基于強跟蹤濾波的車載行進間對準

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

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

基于強跟蹤濾波的車載行進間對準

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

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

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

捷聯慣導系統；行進間對準；卡爾曼濾波；多漸消因子強跟蹤濾波

隨著對車載系統的快速反應能力、惡劣環境生存能力的要求越來越高，具備行進間對準能力的車載捷聯慣導系統逐漸成為研究熱點。

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

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

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

多重漸消因子強跟蹤濾波器是在標準卡爾曼濾波技術上改進的一種濾波算法。標準卡爾曼濾波遞推方程為：

狀態一步預測方程：

一步預測均方誤差方程：

濾波增益方程：

狀態估計方程：

估計均方誤差：

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

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

式中：αi≥1，i=1,2,…,n 為由先驗知識預先確定的系數。可以增大易于突變狀態分量相應的αi，若無系統先驗知識時，取αi=1。式(8)中，

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

多漸消因子的卡爾曼濾波器針對各數據通道以不同的速率進行漸消，可以更好地調整增益矩陣，即使當系統達到穩態時，也能自適應地調整增益矩陣，從而達到增強對模型失配和系統擾動的魯棒性，得到更加精確和穩定的估計結果[3-5]。

2 強跟蹤行進間對準設計

車載行進間對準分為粗對準和精對準兩個階段。

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

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

2.1 坐標系說明

n系：導航坐標系，采用當地地理坐標系（方向為東北天）；

b系：車體坐標系，方向為右前上；

b′系：在慣導系統安裝處的載體系，它與車體固聯，與b系存在安裝誤差角；

in0系：導航系凝固而成的慣性坐標系；

ib0系：載體系凝固而成的慣性坐標系。

2.2 狀態模型建立

假設導航計算坐標系和當地地理坐標系間的失準角誤差為小角度誤差矢量。光纖捷聯慣導與里程計組成組合導航系統。車載環境下，主要是由于安裝、使用環境等因素造成的誤差，包括由慣導系統在載體上的安裝偏差角產生的航向安裝誤差角δψα和俯仰安裝誤差角δθα以及里程計標度因子誤差δKD。其中，里程計標度因數誤差與環境溫度、路面狀況等因素有關[7]。

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

考慮到慣導在載體上的安裝誤差和標度因數誤差，里程計的路程輸出在b′系下的矢量為：

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

導航系下捷聯慣性導航系統的誤差模型為：

結合慣導安裝誤差及里程計標度誤差，選取姿態角誤差、速度誤差、位置誤差、陀螺常值漂移、加速度計零偏和安裝誤差及里程計標度來構成狀態變量X：

2.3 量測方程建立

以慣導解算位置增量和里程計位置增量之差作為強跟蹤濾波器的觀測量。慣導系統的位移增量為：

考慮慣導在載體系下的安裝誤差，則有b系和b′系之間的轉換關系。

里程計的位移量在導航坐標系上的投影為：

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

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

式中：

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

組合導航系統的觀測方程：

根據觀測方程建立相應的觀測矩陣Hk。

2.4 強跟蹤濾波估計過程

根據系統誤差模型和觀測量的建立，建立系統的的狀態方程和量測方程：

式中：X表示系統狀態；Z表示觀測向量；F和H分別為系統的狀態函數和量測函數，W和V為互不相關的零均值高斯白噪聲，

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

① 狀態預測：

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

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

③ 用式(6)計算預測狀態協方差陣。

④ 計算增益矩陣：

⑤ 計算t時刻狀態估計協方差陣和狀態估計值：

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

3 試驗驗證

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

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

圖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 對準600 s時各組航向角誤差Tab. 1 Errors of heading angle at 600 s

4 結 論

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

（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] 肖烜, 王清哲, 付夢印, 等. 里程計輔助陸用慣導行進間對準方法[J]. 中國慣性技術學報, 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初始對準中的應用[J]. 中國慣性技術學報, 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] 秦永元, 朱新穎, 趙長山, 等. 艦載機捷聯慣導自對準方案設計與仿真[J].中國慣性技術學報, 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

國防重點預研項目（51309010303）

趙小明（1961—），男，研究員，博士生導師，從事組合慣性導航系統技術研究。E-mail：zhaoshuai06074@163.com