魯棒無跡四元數卡爾曼濾波初始對準算法?

周曉仁徐 祥趙鶴鳴

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

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

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

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

1 傳統動基座初始對準基本原理

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

具體的離散形式可以參考文獻[8]。 式中，tM＝MΔtg，tk＝kΔtg，Δtg＝tk＋1－tk。 利用離散的觀測矢量和參考矢量來構建tM時刻的矩陣KM:

結合式(2)～式(4)可以求解tM時刻的載體的姿態矩陣。

2 無跡四元數卡爾曼濾波模型

2.1 預測模型

2.2 量測模型

經過離散化:

3 GPS 野值影響與模值匹配權值函數

GPS 接收機容易受到外部環境的影響比如障礙物，多徑效應等[11]，輸出有時會出現野值，其實際速度測量值與理想值的關系如下式所示:

式中:γ表示高斯閾值，在本文中取γ為2.25。 通過該權值函數可以有效得抑制異常量測對狀態估計的影響。

4 魯棒無跡四元數估計器

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

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

4.1 狀態初始化

設k－1 時刻的狀態最優估計向量為＾xk－1，相應的狀態協方差矩陣為Pk－1，最優估計四元數為＾qk－1。

（1）全新的課表管理。學生可以通過系統獲取自己的課表，查看一起上課的同學的微博，還可以實名或匿名向相關教師提問或建議，也可取代原來的評教方式對教師進行評教。

4.2 預測更新

4.3 量測更新

將預測四元數Sigma 點集代入量測方程(27)得到量測預測的Sigma 點集為:

4.4 姿態更新

5 實驗與分析

5.1 仿真實驗

為驗證本文提出的算法，本節設計了仿真實驗。共有5 種算法進行對比。

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

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

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

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

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

表1 傳感器誤差特性

表2 載體運動信息

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

圖1～圖4 分別為載體的運動軌跡，真實姿態角，運動速度。 運動速度的噪聲在本文中設其分布為:

圖1 載體的運動軌跡

圖2 真實水平姿態角

圖3 真實航向角

圖4 載體運動速度

圖5 改進前后的觀測矢量

圖6 陀螺零偏估計

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

圖7 俯仰角誤差

圖8 橫滾角誤差

圖9 航向角誤差

5.2 跑車實驗

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

圖10 車載實驗平臺

表3 自制的MIMU 傳感器特性

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

圖11 載體運動軌跡

圖12 參考水平姿態角

圖13 參考航向角

圖14 GPS 速度輸出

圖15 改進前后的參考矢量

圖16 陀螺零偏估計

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

圖17 俯仰角誤差

圖18 橫滾角誤差

圖19 航向角誤差

6 結論

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