直線型植保無人機(jī)航姿UKF兩級估計算法

沈 躍 孫志偉 沈亞運(yùn) 張大海 錢 鵬 劉 慧

(1.江蘇大學(xué)電氣信息工程學(xué)院， 鎮(zhèn)江 212013； 2.浙江大學(xué)海洋學(xué)院， 舟山 310013)

0 引言

植保無人機(jī)是近年來新興的植保機(jī)械，適應(yīng)我國多樣性和分散性種植地形[1-2]。直線型植保無人機(jī)，相對于常見多軸旋翼植保無人機(jī)，其物理噴幅寬，利于提高作業(yè)效率。但直線型無人機(jī)由于多電機(jī)呈直線式緊密分布，飛行控制器與電機(jī)間距較小，易受電機(jī)旋轉(zhuǎn)產(chǎn)生的磁場干擾，導(dǎo)致航向角估計精度低，同時采用高精度RTK慣導(dǎo)系統(tǒng)會增加無人機(jī)飛行質(zhì)量與制造成本[3]。無人機(jī)多傳感器精確校準(zhǔn)和融合算法，是保證航姿估計準(zhǔn)確性的必要條件。

無人機(jī)傳感器校準(zhǔn)主要包含對加速度計、陀螺儀和磁力計的校準(zhǔn)，其中加速度計和陀螺儀的校準(zhǔn)方法較為成熟[4]，而磁力計容易受電機(jī)感應(yīng)磁場干擾。磁力計校準(zhǔn)可分為離線校準(zhǔn)和實時校準(zhǔn)。孫宏偉等[5]采用帶約束的最小二乘法，通過旋轉(zhuǎn)估計橢球參數(shù)。WU等[6]利用LM算法先進(jìn)行球面擬合，再進(jìn)行橢球擬合計算出靜態(tài)補(bǔ)償參數(shù)，但對大型無人機(jī)校準(zhǔn)操作繁瑣、動態(tài)性能差。蔡浩原等[7]提出一種基于擴(kuò)展卡爾曼濾波器(Extended Kalman filter，EKF)的動態(tài)校準(zhǔn)方法，但陀螺儀偏置影響校準(zhǔn)參數(shù)的估計精度。SLIC等[8]建立了基于單電機(jī)油門指令的磁力計模型，但無法適用多電機(jī)分布的無人機(jī)。目前無人機(jī)磁力計的校準(zhǔn)算法靜態(tài)精度較高，但是無法快速補(bǔ)償無人機(jī)機(jī)動飛行中時變的電磁場干擾，因此高精度的實時磁力計校準(zhǔn)方法還有待進(jìn)一步研究。

無人機(jī)航姿估計的多傳感器融合算法主要有互補(bǔ)濾波[9-10]、梯度下降法[11]和擴(kuò)展卡爾曼濾波[12-14]等。張勇剛等[15]用互補(bǔ)濾波器融合加速度計、陀螺儀估計航姿，抑制了陀螺儀的積分漂移，但磁場干擾直接影響姿態(tài)角估計。彭孝東等[16]采用梯度下降法，在無人機(jī)運(yùn)動加速度較小時精度好。蔡安江等[17]推測四元數(shù)與歐拉角一一對應(yīng)相關(guān)，但該算法在機(jī)動環(huán)境下，相關(guān)性會降低，導(dǎo)致姿態(tài)估計準(zhǔn)確性降低。盧艷軍等[18]基于EKF分別估計姿態(tài)與航向信息，降低了磁場干擾對航向角的影響，但未考慮磁傾角以及軟磁場變化。

針對上述問題，結(jié)合直線型植保無人機(jī)磁場干擾變化快且模值大的特點(diǎn)，本文提出一種應(yīng)用UKF實現(xiàn)一級融合陀螺儀和加速度計估計姿態(tài)、二級融合陀螺儀和和實時校準(zhǔn)的磁力計數(shù)據(jù)求解航向角，進(jìn)而校正一級四元數(shù)的航姿估計算法，大幅減弱磁場干擾對俯仰角和橫滾角的影響，提高航向角解算精度。

1 分級融合航姿估計算法

圖1 分級融合航姿估計算法流程圖Fig.1 Flowchart of hierarchical fusion attitude estimation algorithm

UKF算法流程簡述如下：對狀態(tài)量進(jìn)行無跡變換(Unscented transform，UT)獲得Sigma點(diǎn)集，通過狀態(tài)方程計算出點(diǎn)集變換后數(shù)據(jù)，從而獲得狀態(tài)的預(yù)測均值及其協(xié)方差矩陣；對狀態(tài)預(yù)測進(jìn)行UT變換，獲得Sigma點(diǎn)集，通過量測方程計算出點(diǎn)集變換后數(shù)據(jù)，從而量測的預(yù)測均值及其協(xié)方差矩陣；計算卡爾曼增益，更新狀態(tài)及其協(xié)方差[19]。

2 磁力計實時校準(zhǔn)

2.1 磁力計量測模型

無人機(jī)通常需要考慮地磁場和空間磁場對航姿測量的影響[20-21]。地磁場是航向角估計的重要觀測信息，磁力計是測量空間磁場矢量的傳感器[22]。但磁力計測量易產(chǎn)生誤差：一是生產(chǎn)工藝限制導(dǎo)致的儀表誤差，二是外部磁場干擾造成的誤差。

磁力計儀表誤差包括零偏誤差和尺度因子誤差、非正交誤差等[23-24];外部磁場干擾主要包括無刷電機(jī)的永磁體、載流導(dǎo)線引起的磁力計偏置和機(jī)身金屬連接件導(dǎo)致的三軸非正交誤差等。對2種誤差建立磁力計測量模型，其零位偏移為

(1)

尺度因子矩陣為

(2)

磁力計測量模型的非正交變換矩陣為Kd，磁力計測量模型的三維變換矩陣為

(3)

Mm=KMd+Mb

(4)

Md=K-1(Mm-Mb)

(5)

2.2 LM非線性最小二乘模型

地磁場強(qiáng)度為50～60 μT，植保無人機(jī)在某一范圍內(nèi)作業(yè)時，地磁矢量變化很小，可視為定值，假定地磁場模長為常數(shù)，即

(6)

在僅考慮地磁場的理想環(huán)境下

(7)

式中b——機(jī)體坐標(biāo)系(b系)

e——當(dāng)?shù)氐乩碜鴺?biāo)系(e系)

Mb——理想磁力計輸出矢量

Me——地磁場矢量

無人機(jī)在空中運(yùn)動時，磁力計數(shù)據(jù)點(diǎn)形成一個各軸等長的球面。但在實際環(huán)境中，由式(5)可知，球體三軸產(chǎn)生旋轉(zhuǎn)且非正交、各軸長度不等，導(dǎo)致球心偏離坐標(biāo)原點(diǎn)形成橢球面。

經(jīng)校準(zhǔn)后，磁力計模長為

‖Md‖2=(K-1(Mm-Mb))T(K-1(Mm-Mb))

(8)

將K-1轉(zhuǎn)換為

(9)

并構(gòu)建代價函數(shù)L，通過非線性最小二乘擬合校準(zhǔn)參數(shù)得

(10)

(11)

其中

(12)

(13)

式中xcali——需要校準(zhǔn)的參數(shù)

應(yīng)用列文伯格-馬夸特(Levenberg-Marquardt，LM)算法[25]求解使代價函數(shù)接近于零的參數(shù)。

LM算法初始化：

校準(zhǔn)參數(shù)向量xcali的初始值xcali|0=[1 0 0 1 0 1mbx0mby0mbz0]，其中[mbx0mby0mbz0]T為在空曠處獲得的機(jī)載硬磁零偏，加快算法收斂速度。

地磁場模長的參考值是利用WMM2020世界地磁模型[26]和經(jīng)緯度計算得到。

令阻尼系數(shù)初始值μ0=2，阻尼縮放系數(shù)α=2，迭代次數(shù)初始值k=1。

LM算法迭代更新步驟：

(1) 計算Jacobian矩陣

(14)

式中Mmk——kTs時刻傳感器測量值

Ts——迭代周期

(2) 計算Hessian近似矩陣

(15)

(3) 更新阻尼系數(shù)μk

μk=Sgyroμk-1

(16)

其中

(17)

式中Sgyro——阻尼系數(shù)動態(tài)調(diào)整因子

‖wgyro‖——陀螺儀矢量模值

(4) 計算迭代步長

dk=-(J(xcali|k-1,Mmk)TJ(xcali|k-1,Mmk)+μkI)-1·
J(xcali|k-1,Mmk)TL(xcali|k-1,Mmk)

(18)

式中I——9維單位矩陣

(5) 確定搜索方向

(6) 輸出迭代參數(shù)

xcali|k=xcali|k-1+dk

(19)

利用LM算法依據(jù)地磁場模長不變的特點(diǎn)，實時求解磁力計校準(zhǔn)參數(shù)，減小了磁力計的測量噪聲，增強(qiáng)了磁力計數(shù)據(jù)的抗干擾能力。

3 基于UKF的一級航姿估計

無人機(jī)航姿變化主要依靠對橫滾角、俯仰角和航向角的調(diào)節(jié)[27]。橫滾角和俯仰角的姿態(tài)估計依賴加速度計與陀螺儀融合，加速度計可以實時補(bǔ)償陀螺儀零偏，但是由于重力加速度垂直于水平面，所以無法補(bǔ)償航向角漂移，需要融合磁力計數(shù)據(jù)進(jìn)行補(bǔ)償。下面建立基于UKF的四元數(shù)估計算法模型。

3.1 航姿狀態(tài)方程

建立姿態(tài)預(yù)測狀態(tài)方程

新課標(biāo)指出，在小學(xué)語文教學(xué)過程中應(yīng)該對學(xué)生各種能力的培養(yǎng)提出更高的重視，而閱讀能力作為一種重要的基礎(chǔ)能力，教師更應(yīng)該給予重視。在實際的教學(xué)過程中，教師要重視學(xué)生閱讀能力的培養(yǎng)，創(chuàng)建有趣的情境激發(fā)學(xué)生的閱讀興趣，面對小學(xué)生，情境教學(xué)是一種非常有效的手段，具備特殊的應(yīng)用價值，尤其值得在小學(xué)語文教學(xué)中推廣和運(yùn)用。

xk=f(xk-1)+wk-1

(20)

式中f(xk-1)——狀態(tài)連續(xù)的非線性向量函數(shù)

xk——系統(tǒng)狀態(tài)向量

wk-1——系統(tǒng)噪聲

四元數(shù)微分方程為

(21)

式(21)矩陣形式為

(22)

式(22)進(jìn)行歐拉積分將四元數(shù)更新，得

(23)

(24)

陀螺儀零偏具有低頻特性，隨時間緩慢波動，用一階馬爾可夫過程建模

(25)

式中τ——相關(guān)時間系數(shù)

(26)

3.2 重力加速度觀測方程

建立姿態(tài)的觀測方程

yk=h(xk)+vk

(27)

式中h(xk)——狀態(tài)連續(xù)的非線性向量函數(shù)

yk——量測信息vk——量測噪聲

加速度計測量模型為

(28)

gb——機(jī)體重力加速度

ma——機(jī)體運(yùn)動加速度

ab——加速度計三軸零偏

觀測量選取機(jī)體坐標(biāo)系下重力加速度的比力，將比力旋轉(zhuǎn)至機(jī)體坐標(biāo)系得

(29)

故觀測方程為

(30)

3.3 觀測噪聲協(xié)方差自適應(yīng)調(diào)節(jié)

在微機(jī)動或靜止?fàn)顟B(tài)下，加速度計通過測量重力加速度，可以準(zhǔn)確補(bǔ)償陀螺儀零偏，但是載體機(jī)動時存在線加速度，從式(28)可得到加速度計測量值不僅是重力加速度，而是重力加速度和運(yùn)動加速度的矢量和。而陀螺儀受運(yùn)動加速度影響較小，在大機(jī)動的場景下，觀測量將偏離預(yù)設(shè)模型，根據(jù)加速度計的測量數(shù)據(jù)模值，自適應(yīng)調(diào)節(jié)測量噪聲方差矩陣。

使用分段函數(shù)描述測量噪聲變化

(31)

式中g(shù)——重力加速度模值

磁力計測量噪聲自適應(yīng)調(diào)節(jié)與加速度計類似，以地磁場強(qiáng)度為基準(zhǔn)。

4 基于UKF的二級航姿修正

第2級融合是利用磁力計數(shù)據(jù)對上一級融合的航向角進(jìn)行修正，同時避免磁傾角誤差、磁場干擾等因素影響橫滾角和俯仰角數(shù)據(jù)。

4.1 航向角狀態(tài)方程

通過歐拉角微分方程，可得航向角與三軸角速度的關(guān)系為

(32)

式中φ——橫滾角θ——俯仰角

ψ——航向角

選取航向角為狀態(tài)量，故航向角狀態(tài)預(yù)測方程為

(33)

4.2 磁力計觀測方程

(34)

式中Ry(θ)——繞y軸旋轉(zhuǎn)矩陣

Rx(φ)——繞x軸旋轉(zhuǎn)矩陣

忽略z軸數(shù)據(jù)，獲得磁力計在水平面投影，歸一化后作為航向角觀測量。采用東北天坐標(biāo)系，故地磁水平方向為[0 1 0]T，航向角觀測方程為

(35)

4.3 航向角誤差四元數(shù)

四元數(shù)的角軸表示法為

(36)

式中ε——旋轉(zhuǎn)角u——空間的旋轉(zhuǎn)軸

Qe=(cos(ψ2-ψ1),0,0,sin(ψ2-ψ1))T

(37)

式中ψ1——第1級融合航向角

ψ2——第2級融合航向角

5 試驗平臺與試驗方法

5.1 硬件平臺

直線型植保無人機(jī)航姿測量系統(tǒng)(圖2)主要由核心處理器、六軸微慣性器件和三軸磁力計構(gòu)成。其中，核心處理器選用ST公司的STM32F405RGT6芯片。該芯片處理器為Cortex M4內(nèi)核，最高處理頻率168 MHz，可滿足直線型植保無人機(jī)航姿估計要求。六軸微慣性傳感器選用InvenSense公司的ICM20602型微慣性測量芯片，包含三軸陀螺儀和三軸加速度計。磁力計采用PNI傳感器公司的RM3100型電子羅盤傳感器。微慣性傳感器的采樣頻率設(shè)置為500 Hz，磁力計的采樣頻率為200 Hz，保證無人機(jī)在定點(diǎn)懸停、高速飛行作業(yè)等復(fù)雜工況下航姿的準(zhǔn)確測量與磁力計實時校準(zhǔn)。

圖2 航姿測量系統(tǒng)實物圖Fig.2 Heading and attitude measurement system1.RM3100型磁力計 2.STM32F405RGT6單片機(jī) 3.ICM20602型六軸微慣性傳感器

機(jī)體坐標(biāo)系的坐標(biāo)原點(diǎn)位于六軸微慣性器件的中心處，Xb軸沿機(jī)體縱軸指向前，Yb軸沿機(jī)體橫軸指向左，Zb軸垂直于OXbYb平面沿機(jī)體豎軸向上; 導(dǎo)航坐標(biāo)系OXnYnZn選用地理坐標(biāo)系，即東北天坐標(biāo)系。

為驗證航姿測量系統(tǒng)的精確性與系統(tǒng)的適用性，本文采用MTi-680G型 GNSS/慣性導(dǎo)航系統(tǒng)(INS)模塊數(shù)據(jù)作為基準(zhǔn)，將測量系統(tǒng)的硬件單元搭載在自主設(shè)計的直線型植保無人機(jī)(圖3)進(jìn)行試驗。MTi-680G型模塊集成GNSS/慣性導(dǎo)航系統(tǒng)(INS)，配備RTK GNSS接收器，可提供同步的3D姿態(tài)和航向輸出。MTi-680G型姿態(tài)測量精度0.2°，航向角測量精度1.0°。

圖3 直線型植保無人機(jī)試驗平臺Fig.3 Linear plant protection UAV test platform

圖3所示的直線型植保無人機(jī)試驗平臺，主升電機(jī)為4個颶風(fēng)U3515型電機(jī)，采用豎直桿為姿態(tài)調(diào)整桿，姿態(tài)調(diào)整桿上為2個對稱的TMOTOR乘風(fēng)2207型電機(jī)，通過串級控制完成飛行任務(wù)。

5.2 試驗設(shè)計與結(jié)果分析

植保無人機(jī)作業(yè)時，一般不會出現(xiàn)大幅度機(jī)動，因此本文進(jìn)行直線型植保無人機(jī)小幅度運(yùn)動模式的磁力計校準(zhǔn)試驗。選取周圍3 m內(nèi)無明顯磁場干擾源的空曠場地，人為旋轉(zhuǎn)圖2所示的航姿測量系統(tǒng)，以測量模塊小幅度運(yùn)動(磁場矢量在單位球面一個小區(qū)域內(nèi)運(yùn)動，俯仰角、橫滾角和航向角運(yùn)動范圍均小于90°)的數(shù)據(jù)。試驗共采集7 000個數(shù)據(jù)點(diǎn)，并對旋轉(zhuǎn)過程中LM實時校準(zhǔn)算法、LS橢球擬合法2種方法的局部采樣曲線和局部采樣誤差曲線進(jìn)行對比分析。圖4為試驗過程中2種算法在磁場球面的軌跡曲線；圖5為2種算法校準(zhǔn)過程中產(chǎn)生的誤差曲線。

圖4 局部采樣磁力計校準(zhǔn)對比Fig.4 Comparison diagram of local sampling magnetometer calibration

圖5 局部采樣磁力計校準(zhǔn)誤差對比Fig.5 Comparison diagram of calibration error of local sampling magnetometer

由圖4、5可知，LS橢球擬合法在磁力計原始數(shù)據(jù)集中在小區(qū)域內(nèi)時，校準(zhǔn)后數(shù)據(jù)多數(shù)偏離理想球面(理想磁場矢量活動面)，校準(zhǔn)數(shù)據(jù)發(fā)散嚴(yán)重，校準(zhǔn)效果較差；而本文提出的LM實時校準(zhǔn)算法在相同情況下，校準(zhǔn)后數(shù)據(jù)與原始數(shù)據(jù)區(qū)域重合率更高、誤差更小，校準(zhǔn)效果更好。

如表1所示，在上述試驗環(huán)節(jié)，LS橢球擬合法最大誤差為236.22 μT、均方根誤差為51.19 μT，說明LS橢球擬合法已經(jīng)發(fā)散，無法完成此場景下的磁力計校準(zhǔn)；本文提出的LM實時校準(zhǔn)法最大誤差為4.50 μT、均方根誤差為0.58 μT，在測量模塊小幅度運(yùn)動時，可以完成磁力計實時校準(zhǔn)并快速收斂。

表1 局部采樣磁場校準(zhǔn)試驗結(jié)果Tab.1 Magnetic field interference test results μT

飛行過程中，電機(jī)旋轉(zhuǎn)會產(chǎn)生磁場干擾，為測試LM實時校準(zhǔn)算法在動態(tài)飛行過程中的魯棒性，將電機(jī)作為干擾源，設(shè)計磁場干擾試驗。選取周圍3 m內(nèi)無明顯磁場干擾源的空曠場地，旋轉(zhuǎn)測量系統(tǒng)。由于旋轉(zhuǎn)前期無法快速滿足LM實時校準(zhǔn)法和LS橢球擬合法收斂條件，因此在旋轉(zhuǎn)測量系統(tǒng)8 s后(俯仰角、橫滾角和航向角運(yùn)動范圍均大于90°)加入磁場干擾源，將電機(jī)在距航姿測量系統(tǒng)15 cm處做鐘擺運(yùn)動，采集20 000個數(shù)據(jù)點(diǎn)，進(jìn)行LM實時校準(zhǔn)算法和LS橢球擬合法的對比試驗。圖6為受電機(jī)磁干擾后兩種算法的校準(zhǔn)曲線；圖7為受電機(jī)磁干擾后兩種算法校準(zhǔn)過程中的誤差曲線。

圖6 磁場干擾試驗磁力計校準(zhǔn)對比Fig.6 Calibration comparison diagram of magnetometer in magnetic field interference test

圖7 磁場干擾試驗誤差對比Fig.7 Comparison diagram of magnetic field interference test error

由圖6、7可知，憑借LS橢球擬合提供的實時校準(zhǔn)算法初始值，LM實時校準(zhǔn)法快速收斂。在8s出現(xiàn)磁場干擾后，LM實時校準(zhǔn)法依舊快速估計出了磁力計校準(zhǔn)參數(shù)。如表2所示，LS橢球擬合法最大誤差為29.33 μT、均方根誤差為3.78 μT；LM實時校準(zhǔn)法最大誤差為6.17 μT、均方根誤差為 0.59 μT，均小于LS橢球擬合法校準(zhǔn)產(chǎn)生的誤差。因此，本文提出的基于LM實時校準(zhǔn)算法的航姿測量系統(tǒng)對于時變的磁場干擾，能夠快速收斂，保持高穩(wěn)定性。

表2 磁場干擾試驗結(jié)果Tab.2 Magnetic field interference test results μT

為進(jìn)一步驗證航姿測量系統(tǒng)在實際飛行作業(yè)過程中的測量精度，于2021年4月在江蘇大學(xué)(32°12′15″N,119°30′43″E)進(jìn)行飛行試驗。以直線型植保無人機(jī)為飛行試驗平臺，并以MTi-680G型輸出數(shù)據(jù)作為航姿測量參考值。圖3為飛行試驗的現(xiàn)場圖，航姿測量系統(tǒng)安裝于直線型植保無人機(jī)機(jī)架中心處，與飛行控制系統(tǒng)在同一平面。無人機(jī)在空中分別完成俯仰、橫滾、偏航等動作，采集航姿變化數(shù)據(jù)，同時采用本文提出的分級融合算法與互補(bǔ)濾波分別進(jìn)行估計，比對航姿解算魯棒性。飛行試驗結(jié)果如圖8所示。

由圖8a和圖8b可知，直線型植保無人機(jī)飛行階段，俯仰角和橫滾角動態(tài)跟隨MTi-680G型姿態(tài)變化波形效果較好，沒有因磁場擾動而導(dǎo)致姿態(tài)估計精度降低。同時，航姿估計算法調(diào)節(jié)了加速度計的測量方差，降低運(yùn)動加速度、機(jī)身振動對姿態(tài)解算的影響，UKF濾波器的姿態(tài)解算精度優(yōu)于互補(bǔ)濾波。

圖8 估計對比局部圖Fig.8 Estimation and comparison local map

直線型植保無人機(jī)飛行時，磁場擾動不斷變化，若按照離線校準(zhǔn)參數(shù)進(jìn)行航向角解算，會因受變化的軟磁場干擾產(chǎn)生誤差。而利用本文提出的LM實時校準(zhǔn)法，可以減小磁場擾動對于航向角的觀測信息的影響，如圖8c和圖9所示。

圖9 飛行試驗磁力計模長誤差對比曲線Fig.9 Comparison diagram of mode length error of flight test magnetometer

由圖8c可知，利用LM法實時校準(zhǔn)磁羅盤數(shù)據(jù)，結(jié)合UKF分級融合算法，能夠有效降低磁場擾動影響，提高航向角估計精度。由圖9可知，在動態(tài)飛行試驗中，磁場擾動不斷變化，LS橢球擬合法模長誤差數(shù)據(jù)不穩(wěn)定，而利用LM法實時校準(zhǔn)磁羅盤數(shù)據(jù)，比LS橢球擬合法校準(zhǔn)的數(shù)據(jù)，模長誤差更小。對采用互補(bǔ)濾波和UKF分級融合算法的飛行測試數(shù)據(jù)進(jìn)一步分析，試驗數(shù)據(jù)如表3、4所示。

表3 互補(bǔ)濾波算法飛行測試結(jié)果Tab.3 Flight test results of complementary filter (°)

表4 分級融合算法飛行測試結(jié)果Tab.4 Flight test results of hierarchical fusion algorithm (°)

由表3、4可知，本文研制的基于UKF分級融合算法的直線型植保無人機(jī)航姿測量系統(tǒng)姿態(tài)角均方根誤差不大于0.75°、航向角均方根誤差為1.40°，與互補(bǔ)濾波算法相比，橫滾角與俯仰角精度提高了約0.6°、航向角精度提高1.38°，并且航向角最大誤差也減小了2.98°。

6 結(jié)論

(1)針對直線型植保無人機(jī)飛行過程中，磁場干擾影響航姿解算精度的問題，設(shè)計了一種基于磁力計實時校準(zhǔn)的航姿測量系統(tǒng)。利用地磁場矢量變化緩慢的特點(diǎn)，使用 LM法實時計算磁力計校準(zhǔn)參數(shù)；采用自適應(yīng)無跡卡爾曼濾波器，結(jié)合六軸微慣性和三軸磁力器件設(shè)計了低成本硬件方案。

(2)采用電機(jī)作為干擾源，旋轉(zhuǎn)航姿測量模塊進(jìn)行試驗。試驗結(jié)果表明，在外部磁場干擾高達(dá)30.97 μT時，實時校準(zhǔn)算法依然可以快速計算出磁力計校準(zhǔn)參數(shù)，模長均方根誤差為0.59 μT，最大誤差為6.17 μT，提高了航向角觀測信息精度。

(3)以自主設(shè)計的直線型植保無人機(jī)為飛行平臺，采用無跡卡爾曼濾波器融合陀螺儀和加速度計數(shù)據(jù)估計姿態(tài)，利用實時校準(zhǔn)后磁力計數(shù)據(jù)與角速度信息融合修正航向角，增強(qiáng)了航向跟蹤能力。飛行試驗中，本文研制的直線型植保無人機(jī)航姿測量系統(tǒng)與互補(bǔ)濾波算法相比，橫滾角與俯仰角精度提高0.6°、航向角精度提高1.38°。算法有效抑制了磁力計測量中的干擾，并且干擾沒有耦合到姿態(tài)估計，保證了飛機(jī)的穩(wěn)定飛行，提高了航向角估計精度。

(4)設(shè)計的直線型植保無人機(jī)航姿測量系統(tǒng)仍需進(jìn)行飛行植保作業(yè)進(jìn)一步驗證，并根據(jù)飛行數(shù)據(jù)進(jìn)一步探索融合算法優(yōu)化方向。