本發(fā)明涉及測(cè)繪領(lǐng)域，具體涉及一種多軸無人機(jī)高度更新方法。
背景技術(shù)：
MEMS(Micro-Electro-Mechanical System，微機(jī)電系統(tǒng))加速度計(jì)和氣壓計(jì)是可以測(cè)量無人機(jī)相對(duì)高度的器件。加速度計(jì)和氣壓計(jì)測(cè)量高度，優(yōu)缺點(diǎn)共存：加速度計(jì)存在長(zhǎng)時(shí)間積分，高度短期測(cè)量準(zhǔn)確(忽略噪聲干擾)，長(zhǎng)期測(cè)量不精確；氣壓計(jì)精度達(dá)到10cm(氣壓計(jì)輸出的數(shù)據(jù)直接轉(zhuǎn)化為高度的話會(huì)有幾十厘米的浮動(dòng)，直接用于高度控制極不穩(wěn)定)短期測(cè)量不精確，長(zhǎng)期測(cè)量精確。此外，無人機(jī)由于電池電量過高，會(huì)出現(xiàn)定高上升速度過高，出現(xiàn)炸機(jī)，因此也要解決定高上升速度過快問題。
技術(shù)實(shí)現(xiàn)要素：
本發(fā)明提供了一種多軸無人機(jī)高度更新方法，解決加速度計(jì)存在長(zhǎng)時(shí)間積分，高度短期測(cè)量準(zhǔn)確(忽略噪聲干擾)，長(zhǎng)期測(cè)量不精確；氣壓計(jì)精度達(dá)到10cm(氣壓計(jì)輸出的數(shù)據(jù)直接轉(zhuǎn)化為高度的話會(huì)有幾十厘米的浮動(dòng)，直接用于高度控制極不穩(wěn)定)，實(shí)現(xiàn)了長(zhǎng)期測(cè)量更加精確。
本發(fā)明的技術(shù)方案為：
一種多軸無人機(jī)高度更新方法，包括如下步驟：
步驟S1：判斷無人機(jī)的氣壓計(jì)數(shù)據(jù)是否更新，如果更新，執(zhí)行S2；如果沒有更新，執(zhí)行S3；
步驟S2:通過氣壓計(jì)獲取當(dāng)前飛行高度，進(jìn)行高度更新，之后進(jìn)行當(dāng)前氣壓計(jì)修正系數(shù)計(jì)算；
步驟S3：判斷加速度計(jì)數(shù)據(jù)是否更新，如果更新，執(zhí)行S4；如果沒有更新，執(zhí)行S6；
步驟S4：將機(jī)體坐標(biāo)系修正后的加速度轉(zhuǎn)換為地理坐標(biāo)系下的加速度；
步驟S5：加速度偏移向量校正；偏移向量轉(zhuǎn)移到機(jī)體坐標(biāo)系；
步驟S6：加速度推算高度和速度信息；利用氣壓計(jì)校準(zhǔn)系數(shù)進(jìn)行修正高度和速度信息；
步驟S7：返回執(zhí)行S1判斷氣壓計(jì)數(shù)據(jù)是否更新。
上述的多軸無人機(jī)高度更新方法，其中，所述無人機(jī)為四軸無人機(jī)。
上述的多軸無人機(jī)高度更新方法，其中，在步驟S2中，通過氣壓計(jì)獲取當(dāng)前飛行高度的公式為：
公式一中，Hb、pb和Tb分別為相應(yīng)大氣層的重力勢(shì)高度、大氣壓力以及大氣層大氣溫度的下限值；β為溫度垂直變化率；R為空氣氣體常數(shù)；gn為自由落體加速度；ph為當(dāng)前所測(cè)得的大氣靜壓。
將起飛點(diǎn)時(shí)氣壓計(jì)所測(cè)得的高度作為基準(zhǔn)高度，與當(dāng)前氣壓高度計(jì)測(cè)量值做差，得到定位高度h測(cè)高方程公式：
hat＝h+b+ε1 公式二；
公式二中，h為無人機(jī)的真實(shí)定位高度，b為環(huán)境影響所造成的誤差，ε1為量測(cè)噪聲；
當(dāng)前氣壓計(jì)修正系數(shù)計(jì)算方法：
corr_bar＝-H-z_est0 公式3；
式中，corr_bar為氣壓計(jì)修正系數(shù)，z_est0為加速度計(jì)Z軸高度。
上述的多軸無人機(jī)高度更新方法，其中，在步驟S5中，加速度偏移向量校正的公式為：
accel_bias＝accel_bias-corr_bar*para*para 公式4；
公式4中，accel_bias為校正后的加速度偏移向量，para為加速度計(jì)Z軸校正系數(shù)；
偏移向量轉(zhuǎn)移到機(jī)體坐標(biāo)系的步驟包括：
將地理坐標(biāo)系利用四元素轉(zhuǎn)換為載體坐標(biāo)系時(shí)，其方向余弦矩陣為
將偏移向量轉(zhuǎn)換到機(jī)體坐標(biāo)系公式：
acc_bias＝acc_bias+Rot_matrix[x][y]*accel_bias*para1*dt 公式6；
公式6中，acc_bias為機(jī)體坐標(biāo)系下的偏移向量，Rot_matrix[x][y]為對(duì)應(yīng)的公式5的旋轉(zhuǎn)矩陣，para1為偏移向量積分修正系數(shù)，dt為積分時(shí)間間隔。
上述的多軸無人機(jī)高度更新方法，其中，在步驟S6中，
設(shè)捷聯(lián)式加速度計(jì)沿?zé)o人機(jī)的縱軸、橫軸和垂直軸方向與飛機(jī)固聯(lián)，加速度計(jì)放置在無人機(jī)的重心處，忽略角運(yùn)動(dòng)對(duì)加速度計(jì)輸出的影響，則垂向加速度計(jì)的量測(cè)方程為：
aacc＝ag+ε3 公式7；
公式7中，aacc為垂向加速度，ag為真實(shí)的垂向加速度，ε3為加速度計(jì)對(duì)高度的量測(cè)噪聲；
飛機(jī)相對(duì)于起飛地面的飛行高度hacc、垂向速度vacc和垂向加速度aacc之間的關(guān)系表示為：
本發(fā)明將加速度計(jì)短期測(cè)量精確與氣壓計(jì)長(zhǎng)期測(cè)量不精確的特點(diǎn)結(jié)合在一起，保證了無人機(jī)上升過程中高度長(zhǎng)期的精確穩(wěn)定性。
附圖說明
通過閱讀參照以下附圖對(duì)非限制性實(shí)施例所作的詳細(xì)描述，本發(fā)明及其特征、外形和優(yōu)點(diǎn)將會(huì)變得更明顯。在全部附圖中相同的標(biāo)記指示相同的部分。并未刻意按照比例繪制附圖，重點(diǎn)在于示出本發(fā)明的主旨。
圖1為本發(fā)明提供的一種多軸無人機(jī)高度更新方法的流程圖。
具體實(shí)施方式
在下文的描述中，給出了大量具體的細(xì)節(jié)以便提供對(duì)本發(fā)明更為徹底的理解。然而，對(duì)于本領(lǐng)域技術(shù)人員而言顯而易見的是，本發(fā)明可以無需一個(gè)或多個(gè)這些細(xì)節(jié)而得以實(shí)施。在其他的例子中，為了避免與本發(fā)明發(fā)生混淆，對(duì)于本領(lǐng)域公知的一些技術(shù)特征未進(jìn)行描述。
為了徹底理解本發(fā)明，將在下列的描述中提出詳細(xì)的步驟以及詳細(xì)的結(jié)構(gòu)，以便闡釋本發(fā)明的技術(shù)方案。本發(fā)明的較佳實(shí)施例詳細(xì)描述如下，然而除了這些詳細(xì)描述外，本發(fā)明還可以具有其他實(shí)施方式。
針對(duì)現(xiàn)有技術(shù)中無人機(jī)定高存在的問題，本發(fā)明提供了提高小四軸定高精度方法。
本發(fā)明技術(shù)方案為，利用互補(bǔ)濾波算法，可以實(shí)現(xiàn)加速度計(jì)和氣壓計(jì)的優(yōu)勢(shì)結(jié)合，實(shí)現(xiàn)無人機(jī)定高長(zhǎng)時(shí)間穩(wěn)定性和準(zhǔn)確性；根據(jù)電池電量，改變無人機(jī)上升的速度，保證上升的穩(wěn)定性。
在無人機(jī)定高中，所說的高度是地理坐標(biāo)系下的相對(duì)高度，所獲取的加速度計(jì)Z軸加速度信息是相對(duì)于當(dāng)前機(jī)體坐標(biāo)系下的，因此需要進(jìn)行機(jī)體坐標(biāo)系和地理坐標(biāo)系加速度的實(shí)時(shí)轉(zhuǎn)換，下面簡(jiǎn)單的介紹進(jìn)行高度求解的方法：
在定高過程中，需要的速度和位置信息是由地理坐標(biāo)系下的加速度通過一次積分，二次積分獲得，而地理坐標(biāo)系下的加速度是通過機(jī)體坐標(biāo)系測(cè)量的加速度減去機(jī)體偏移向量，再轉(zhuǎn)換到地理坐標(biāo)系求得的。在加速度計(jì)和氣壓計(jì)數(shù)據(jù)融合的過程中，氣壓計(jì)在數(shù)據(jù)更新時(shí)，計(jì)算出加速度偏移量校正系數(shù)，對(duì)地理坐標(biāo)系下的加速度偏移量進(jìn)行修正。
由于加速度計(jì)XY軸所在平面與水平面上存在傾斜角度，因此Z軸輸出的數(shù)據(jù)是重力在Z軸當(dāng)前角度的一個(gè)分量，是不準(zhǔn)確的，所以需要對(duì)Z軸的加速度計(jì)進(jìn)行傾角補(bǔ)償，這樣得到的才是垂直方向上的當(dāng)前重力。
本發(fā)明提供了一種多軸無人機(jī)高度更新方法，參照?qǐng)D1所示，包括如下步驟：
步驟S1：判斷無人機(jī)的氣壓計(jì)數(shù)據(jù)是否更新，如果更新，執(zhí)行S2；如果沒有更新，執(zhí)行S3。
步驟S2：通過氣壓計(jì)獲取當(dāng)前飛行高度，進(jìn)行高度更新，之后進(jìn)行當(dāng)前氣壓計(jì)修正系數(shù)計(jì)算。具體的，通過氣壓計(jì)獲取當(dāng)前飛行高度的公式為：
公式一中，Hb、pb和Tb分別為相應(yīng)大氣層的重力勢(shì)高度、大氣壓力以及大氣層大氣溫度的下限值；β為溫度垂直變化率；R為空氣氣體常數(shù)；gn為自由落體加速度；ph為當(dāng)前所測(cè)得的大氣靜壓。
將起飛點(diǎn)時(shí)氣壓計(jì)所測(cè)得的高度作為基準(zhǔn)高度，與當(dāng)前氣壓高度計(jì)測(cè)量值做差，得到定位高度h測(cè)高方程公式：
hat＝h+b+ε1 公式二；
公式二中，h為無人機(jī)的真實(shí)定位高度，b為環(huán)境影響所造成的誤差，ε1為量測(cè)噪聲；
當(dāng)前氣壓計(jì)修正系數(shù)計(jì)算方法：
corr_bar＝-H-z_est0 公式3；
公式3中，corr_bar為氣壓計(jì)修正系數(shù)，z_est0為加速度計(jì)Z軸高度。
步驟S3：判斷加速度計(jì)數(shù)據(jù)是否更新，如果更新，執(zhí)行S4；如果沒有更新，執(zhí)行S6。
步驟S4：將機(jī)體坐標(biāo)系修正后的加速度轉(zhuǎn)換為地理坐標(biāo)系下的加速度。
步驟S5：加速度偏移向量校正；偏移向量轉(zhuǎn)移到機(jī)體坐標(biāo)系。具體的，加速度偏移向量校正的公式為：
accel_bias＝accel_bias-corr_bar*para*para 公式4；
公式4中，accel_bias為校正后的加速度偏移向量，para為加速度計(jì)Z軸校正系數(shù)；
偏移向量轉(zhuǎn)移到機(jī)體坐標(biāo)系的步驟包括：
將地理坐標(biāo)系利用四元素轉(zhuǎn)換為載體坐標(biāo)系時(shí)，其方向余弦矩陣為
將偏移向量轉(zhuǎn)換到機(jī)體坐標(biāo)系公式：
acc_bias＝acc_bias+Rot_matrix[x][y]*accel_bias*para1*dt 公式6；
公式6中，acc_bias為機(jī)體坐標(biāo)系下的偏移向量，Rot_matrix[x][y]為對(duì)應(yīng)的公式5的旋轉(zhuǎn)矩陣，para1為偏移向量積分修正系數(shù)，dt為積分時(shí)間間隔。
步驟S6：加速度推算高度和速度信息；利用氣壓計(jì)校準(zhǔn)系數(shù)進(jìn)行修正高度和速度信息。具體的，設(shè)捷聯(lián)式加速度計(jì)沿?zé)o人機(jī)的縱軸、橫軸和垂直軸方向與飛機(jī)固聯(lián)，加速度計(jì)放置在無人機(jī)的重心處，忽略角運(yùn)動(dòng)對(duì)加速度計(jì)輸出的影響，則垂向加速度計(jì)的量測(cè)方程為：
aacc＝ag+ε3 公式7；
公式7中，aacc為垂向加速度，ag為真實(shí)的垂向加速度，ε3為加速度計(jì)對(duì)高度的量測(cè)噪聲；
飛機(jī)相對(duì)于起飛地面的飛行高度hacc、垂向速度vacc和垂向加速度aacc之間的關(guān)系表示為：
步驟S7：返回執(zhí)行S1判斷氣壓計(jì)數(shù)據(jù)是否更新。
本發(fā)明利用互補(bǔ)濾波算法，可以實(shí)現(xiàn)加速度計(jì)和氣壓計(jì)的優(yōu)勢(shì)結(jié)合，實(shí)現(xiàn)無人機(jī)定高長(zhǎng)時(shí)間穩(wěn)定性和準(zhǔn)確性；根據(jù)電池電量，改變無人機(jī)上升的速度，保證上升的穩(wěn)定性。
以上對(duì)本發(fā)明的較佳實(shí)施例進(jìn)行了描述。需要理解的是，本發(fā)明并不局限于上述特定實(shí)施方式，其中未盡詳細(xì)描述的設(shè)備和結(jié)構(gòu)應(yīng)該理解為用本領(lǐng)域中的普通方式予以實(shí)施；任何熟悉本領(lǐng)域的技術(shù)人員，在不脫離本發(fā)明技術(shù)方案范圍情況下，都可利用上述揭示的方法和技術(shù)內(nèi)容對(duì)本發(fā)明技術(shù)方案做出許多可能的變動(dòng)和修飾，或修改為等同變化的等效實(shí)施例，這并不影響本發(fā)明的實(shí)質(zhì)內(nèi)容。因此，凡是未脫離本發(fā)明技術(shù)方案的內(nèi)容，依據(jù)本發(fā)明的技術(shù)實(shí)質(zhì)對(duì)以上實(shí)施例所做的任何簡(jiǎn)單修改、等同變化及修飾，均仍屬于本發(fā)明技術(shù)方案保護(hù)的范圍內(nèi)。