謝 波，江一夫，嚴(yán)恭敏，陳 勇

（1. 西安航天精密機(jī)電研究所，西安 710100；2. 西北工業(yè)大學(xué)自動化學(xué)院，西安 710072）

捷聯(lián)慣導(dǎo)基于地球系的動基座間接精對準(zhǔn)算法

謝 波1，江一夫1，嚴(yán)恭敏2，陳 勇1

（1. 西安航天精密機(jī)電研究所，西安 710100；2. 西北工業(yè)大學(xué)自動化學(xué)院，西安 710072）

為了將慣性凝固思想延伸到行進(jìn)間精對準(zhǔn)中并提升計(jì)算效率，提出了基于地球系的間接精對準(zhǔn)算法。描述了地球系下的捷聯(lián)慣導(dǎo)/里程計(jì)系統(tǒng)模型和相應(yīng)的Ψ角誤差模型?？紤]安裝偏差角、桿臂等因素，建立了相應(yīng)的卡爾曼濾波方程。六組行車軌跡的行進(jìn)間對準(zhǔn)結(jié)果表明，相對于地理系精對準(zhǔn)算法，地球系間接算法整體對準(zhǔn)性能更加優(yōu)越，系統(tǒng)的穩(wěn)定性和快速性得到提高，在對準(zhǔn)第600 s方位失準(zhǔn)角可以穩(wěn)定在 1 mil(1σ)的誤差限內(nèi)。另外，地球系濾波算法具有更好的初始化參數(shù)適應(yīng)性,有利于工程實(shí)現(xiàn)。

行進(jìn)間對準(zhǔn)；地球坐標(biāo)系；里程計(jì)；卡爾曼濾波

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

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

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

本文先推導(dǎo)地球系下的系統(tǒng)模型和卡爾曼濾波精對準(zhǔn)模型，之后給出地球系間接精對準(zhǔn)算法的詳細(xì)運(yùn)算流程，最后利用試驗(yàn)結(jié)果比較地理系算法和地球系間接算法的對準(zhǔn)性能。結(jié)果表明地球系算法確實(shí)可以提升系統(tǒng)的快速性和穩(wěn)定性，其整體對準(zhǔn)性能更加優(yōu)越，并且卡爾曼濾波器對初始化參數(shù)不敏感，有利于在工程上實(shí)現(xiàn)濾波參數(shù)的固化。

1 地球系下的系統(tǒng)模型描述

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

假設(shè)里程計(jì)和慣組剛性固聯(lián)，以 m表示原點(diǎn)在OD安裝輪軸中心的車體坐標(biāo)系， lb表示m系原點(diǎn)相對 b系原點(diǎn)的安裝桿臂，則里程計(jì)和捷聯(lián)慣導(dǎo)之間存在如圖 1所示的安裝角度關(guān)系和位置關(guān)系。

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

圖1 慣組坐標(biāo)系與車體坐標(biāo)系的相互關(guān)系Fig.1 Relationship between IMU frame and vehicle frame

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

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

2 地球系間接精對準(zhǔn)算法

慣性導(dǎo)航系統(tǒng)誤差模型是卡爾曼濾波器執(zhí)行的數(shù)學(xué)載體，按照通用Ψ角誤差模型，地球系下的捷聯(lián)慣導(dǎo)誤差傳播方程可以表示為[11]：

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

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

而認(rèn)為里程計(jì)與捷聯(lián)慣導(dǎo)之間的安裝桿臂在載車運(yùn)行過程中變化不大，因此直接利用預(yù)標(biāo)定結(jié)果，濾波過程中不再反饋校正。因此，選取如下18維狀態(tài)量：

由IMU捷聯(lián)解算e系速度可以計(jì)算得到圖1所示“o”點(diǎn)的e系地速：

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

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

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

3 地球系間接精對準(zhǔn)方案的具體實(shí)現(xiàn)

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

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

圖2 ECEF系間接精對準(zhǔn)算法的實(shí)現(xiàn)流程Fig.2 Flow chart of ECEF-frame indirect fine-alignment

4 實(shí)驗(yàn)驗(yàn)證

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

行進(jìn)間對準(zhǔn)從載車離開原點(diǎn)時刻開始，完成100 s慣性系粗對準(zhǔn)后分別按照本文所提地球系間接算法和地理系算法開始精對準(zhǔn)，且兩種算法都采用各自固化的最優(yōu)初始化參數(shù)。圖4、5是以靜止精對準(zhǔn)轉(zhuǎn)導(dǎo)航狀態(tài)的姿態(tài)結(jié)果為參考值的方位失準(zhǔn)角收斂曲線。

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

以上結(jié)論說明，地球系間接精對準(zhǔn)算法具有更好的魯棒性和快速性。收斂過程中遇到量測擾動時，方位失準(zhǔn)角不會出現(xiàn)大幅波動?；旧狭M軌跡在對準(zhǔn)第600 s就可以達(dá)到1 mil的方位對準(zhǔn)精度。

圖3 車載實(shí)驗(yàn)行車軌跡Fig.3 Trajectory of vehicular test

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

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

5 結(jié) 論

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

（

）：

[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] 嚴(yán)恭敏，翁浚，白亮，等. 基于慣性參考系的動基座初始對準(zhǔn)與定位導(dǎo)航[J]. 系統(tǒng)工程與電子技術(shù)，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ì)輔助陸用慣導(dǎo)行進(jìn)間對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報，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ìn)間對準(zhǔn)方法[J]. 兵工學(xué)報，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的彈載捷聯(lián)慣導(dǎo)動基座傳遞對準(zhǔn)技術(shù)[J]. 中國慣性技術(shù)學(xué)報，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

國家自然科學(xué)基金（61273333）；總裝備部慣性技術(shù)預(yù)研基金（51309040501）

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