基于強(qiáng)跟蹤濾波的車載行進(jìn)間對(duì)準(zhǔn)

趙小明，趙 帥，郭永剛，王曉亮，周凌峰，王 強(qiáng)

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

基于強(qiáng)跟蹤濾波的車載行進(jìn)間對(duì)準(zhǔn)

趙小明1，趙 帥1，郭永剛1，王曉亮1，周凌峰2，王 強(qiáng)3

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

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

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

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

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

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

1 多重漸消因子強(qiáng)跟蹤濾波

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

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

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

濾波增益方程：

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

估計(jì)均方誤差：

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

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

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

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

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

2 強(qiáng)跟蹤行進(jìn)間對(duì)準(zhǔn)設(shè)計(jì)

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

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

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

2.1 坐標(biāo)系說明

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

b系：車體坐標(biāo)系，方向?yàn)橛仪吧希?/p>

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

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

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

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

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

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

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

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

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

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

2.3 量測(cè)方程建立

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

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

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

實(shí)際里程計(jì)位移輸出在n系下分量為：

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

式中：

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

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

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

2.4 強(qiáng)跟蹤濾波估計(jì)過程

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

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

多漸消因子強(qiáng)跟蹤濾波過程：

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

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

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

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

④ 計(jì)算增益矩陣：

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

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

3 試驗(yàn)驗(yàn)證

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

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

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

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

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

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

4 結(jié) 論

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

（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] 肖烜, 王清哲, 付夢(mèng)印, 等. 里程計(jì)輔助陸用慣導(dǎo)行進(jìn)間對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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初始對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 秦永元, 朱新穎, 趙長(zhǎng)山, 等. 艦載機(jī)捷聯(lián)慣導(dǎo)自對(duì)準(zhǔn)方案設(shè)計(jì)與仿真[J].中國(guó)慣性技術(shù)學(xué)報(bào), 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

國(guó)防重點(diǎn)預(yù)研項(xiàng)目（51309010303）

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