趙小明，趙 帥，郭永剛，王曉亮，周凌峰，王 強

（1. 天津航海儀器研究所，天津 300131；2. 哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001；3. 北京總裝北京軍事代表局駐天津地區(qū)軍事代表室，天津 300131）

基于強跟蹤濾波的車載行進間對準(zhǔn)

趙小明1，趙 帥1，郭永剛1，王曉亮1，周凌峰2，王 強3

（1. 天津航海儀器研究所，天津 300131；2. 哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001；3. 北京總裝北京軍事代表局駐天津地區(qū)軍事代表室，天津 300131）

針對車載行進間對準(zhǔn)過程中存在復(fù)雜路面和未知干擾的情況，提出基于強跟蹤濾波的里程計輔助車載捷聯(lián)慣導(dǎo)行進間對準(zhǔn)方法。采用多重漸消因子的強跟蹤濾波器進行車載行進間精對準(zhǔn)。多重漸消因子的強跟蹤濾波器利用卡爾曼濾波取得最佳增益時殘差序列互不相關(guān)的性質(zhì)，在線自適應(yīng)地調(diào)整漸消因子，對未知干擾有較強的魯棒性。建立行進間對準(zhǔn)的狀態(tài)方程與觀測方程，針對三種不同路況進行了8次跑車行進間對準(zhǔn)試驗。試驗結(jié)果表明：強跟蹤濾波能適應(yīng)惡劣復(fù)雜路況；精對準(zhǔn)后航向誤差(1σ)≤3.6′，滿足指標(biāo)要求。

捷聯(lián)慣導(dǎo)系統(tǒng)；行進間對準(zhǔn)；卡爾曼濾波；多漸消因子強跟蹤濾波

隨著對車載系統(tǒng)的快速反應(yīng)能力、惡劣環(huán)境生存能力的要求越來越高，具備行進間對準(zhǔn)能力的車載捷聯(lián)慣導(dǎo)系統(tǒng)逐漸成為研究熱點。

行進間對準(zhǔn)在行車過程中實現(xiàn)對慣性導(dǎo)航系統(tǒng)的補償和修正，通常需要利用外部設(shè)備（GPS或里程計）提供載體運動信息。GPS能直接提供載體位置和速度，對準(zhǔn)速度快、精度高，但是其信號易受遮擋導(dǎo)致關(guān)鍵時刻可利用性低[1]；相比較，里程計輔助行進間對準(zhǔn)具有全自主性，但對數(shù)據(jù)處理技術(shù)有較高要求。國內(nèi)嚴(yán)恭敏、肖烜[2]等學(xué)者對里程計輔助載車行進間對準(zhǔn)進行了相關(guān)研究。

里程計輔助下的捷聯(lián)慣導(dǎo)行進間對準(zhǔn)包括粗對準(zhǔn)和精對準(zhǔn)。精對準(zhǔn)過程常采用卡爾曼濾波算法，然而實際對準(zhǔn)過程中，由于存在復(fù)雜路面和未知干擾的情況，系統(tǒng)噪聲統(tǒng)計特性經(jīng)常發(fā)生變化，導(dǎo)致卡爾曼濾波性能下降，嚴(yán)重時甚至發(fā)散。本文提出將強跟蹤濾波引入行進間對準(zhǔn)，以克服標(biāo)準(zhǔn)卡爾曼濾波魯棒性差的不足。跑車試驗表明，強跟蹤濾波行進間對準(zhǔn)在復(fù)雜路面情況下有較好的對準(zhǔn)精度。

1 多重漸消因子強跟蹤濾波

多重漸消因子強跟蹤濾波器是在標(biāo)準(zhǔn)卡爾曼濾波技術(shù)上改進的一種濾波算法。標(biāo)準(zhǔn)卡爾曼濾波遞推方程為：

狀態(tài)一步預(yù)測方程：

一步預(yù)測均方誤差方程：

濾波增益方程：

狀態(tài)估計方程：

估計均方誤差：

多重漸消因子強跟蹤濾波算法是在式(2)中引入一個對角矩陣漸消因子，使得不同時刻的殘差序列處處保持正交：

式中：F為狀態(tài)向量X為從tk-1時刻轉(zhuǎn)移到tk時刻的轉(zhuǎn)移矩陣；Dk+1為多重漸消因子的對角矩陣，可由下面方法確定：

式中：αi≥1，i=1,2,…,n 為由先驗知識預(yù)先確定的系數(shù)?？梢栽龃笠子谕蛔儬顟B(tài)分量相應(yīng)的αi，若無系統(tǒng)先驗知識時，取αi=1。式(8)中，

式中：tr[·]為求跡算子；Q為系統(tǒng)噪聲方差陣；R為量測噪聲方差陣；Pk-1為tk-1時刻最優(yōu)濾波值誤差協(xié)方差陣；lk≥1為弱化因子；V0,k為均方誤差值；Hk為tk時刻量測矢量Zk與狀態(tài)向量X間的量測系數(shù)矩陣。

多漸消因子的卡爾曼濾波器針對各數(shù)據(jù)通道以不同的速率進行漸消，可以更好地調(diào)整增益矩陣，即使當(dāng)系統(tǒng)達到穩(wěn)態(tài)時，也能自適應(yīng)地調(diào)整增益矩陣，從而達到增強對模型失配和系統(tǒng)擾動的魯棒性，得到更加精確和穩(wěn)定的估計結(jié)果[3-5]。

2 強跟蹤行進間對準(zhǔn)設(shè)計

車載行進間對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)兩個階段。

在粗對準(zhǔn)階段，參照文獻[6]，建立載體慣性凝固坐標(biāo)系，將姿態(tài)陣分解為三個矩陣求解，根據(jù)陀螺和加速度計的輸出直接解算出姿態(tài)陣：

在精對準(zhǔn)階段：慣導(dǎo)系統(tǒng)提供車輛的姿態(tài)信息；里程計提供載體運動信息，建立失準(zhǔn)角誤差模型，運用濾波器估計出車輛的姿態(tài)失準(zhǔn)角、陀螺常值漂移和加速度計零偏，根據(jù)濾波得到的車輛姿態(tài)失準(zhǔn)角修正輸出的車輛姿態(tài)，根據(jù)陀螺常值漂移與加速度計零偏估計值修正慣性敏感器件誤差初始值。重復(fù)此過程，直至濾波估計量收斂，完成行進間對準(zhǔn)。

2.1 坐標(biāo)系說明

n系：導(dǎo)航坐標(biāo)系，采用當(dāng)?shù)氐乩碜鴺?biāo)系（方向為東北天）；

b系：車體坐標(biāo)系，方向為右前上；

b′系：在慣導(dǎo)系統(tǒng)安裝處的載體系，它與車體固聯(lián)，與b系存在安裝誤差角；

in0系：導(dǎo)航系凝固而成的慣性坐標(biāo)系；

ib0系：載體系凝固而成的慣性坐標(biāo)系。

2.2 狀態(tài)模型建立

假設(shè)導(dǎo)航計算坐標(biāo)系和當(dāng)?shù)氐乩碜鴺?biāo)系間的失準(zhǔn)角誤差為小角度誤差矢量。光纖捷聯(lián)慣導(dǎo)與里程計組成組合導(dǎo)航系統(tǒng)。車載環(huán)境下，主要是由于安裝、使用環(huán)境等因素造成的誤差，包括由慣導(dǎo)系統(tǒng)在載體上的安裝偏差角產(chǎn)生的航向安裝誤差角δψα和俯仰安裝誤差角δθα以及里程計標(biāo)度因子誤差δKD。其中，里程計標(biāo)度因數(shù)誤差與環(huán)境溫度、路面狀況等因素有關(guān)[7]。

設(shè)里程計在載體系下的輸出矢量為：

考慮到慣導(dǎo)在載體上的安裝誤差和標(biāo)度因數(shù)誤差，里程計的路程輸出在b′系下的矢量為：

K為里程計標(biāo)度因數(shù)修正量。上述修正量的誤差記為δψα、δθα和δKD，實際中通常假設(shè)上述誤差量為常值，從而安裝誤差及里程計標(biāo)度誤差模型為：

導(dǎo)航系下捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差模型為：

結(jié)合慣導(dǎo)安裝誤差及里程計標(biāo)度誤差，選取姿態(tài)角誤差、速度誤差、位置誤差、陀螺常值漂移、加速度計零偏和安裝誤差及里程計標(biāo)度來構(gòu)成狀態(tài)變量X：

2.3 量測方程建立

以慣導(dǎo)解算位置增量和里程計位置增量之差作為強跟蹤濾波器的觀測量。慣導(dǎo)系統(tǒng)的位移增量為：

考慮慣導(dǎo)在載體系下的安裝誤差，則有b系和b′系之間的轉(zhuǎn)換關(guān)系。

里程計的位移量在導(dǎo)航坐標(biāo)系上的投影為：

實際里程計位移輸出在n系下分量為：

將式(19)展開，并忽略關(guān)于δψα、δθα和δKD的高階小量得：

式中：

里程計位移增量在n系下的矢量誤差方程：

組合導(dǎo)航系統(tǒng)的觀測方程：

根據(jù)觀測方程建立相應(yīng)的觀測矩陣Hk。

2.4 強跟蹤濾波估計過程

根據(jù)系統(tǒng)誤差模型和觀測量的建立，建立系統(tǒng)的的狀態(tài)方程和量測方程：

式中：X表示系統(tǒng)狀態(tài)；Z表示觀測向量；F和H分別為系統(tǒng)的狀態(tài)函數(shù)和量測函數(shù)，W和V為互不相關(guān)的零均值高斯白噪聲，

多漸消因子強跟蹤濾波過程：

① 狀態(tài)預(yù)測：

② 計算帶多重次優(yōu)漸消因子的對角矩陣，其中，

式中，0≤ρ≤1為遺忘因子。

③ 用式(6)計算預(yù)測狀態(tài)協(xié)方差陣。

④ 計算增益矩陣：

⑤ 計算t時刻狀態(tài)估計協(xié)方差陣和狀態(tài)估計值：

根據(jù)以上步驟，編輯多重漸消因子的強跟蹤濾波導(dǎo)航解算程序。

3 試驗驗證

采用的光纖陀螺零偏穩(wěn)定性為0.01 (°)/h，加速度計零偏穩(wěn)定性為50 μg，捷聯(lián)慣導(dǎo)采樣周期為0.01 s。行進間粗對準(zhǔn)時間50 s，精對準(zhǔn)時間10 min。為繼續(xù)觀察收斂情況，將試驗時間延長至800 s。共進行8組跑車試驗，行車路況包括三種：① 一般路況（第1、6組）；② 上下坡路況（第2、3、5組）；③ 沙土路（第4、7、8組）。試驗過程中對載車加減速過程及行駛速度不做特殊要求。

圖1、圖2和圖3分別為精對準(zhǔn)過程中航向角誤差的收斂曲線。由于俯仰角和橫滾角誤差對準(zhǔn)收斂速度快，而航向角誤差收斂速度慢，因此本文通過研究航向角誤差的對準(zhǔn)精度和對準(zhǔn)時間來說明提出的濾波算法的有效性。從圖1～3中可以看出，第600 s時誤差曲線基本達到平穩(wěn)。取每種工況下第600 s時的航向角誤差列于表1。從表1知，第600 s時使用強跟蹤濾波器對準(zhǔn)的航向角誤差為3.58′（1σ），滿足行進間對準(zhǔn)的精度要求，表明當(dāng)存在復(fù)雜路面和外界擾動時，所設(shè)計的強跟蹤濾波方法能夠較好地估計載車姿態(tài)失準(zhǔn)角，實現(xiàn)行進間對準(zhǔn)。

圖1 第1、6組試驗航向角誤差曲線Fig.1 Experiments 1 and 6 for heading angle

圖2 第2、3、5組試驗航向角誤差曲線Fig.2 Experiments 2, 3 and 5 for heading angle

圖3 第4、7、8組試驗航向角誤差曲線Fig.3 Experiments 4, 7, and 8 for heading angle

表1 對準(zhǔn)600 s時各組航向角誤差Tab. 1 Errors of heading angle at 600 s

4 結(jié) 論

本文提出將強跟蹤濾波算法應(yīng)用于車載行進間對準(zhǔn)，通過分析多重漸消因子強跟蹤濾波的原理，建立車載行進間對準(zhǔn)的狀態(tài)模型和量測模型，設(shè)計出相應(yīng)的強跟蹤濾波器。對不同路況進行了8組跑車試驗，結(jié)果表明，強跟蹤濾波算法適用于包含復(fù)雜路況和外界干擾的車載行進間對準(zhǔn)情況，并從對準(zhǔn)精度和對準(zhǔn)時間上證明了算法的有效性。

（References）：

[1] Hong Woonseon, Han Kyungjun, Lee Chulsoo, Paik Boksu. Three stage in flight alignment with covariance shaping adaptive filter for the strapdown inertial navigation system(SDINS)[C]//AIAA Guidance, Navigation and Control Conference. Toronto, Ontario, Canada, 2010: 355-361.

[2] 肖烜, 王清哲, 付夢印, 等. 里程計輔助陸用慣導(dǎo)行進間對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報, 2012, 20(2): 140-145. Xiao Xuan, Wang Qing-zhe, Fu Meng-ying, et al. INS in-motion alignment for land-vehicle aided by odometer [J]. Journal of Chinese Inertial Technology, 2012, 20(2): 140-145.

[3] Guo Z, Miao L J, Zhao H S, et al. An improved strong tracking UKF algorithm and its application in SINS initial alignment under large azimuth misalignment angles [J]. Acta Aeronautica Sinica, 2014, 35(1): 203-214.

[4] Gao Wei-xi, Miao Ling-juan, Ni Mao-lin, et al. Multiple fading factors Kalman filter for SINS static alignment application[J]. Chinese Journal of Aeronautics, 2011, 24(4): 476-483.

[5] 錢華明, 葛磊, 彭宇, 等. 多漸消因子卡爾曼濾波及其在SINS初始對準(zhǔn)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2012, 20(3): 287-291. Qian Hua-ming, Ge lei, Peng Yu, et al. Multiple fading Kalman filter and its application in SINS initial alignment[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 287-291.

[6] 秦永元, 朱新穎, 趙長山, 等. 艦載機捷聯(lián)慣導(dǎo)自對準(zhǔn)方案設(shè)計與仿真[J].中國慣性技術(shù)學(xué)報, 2008, 16(1): 28-33. Qin Yong-yuan, Zhu Xin-ying, Zhao Chang-shan, et al. Design and simulation on SINS self-alignment for carrier born aircraft[J]. Journal of Chinese Inertial Technology, 2008, 16(1): 28-33.

[7] Georgy J, Noureldin A, Korenberg M, et al. Low-cost three-dimensional navigation solution for PISS/GPS integration using mixture particle filter[J]. IEEE Transactions on Vehicular Technology, 2010, 59(2): 599-615.

In-motion alignment based on strong tracking filter

ZHAO Xiao-ming1, ZHAO Shuai1, WANG Xiao-liang1, ZHOU Ling-feng2, WANG Qiang3
(1. Tianjin Navigation Instrument Research Institute, Tianjin 300131, China; 2. Automation College, Harbin Engineering University, Harbin 150001, China; 3. Beijing Military Representative Assembly of Beijing Military Representative Office in Tianjin Area, Tianjin 300131, China)

A new odometer-aided in-motion alignment algorithm based on strong tracking filter (STF) is proposed for vehicles driving in complicated environment. The STF with multiple fading factors is used in the fine alignment stage based on the property that the sequences of residuals are mutually uncorrelated when Kalman filter gain is optimal. The algorithm can adjust multiple fading factors adaptively, so it is robust to unknown external disturbances. The state equations and observation equations of in-motion alignment algorithm is established, and eight groups of ground-based navigation experiments are carried out in three different road conditions. The results show that this method is effective in complicated environment, and the estimated accuracy of heading error angle is less than 3.6′ (1σ), which satisfy the requirements of the in-motion alignment.

strapdown inertial navigation system; in-motion alignment; Kalman filter; multiple fading factor strong tracking filter

U666.1

A

1005-6734(2015)02-0141-04

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

2011-11-14；

2012-01-20

國防重點預(yù)研項目（51309010303）

趙小明（1961—），男，研究員，博士生導(dǎo)師，從事組合慣性導(dǎo)航系統(tǒng)技術(shù)研究。E-mail：zhaoshuai06074@163.com