趙健康，崔 超，朱建斌

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

0 引 言

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

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

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

本文的組織結(jié)構(gòu)如下：第2小節(jié)推演了系統(tǒng)模型和算法步驟，第3小節(jié)展示了仿真和實(shí)驗(yàn)結(jié)果，第4小節(jié)總結(jié)了本文的內(nèi)容。

1 理論背景

1.1 坐標(biāo)系定義

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

1.2 傳感器測量模型

由于微型無人機(jī)體積小、載荷有限，因此只能用低功耗、低成本的傳感器來構(gòu)造導(dǎo)航系統(tǒng)?；谖C(jī)電系統(tǒng)(Micro-Electro-Mechanical System, MEMS)技術(shù)的9-D MIMU芯片，集成了三軸磁強(qiáng)計、加速度計和陀螺儀，被廣泛應(yīng)用于微小型無人機(jī)的設(shè)計。此外，為了能夠?qū)崿F(xiàn)無人機(jī)在室外的定點(diǎn)飛行、航跡規(guī)劃等高級任務(wù)，無人機(jī)需要搭載專用的GPS模塊來為其提供位置、速度信息。下文將分別描述4種傳感器的測量模型。

1.2.1 磁強(qiáng)計

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

(1)

Mm-error=Mm-nonOrthMm-scaleMm-soft

(2)

1.2.2 陀螺儀

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

(3)

Mgyro-error=Mgyro-alignMgyro-nonOrthMgyro-scale

(4)

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

1.2.3 加速度計

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

(5)

Macc-error=Macc-alignMacc-nonOrthMacc-scale

(6)

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

1.2.4 GPS

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

記載體起飛地點(diǎn)或自定義的某一地點(diǎn)為起始點(diǎn)Phome(Bhome,Lhome,hhome)，載體在當(dāng)前位置的GPS位置測量值為Puav(Buav,Luav,huav)。取輔助點(diǎn)Paux(Buav,Lhome)，已知連接Phome、Paux和Puav三點(diǎn)可以構(gòu)成一個曲面直角三角形。當(dāng)無人機(jī)在小區(qū)域內(nèi)飛行時，可以假設(shè)地球?yàn)闃?biāo)準(zhǔn)球體，通過Haversine[7]公式計算無人機(jī)在以起始點(diǎn)為原點(diǎn)的導(dǎo)航坐標(biāo)系下的位置

(7)

(8)

PD,GPS=huav-hhome

(9)

1.2.5 測量模型簡化

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

(10)

(11)

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

(12)

(13)

1.3 基于橢球模型的磁強(qiáng)計干擾補(bǔ)償

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

(14)

在較小的飛行區(qū)域內(nèi)，地磁場強(qiáng)度與方向相對恒定，根據(jù)式(14)可以得到

(15)

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

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

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

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

(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)各項(xiàng)對應(yīng)，可以得到如下關(guān)系

(23)

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

(24)

ax2+cy2+jz2+px+qy+rz+s=0，對應(yīng)地有

利用磁強(qiáng)計在載體運(yùn)動過程中的測量數(shù)據(jù)，擬合得到橢球模型式(24)，就可以對磁強(qiáng)計的測量誤差進(jìn)行補(bǔ)償。

將式(24)記為

F(a,mb)=0

(25)

(26)

其中

D=

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

(27)

式中，λ為拉格朗日乘數(shù)。

對式(27)求偏導(dǎo)數(shù)得

(28)

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

(29)

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

至此，并利用式(14)來對測量得到的磁強(qiáng)計數(shù)據(jù)進(jìn)行修正。

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

2 兩級EKF組合導(dǎo)航系統(tǒng)的魯棒性設(shè)計

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

2.1 單位重力向量估計

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

系統(tǒng)模型可以用如下微分方程描述。

(30)

式中，符號^表示估計量，·表示導(dǎo)數(shù)，q=[q0,q1,q2,q3]T表示導(dǎo)航系與機(jī)體系之間相對旋轉(zhuǎn)的四元數(shù)，一個四元數(shù)由一個實(shí)數(shù)和一個向量組成，q0表示實(shí)數(shù)，v=[q1,q2,q3]T表示向量；ω=[ωx,ωy,ωz]T表示機(jī)體坐標(biāo)系下陀螺儀的測量數(shù)據(jù)；V=[vN,vE,vD]T為導(dǎo)航系中載體的運(yùn)動速度；fb=[fx,fy,fz]T表示機(jī)體坐標(biāo)系下加速度計的測量數(shù)據(jù)；g表示當(dāng)?shù)刂亓铀俣认蛄?；bgyro=[bgyro,x,bgyro,y,bgyro,z]T和bacc=[bacc,x,bacc,y,bacc,z]T分別為陀螺儀和加速度計的三軸測量偏差估計值。

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

(31)

將量測向量定義為

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

(32)

將量測模型描述為

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

(33)

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

由式(30)可知，EKF A沒有利用磁強(qiáng)計的數(shù)據(jù)，可以通過對載體速度的觀測，來消除載體線加速度對姿態(tài)的影響。同時，利用估計出的四元數(shù)構(gòu)建旋轉(zhuǎn)矩陣

(34)

進(jìn)而求得單位重力向量的估計

2.2 磁場矢量正交標(biāo)準(zhǔn)化

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

(35)

(36)

(37)

(38)

顯然，當(dāng)載體坐標(biāo)系與導(dǎo)航坐標(biāo)系重合時，有

2.3 組合導(dǎo)航系統(tǒng)設(shè)計

為實(shí)現(xiàn)各個傳感器的特性互補(bǔ)，本文設(shè)計了EKF濾波器將全部傳感器的測量值融合到導(dǎo)航系統(tǒng)狀態(tài)的估計中，并將其記為EKF B。

系統(tǒng)模型可以用如下微分方程描述

(39)

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

(40)

將量測向量定義為

可將量測模型描述為

(41)

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

(42)

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

運(yùn)動過程中的微型無人機(jī)的航向信息可以通過載體在導(dǎo)航系中的速度及磁強(qiáng)計計算獲得；而處于懸停狀態(tài)或緩慢運(yùn)動狀態(tài)的無人機(jī)則僅能依靠磁強(qiáng)計來獲取當(dāng)前的航向。因此，磁強(qiáng)計對微型無人機(jī)的航向及其他狀態(tài)的正確估計起到了至關(guān)重要的作用。

為了能夠在磁場異常的情況下依然能夠維持微型無人機(jī)導(dǎo)航系統(tǒng)的基本功能，使異常的磁場測量數(shù)據(jù)不對導(dǎo)航系統(tǒng)中其他狀態(tài)量的估計產(chǎn)生污染，有必要建立磁場異常判別機(jī)制，以便在進(jìn)行多傳感器數(shù)據(jù)融合時減小其權(quán)重。

依據(jù)式(24)、式(25)，F(xiàn)(a,mb)表示數(shù)據(jù)點(diǎn)到橢球面的距離，當(dāng)橢球模型能夠?qū)Ξ?dāng)前磁強(qiáng)計的測量值進(jìn)行理想的修正時，則有F(a,mb)=0；當(dāng)磁強(qiáng)計的測量受到其他磁場噪聲的干擾時，F(xiàn)(a,mb)將會與零值產(chǎn)生一定的偏差。定義磁強(qiáng)計的當(dāng)前測量值與橢球校正模型的適配指數(shù)為

ξ=e|F(a,mb)|

(43)

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

(44)

相應(yīng)的磁強(qiáng)計測量值的等價協(xié)方差矩陣為：

(45)

3 仿真實(shí)驗(yàn)及結(jié)果分析

3.1 磁強(qiáng)計橢球模型修正實(shí)驗(yàn)

本小節(jié)設(shè)定三軸磁強(qiáng)計的模型誤差為

仿真數(shù)據(jù)利用隨機(jī)的姿態(tài)四元數(shù)模擬不同的載體姿態(tài)，并結(jié)合磁強(qiáng)計的誤差模型得到仿真的磁強(qiáng)計的輸出。圖2為磁強(qiáng)計原始測量數(shù)據(jù)的橢球擬合立體視圖，可以看到由于模型誤差的存在，磁強(qiáng)計的測量點(diǎn)分布在一個橢球的表面上。

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

經(jīng)過本文1.3小節(jié)的算法校正，求解結(jié)果為

可見本文對磁強(qiáng)計的校正算法能夠有效估計磁強(qiáng)計的誤差模型參數(shù)。圖3給出了校正后的磁強(qiáng)計測量數(shù)據(jù)的橢球擬合圖?？梢钥吹剑艔?qiáng)計的輸出數(shù)據(jù)已經(jīng)被較好地校正成為球體，說明基于橢球模型的修正能夠降低磁強(qiáng)計在測量時被引入的誤差。

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

3.2 導(dǎo)航姿態(tài)信息估計實(shí)驗(yàn)

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

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

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

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

表1 EKF算法參數(shù)

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

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

4 結(jié) 論

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