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

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

0 引言

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

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

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

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

1 分級(jí)融合航姿估計(jì)算法

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

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

2 磁力計(jì)實(shí)時(shí)校準(zhǔn)

2.1 磁力計(jì)量測(cè)模型

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

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

(1)

尺度因子矩陣為

(2)

磁力計(jì)測(cè)量模型的非正交變換矩陣為Kd，磁力計(jì)測(cè)量模型的三維變換矩陣為

(3)

Mm=KMd+Mb

(4)

Md=K-1(Mm-Mb)

(5)

2.2 LM非線性最小二乘模型

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

(6)

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

(7)

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

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

Mb——理想磁力計(jì)輸出矢量

Me——地磁場(chǎng)矢量

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

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

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

(8)

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

(9)

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

(10)

(11)

其中

(12)

(13)

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

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

LM算法初始化：

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

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

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

LM算法迭代更新步驟：

(1) 計(jì)算Jacobian矩陣

(14)

式中Mmk——kTs時(shí)刻傳感器測(cè)量值

Ts——迭代周期

(2) 計(jì)算Hessian近似矩陣

(15)

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

μk=Sgyroμk-1

(16)

其中

(17)

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

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

(4) 計(jì)算迭代步長(zhǎng)

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ù)地磁場(chǎng)模長(zhǎng)不變的特點(diǎn)，實(shí)時(shí)求解磁力計(jì)校準(zhǔn)參數(shù)，減小了磁力計(jì)的測(cè)量噪聲，增強(qiáng)了磁力計(jì)數(shù)據(jù)的抗干擾能力。

3 基于UKF的一級(jí)航姿估計(jì)

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

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

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

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

陀螺儀零偏具有低頻特性，隨時(shí)間緩慢波動(dòng)，用一階馬爾可夫過(guò)程建模

(25)

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

(26)

3.2 重力加速度觀測(cè)方程

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

yk=h(xk)+vk

(27)

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

yk——量測(cè)信息vk——量測(cè)噪聲

加速度計(jì)測(cè)量模型為

(28)

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

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

ab——加速度計(jì)三軸零偏

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

(29)

故觀測(cè)方程為

(30)

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

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

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

(31)

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

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

4 基于UKF的二級(jí)航姿修正

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

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

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

(32)

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

ψ——航向角

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

(33)

4.2 磁力計(jì)觀測(cè)方程

(34)

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

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

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

(35)

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

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

(36)

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

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

(37)

式中ψ1——第1級(jí)融合航向角

ψ2——第2級(jí)融合航向角

5 試驗(yàn)平臺(tái)與試驗(yàn)方法

5.1 硬件平臺(tái)

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

圖2 航姿測(cè)量系統(tǒng)實(shí)物圖Fig.2 Heading and attitude measurement system1.RM3100型磁力計(jì) 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)系。

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

圖3 直線型植保無(wú)人機(jī)試驗(yàn)平臺(tái)Fig.3 Linear plant protection UAV test platform

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

5.2 試驗(yàn)設(shè)計(jì)與結(jié)果分析

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

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

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

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

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

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

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

圖6 磁場(chǎng)干擾試驗(yàn)磁力計(jì)校準(zhǔn)對(duì)比Fig.6 Calibration comparison diagram of magnetometer in magnetic field interference test

圖7 磁場(chǎng)干擾試驗(yàn)誤差對(duì)比Fig.7 Comparison diagram of magnetic field interference test error

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

表2 磁場(chǎng)干擾試驗(yàn)結(jié)果Tab.2 Magnetic field interference test results μT

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

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

圖8 估計(jì)對(duì)比局部圖Fig.8 Estimation and comparison local map

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

圖9 飛行試驗(yàn)磁力計(jì)模長(zhǎng)誤差對(duì)比曲線Fig.9 Comparison diagram of mode length error of flight test magnetometer

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

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

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

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

6 結(jié)論

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

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

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

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