捷聯慣導基于地球系的動基座間接精對準算法

謝 波，江一夫，嚴恭敏，陳 勇

（1. 西安航天精密機電研究所，西安 710100；2. 西北工業大學自動化學院，西安 710072）

捷聯慣導基于地球系的動基座間接精對準算法

謝 波1，江一夫1，嚴恭敏2，陳 勇1

（1. 西安航天精密機電研究所，西安 710100；2. 西北工業大學自動化學院，西安 710072）

為了將慣性凝固思想延伸到行進間精對準中并提升計算效率，提出了基于地球系的間接精對準算法。描述了地球系下的捷聯慣導/里程計系統模型和相應的Ψ角誤差模型。考慮安裝偏差角、桿臂等因素，建立了相應的卡爾曼濾波方程。六組行車軌跡的行進間對準結果表明，相對于地理系精對準算法，地球系間接算法整體對準性能更加優越，系統的穩定性和快速性得到提高，在對準第600 s方位失準角可以穩定在 1 mil(1σ)的誤差限內。另外，地球系濾波算法具有更好的初始化參數適應性,有利于工程實現。

行進間對準；地球坐標系；里程計；卡爾曼濾波

慣導系統受限于工作環境，往往需要在動態條件下完成初始對準。行車狀態下的對準不可避免地需要外部設備提供的載體運動信息以濾除行進狀態下的機動加速度和干擾加速度[1]。由于里程計(OD)輸出噪聲小、成本低廉、安裝簡易，同時與慣性傳感器一樣，具有自主、隱蔽、高采樣率等特點，因而常作為車載慣性基定位定向系統的輔助信息源，本文也主要針對里程計輔助的車載捷聯慣導系統行進間對準。

為了提高對準快速性并保證誤差模型的魯棒性，工程上的行進間對準遵循先粗對準后精對準的流程。文獻[2-4]應用慣性凝固思想，較好地解決了動態條件下粗略姿態信息快速獲取的問題。但對于后續精對準，絕大多數學者[5-7]都選擇利用濾波算法直接估計相對地理系的姿態。為了將慣性系算法原理延伸到精對準中，文獻[8-9]提出了引入慣性坐標系作為中間坐標系的間接算法，但兩篇文獻都沒有說明這種算法的優勢所在，也沒有給出慣性系的重力模型和地球引力加速度誤差模型，需要在不同坐標系之間的反復轉換導航參數，這無疑增加了算法的繁復度和計算量。

事實上，地球系間接對準也是相對宏觀空間的一種對準算法，地球相對慣性空間的單一自轉角運動決定了地球系算法原理和慣性凝固思想在本質上的等同性。受此啟發，本文詳細推導了里程計輔助下的地球系間接精對準算法。一方面，地球系算法將粗對準中慣性凝固思想延伸到了精對準中，提升了精對準的魯棒性和抗干擾性；另一方面，相對地理系算法，地球系的捷聯解算和卡爾曼濾波運算可以分別提升40%和10%的計算效率[10]，有利于在工程實踐中實現一些復雜的高精度算法。

本文先推導地球系下的系統模型和卡爾曼濾波精對準模型，之后給出地球系間接精對準算法的詳細運算流程，最后利用試驗結果比較地理系算法和地球系間接算法的對準性能。結果表明地球系算法確實可以提升系統的快速性和穩定性，其整體對準性能更加優越，并且卡爾曼濾波器對初始化參數不敏感，有利于在工程上實現濾波參數的固化。

1 地球系下的系統模型描述

由于SINS/OD系統以地球系作為參考系，那么在新的坐標系下重新描述系統模型是非常必要的。以 e表示地球系，b表示捷聯慣導坐標系，利用SINS輸出量測可以寫出e系地心位置矢量 re、e系地速 ve以及姿態矩陣的動態微分方程：

假設里程計和慣組剛性固聯，以 m表示原點在OD安裝輪軸中心的車體坐標系， lb表示m系原點相對 b系原點的安裝桿臂，則里程計和捷聯慣導之間存在如圖 1所示的安裝角度關系和位置關系。

里程計測量的是圖1所示“o”點在m系的前向速度，同時考慮車輛運動約束，則里程計的輸出 yod滿足

圖1 慣組坐標系與車體坐標系的相互關系Fig.1 Relationship between IMU frame and vehicle frame

式中， fv表示將OD輸出轉換為載車前向速度的標度因數。假設m系相對b系存在俯仰安裝偏差角 αθ、橫滾安裝偏差角 αγ和方位安裝偏差角 αψ，此時式(3)可以改寫為：

式(4)也說明橫滾安裝偏差角 αγ與OD量測無關。

2 地球系間接精對準算法

慣性導航系統誤差模型是卡爾曼濾波器執行的數學載體，按照通用Ψ角誤差模型，地球系下的捷聯慣導誤差傳播方程可以表示為[11]：

式中， δre表示地球系地心位置矢量誤差， δve表示地球系地速誤差，φe為姿態角誤差矢量，εb、▽b分別為陀螺常值漂移和加計零偏。重力加速度誤差矢量 δge由兩部分組成：一是由不精確的瞬時載體位置計算重力加速度產生的計算誤差 Δge；二是地球各點的擾動重力矢量 δgn，其中包括垂線偏差和重力異常。一般情況下忽略重力擾動矢量或者將其考慮為e系加計誤差▽e的一部分，把兩者作為一個整體來對待；重力加速度計算誤差 Δge的一階近似表達式可以寫為：

考慮到人員數量變動、車體減振等因素的影響，捷聯慣導與里程計之間的姿態安裝角是實時變化的；同時不同行駛路面條件、環境溫度、輪胎狀況和履帶松緊將引起OD標度因數的變化。因此在建立Kalman濾波模型時，將安裝偏差角估計誤差矢量η和OD標度因數誤差 Δfv作為狀態量的一部分，實際中假設上述誤差量為未知的隨機常值，即：

而認為里程計與捷聯慣導之間的安裝桿臂在載車運行過程中變化不大，因此直接利用預標定結果，濾波過程中不再反饋校正。因此，選取如下18維狀態量：

由IMU捷聯解算e系速度可以計算得到圖1所示“o”點的e系地速：

式中， ?od為里程計的輸入噪聲，以兩者速度之差作為觀測量，同時忽略高階誤差項，可得：

至此可以寫出卡爾曼濾波器的系統方程和量測方程：

式中， F (t)為系統矩陣，可以由式(5)(7)得到； Hk為量測矩陣，可以由式(10)得到； w (t)、?k分別為系統噪聲和量測噪聲。

3 地球系間接精對準方案的具體實現

行進間精對準算法需要在已知粗略姿態信息的條件下才能達到線性最優，本文動態條件下的粗對準依然利用慣性凝固的思想來實現，在文獻[2]中已經給出了具體算法。本文在100 s慣性系粗對準后初始化卡爾曼濾波器，之后切換到地球系行進間間接精對準開始實時姿態估計，同時反饋校正器件誤差。最終相對地理系的姿態矩陣通過兩個矩陣間接計算得到。

算法的具體流程如圖2所示。

圖2 ECEF系間接精對準算法的實現流程Fig.2 Flow chart of ECEF-frame indirect fine-alignment

4 實驗驗證

車載實驗采用的慣導系統陀螺零偏為0.03 (°)/h，加計零偏在100 μg的水平，里程計分辨率約為43 mm/脈沖。為了驗證地球系Kalman濾波精對準算法的有效性，分三次啟電進行了三組車載實驗，每組實驗包括一段自由行車軌跡和一段矩形軌跡。圖3給出了6段行車軌跡的具體路徑，其中前三段軌跡為自由行車軌跡，后三段為設定的矩形軌跡，*和o分別表示軌跡的起訖點。

行進間對準從載車離開原點時刻開始，完成100 s慣性系粗對準后分別按照本文所提地球系間接算法和地理系算法開始精對準，且兩種算法都采用各自固化的最優初始化參數。圖4、5是以靜止精對準轉導航狀態的姿態結果為參考值的方位失準角收斂曲線。

圖4給出了前三組自由行車軌跡的方位失準角收斂曲線，圖5為后三組矩形軌跡的方位角估計誤差曲線。由圖4和圖5可以發現，對于軌跡2、5、6，地球算法的方位精度在170 s左右就已經穩定在相當高的一個水平，而地理系算法仍然存在大幅的誤差波動。同樣，對于軌跡 1、3、4，地球系間接算法的收斂趨勢也要平穩得多。雖然兩種算法在700 s左右對準精度基本一致，但是地球系算法能更早的穩定在1 mil(1σ)的誤差限內。另外，離線處理軌跡數據可以明顯發現，地理系濾波算法對初始化參數的變化非常敏感，小幅改變初始條件對系統性能影響很大，而地球算法卻對濾波初始化參數具有很好的適應性，這也是地球系算法在工程應用上的優勢所在。

以上結論說明，地球系間接精對準算法具有更好的魯棒性和快速性。收斂過程中遇到量測擾動時，方位失準角不會出現大幅波動。基本上六組軌跡在對準第600 s就可以達到1 mil的方位對準精度。

圖3 車載實驗行車軌跡Fig.3 Trajectory of vehicular test

圖4 三組自由行車軌跡的方位失準角Fig.4 Yaw errors of the three free traveling paths

圖5 三組矩形軌跡的方位失準角Fig.5 Yaw errors of the three rectangle paths

5 結 論

本文將慣性凝固算法原理延伸到了行進間精對準，為慣性系粗對準提出了一種基于地球參考系的后續間接精對準算法，提升了精對準卡爾曼濾波器的魯棒性和抗干擾性。六組車載實驗表明，相對地理系濾波算法，本文所提間接精對準算法收斂趨勢更加平穩，穩定時間更短，極大地提升了整體對準性能。由實驗結果可以看出，地球系算法在對準第600 s就能滿足優于1 mil的方位對準精度要求。另外算法大幅提升了計算效率，對動態條件下濾波參數初始化的要求也寬松許多，有利于算法的工程實現。

（

）：

[1] Silson P. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J]. IEEE Transactions on instrument and measurement, 2011, 60(6): 1930-1941.

[2] 嚴恭敏，翁浚，白亮，等. 基于慣性參考系的動基座初始對準與定位導航[J]. 系統工程與電子技術，2011，33 (3)：618-621.

YAN Gong-min, WENG Jun, BAI Liang, et al. Initial in-movement alignment and position determination based on inertial reference frame[J]. Systems Engineering and Electronics, 2011, 33(3): 618-621.

[3] Wu Y X, Pan X F. Velocity/position integration formula part I: application to in-flight coarse alignment[J]. IEEE Transactions on Aerospace and Electronic Systems, 2013, 49(2): 1006-1023.

[4] Jiang Y F, Xie B, Weng J. SINS in-motion alignment and position determination for land-vehicle based on quaternion Kalman filter[C]// The 32nd Chinese Control Conference. Xi’an, China, July 26-28, 2013.

[5] 肖烜，王清哲，付夢印，等. 里程計輔助陸用慣導行進間對準方法[J]. 中國慣性技術學報，2012，20(2)：140-145.

XIAO Xuan, WANG Qing-zhe, FU Meng-yin, et al. INS in-motion alignment for land-vehicle aided by odometer[J]. Journal of Chinese Inertial Technology, 2012 20(2): 140-145.

[6] Seo J, Lee H K, Lee J G, et al. Lever arm compensation for GPS/INS/odometer integrated system[J]. International Journal of Control, Automation, and Systems, 2006 4(2): 247-254.

[7] Ali J, Ushaq M. A consistent and robust Kalman filter design for in-motion alignment of inertial navigation system[J]. Measurement 2009 42: 577-582.

[8] 繆玲娟，高偉熙，沈軍，等. 基于慣性系采用 Kalman濾波的車載SINS行進間對準方法[J]. 兵工學報，2013，34(2)：143-148.

MIAO Ling-juan, GAO Wei-xi, SHEN Jun, et al. SINS initial alignment algorithm base on inertial frame[J]. Acta Armamentarii, 2013 34(2): 143-148.

[9] 吳楓，秦永元，成妍. 基于GPS的彈載捷聯慣導動基座傳遞對準技術[J]. 中國慣性技術學報，2013，21(1)：56-60.

WU Feng, QIN Yong-yuan, CHENG Yan. Transfer alignment for missile-borne SINS using airborne GPS on moving base[J]. Journal of Chinese Inertial Technology, 2013 21(1): 56-60.

[10] Wei M, Schwarz K P. A strapdown inertial algorithm using an earth-fixed Cartesian frame[J]. Journal of the institute of navigation, 1990, 37(2):153-167.

[11] Godha S. Performance evaluation of low cost MEMS-based IMU integrated with GPS for land vehicle navigation application[D]. Calgary: University of Calgary, 2006.

Indirect fine-alignment algorithm of in-motion SINS based on ECEF-frame

XIE-Bo1, JIANG Yi-fu1, YAN Gong-min2, CHEN Yong1
(1. Xi’an aerospace precision electromechanical institute, Xi’an 710100, China; 2. College of Automation, Northwestern Polytechnical University, Xi’an 710072, China)

For the purpose of extending the application of inertia freezing methodology into moving-base fine-alignment and promoting computational efficiency, an indirect fine-alignment algorithm is presented which introduces earth-centered and earth-fixed (ECEF) frame as intermediate reference. The mathematical formulation of the inertial sensor/odometer integration system is given within ECEF frame, and the corresponding ψ-angle error model is outlined. By taking the SINS-vehicle misaligning angle and lever arm into account, the derivation of the appropriate Kalman filter (KF) is discussed. The proposed algorithm is verified through experimental data collected from six traveling routes. In comparison with geographic-frame KF algorithm, the ECEF-frame indirect approach has shorter stabilizing time and performs better in robustness, and a better overall accuracy of 1 mil(1σ) azimuth error within 600 s can be achieved. The ECEF-frame KF is less sensitive to changes of initialization parameters, which is useful for practical application.

in-motion alignment; ECEF frame; odometer; Kalman filter

1005-6734(2014)05-0593-04

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

U666.1

A

2014-05-18；

2014-09-03

國家自然科學基金（61273333）；總裝備部慣性技術預研基金（51309040501）

謝波（1978—），男，高級工程師，主要從事慣性導航技術研究。E-mail：reyoble@126.com