魯棒無(wú)跡四元數(shù)卡爾曼濾波初始對(duì)準(zhǔn)算法?

周曉仁徐 祥趙鶴鳴

(蘇州大學(xué)電子信息學(xué)院，江蘇 蘇州 215006)

捷聯(lián)慣性導(dǎo)航系統(tǒng)的結(jié)果通過(guò)航位推算獲得，在導(dǎo)航過(guò)程之前需要獲得載體的位置，速度和姿態(tài)的三個(gè)初始信息。 通常載體的初始位置和初始速度可以由GPS 獲取，而初始姿態(tài)需要通過(guò)初始對(duì)準(zhǔn)提供。初始對(duì)準(zhǔn)有兩個(gè)的過(guò)程:粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)。 粗對(duì)準(zhǔn)提供一個(gè)粗略的姿態(tài)矩陣，從而精對(duì)準(zhǔn)才能滿(mǎn)足失準(zhǔn)角誤差模型來(lái)對(duì)姿態(tài)矩陣進(jìn)行更準(zhǔn)確得估計(jì)。

為提高粗對(duì)準(zhǔn)的精度，眾多學(xué)者提出了改進(jìn)的算法。 文獻(xiàn)[1－2]提出了將姿態(tài)矩陣用慣性系分解的方法，減小了基座搖擺帶來(lái)的影響。 在慣性系對(duì)準(zhǔn)方法提出之后，產(chǎn)生了眾多改進(jìn)的基于重力視矢量運(yùn)動(dòng)的搖擺基座粗對(duì)準(zhǔn)算法，文獻(xiàn)[3－4]提出了參數(shù)識(shí)別的算法，提高了初始載體系下的矢量的精度，從而提高了粗對(duì)準(zhǔn)的精度。 文獻(xiàn)[5]提出了基于最優(yōu)化的初始姿態(tài)確定方法(optimization-based alignment，OBA)，該方法能夠充分利用觀測(cè)和參考矢量，通過(guò)連續(xù)定姿的方式加快了粗對(duì)準(zhǔn)的速度。 文獻(xiàn)[6]提出了速度/位置積分公式，使用外部輔助設(shè)備GPS 構(gòu)造觀測(cè)矢量和參考矢量，通過(guò)OBA 法完成動(dòng)基座初始對(duì)準(zhǔn)。 文獻(xiàn)[7－8]提出了回溯法來(lái)縮短初始對(duì)準(zhǔn)的時(shí)間。 但以上的方法并不適用于低精度傳感器，因?yàn)榈途葌鞲衅鞯耐勇萘闫斐傻睦鄯e誤差會(huì)造成參考矢量精度的下降，從而導(dǎo)致對(duì)準(zhǔn)結(jié)果不理想。 文獻(xiàn)[9－10]采用非線性姿態(tài)估計(jì)的算法，將陀螺零偏加入預(yù)測(cè)方程，提高了粗對(duì)準(zhǔn)的穩(wěn)定性和精度。 然而，該算法的魯棒性不足，不能抑制GPS 野值帶來(lái)的影響。 文獻(xiàn)[11]提出了由位置軌跡構(gòu)造矢量的粗對(duì)準(zhǔn)方法，該方法雖然在一定程度上能夠抑制野值的影響，但是在根本上未去除野值。文獻(xiàn)[12]提出了基于矢量重構(gòu)的魯棒初始對(duì)準(zhǔn)方法，但是該方法只能用于慣導(dǎo)系統(tǒng)與多普勒速度儀(doppler velocity log，DVL)組合導(dǎo)航系統(tǒng)。 文獻(xiàn)[13]提出了一種基于Huber 的無(wú)跡濾波算法，并將其應(yīng)用于無(wú)人機(jī)編隊(duì)飛行中相對(duì)位置、速度和姿態(tài)的精確估計(jì)。 該算法通過(guò)改變量測(cè)噪聲方差矩陣，修改量測(cè)更新公式來(lái)消除野值的影響，但與本文相比，未能在觀測(cè)矢量中體現(xiàn)出消除野值的效果。

為解決上述的在低精度傳感器初始對(duì)準(zhǔn)時(shí)的兩個(gè)問(wèn)題:陀螺零偏和GPS 野值影響，本文使用模值匹配法，把規(guī)范化模值平方誤差作為量測(cè)殘差，構(gòu)造了改進(jìn)的觀測(cè)矢量， 結(jié)合無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)推導(dǎo)了基于四元數(shù)的魯棒無(wú)跡卡爾曼濾波算法，在未修改量測(cè)更新公式的情況下完成了對(duì)載體姿態(tài)與陀螺零偏的估計(jì)，抑制了陀螺零偏和GPS 野值的影響，提高了初始對(duì)準(zhǔn)的精度。

1 傳統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)基本原理

定義i 為初始時(shí)刻的地球坐標(biāo)系且相對(duì)于慣性坐標(biāo)系靜止。e為與地球固連的地球坐標(biāo)系。n為導(dǎo)航坐標(biāo)系，原點(diǎn)在載體中心，x，y，z三軸分別指向東，北，天方向。b為載體坐標(biāo)系，原點(diǎn)在載體中心，x，y，z三軸分別指向載體的右，前，上方向。n0為導(dǎo)航慣性坐標(biāo)系，與初始時(shí)刻的n系重合。b0為載體慣性坐標(biāo)系，與初始時(shí)刻的b系重合。 則比力方程可以表示為:

具體的離散形式可以參考文獻(xiàn)[8]。 式中，tM＝MΔtg，tk＝kΔtg，Δtg＝tk＋1－tk。 利用離散的觀測(cè)矢量和參考矢量來(lái)構(gòu)建tM時(shí)刻的矩陣KM:

結(jié)合式(2)～式(4)可以求解tM時(shí)刻的載體的姿態(tài)矩陣。

2 無(wú)跡四元數(shù)卡爾曼濾波模型

2.1 預(yù)測(cè)模型

2.2 量測(cè)模型

經(jīng)過(guò)離散化:

3 GPS 野值影響與模值匹配權(quán)值函數(shù)

GPS 接收機(jī)容易受到外部環(huán)境的影響比如障礙物，多徑效應(yīng)等[11]，輸出有時(shí)會(huì)出現(xiàn)野值，其實(shí)際速度測(cè)量值與理想值的關(guān)系如下式所示:

式中:γ表示高斯閾值，在本文中取γ為2.25。 通過(guò)該權(quán)值函數(shù)可以有效得抑制異常量測(cè)對(duì)狀態(tài)估計(jì)的影響。

4 魯棒無(wú)跡四元數(shù)估計(jì)器

將UKF 應(yīng)用于估計(jì)載體的姿態(tài)四元數(shù)，完成初始對(duì)準(zhǔn)的方法稱(chēng)為無(wú)跡四元數(shù)估計(jì)器(unscented quaternion estimator，USQUE)。 通常，為了滿(mǎn)足運(yùn)算過(guò)程中四元數(shù)保持歸一化的條件，在USQUE 中采用修正羅德里格參數(shù)( modified Rodrigues parameters，MRP)作為狀態(tài)向量。 本章結(jié)合USQUE與權(quán)值函數(shù)推導(dǎo)了魯棒無(wú)跡四元數(shù)估計(jì)器(robust unscented quaternion estimator，RUSQUE)。

式中:a為一個(gè)取值范圍為0 到1 的常數(shù)，f是比例因子。 在RUSQUE 中，設(shè)狀態(tài)向量xk＝[δpk；εk]，εk為k時(shí)刻的陀螺零偏狀態(tài)向量。

4.1 狀態(tài)初始化

設(shè)k－1 時(shí)刻的狀態(tài)最優(yōu)估計(jì)向量為＾xk－1，相應(yīng)的狀態(tài)協(xié)方差矩陣為Pk－1，最優(yōu)估計(jì)四元數(shù)為＾qk－1。

（1）全新的課表管理。學(xué)生可以通過(guò)系統(tǒng)獲取自己的課表，查看一起上課的同學(xué)的微博，還可以實(shí)名或匿名向相關(guān)教師提問(wèn)或建議，也可取代原來(lái)的評(píng)教方式對(duì)教師進(jìn)行評(píng)教。

4.2 預(yù)測(cè)更新

4.3 量測(cè)更新

將預(yù)測(cè)四元數(shù)Sigma 點(diǎn)集代入量測(cè)方程(27)得到量測(cè)預(yù)測(cè)的Sigma 點(diǎn)集為:

4.4 姿態(tài)更新

5 實(shí)驗(yàn)與分析

5.1 仿真實(shí)驗(yàn)

為驗(yàn)證本文提出的算法，本節(jié)設(shè)計(jì)了仿真實(shí)驗(yàn)。共有5 種算法進(jìn)行對(duì)比。

方案1:OBA 法，GPS 速度含野值。

方案2:RUSQUE 法，GPS 速度含野值。

方案3:USQUE 法，GPS 速度含野值。

方案4:OBA 法，GPS 速度不含野值。

方案5:USQUE 法，GPS 速度不含野值。

表1 傳感器誤差特性

表2 載體運(yùn)動(dòng)信息

設(shè)定慣性傳感器數(shù)據(jù)輸出頻率為200 Hz，GPS數(shù)據(jù)輸出頻率為1 Hz，GPS 量測(cè)噪聲設(shè)置:速度為0.1 m/s，位置為1 m。

圖1～圖4 分別為載體的運(yùn)動(dòng)軌跡，真實(shí)姿態(tài)角，運(yùn)動(dòng)速度。 運(yùn)動(dòng)速度的噪聲在本文中設(shè)其分布為:

圖1 載體的運(yùn)動(dòng)軌跡

圖2 真實(shí)水平姿態(tài)角

圖3 真實(shí)航向角

圖4 載體運(yùn)動(dòng)速度

圖5 改進(jìn)前后的觀測(cè)矢量

圖6 陀螺零偏估計(jì)

圖7～圖9 分別為俯仰角，橫滾角，航向角的對(duì)準(zhǔn)誤差曲線圖。 當(dāng)GPS 速度中包含野值時(shí)，OBA 法與USQUE 法受到異常觀測(cè)矢量的影響，導(dǎo)致姿態(tài)角誤差曲線產(chǎn)生鋸齒狀的抖動(dòng)。 RUSQUE 法由于改進(jìn)的參考矢量有效得去除了野值，對(duì)準(zhǔn)誤差曲線趨于平穩(wěn)。 當(dāng)GPS 速度中沒(méi)有野值時(shí)，OBA 法與USQUE 法的航向角誤差曲線有著明顯的差異，OBA 法由于陀螺儀的零偏造成的累積誤差而導(dǎo)致曲線呈現(xiàn)波動(dòng)特性。 方案2，4 和5 在經(jīng)過(guò)21 s 后，俯仰角誤差小于0.1°，橫滾角誤差小于0.15°。 在航向角方面，281 s～381 s 間三種方案的誤差均值分別為－0.713 9°，3.032 0°和－0.684 8°，誤差標(biāo)準(zhǔn)差分別為0.015 7°，0.659 2°和0.014 4°。 仿真實(shí)驗(yàn)驗(yàn)證了RUSQUE 法的魯棒性以及對(duì)于陀螺零偏抑制的有效性。

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 航向角誤差

5.2 跑車(chē)實(shí)驗(yàn)

本節(jié)設(shè)計(jì)了跑車(chē)實(shí)驗(yàn)來(lái)進(jìn)一步驗(yàn)證本文提出的算法。 車(chē)載導(dǎo)航設(shè)備如圖10 所示。 跑車(chē)實(shí)驗(yàn)過(guò)程中使用自制的微型慣性測(cè)量單元(miniature inertial measurement unit，MIMU)，采樣頻率為200 Hz。 其陀螺儀和加速度計(jì)性能參數(shù)見(jiàn)表3 所示。 GPS 的采樣頻率為1 Hz，光纖慣導(dǎo)系統(tǒng)的采樣頻率為200 Hz。 采用光纖慣導(dǎo)與GPS 組合導(dǎo)航系統(tǒng)傳遞對(duì)準(zhǔn)的結(jié)果作為參考基準(zhǔn)。

圖10 車(chē)載實(shí)驗(yàn)平臺(tái)

表3 自制的MIMU 傳感器特性

圖11～圖14 分別為載體的運(yùn)動(dòng)軌跡，參考的姿態(tài)角以及GPS 的速度輸出。 圖15 為改進(jìn)前后的觀測(cè)矢量，由圖可得RUSQUE 法濾除了觀測(cè)矢量中的野值。 圖16 為RUSQUE 算法與傳遞對(duì)準(zhǔn)估計(jì)的陀螺零偏對(duì)比曲線。

圖11 載體運(yùn)動(dòng)軌跡

圖12 參考水平姿態(tài)角

圖13 參考航向角

圖14 GPS 速度輸出

圖15 改進(jìn)前后的參考矢量

圖16 陀螺零偏估計(jì)

圖17～圖19 為五種方案的對(duì)準(zhǔn)結(jié)果圖。 在野值的影響下，OBA 法與USQUE 法的對(duì)準(zhǔn)曲線圖都出現(xiàn)了鋸齒狀的波動(dòng)，在航向誤差圖中尤為明顯，使得兩種算法的對(duì)準(zhǔn)結(jié)果不理想。 但是RUSQUE 算法去除了觀測(cè)矢量中的野值，所以對(duì)準(zhǔn)誤差曲線呈現(xiàn)了平穩(wěn)的特性。 當(dāng)沒(méi)有野值時(shí)，OBA 法的航向角誤差曲線由于陀螺零偏累積誤差呈波動(dòng)特性，USQUE 法因在狀態(tài)方程中去除了陀螺零偏使得航向角誤差曲線趨于平穩(wěn)。 當(dāng)對(duì)準(zhǔn)過(guò)程經(jīng)過(guò)15s 后，方案2，4 和5 的俯仰角小于0.1°，橫滾角誤差小于0.13°。 航向角方面，200 s～300 s 之間三種方案的誤差均值分別為－0.371 8°，0.241 4°和－0.351 5°，標(biāo)準(zhǔn)差分別為0.046 5°，0.118 4°和0.049 6°。 雖然方案4 的均值優(yōu)于方案2 與5，但是陀螺零偏的影響導(dǎo)致其誤差標(biāo)準(zhǔn)差將近于另兩者的3 倍，因此跑車(chē)實(shí)驗(yàn)也驗(yàn)證了本文提出的算法的魯棒性以及對(duì)于陀螺零偏累積誤差的抑制效果。

圖17 俯仰角誤差

圖18 橫滾角誤差

圖19 航向角誤差

6 結(jié)論

針對(duì)傳統(tǒng)算法在GPS 輔助的低精度慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)中易受到陀螺零偏和GPS 野值的影響，本文提出了魯棒無(wú)跡四元數(shù)姿態(tài)估計(jì)算法。 仿真和跑車(chē)實(shí)驗(yàn)表明，當(dāng)GPS 中含有野值時(shí)，該算法能濾除觀測(cè)矢量中的野值，同時(shí)陀螺零偏導(dǎo)致航向角誤差曲線呈波動(dòng)特性的問(wèn)題也能得到改善，驗(yàn)證了本文設(shè)計(jì)的算法相較于傳統(tǒng)的算法更有優(yōu)勢(shì)。