周曉仁徐 祥趙鶴鳴

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

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

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

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

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

定義i 為初始時(shí)刻的地球坐標(biāo)系且相對于慣性坐標(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。 利用離散的觀測矢量和參考矢量來構(gòu)建tM時(shí)刻的矩陣KM:

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

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

2.1 預(yù)測模型

2.2 量測模型

經(jīng)過離散化:

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

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

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

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

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

式中:a為一個取值范圍為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é)生可以通過系統(tǒng)獲取自己的課表，查看一起上課的同學(xué)的微博，還可以實(shí)名或匿名向相關(guān)教師提問或建議，也可取代原來的評教方式對教師進(jìn)行評教。

4.2 預(yù)測更新

4.3 量測更新

將預(yù)測四元數(shù)Sigma 點(diǎn)集代入量測方程(27)得到量測預(yù)測的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)行對比。

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

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

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

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

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

表1 傳感器誤差特性

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

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

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

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

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

圖3 真實(shí)航向角

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

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

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

圖7～圖9 分別為俯仰角，橫滾角，航向角的對準(zhǔn)誤差曲線圖。 當(dāng)GPS 速度中包含野值時(shí)，OBA 法與USQUE 法受到異常觀測矢量的影響，導(dǎo)致姿態(tài)角誤差曲線產(chǎn)生鋸齒狀的抖動。 RUSQUE 法由于改進(jìn)的參考矢量有效得去除了野值，對準(zhǔn)誤差曲線趨于平穩(wěn)。 當(dāng)GPS 速度中沒有野值時(shí)，OBA 法與USQUE 法的航向角誤差曲線有著明顯的差異，OBA 法由于陀螺儀的零偏造成的累積誤差而導(dǎo)致曲線呈現(xiàn)波動特性。 方案2，4 和5 在經(jīng)過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 法的魯棒性以及對于陀螺零偏抑制的有效性。

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 航向角誤差

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

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

圖10 車載實(shí)驗(yàn)平臺

表3 自制的MIMU 傳感器特性

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

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

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

圖13 參考航向角

圖14 GPS 速度輸出

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

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

圖17～圖19 為五種方案的對準(zhǔn)結(jié)果圖。 在野值的影響下，OBA 法與USQUE 法的對準(zhǔn)曲線圖都出現(xiàn)了鋸齒狀的波動，在航向誤差圖中尤為明顯，使得兩種算法的對準(zhǔn)結(jié)果不理想。 但是RUSQUE 算法去除了觀測矢量中的野值，所以對準(zhǔn)誤差曲線呈現(xiàn)了平穩(wěn)的特性。 當(dāng)沒有野值時(shí)，OBA 法的航向角誤差曲線由于陀螺零偏累積誤差呈波動特性，USQUE 法因在狀態(tài)方程中去除了陀螺零偏使得航向角誤差曲線趨于平穩(wěn)。 當(dāng)對準(zhǔn)過程經(jīng)過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 倍，因此跑車實(shí)驗(yàn)也驗(yàn)證了本文提出的算法的魯棒性以及對于陀螺零偏累積誤差的抑制效果。

圖17 俯仰角誤差

圖18 橫滾角誤差

圖19 航向角誤差

6 結(jié)論

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