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

沈 躍 孫志偉 沈亞運 張大海 錢 鵬 劉 慧

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

0 引言

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

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

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

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

1 分級融合航姿估計算法

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

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

2 磁力計實時校準

2.1 磁力計量測模型

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

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

(1)

尺度因子矩陣為

(2)

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

(3)

Mm=KMd+Mb

(4)

Md=K-1(Mm-Mb)

(5)

2.2 LM非線性最小二乘模型

地磁場強度為50～60 μT，植保無人機在某一范圍內作業時，地磁矢量變化很小，可視為定值，假定地磁場模長為常數，即

(6)

在僅考慮地磁場的理想環境下

(7)

式中b——機體坐標系(b系)

e——當地地理坐標系(e系)

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

Me——地磁場矢量

無人機在空中運動時，磁力計數據點形成一個各軸等長的球面。但在實際環境中，由式(5)可知，球體三軸產生旋轉且非正交、各軸長度不等，導致球心偏離坐標原點形成橢球面。

經校準后，磁力計模長為

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

(8)

將K-1轉換為

(9)

并構建代價函數L，通過非線性最小二乘擬合校準參數得

(10)

(11)

其中

(12)

(13)

式中xcali——需要校準的參數

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

LM算法初始化：

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

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

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

LM算法迭代更新步驟：

(1) 計算Jacobian矩陣

(14)

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

Ts——迭代周期

(2) 計算Hessian近似矩陣

(15)

(3) 更新阻尼系數μk

μk=Sgyroμk-1

(16)

其中

(17)

式中Sgyro——阻尼系數動態調整因子

‖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) 輸出迭代參數

xcali|k=xcali|k-1+dk

(19)

利用LM算法依據地磁場模長不變的特點，實時求解磁力計校準參數，減小了磁力計的測量噪聲，增強了磁力計數據的抗干擾能力。

3 基于UKF的一級航姿估計

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

3.1 航姿狀態方程

建立姿態預測狀態方程

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

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

(20)

式中f(xk-1)——狀態連續的非線性向量函數

xk——系統狀態向量

wk-1——系統噪聲

四元數微分方程為

(21)

式(21)矩陣形式為

(22)

式(22)進行歐拉積分將四元數更新，得

(23)

(24)

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

(25)

式中τ——相關時間系數

(26)

3.2 重力加速度觀測方程

建立姿態的觀測方程

yk=h(xk)+vk

(27)

式中h(xk)——狀態連續的非線性向量函數

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

加速度計測量模型為

(28)

gb——機體重力加速度

ma——機體運動加速度

ab——加速度計三軸零偏

觀測量選取機體坐標系下重力加速度的比力，將比力旋轉至機體坐標系得

(29)

故觀測方程為

(30)

3.3 觀測噪聲協方差自適應調節

在微機動或靜止狀態下，加速度計通過測量重力加速度，可以準確補償陀螺儀零偏，但是載體機動時存在線加速度，從式(28)可得到加速度計測量值不僅是重力加速度，而是重力加速度和運動加速度的矢量和。而陀螺儀受運動加速度影響較小，在大機動的場景下，觀測量將偏離預設模型，根據加速度計的測量數據模值，自適應調節測量噪聲方差矩陣。

使用分段函數描述測量噪聲變化

(31)

式中g——重力加速度模值

磁力計測量噪聲自適應調節與加速度計類似，以地磁場強度為基準。

4 基于UKF的二級航姿修正

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

4.1 航向角狀態方程

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

(32)

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

ψ——航向角

選取航向角為狀態量，故航向角狀態預測方程為

(33)

4.2 磁力計觀測方程

(34)

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

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

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

(35)

4.3 航向角誤差四元數

四元數的角軸表示法為

(36)

式中ε——旋轉角u——空間的旋轉軸

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

(37)

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

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

5 試驗平臺與試驗方法

5.1 硬件平臺

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

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

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

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

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

圖3所示的直線型植保無人機試驗平臺，主升電機為4個颶風U3515型電機，采用豎直桿為姿態調整桿，姿態調整桿上為2個對稱的TMOTOR乘風2207型電機，通過串級控制完成飛行任務。

5.2 試驗設計與結果分析

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

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

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

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

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

表1 局部采樣磁場校準試驗結果Tab.1 Magnetic field interference test results μT

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

圖6 磁場干擾試驗磁力計校準對比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橢球擬合提供的實時校準算法初始值，LM實時校準法快速收斂。在8s出現磁場干擾后，LM實時校準法依舊快速估計出了磁力計校準參數。如表2所示，LS橢球擬合法最大誤差為29.33 μT、均方根誤差為3.78 μT；LM實時校準法最大誤差為6.17 μT、均方根誤差為 0.59 μT，均小于LS橢球擬合法校準產生的誤差。因此，本文提出的基于LM實時校準算法的航姿測量系統對于時變的磁場干擾，能夠快速收斂，保持高穩定性。

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

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

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

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

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

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

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

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

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

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

6 結論

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

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

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

(4)設計的直線型植保無人機航姿測量系統仍需進行飛行植保作業進一步驗證，并根據飛行數據進一步探索融合算法優化方向。