趙鵬飛 齊建宇

1.北京航天自動控制研究所，北京 100854 2.宇航智能控制技術(shù)國家級重點(diǎn)實(shí)驗(yàn)室，北京 100854

捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)初始對準(zhǔn)的目的是為了獲得載體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系的坐標(biāo)變換矩陣，即姿態(tài)矩陣，對準(zhǔn)精度直接影響慣導(dǎo)系統(tǒng)的導(dǎo)航精度。SINS初始對準(zhǔn)通常可分為粗對準(zhǔn)和精對準(zhǔn)2個階段。在粗對準(zhǔn)階段，利用地球自轉(zhuǎn)角速度和重力加速度作為參考量，利用加速度計及陀螺儀的輸出求取粗略的姿態(tài)矩陣；在精對準(zhǔn)階段，建立導(dǎo)航計算坐標(biāo)系和真實(shí)導(dǎo)航坐標(biāo)系間的失準(zhǔn)角模型，通過卡爾曼濾波估計出失準(zhǔn)角，從而獲得準(zhǔn)確的姿態(tài)矩陣。

經(jīng)典的基于Φ角法或ψ角法推導(dǎo)的誤差模型是在小失準(zhǔn)角條件下獲得的，利用Kalman濾波最優(yōu)估計能有效解決小失準(zhǔn)角條件下的SINS初始對準(zhǔn)問題[1]。文獻(xiàn)[2-3]研究了方位大失準(zhǔn)角對準(zhǔn)誤差模型。建立大失準(zhǔn)角模型的主要目的是為了在方位角未知的情況下使模型更符合實(shí)際。當(dāng)系統(tǒng)加電啟動后，若方位角信息未知，方位失準(zhǔn)角不是小量時，系統(tǒng)模型是非線性的，因此失準(zhǔn)角為小量時的線性誤差方程不能表示大失準(zhǔn)角時的誤差傳播特性。本文在游動方位角坐標(biāo)系下推導(dǎo)了SINS的方位大失準(zhǔn)角誤差模型，并在靜基座和動基座下分別對模型進(jìn)行了驗(yàn)證，結(jié)果表明誤差模型在方位大失準(zhǔn)角情況下是有效的。

1 SINS方位大失準(zhǔn)角誤差模型

記地心慣性系為i系，地球坐標(biāo)系為e系，機(jī)體坐標(biāo)系為b系，導(dǎo)航坐標(biāo)系為n系。由n系依次經(jīng)過3次歐拉角ψG,θ,γ轉(zhuǎn)動到b系。

1.1 導(dǎo)航方程

文中導(dǎo)航坐標(biāo)系采用游動方位角坐標(biāo)系，真實(shí)方位角ψ與游動方位角a的關(guān)系為ψ=ψG-a。SINS導(dǎo)航方程主要由速度更新、位置更新和姿態(tài)更新方程組成。位置矩陣滿足以下微分方程：

速度微分方程為

姿態(tài)矩陣微分方程為

1.2 方位大失準(zhǔn)角誤差模型

為避免誤差模型的非線性問題，方位大失準(zhǔn)角誤差模型公式常用2個方位角誤差變量來表示，例如δsin(a)和δcos(a)[2]或者δsin(ψ)和δ[1-cos(ψ)][4]。關(guān)于這2種不同的方位角誤差變量設(shè)置的對比可在參考文獻(xiàn)[5]中查到。以上2種模型都適用于靜基座對準(zhǔn)，可以通過KF算法進(jìn)行對準(zhǔn)濾波，系泊狀態(tài)下的對準(zhǔn)也可以使用上述模型。另外，也可直接用游動方位角誤差δα作為狀態(tài)變量，通過EKF進(jìn)行對準(zhǔn)濾波。本文的推導(dǎo)使用δα作為狀態(tài)變量。

1.2.1 速度誤差方程

當(dāng)不考慮任何誤差時，速度的理想值由下式確定：

(1)

實(shí)際含誤差的導(dǎo)航解算微分方程為

(2)

其中“s”和“c”表示sin和cos函數(shù)(下同)，令

由加速度計測量，有

(3)

由式(3)得

(4)

式(2)減去式(1)，并將式(4)帶入，忽略δgn的影響，并略去二階小量，得SINS速度誤差方程，

(5)

上述誤差分析中，利用到

其中，h表示慣導(dǎo)高度，Rx和Ry表示沿導(dǎo)航坐標(biāo)系x和y方向的曲率半徑，滿足

注意到式(5)不同于文獻(xiàn)[3]中另一種速度誤差方程的形式：

(6)

1.2.2 姿態(tài)誤差方程

引入向量

(7)

(8)

(9)

(10)

由式(7)和(8)可得：

對應(yīng)向量方程為

(11)

(12)

上式可寫成

(13)

為了避免非線性問題，在精對準(zhǔn)過程中可對游動方位角的正弦函數(shù)和余弦函數(shù)的誤差進(jìn)行建模，狀態(tài)向量采用δsa和δca，系統(tǒng)誤差模型可用線性KF進(jìn)行濾波。而當(dāng)進(jìn)行動基座對準(zhǔn)時，則需要使用EKF非線性模型濾波方法。

另外，位置誤差方程的推導(dǎo)過程中不需要對姿態(tài)誤差角作小角度假設(shè)，將文獻(xiàn)[2]中給出的位置微分方程的諸量用理論值與誤差量之和代替，即得適用于大失準(zhǔn)角的位置誤差方程。

1.2.3 SINS初始對準(zhǔn)濾波模型

(14)

其中F(t)為f(X(t),t)的雅可比矩陣，H(t)為h(X(t),t)的雅可比矩陣。

對式(14)作離散化處理，設(shè)采樣周期為T，離散化后的狀態(tài)方程和量測方程為

當(dāng)T為小量時，

仿照線性卡爾曼濾波基本方程，可寫出擴(kuò)展卡爾曼濾波遞推公式為

2 仿真

進(jìn)行了靜基座初始對準(zhǔn)計算，計算數(shù)據(jù)來源于真實(shí)慣導(dǎo)系統(tǒng)輸出。其中粗對準(zhǔn)采用解析法粗對準(zhǔn)[6]。精對準(zhǔn)使用上文推導(dǎo)的模型進(jìn)行EKF濾波，對準(zhǔn)過程姿態(tài)角變化曲線如圖1所示。圖1說明利用方位大失準(zhǔn)角模型濾波適用于小失準(zhǔn)角的情形。

圖1 靜基座精對準(zhǔn)姿態(tài)角變化

在上述粗對準(zhǔn)結(jié)果中將方位角加入1個失準(zhǔn)角-175°，對準(zhǔn)過程姿態(tài)角變化曲線如圖2所示，可見該模型能迅速修正大方位失準(zhǔn)角。

圖2 方位大失準(zhǔn)角情形

進(jìn)一步分析失準(zhǔn)角分別為-65°,-125°,-175°時方位角的收斂情況，可以看出初始失準(zhǔn)角越大，濾波收斂時間越長。因此，在初始方位角誤差較大時，采用反饋修正的方法不斷減小失準(zhǔn)角誤差，對初始對準(zhǔn)是有益的。當(dāng)失準(zhǔn)角降低至比較小時，濾波模型可由方位大失準(zhǔn)角模型轉(zhuǎn)為使用小失準(zhǔn)角線性模型，從而降低計算量。

圖3 不同失準(zhǔn)角情況的收斂曲線

表1給出了方位失準(zhǔn)角為-65°時，EKF濾波穩(wěn)定后(對準(zhǔn)結(jié)束前30s內(nèi))姿態(tài)誤差的估計均值，表中還給出了使用δsa和δca模型處理同樣數(shù)據(jù)的KF對準(zhǔn)精度。對于靜基座對準(zhǔn)，采用δsa和δca線性模型能夠獲得和采用非線性誤差方程同樣的精度。

表1 靜基座對準(zhǔn)精度比較

本文推導(dǎo)的模型同樣適用于動基座下的初始對準(zhǔn)。采用相同的慣組在運(yùn)動狀態(tài)下的數(shù)據(jù)進(jìn)行初始對準(zhǔn)。對準(zhǔn)開始首先假設(shè)初始姿態(tài)角均為0°，建立初始姿態(tài)矩陣。實(shí)際對準(zhǔn)情況為水平姿態(tài)角在0°附近，方位失準(zhǔn)角未知，符合模型假設(shè)。使用本文建立的模型進(jìn)行Kalman濾波，用反饋校正，初始對準(zhǔn)時間870s。姿態(tài)角誤差曲線如圖4所示，可見水平失準(zhǔn)角收斂較快，方位失準(zhǔn)角收斂較慢。

圖4 姿態(tài)角誤差曲線

3 結(jié)論

本文討論了捷聯(lián)慣導(dǎo)系統(tǒng)在方位大失準(zhǔn)角下的對準(zhǔn)問題。當(dāng)姿態(tài)誤差角不能被看作小角度時，采用經(jīng)典的Φ角法或ψ角法推導(dǎo)的誤差模型將會帶來很大的模型誤差。在游動方位角坐標(biāo)系下推導(dǎo)了方位大失準(zhǔn)角條件下的SINS初始對準(zhǔn)誤差模型，給出了游動方位角坐標(biāo)系下誤差模型的表達(dá)式。

對SINS在方位大失準(zhǔn)角下的靜基座和動基座初始對準(zhǔn)進(jìn)行了試驗(yàn)研究，結(jié)果表明所提出的SINS非線性誤差模型可有效濾除大方位失準(zhǔn)角。方位失準(zhǔn)角越大，濾波收斂所需的時間就越長，可通過反饋修正的方法減小方位失準(zhǔn)角。

參 考 文 獻(xiàn)

[1] 秦永元, 張洪鉞, 汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社,1998.

[2] Pham T M.Kalman Filter Mechanization for INS Airstart[J]. IEEE AES Systems,Jan.1992.

[3] Dmitriyev S P.Nonlinear Filtering Methods Application in INS Alignment[C]. IEEE Transaction on Aerospace and Electronic Systems, Jan. 1997.

[4] Scherzinger B M. Inertial Navigation Error Models for Large Heading Uncertainty[C]. IEEE Position Location and Navigation Symposium, 1996.

[5] Rogers R M. Large Azimuth INS Error Models for In-Motion Alignment[C]. ION National Technical Meeting Paper, 2001.

[6] 陳哲.捷聯(lián)慣導(dǎo)系統(tǒng)原理[M].北京：宇航出版社,1986:128-132.