趙健康，崔 超，朱建斌

(1. 上海交通大學 電子信息與電氣工程學院·上海·200240;2.深圳高速工程顧問有限公司·深圳·518094)

0 引 言

隨著微型無人機技術的發展與無人機行業的爆發式增長，基于9軸微型慣性測量單元(9 Degree of Freedom Micro Inertial Measurement Unit,9-D MIMU)與全球定位系統(Global Position System, GPS)的低成本組合導航系統日益成為了工程研究與應用的熱點[1-2]。

9-D MIMU以成本低廉的優勢被廣泛應用于消費級無人機系統，但由于其測量噪聲過大，僅能在很短的時間內對載體的運動狀態進行有效的測量與估計。GPS定位技術成熟，能在較長時間內保持測量數據的穩定并維持一定程度的精度。鑒于此，科研及工程人員設計了諸多將9-D MIMU與GPS的測量數據有效融合的組合導航算法，進而估計出較為準確的載體運動狀態。在實際應用過程中，加速度計與陀螺儀的誤差標定已經能夠滿足中低端的應用需求，而磁強計的測量過程容易受到自身制造誤差及不確定的外界擾動的影響，給組合導航算法的穩定性與可靠性帶來了一定的壓力。如何設計魯棒性算法、有效地降低磁強計的傳感器的測量誤差對導航狀態估計的影響，便成為了研究人員關注的重點。文獻[3]提出了使用橢球模型來對磁強計的測量噪聲進行建模，取得了良好的磁強計誤差修正效果；文獻[4]利用陀螺儀的測量值進行了輔助，實時修正了磁強計的誤差參數；文獻[5]使用兩個磁強計互補的原理消除了磁強計不規律和長期的誤差干擾；文獻[6]采用優化的方法，利用全球導航衛星系統(Global Navigation Satellite System, GNSS)的信息和加速度信息消除了磁場的干擾對導航系統的影響。

上述方法均能取得一定的效果，但是文獻[3-4]并未考慮磁傾角和磁偏角對導航系統的影響；文獻[7]建立在雙磁強計的基礎上，而多數硬件平臺不滿足改造條件；文獻[6]利用了優化的方式，同時利用了GNSS信息和加速度計信息來減小磁強計所受干擾對導航系統的影響，實驗效果雖然良好，但是其優化算法的計算量大，不便于在低成本的嵌入式平臺上實現。本文在橢球模型修正磁強計的基礎上，提出了一種基于兩級擴展卡爾曼濾波器(Extended Kalman Filter，EKF )的9-D MIMU/GPS微型無人機組合導航系統的魯棒性設計：首先，建立第一級EKF，融合加速度計、陀螺儀和GPS的速度觀測信息，估計出載體坐標系下的單位重力矢量；而后，利用單位重力矢量將修正后的磁強計測量值正交單位化，消除磁傾角的影響；最后，建立第二級EKF，將正交單位化的磁強計測量數據和GPS的位置、速度信息作為觀測量，將載體的姿態、速度、位置及9-D MIMU的誤差作為狀態量進行有效的估計。實驗仿真結果表明，本文的算法設計能夠針對性地降低磁強計原始測量數據中包含的誤差對導航系統的影響。

本文的組織結構如下：第2小節推演了系統模型和算法步驟，第3小節展示了仿真和實驗結果，第4小節總結了本文的內容。

1 理論背景

1.1 坐標系定義

為了能夠更加清晰地描述各個傳感器的測量模型和標定方法，我們首先定義本文所使用的坐標系：載體坐標系(b )與9-D MIMU傳感器固連，xb指向載體前方并與橫滾角φ關聯，yb指向載體右方并與俯仰角θ關聯，zb指向載體下方并與偏航角ψ關聯；取“北-東-地(NED)”坐標系作為導航坐標系(n )，xn指向當地北方，yn指向當地東方，zn指向地球表面并豎直向下。

1.2 傳感器測量模型

由于微型無人機體積小、載荷有限，因此只能用低功耗、低成本的傳感器來構造導航系統。基于微機電系統(Micro-Electro-Mechanical System, MEMS)技術的9-D MIMU芯片，集成了三軸磁強計、加速度計和陀螺儀，被廣泛應用于微小型無人機的設計。此外，為了能夠實現無人機在室外的定點飛行、航跡規劃等高級任務，無人機需要搭載專用的GPS模塊來為其提供位置、速度信息。下文將分別描述4種傳感器的測量模型。

1.2.1 磁強計

磁強計可以敏感并測量傳感器周圍的磁場環境，包括地磁場和其他磁場干擾。三軸磁強計的測量模型[3]可以表述為

(1)

Mm-error=Mm-nonOrthMm-scaleMm-soft

(2)

1.2.2 陀螺儀

陀螺儀可以敏感并測量載體的三維旋轉角速度，其測量模型可以表示為

(3)

Mgyro-error=Mgyro-alignMgyro-nonOrthMgyro-scale

(4)

式中，Mgyro-error是一個3×3的矩陣，描述了陀螺儀與磁強計之間的非對齊誤差Mgyro-align、陀螺儀的三軸非正交誤差Mgyro-nonOrth和三軸增益誤差Mgyro-scale。bgyro表示陀螺儀的常值偏差。εgyro表示陀螺儀的測量噪聲，并假設其滿足白噪聲特性。

1.2.3 加速度計

加速度計可以敏感并測量載體的三軸加速度，其測量模型可以表示為

(5)

Macc-error=Macc-alignMacc-nonOrthMacc-scale

(6)

式中,Macc-error是一個3×3的矩陣，描述了加速度計與磁強計之間的非對齊誤差Macc-align、加速度計的三軸非正交誤差Macc-nonOrth,以及三軸增益誤差Macc-scale。bacc表示加速度計的常值偏差，εacc表示加速度計的測量噪聲，并假設其滿足白噪聲特性。

1.2.4 GPS

由于技術成熟、成本低廉等特點，GPS被廣泛應用于汽車、飛行器等室外導航領域，也成為了微型無人機室外飛行的常用傳感器。通常而言，GPS接收機可以提供載體所在位置的經度L、緯度B、相對于平均海平面的高度hMSL,GPS，以及載體在導航坐標系(北-東-地)下的速度向量vGPS=[vN,GPS,vE,GPS,vD,GPS]T。為了方便融合各個傳感器的測量數據，需要將GPS接收機的各個測量數據轉換至導航坐標系中。

記載體起飛地點或自定義的某一地點為起始點Phome(Bhome,Lhome,hhome)，載體在當前位置的GPS位置測量值為Puav(Buav,Luav,huav)。取輔助點Paux(Buav,Lhome)，已知連接Phome、Paux和Puav三點可以構成一個曲面直角三角形。當無人機在小區域內飛行時，可以假設地球為標準球體，通過Haversine[7]公式計算無人機在以起始點為原點的導航坐標系下的位置

(7)

(8)

PD,GPS=huav-hhome

(9)

1.2.5 測量模型簡化

由于MEMS技術工藝的限制，并出于降低系統復雜度的考慮，本文將加速度計和陀螺儀的測量模型簡化為

(10)

(11)

同時，也忽略GPS模塊測量過程中其他誤差對測量的影響，假定GPS的測量噪聲滿足零均值高斯白噪聲假設，即有

(12)

(13)

1.3 基于橢球模型的磁強計干擾補償

在忽略測量噪聲nm的情況下，對磁強計的誤差修正模型可以由式(1)、式(2)得到

(14)

在較小的飛行區域內，地磁場強度與方向相對恒定，根據式(14)可以得到

(15)

式中，G=(Mm-error)-1。

在沒有磁場干擾的情況下，即bm=0，Mm-error=

G=I，I為單位矩陣，真實磁場的測量值分布在一個球心在原點的球面上。對應地，在有磁場干擾的情況下，磁強計的實際測量值應分布在一個中心偏移bm的橢球面上，橢球的形狀由Mm-error決定。

式(15)可以進一步整理為

(16)

而橢球是一個二次曲面，其一般方程表示為[3,8]

ax2+bxy+cy2+dxz+eyz+jz2+
px+qy+rz+s=0

(17)

改寫為矩陣形式，為

(X-X0)TA(X-X0)=1

(18)

可以展開為

(19)

其中

(20)

(21)

(22)

將式(16)與式(19)各項對應，可以得到如下關系

(23)

ax2+cy2+jz2+px+qy+rz+s=0

(24)

ax2+cy2+jz2+px+qy+rz+s=0，對應地有

利用磁強計在載體運動過程中的測量數據，擬合得到橢球模型式(24)，就可以對磁強計的測量誤差進行補償。

將式(24)記為

F(a,mb)=0

(25)

(26)

其中

D=

求解式(26)屬于靜態約束條件下的極值約束問題，可以利用拉格朗日乘數法求解，構造增廣代價函數

(27)

式中，λ為拉格朗日乘數。

對式(27)求偏導數得

(28)

將其置零，并結合約束條件，可以得到

(29)

即將橢球擬合問題轉換為矩陣DTD相對于矩陣B的廣義特征值問題，最終通過求解得到橢球模型的參數a。然后，利用a和式(21)得到矩陣A。由于A為對角矩陣，容易得到

至此，并利用式(14)來對測量得到的磁強計數據進行修正。

由于矩陣A不受橢球方程常值系數s的影響，利用矩陣G和bm得到的磁強計修正值與真實的磁場強度呈某一固定倍數關系，但不影響磁強計對磁場方向的敏感。

2 兩級EKF組合導航系統的魯棒性設計

完整的系統模型可以分為3個部分：(1)基于加速度計、陀螺儀和GPS速度信息的單位重力向量估計；(2)磁強計測量正交標準化；(3)基于加速度計、陀螺儀、單位磁場矢量和GPS的組合導航。系統模型原理如圖1所示。

2.1 單位重力向量估計

加速度能夠敏感地球的重力場，但是載體的三軸線加速運動會極大地影響加速度計對重力場的測量。為了能夠更好地估計出載體所在環境的重力場，本文設計了一個EKF濾波器，記為EKF A，融合GPS的速度測量值和陀螺儀的角速度測量值，消除載體的線加速運動對重力估計的影響。

系統模型可以用如下微分方程描述。

(30)

式中，符號^表示估計量，·表示導數，q=[q0,q1,q2,q3]T表示導航系與機體系之間相對旋轉的四元數，一個四元數由一個實數和一個向量組成，q0表示實數，v=[q1,q2,q3]T表示向量；ω=[ωx,ωy,ωz]T表示機體坐標系下陀螺儀的測量數據；V=[vN,vE,vD]T為導航系中載體的運動速度；fb=[fx,fy,fz]T表示機體坐標系下加速度計的測量數據；g表示當地重力加速度向量；bgyro=[bgyro,x,bgyro,y,bgyro,z]T和bacc=[bacc,x,bacc,y,bacc,z]T分別為陀螺儀和加速度計的三軸測量偏差估計值。

根據式(30)所示的微分方程，將EKF A的狀態向量定義為

(31)

將量測向量定義為

Z=[vN,GPSvE,GPSvD,GPS]T

(32)

將量測模型描述為

[vN,GPSvE,GPSvD,GPS]T=[vNvEvD]T

(33)

基于式(30)、式(31)、式(32)、式(33)，可以利用擴展卡爾曼算法將模型線性化，并迭代估計出導航狀態量[11]。

由式(30)可知，EKF A沒有利用磁強計的數據，可以通過對載體速度的觀測，來消除載體線加速度對姿態的影響。同時，利用估計出的四元數構建旋轉矩陣

(34)

進而求得單位重力向量的估計

2.2 磁場矢量正交標準化

地球磁場的方向并非嚴格指北，還存在著磁偏角和磁傾角的影響[10]，因此北半球的實際地磁場矢量除了北向分量外，還包含東向分量和豎直向下的分量。本文基于水平方向磁場矢量與重力向量相互垂直的關系，設計了如下方式來獲取與單位重力向量正交的單位磁場矢量，消除磁傾角對導航系統的影響。

(35)

(36)

(37)

(38)

顯然，當載體坐標系與導航坐標系重合時，有

2.3 組合導航系統設計

為實現各個傳感器的特性互補，本文設計了EKF濾波器將全部傳感器的測量值融合到導航系統狀態的估計中，并將其記為EKF B。

系統模型可以用如下微分方程描述

(39)

根據式(39)所示的微分方程，將EKF B的狀態向量定義為

(40)

將量測向量定義為

可將量測模型描述為

(41)

式中，C(ξ)為磁偏角修正因子，ξ表示磁偏角，用來修正地理北極與地磁北極之間的誤差，C(ξ)可以用矩陣表示為

(42)

與EKF A類似，基于式(30)～式(33)，可以利用擴展卡爾曼算法將模型線性化并迭代估計出導航狀態量[11]。

運動過程中的微型無人機的航向信息可以通過載體在導航系中的速度及磁強計計算獲得；而處于懸停狀態或緩慢運動狀態的無人機則僅能依靠磁強計來獲取當前的航向。因此，磁強計對微型無人機的航向及其他狀態的正確估計起到了至關重要的作用。

為了能夠在磁場異常的情況下依然能夠維持微型無人機導航系統的基本功能，使異常的磁場測量數據不對導航系統中其他狀態量的估計產生污染，有必要建立磁場異常判別機制，以便在進行多傳感器數據融合時減小其權重。

依據式(24)、式(25)，F(a,mb)表示數據點到橢球面的距離，當橢球模型能夠對當前磁強計的測量值進行理想的修正時，則有F(a,mb)=0；當磁強計的測量受到其他磁場噪聲的干擾時，F(a,mb)將會與零值產生一定的偏差。定義磁強計的當前測量值與橢球校正模型的適配指數為

ξ=e|F(a,mb)|

(43)

ξ越接近于1，說明磁強計的當前測量值越能與橢球校正模型契合；否則，說明當前磁強計受到了其他額外異常的磁場噪聲的干擾。為了隔離異常的磁強計測量值，本文利用式(43)構造了方差膨脹因子，依據ξ的大小和自定義的臨界異常參數TD、k1、k2，將磁場異常程度劃分為正常、臨界異常和嚴重異常3個等級

(44)

相應的磁強計測量值的等價協方差矩陣為：

(45)

3 仿真實驗及結果分析

3.1 磁強計橢球模型修正實驗

本小節設定三軸磁強計的模型誤差為

仿真數據利用隨機的姿態四元數模擬不同的載體姿態，并結合磁強計的誤差模型得到仿真的磁強計的輸出。圖2為磁強計原始測量數據的橢球擬合立體視圖，可以看到由于模型誤差的存在，磁強計的測量點分布在一個橢球的表面上。

圖2 磁強計原始測量數據的橢球擬合立體視圖Fig.2 Stereo view of the original measurement data of magnetometer based on ellipsoid fitting

經過本文1.3小節的算法校正，求解結果為

可見本文對磁強計的校正算法能夠有效估計磁強計的誤差模型參數。圖3給出了校正后的磁強計測量數據的橢球擬合圖。可以看到，磁強計的輸出數據已經被較好地校正成為球體，說明基于橢球模型的修正能夠降低磁強計在測量時被引入的誤差。

圖3 經橢球模型校正后的立體視圖Fig.3 Stereo view of the measurement data of magnetometer corrected by ellipsoid model

3.2 導航姿態信息估計實驗

為了驗證基于磁強計橢球校正模型所設計的抗差卡爾曼濾波器的魯棒性，本文將導航系統的姿態估計作為研究重點，并基于參考導航數據集中的位置、速度及載體姿態角等信息，利用1.2節各個傳感器的測量模型，仿真生成了陀螺儀、加速度計、磁強計和GPS的測量數據，并在磁強計的測量數據上選取測量區間[2000，3000]疊加了正弦波干擾，用來模擬載體在飛行過程中所受到的緩慢變化的磁場的干擾。圖4展示了三軸磁強計上所疊加的測量噪聲，圖5展示了疊加正弦波噪聲后的磁強計測量值。

圖4 正弦波形式的磁場干擾Fig.4 Sinusoidal magnetic field interference

圖5 受正弦波干擾的磁場測量值與原始值的對比圖Fig.5 Comparison of magnetic field measurements disturbed by sinusoidal noise with original values

在仿真實驗中，首先使用基于式(39)所描述的微分模型設計EKF算法，來解算各個導航狀態量，濾波器的參數如表1所示。從圖6可以看出，由于磁強計測量量上所疊加的正弦波的干擾，導航系統的姿態估計結果相應地出現了波動。與此對比，本文基于磁強計的橢球模型來對異常的磁強計測量值進行判別，設計抗差EKF算法估計各個導航狀態量，濾波器參數與表1相同，估計結果如圖7所示。對比兩種算法的估計結果，顯然后者能夠有效抑制異常磁場測量值對導航系統的影響，得到更加可靠、穩定的導航狀態量的估計值。

表1 EKF算法參數

圖6 受正弦波噪聲干擾的三軸姿態解算結果與 參考值的對比圖Fig.6 Comparisons between the calculated results of three- axis attitude disturbed by sinusoidal noise and the reference values

圖7 基于抗差EKF求解的三軸姿態解算 結果與參考值的對比圖Fig.7 Comparisons between the results of three-axis attitude calculation based on robust EKF esti- mation and the reference values

4 結 論

本文針對磁強計在低成本微型無人機上應用時容易受到外界干擾，并影響導航系統其他狀態量的準確估計的問題，提出了一種基于兩級EKF的9-D MIMU/GPS微型無人機組合導航系統的魯棒性設計。利用磁強計的橢球模型對磁強計的測量誤差進行了建模，校正了磁強計在消費級微型無人機導航系統中的主要誤差項，并建立了磁強計測量異常判別機制，增強了導航系統的魯棒性。實驗表明，本文對導航系統的設計能夠有效校正磁強計原始測量數據中所包含的誤差，降低導航系統對異常磁場測量的敏感程度。