基于重構偽地球坐標系的捷聯(lián)慣導初始對準算法

劉 猛，高延濱，李光春，趙 丹

（1. 哈爾濱工程大學 自動化學院，哈爾濱 150001；2. 許繼集團有限公司，河南 許昌 461000）

基于重構偽地球坐標系的捷聯(lián)慣導初始對準算法

劉 猛1，高延濱1，李光春1，趙 丹2

（1. 哈爾濱工程大學 自動化學院，哈爾濱 150001；2. 許繼集團有限公司，河南 許昌 461000）

針對捷聯(lián)慣性導航系統(tǒng)的方位誤差對系統(tǒng)誤差特別敏感，容易引起閉環(huán)卡爾曼濾波初始對準的發(fā)散，提出了一種基于重構偽地球坐標系慣導機械編排的初始對準算法。重構偽地球坐標系慣導編排方案在初始位置實現(xiàn)了線性運動誤差和方位誤差之間解耦，從而消除了導航坐標系旋轉角速度誤差對方位對準的影響。因此該算法可以減小由系統(tǒng)誤差引起的方位對準估計振蕩，從而降低了對準系統(tǒng)發(fā)散的可能性，進而提高對準系統(tǒng)的穩(wěn)定性，并改善了捷聯(lián)慣導初始對準的性能。另外，它不僅適用于常規(guī)緯度初始對準，也可以解決極區(qū)靜態(tài)對準問題。最后，常規(guī)緯度和極區(qū)靜態(tài)對準仿真證明了該算法具有優(yōu)良性能。

初始對準；偽地球坐標系；解耦；極區(qū)靜態(tài)對準；捷聯(lián)慣性導航

捷聯(lián)慣性導航（SINS）因其獨特的優(yōu)點被廣泛用于定位、定向和導航。然而在進入導航之前，精確的初始對準必須要完成[1]，其結果的好壞將直接影響導航的精度，因此初始對準對 SINS是極其重要的。另外隨著北極航道的開辟，極區(qū)導航變得越來越重要。但是由于極區(qū)特殊的地理、電磁條件，使得各種導航手段無法正常工作。慣性導航（INS）因其自主性、隱蔽性和信息的完備性使其成為極區(qū)導航的首選[2]。

對于極區(qū)慣性導航，游移方位機械編排、格網坐標機械編排和橫向地球坐標機械編排已經被提出用于解決極區(qū)慣性導航問題。但是慣導的極區(qū)初始對準仍然是一個挑戰(zhàn)[3-5]。因此極區(qū)對準也是極區(qū)慣性導航亟待解決的一個重要的問題。

對于捷聯(lián)慣導初始對準，其通常分為粗對準和精對準兩個階段[6]。在進行對準時，閉環(huán)卡爾曼濾波是精對準階段常用方法之一。然而由于方位對準的不完全可觀測以及線性運動誤差和方位誤差之間耦合，卡爾曼濾波的方位對準對系統(tǒng)誤差特別敏感，容易引起閉環(huán)系統(tǒng)的發(fā)散[7-8]。特別是在較差的粗對準情況下，系統(tǒng)的線性運動誤差對 SINS方位對準的影響將是更嚴重的。另外，由于 SINS東向線性運動的誤差對方位誤差的影響是隨著緯度成正切函數(shù)增加的，因此在極區(qū)一個很小東向線性運動誤差就可能引起非常大方位誤差波動，從而造成對準系統(tǒng)的發(fā)散。這也是極區(qū)對準的一個難點。

為了改善閉環(huán)卡爾曼濾波初始對準的性能，常用的方法有變增益卡爾曼濾波、設置較大的初始噪聲矩陣及提高粗對準精度方法等[9-11]。對于變增益卡爾曼濾波，其增益是很難預測和控制的，而設置較大的量測噪聲矩陣以犧牲對準時間和對準精度為代價來保證系統(tǒng)穩(wěn)定，另外精確的粗對準在極區(qū)也是非常困難得到的。并且我們也可以注意到，所有的這些方法皆是從外部因素考慮解決卡爾曼濾波對準的穩(wěn)定性，例如從粗對準精度、過程噪聲矩陣和量測噪聲矩陣等因素。

針對上述問題，本文提出了一種基于重構偽地球坐標系慣導編排的初始對準算法。該算法是從固有慣性導航誤差模型（內部固有因素）出發(fā)解決捷聯(lián)慣導卡爾曼濾波初始對準問題。最后通過常規(guī)緯度和極區(qū)靜態(tài)對準仿真對該算法進行驗證。

1 偽地球坐標系的定義

假設載體的初始經緯度為 λ0和 φ0，則偽地球坐標系Oxpypzp可以通過原地球坐標系Oxeyeze兩次旋轉得到的。如圖1所示，在偽地球坐標系中，其坐標原點仍在地球中心，Np是偽北極點，Oxp軸與原Oze重合，Oyp軸是指向載體初始位置在赤道平面的投影點，Ozp在赤道平面內垂直于Oyp軸。其具體的旋轉過程為

由以上可知，地球坐標系和偽地球坐標系之間的變換關系為

在偽地球坐標系中，載體位置的偽經緯坐標用 λp和φp來表示。根據(jù)偽地球坐標系的定義，如圖1所示，偽赤道同載體初始位置所在的子午線圈是重合的，則載體的初始偽經緯度為

偽地理坐標系的定義也是同正常地理坐標系相似的。如圖1所示，Oxtpytpztp為偽東-北-天坐標系，其坐標原點在載體的重心，Oxtp與Oytp、Oztp遵從右手定則，Oytp指向偽北向，Oztp沿垂直方向指向天向。則偽地理坐標系和地理坐標系Oxtytzt之間有如下的變換關系：

將式(1)(2)代入式(3)可以得到，在初始位置偽地理坐標系和地理坐標系之間的關系為

由公式(4)可知，在初始位置，偽地理坐標系(偽東-北-天)和傳統(tǒng)的南-東-天地理坐標系是重合的。因此如果在偽地理坐標系進行機械編排來完成靜態(tài)初始對準，它將是容易實現(xiàn)的，且不會增加任何附加的困難。

2 偽地球坐標系捷聯(lián)慣導的機械編排

基于偽地球坐標系慣性導航機械編排與原地球坐標系指北方位系統(tǒng)是相似的。在偽地球坐標系中，捷聯(lián)慣性導航系統(tǒng)則采用偽指北方位系統(tǒng)進行編排，偽地理坐標系被選為導航坐標系，故數(shù)學平臺的指令角速度也將發(fā)生改變。根據(jù)偽地理坐標系的定義可知，僅因地球自轉而引起偽地理坐標系的旋轉角速度和傳統(tǒng)指北方位編排是不一致的，并且可以表示為

式中，Ω表示地球自轉角速度。

根據(jù)上文的分析，偽地球坐標系下的捷聯(lián)慣導機械編排可以表示為如圖2所示，且捷聯(lián)慣導系統(tǒng)的姿態(tài)、速度和位置更新微分方程為

圖2 偽地球坐標系捷聯(lián)慣導系統(tǒng)機械編排Fig.2 Mechanization of strapdown inertial navigation system under pseudo-Earth frame

3 偽地球坐標系捷聯(lián)慣導靜態(tài)誤差方程

根據(jù)偽地球坐標系下捷聯(lián)慣導機械編排，系統(tǒng)的靜態(tài)誤差方程將是容易得到的，則在小失準角的情況下，偽指北方位系統(tǒng)的靜態(tài)誤差方程為

式中：δVEp、δ VNp分別為偽東向和偽北向速度誤差；φxp、φyp、φzp為偽地球坐標系中的平臺姿態(tài)誤差；δλp、δφp為偽經緯度誤差；?Ep、?Np和εEp、εNp、εUp分別為加速度計的零偏和陀螺的漂移在偽地理坐標系中的投影。

4 偽地球坐標系下的靜態(tài)卡爾曼濾波模型

靜基座時，陀螺的漂移和零偏通常被認為是由常值誤差和白噪聲構成，即?˙=0，ε˙=0。忽略位置誤差和天向速度誤差，偽地球坐標系下捷聯(lián)慣導靜基座初始對準的狀態(tài)方程為

式中：X=[δVEpδVNpφxpφypφzp?Ep?NpεEpεNpεUp]T；F(t)為系統(tǒng)狀態(tài)轉移矩陣；W(t)為系統(tǒng)過程噪聲，為N(0,Q)的高斯白噪聲。

由公式(2)可知，在初始位置，偽緯度總為零，即φp0=0°。因此根據(jù)式(9) ～ (15)，偽地球坐標系慣導編排的靜態(tài)初始對準的狀態(tài)轉移矩陣為

分析偽地球坐標系捷聯(lián)慣導初始對準的狀態(tài)轉移矩陣，容易發(fā)現(xiàn)矩陣F21第三行的所有元素都為零。這意味著線性運動誤差和方位誤差之間實現(xiàn)了解耦，因此在偽地球坐標系進行初始對準，可以消除了導航坐標系旋轉角速度誤差（即δω或在傳統(tǒng)編排下為δω）對方位對準的影響。另外，由于偽地球坐標系是根據(jù)載體初始位置重構的，因此它不受載體初始位置的影響，也可用于極區(qū)靜態(tài)初始對準。

在初始對準過程中，由于失準角的存在，系統(tǒng)將會產生線性運動誤差，從而引起導航坐標系旋轉角速度誤差。另外，由于方位對準是不完全可觀測的，因此在傳統(tǒng)指北方位編排下，導航坐標系旋轉角速度誤差將會引較大的方位對準估計振蕩，從而可能會造成閉環(huán)對準系統(tǒng)的發(fā)散，以至于對準失敗。特別當粗對準精度較低的情況下，在卡爾曼濾波對準的開始階段，它的影響將是更加嚴重的。而本文所建議的偽地球坐標系慣導機械編排在初始位置實現(xiàn)了線性運動誤差和方位誤差之間解耦，消除了導航坐標系旋轉角速度誤差對方位對準的影響，因此該算法將具有優(yōu)良的初始對準性能。此外，由于在傳統(tǒng)指北方位編排下，導航坐標系旋轉角速度誤差對方位誤差的影響是隨緯度成正切函數(shù)增加的，所以在極地地區(qū)，其對于閉環(huán)系統(tǒng)的影響將是更嚴重的。因此線性運動誤差和方位誤差之間解耦對極區(qū)初始對準將是非常有利的。由于其不受載體初始位置的影響，故它不僅可以用于常規(guī)緯度靜態(tài)初始對準，同樣也可以用于極地地區(qū)的靜態(tài)初始對準，從而解決極區(qū)靜態(tài)初始對準問題。

綜上所述，在偽地球坐標下進行初始對準是從慣導系統(tǒng)建模方面消除了導航坐標系旋轉角速度誤差對方位對準的影響，從而減小了系統(tǒng)誤差引起估計震蕩，進而提高系統(tǒng)方位對準的穩(wěn)定性。該算法有望輔助其它從外部因素考慮解決卡爾曼濾波對準穩(wěn)定性的方法，進一步改善初始對準性能。

對于卡爾曼濾波模型的量測方程，選取兩個水平偽速度誤差δVEp、δVNp為觀測量，則系統(tǒng)的量測方程為

式中：H=[I2×202×8]；V(t)為系統(tǒng)觀測噪聲，為N(0,R)的高斯白噪聲。

為了驗證基于偽地球坐標系慣導機械編排的初始對準算法的優(yōu)越性能，本節(jié)分別用常規(guī)緯度和極區(qū)靜態(tài)閉環(huán)卡爾曼濾波初始對準仿真對其進行驗證。

5.1 常規(guī)緯度靜態(tài)初始對準仿真

仿真參數(shù)設置如下：初始位置φ0=45°，λ0=126°；載體的真實姿態(tài)角θ=γ=ψ=0°；陀螺常值漂移為0.01 (°)/h，隨機游走系數(shù)為0.001 (°)/h；加速度計常值零偏為1×10-4g，隨機游走系數(shù)為1×10-5g；采樣頻率為100 Hz；卡爾曼濾波步長為0.1 s。

設捷聯(lián)慣導系統(tǒng)粗對準失準角為φx=φy=0.2°，φz=0.5°，分別用正常地理坐標系和偽地理坐標系下兩種機械編排進行仿真，其仿真結果如圖3所示。在圖3中，實線表示使用傳統(tǒng)指北方位編排的初始對準結果，虛線表示使用偽地理坐標系機械編排的初始對準結果，圖中的綠色線表示協(xié)方差矩陣Pk中方位估計誤差的兩個平方根。由圖3可知，在偽地理坐標系下進行初始對準，其估計振蕩明顯比傳統(tǒng)的機械編排小，且收斂時間也要比傳統(tǒng)方法短。因此在相同的外部環(huán)境下，基于偽地球坐標系慣導機械編排的初始對準可以消除導航坐標系旋轉角速度誤差對方位對準的影響，從而減小由系統(tǒng)誤差引起估計震蕩，進而改善靜態(tài)初始對準性能。

5 仿真與分析

圖3 兩種機械編排下的靜態(tài)對準結果對比Fig.3 Comparison on static alignment results under the two mechanizations

然而當粗對準的失準角為φx=φy=0.3°、φz=1°時，仍使用與上文較小粗對準失準角仿真相同的卡爾曼濾波初始噪聲參數(shù)，則在傳統(tǒng)機械編排的卡爾曼估計是發(fā)散的。于是采用較大初始噪聲矩陣來保持卡爾曼濾波器的穩(wěn)定，其仿真結果如圖4所示。圖5為采用與上文較小粗對準失準角仿真相同的卡爾曼濾波初始參數(shù)的偽地球坐標系靜態(tài)初始對準仿真結果。由圖4和圖5可知，使用這兩種算法的方位對準精度和收斂時間分別為3.3′、0.8′和100 s、50 s。因而加大初始噪聲矩陣雖然可以保持濾波系統(tǒng)的穩(wěn)定，但是其收斂速度明顯變慢，而且對準精度也是下降的。因此增大初始噪聲矩陣的方法是以犧牲對準性能為代價來保持初始對準系統(tǒng)的穩(wěn)定，故此方法是不可取的。而在較大的粗對準誤差下，基于偽地球坐標系慣導機械編排的靜態(tài)對準性能仍然是較好的。因此基于偽地球坐標系慣導機械編排的靜態(tài)對準可以減小由系統(tǒng)誤差引起估計振蕩，從而降低了初始對準系統(tǒng)發(fā)散的可能性，并提高系統(tǒng)方位對準的穩(wěn)定性，進而改善捷聯(lián)慣導系統(tǒng)的初始對準性能。

圖 4 較大的初始噪聲矩陣的靜態(tài)對準結果Fig.4 Static alignment results using the noise matrix with larger initial value

圖5 偽坐標系下靜態(tài)對準結果Fig.5 Static alignment results under pseudo frame without correction

5.2 極區(qū)靜態(tài)初始對準仿真

為了進一步驗證偽地球坐標系下初始對準算法優(yōu)越性，對極區(qū)靜態(tài)初始對準也進行了仿真。由于隨著緯度的升高，地理坐標系的地球自轉北向分量越來越小，最終會為零，因此慣性導航靜態(tài)對準在極點和極點附近將是不能實現(xiàn)的，對極點及極點附近的靜態(tài)對準研究也將是無意義的，故將極區(qū)靜態(tài)對準仿真的初始位置設為φ0=89°，λ0=126°。另外考慮到慣性導航靜態(tài)對準系統(tǒng)的方位對準精度不僅與東向陀螺的常值漂移成正比，且與緯度余弦函數(shù)成反比，因此隨著緯度的升高，靜態(tài)對準的精度會下降。故在極區(qū)靜態(tài)對準仿真中選用了較高精度的慣性測量單元，其參數(shù)設置如下：陀螺零偏為 0.001 (°)/h，隨機游走系數(shù)為0.0001 (°)/h；加速度計零偏為5×10-5g，隨機游走系數(shù)為5×10-6g；載體的真實姿態(tài)角θ=γ=ψ=0°；采樣頻率為100 Hz；卡爾曼濾波步長為0.1 s。

由于極地地區(qū)高精度的方位粗對準是難以獲得的，故設其粗對準失準角為φx=φy=0.5°，φz=3°。圖6為采用偽地球坐標系在極地地區(qū)89°緯度下的仿真結果。由圖6可知，在極地地區(qū)，其對準性能仍是比較好的。因此基于重構的偽地球坐標系的初始算法可以用來解決極地靜態(tài)初始對準問題。

圖6 極區(qū)靜態(tài)對準結果Fig.6 Static alignment results in polar region

6 結 論

本文提出了一種基于重構偽地球坐標系的捷聯(lián)慣導初始對準算法。該算法是從固有慣性導航誤差模型（內部固有因素）出發(fā)解決捷聯(lián)慣導卡爾曼濾波初始對準的穩(wěn)定性問題。與傳統(tǒng)機械編排相比，它實現(xiàn)了線性運動誤差和方位誤差之間解耦，從而消除了導航坐標系旋轉角速度誤差對方位對準的影響。因此該算法可以減小由系統(tǒng)誤差引起估計振蕩，從而提高了系統(tǒng)方位對準的穩(wěn)定性，進而改善了靜態(tài)卡爾曼濾波初始對準性能。由于偽地球坐標系的重構不受載體初始位置的影響，所以它也可以解決極地靜態(tài)初始對準問題。常規(guī)緯度靜態(tài)對準和極區(qū)靜態(tài)對準仿真已經驗證了此算法的優(yōu)越性能，結果顯示基于重構偽地球坐標系的捷聯(lián)慣導初始對準算法具有較好的性能。

該算法有望輔助其它從外部因素考慮解決卡爾曼濾波對準穩(wěn)定性的方法，進一步改善初始對準性能。盡管本文只驗證了靜態(tài)基座初始對準，但是該算法也可以用在準靜基座初始對準，并為其它條件下初始對準提供一定的參考價值。

參考文獻（References）：

[1] Chang L, Li J, Chen S. Initial alignment by attitude estimation for strapdown inertial navigation systems[J]. IEEE Transactions on Instrumentation & Measurement, 2015, 64(3): 784-794

[2] 朱啟舉, 秦永元, 周琪. 極區(qū)航空導航綜述[J]. 測控技術, 2014, 33(10): 5-8. Zhu Qi-ju, Qin Yong-yuan, Zhou Qi. Summary of polar air navigation[J]. Measurement & Control Technology, 2014, 33(10): 5-8.

[3] Salychev O S. Applied inertial navigation: problems and solutions[M]. Russia: BMSTU Press, Moscow, 2004, p14-15.

[4] 周琪, 岳亞洲, 張曉冬,等. 極區(qū)飛行間接格網慣性導航算法[J]. 中國慣性技術學報, 2014, 22(1): 18-17. Zhou Qi, Yue Ya-zhou, Zhang Xiao-dong, et al. Indirect grid inertial navigation mechanization for transpolar aircraft[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 18-17.

[5] Li Q, Ben Y, Yu F, et al. System reset of transversal strapdown INS for ship in polar region[J]. Measurement, 2014, 60: 247-257.

[6] Hu J, Cheng X. A new in-motion initial alignment for land-vehicle SINS/OD integrated system[C]//2014 IEEE/ ION Position, Location and Navigation Symposium. 2014: 407-412.

[7] Liu X, Liu X, Song Q, et al. A novel self-alignment method for SINS based on three vectors of gravitational apparent motion in inertial frame[J]. Measurement, 2015, 62: 47-62.

[8] Wu Y, Zhang H, Wu M, et al. Observability of strapdown INS alignment: a global perspective[J]. IEEE Transactions on Aerospace & Electronic Systems, 2011, 48(1): 78-102.

[9] Li Q, Ben Y, Yang J. Coarse alignment for Fiber Optic Gyro SINS with external velocity aid[J]. Optik - International Journal for Light and Electron Optics, 2014, 125(16): 4241-4245.

[10] Lü S, Xie L, Chen J. New techniques for initial alignment of strapdown inertial navigation system[J]. Journal of the Franklin Institute, 2009, 346(10): 1021-1037.

[11] Fang J, Yang S. Study on innovation adaptive EKF for in-flight alignment of airborne POS[J]. IEEE Transactions on Instrumentation & Measurement, 2011, 60(4): 1378-1388.

Initial alignment algorithm for SINS based on reconstructed pseudo-Earth frame

LIU Meng1, GAO Yan-bin1, LI Guang-chun1, ZHAO Dan2
(1. College of Automation, Harbin Engineering University, Harbin 150001, China; 2. XJ Group Corporation, Xuchang 461000, China)

In view that the SINS’s heading error is very sensitive to system errors, which may lead to divergence in the initial alignment by closed-loop Kalman filtering, this paper proposes an initial alignment mechanization based on reconstructed pseudo-Earth frame. The decoupling between the level linear movement error and the heading error is realized at the initial position, eliminating the coordinate rotation angular velocity error’s influence on the azimuth alignment. Therefore the proposed algorithm can decrease the oscillation of azimuth alignment caused by the system errors, and improve the SINS’s alignment stability. In addition, it can also be applied to the static alignment in polar region. Simulations on static alignments in conventional latitude and in polar regions both show that the proposed algorithm has superior performances.

initial alignment; pseudo-Earth frame; decoupling; polar static alignment; strapdown inertial navigation system

U666.1

：A

2016-08-10；

：2016-11-04

中國國家科學技術部國際科技合作項目（2014DFR10010）

劉猛（1988—），男，博士研究生，從事慣性技術研究。E-mail: liumeng_0304@163.com

聯(lián) 系 人：高延濱（1963—），男，教授，博士生導師。E-mail: gaoyanbin@hrbeu.edu.cn

1005-6734(2016)06-0710-06

10.13695/j.cnki.12-1222/o3.2016.06.003