趙健康，崔 超，朱建斌

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

0 引 言

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

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

為了能夠更加清晰地描述各個(gè)傳感器的測(cè)量模型和標(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 傳感器測(cè)量模型

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

1.2.1 磁強(qiáng)計(jì)

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

(1)

Mm-error=Mm-nonOrthMm-scaleMm-soft

(2)

1.2.2 陀螺儀

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

(3)

Mgyro-error=Mgyro-alignMgyro-nonOrthMgyro-scale

(4)

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

1.2.3 加速度計(jì)

加速度計(jì)可以敏感并測(cè)量載體的三軸加速度，其測(cè)量模型可以表示為

(5)

Macc-error=Macc-alignMacc-nonOrthMacc-scale

(6)

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

1.2.4 GPS

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

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

(7)

(8)

PD,GPS=huav-hhome

(9)

1.2.5 測(cè)量模型簡(jiǎn)化

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

(10)

(11)

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

(12)

(13)

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

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

(14)

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

(15)

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

在沒(méi)有磁場(chǎng)干擾的情況下，即bm=0，Mm-error=

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

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

(16)

而橢球是一個(gè)二次曲面，其一般方程表示為[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)對(duì)應(yīng)，可以得到如下關(guān)系

(23)

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

(24)

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

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

將式(24)記為

F(a,mb)=0

(25)

(26)

其中

D=

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

(27)

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

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

(28)

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

(29)

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

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

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

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

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

2.1 單位重力向量估計(jì)

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

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

(30)

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

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

(31)

將量測(cè)向量定義為

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

(32)

將量測(cè)模型描述為

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

(33)

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

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

(34)

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

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

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

(35)

(36)

(37)

(38)

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

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

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

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

(39)

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

(40)

將量測(cè)向量定義為

可將量測(cè)模型描述為

(41)

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

(42)

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

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

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

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

ξ=e|F(a,mb)|

(43)

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

(44)

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

(45)

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

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

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

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

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

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

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

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

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

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

圖4 正弦波形式的磁場(chǎng)干擾Fig.4 Sinusoidal magnetic field interference

圖5 受正弦波干擾的磁場(chǎng)測(cè)量值與原始值的對(duì)比圖Fig.5 Comparison of magnetic field measurements disturbed by sinusoidal noise with original values

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

表1 EKF算法參數(shù)

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

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

4 結(jié) 論

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