基于四元數姿態估計的捷聯慣導初始對準算法

周曉仁，趙鶴鳴

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

捷聯慣導系統定位的本質是依靠積分來獲取載體的姿態，速度和位置的信息，所以進行導航的前提是要有三者的初始信息。初始位置和速度可以從全球定位系統(global positioning system，GPS)獲得，而初始姿態獲得的途徑是初始對準，它直接影響到捷聯慣導系統的導航精度。

捷聯慣導系統初始對準的重點在于初始姿態陣的求解，它通常可分為濾波估計和確定性算法兩種方法[1]。初始姿態的確定算法早期研究主要源于解決衛星姿態的確定問題。1967年，Wahba將“根據矢量觀測來確定姿態的問題”描述為帶約束的最小二乘問題，首先給出了表達式，即所謂的Wahba問題[2]。后來許多學者經過對Wahba問題的研究后推導出了眾多在矢量觀測的基礎上求解姿態的算法[3－6]。在確定性算法中，近年來文獻[7－8]提出的基于最優化的對準(optimization-based alignment，OBA)算法以及速度積分公式法在中高精度捷聯慣導系統動基座初始對準中得到了廣泛的應用，該方法通過加入外部輔助設備如GPS或者里程計等的信息，能夠構造出多組的參考矢量與觀測矢量。然后采用最優估計法，可以抑制因為觀測矢量異常而造成的影響并且有效地加快了初始對準的速度[9－13]。然而對于微機電(micro electro mechanical system，MEMS)低精度捷聯慣導系統，其陀螺儀的零偏一般是幾度甚至為幾十度每小時，這就會大幅度加劇觀測矢量的累計誤差，從而導致對準結果不理想。

另一種求解姿態的算法為濾波估計法，最常用的技術就是擴展卡爾曼濾波(extended Kalman filter，EKF)。狀態估計法采用動力學模型對姿態參數及相關誤差參數進行實時估計[14]。但是擴展卡爾曼濾波容易引入線性誤差，從而降低濾波精度，且部分非線性函數的Jacobian矩陣計算困難。文獻[15]提出了無跡卡爾曼濾波(unscented Kalman filter，UKF)，與EKF不同，UKF用一系列近似高斯分布的采樣點，通過無跡變換遞推和更新狀態與誤差協方差，而不需要計算Jacobian矩陣，對非線性方程的精度相當于二階泰勒展開。

濾波估計法的優勢在于能夠把陀螺的零偏加入狀態向量中進行估計，從而能夠抑制陀螺零偏對于觀測矢量的影響。本文采用UKF四元數姿態估計進行初始對準，最后，進行了仿真與跑車實驗驗證了所提算法的正確性。

1 姿態確定初始對準基本原理

取i為對準起始時刻地球系作為初始慣性系，并且不隨地球運動而改變。b為右－前－上載體坐標系；n為東－北－天導航坐標系；n0為對準起始時刻導航坐標系，與對準起始時刻的n系重合，并且相對慣性坐標系不變；b0為對準起始時刻載體坐標系，與對準起始時刻的b系重合，并且相對慣性坐標系不變。慣導系統的比力方程可以表示為:

式中，fb表示比力，gn表示地球重力加速度在導航系的投影。由方向余弦矩陣的鏈式法則可知:

將式(2)代入式(1)并整理可得:

將式(4)兩邊同時積分可得:

式中，

由于GPS采樣時間與捷聯慣性導航系統(strapdown inertial navigation system，SINS)采樣時間不一致，此處假設GPS采樣時間為Δtg，SINS采樣時間為Δts，并且有Δtg＝SΔts(S∈R＋)。以GPS量測更新時刻計算離散化矢量。經過離散化可得

式中，t＝MΔtg，tk＝kΔtg，Δtg＝tk＋1－tk。具體的離散計算方式可以參考文獻[8]。

式(8)是典型的Wahba問題，其對應的Wahba損失函數為

2 姿態估計初始對準基本原理

由第一章的分析可知，姿態確定算法通過求解最優初始姿態矩陣，再結合式(2)、式(3)來計算姿態矩陣。但是，由于陀螺零偏和噪聲的影響造成陀螺儀輸出的值與真值之間存在誤差，由式(2)、式(3)可知在低精度傳感器下姿態陣的準確性將會受到很大的影響。姿態估計是指建立預測模型和量測模型，然后通過濾波算法來對姿態矩陣迭代計算。在本文中，預測模型根據姿態矩陣的更新方程建立，量測模型根據速度積分公式法所建立。在本文中，將陀螺零偏加入到姿態更新方程中可以有效地減少陀螺零偏所帶來的影響。

2.1 預測模型

在姿態估計中，陀螺儀的模型如下所示:

該式為姿態估計的預測模型。本文采用了二子樣算法進行姿態矩陣的更新。記四元數的預測模型為:

2.2 量測模型

由文獻[16]可知，方向余弦矩陣的鏈式法則:

將式(16)代入慣導比力方程式(1)整理可得:

式中，

經過離散化:

該式為姿態估計的量測模型，其中

記四元數的量測模型為:

式中，η為量測噪聲，是均值為零的高斯白噪聲。若，則

3 無跡四元數姿態估計

將UKF用于四元數姿態估計的方法稱為無跡四元數估計器(unscented quaternion estimator，USQUE)。若采用四元數作為狀態變量參與姿態估計會有四元數歸一化的問題。Lefferts提出了成立條件:在姿態誤差比較小的情況下有效，但是由于累計的舍入誤差會影響進一步的運算[17]。所以如果四元數作為狀態變量，那么四元數的歸一化約束條件很有可能不再成立。通常，在USQUE中采用無約束條件的三元素參數在濾波過程中作為狀態向量來進行運算。在所有的三元素參數中，修正羅德里格參數(modified Rodrigues parameters，MRP)的非奇異角度最大(為2π)，所以將MRP作為首選姿態參數[18]。

若有誤差四元數為δq＝[δρTδq4]T，則對應的MRP為:

式中，a為一個取值范圍為0到1的常數，并且f是一個比例因子。在USQUE中，設狀態向量xk＝[δpkεk]。

3.1 算法初始化

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

3.2 預測更新

先計算Sigma點集χk－1:

式中，n為狀態向量的維數，κ為滿足加權和的系數，通常κ＝3－n。向量矩陣中其中第i(i∈[1，2n＋1])個向量的誤差四元數δqk－1(i)＝可以由式(26)的逆運算得到:

將姿態方向余弦矩陣以四元數的形式和陀螺零偏的Sigma點集代入式(15)的姿態更新方程(預測模型)得到預測四元數Sigma點集:

由于陀螺零偏可以視為常值，所以預測陀螺零偏Sigma點集為:

預測姿態四元數取預測四元數Sigma點集的第一組向量:

預測誤差四元數Sigma點集為:

因為，由式(26)得對應的預測MRP的Sigma點集為:

3.3 量測更新

量測預測的Sigma點集為:

量測預測的均值和協方差矩陣為:

式中，Rk為量測噪聲矩陣。互相關協方差矩陣為:

濾波增益為:

狀態估計向量為:

狀態估計協方差陣:

3.4 姿態更新

最優四元數估計為:

4 實驗與分析

4.1 仿真實驗

為驗證本文提出的算法，本節設計了仿真實驗，將USQUE法與OBA法(文獻[8])以及改進無跡四元數估計器(modified unscented quaternion estimator，MUSQUE)(文獻[19])進行對比，從算法的對準精度進行驗證分析。

仿真實驗過程中，傳感器誤差參數設定如表1所示。設定慣性傳感器數據輸出頻率為200 Hz，GPS數據輸出頻率為1 Hz，GPS量測噪聲設置:速度為0.1 m/s，位置為1 m。

表1 傳感器誤差特性

表2 載體運動信息

載體運動軌跡如圖1所示，載體的真實姿態角如圖2、圖3所示。運動過程持續300 s，運動過程中載體進行轉向運動。圖4為載體的運動速度。

圖1 載體運動軌跡圖

圖2 真實水平姿態角

圖3 真實航向角

圖4 載體運動速度

圖5～圖7分別表示俯仰角，橫滾角和航向角的對準誤差曲線。由圖5和圖6可知，三種方法的對準誤差在300 s左右可以達到相似的對準精度，俯仰角誤差小于0.05°，橫滾角誤差小于0.1°。圖7的航向角對準誤差表明，傳統OBA法隨著對準時間的增加會因為陀螺儀的零偏導致的累計誤差使得對準誤差曲線呈現波動特性。而采用USQUE法和MUSQUE法后對準誤差更加穩定。200 s～300 s之間OBA法的航向角誤差均值為0.983 7°，USQUE法的航向角誤差均值為0.462 7°，MUSQUE法的航向角誤差均值為0.566 7°。因此USQUE法的精度相較于傳統OBA法更高且與MUSQUE法的精度相近。

圖5 俯仰誤差

圖6 橫滾誤差

圖7 航向誤差

4.2 跑車實驗

為驗證本文提出的算法，現設計跑車實驗進行驗證。實驗過程中采用自制的微型慣性測量單元(miniature inertial measurement unit，MIMU)，慣性傳感器采樣頻率為200Hz。其陀螺儀和加速度計性能參數參見表3所示。

表3 自制的MIMU傳感器特性

GPS的采樣頻率為1 Hz。在對準之前，系統工作在GPS與慣導組合導航模式下，以光纖慣性導航與GPS組合導航系統的輸出姿態值作為參考值進行精度評估，光纖陀螺和加速度計的零偏穩定性分別為0.01 °/h，100μg，采樣頻率為200 Hz。圖8～圖11分別為車載實驗平臺，載體運動軌跡和載體參考姿態角。圖12為GPS速度輸出，圖13～圖15分別為俯仰角，橫滾角和航向角對準誤差曲線圖。

圖8 車載實驗平臺

圖11 參考航向角

圖12 GPS速度輸出

圖13 俯仰誤差

圖15 航向誤差

圖9 載體運動軌跡

圖10 參考水平姿態角

從圖13～圖14中可以看出三種方法的兩個水平姿態誤差角在經過100 s后都在0.2°以下。圖15的航向角誤差曲線表明了OBA法在陀螺零偏的影響下得到的對準精度不如USQUE法和MUSUQE法。200 s～300 s之間OBA法的航向角誤差均值為0.681 2°，USQUE法的航向角誤差均值為0.201 4°，MUSQUE法的航向角誤差均值為0.130 1°。因此跑車實驗進一步驗證了:在航向角誤差方面，USQUE法具有與MUSQUE法相似的精度，且比OBA算法更優。

圖14 橫滾誤差

5 結論

針對姿態確定性算法在低精度捷聯慣導系統初始對準中易受陀螺零偏的影響，本文研究了一種基于UKF的四元數姿態估計的動基座初始對準算法USQUE。通過對陀螺零偏進行建模并加入狀態向量進行估計，從而減小零偏的影響。通過仿真與跑車實驗，驗證了本文設計的算法在航向角對準精度上相較于傳統的方法更有優勢，并且在精度上與MUSQUE法相似。