前后向迭代濾波在SINS/OD組合導(dǎo)航中的應(yīng)用

黃湘遠(yuǎn)，湯霞清，武 萌，藺 博

?

前后向迭代濾波在SINS/OD組合導(dǎo)航中的應(yīng)用

黃湘遠(yuǎn)，湯霞清，武 萌，藺 博

（裝甲兵工程學(xué)院控制工程系，北京，100072）

為了提高SINS/OD組合導(dǎo)航的精度，推導(dǎo)了載體坐標(biāo)系下單位時(shí)間內(nèi)位移增量之差的觀測(cè)模型，利用殘差檢測(cè)對(duì)OD信號(hào)進(jìn)行兩層故障檢測(cè)和隔離，使用前后向迭代濾波完成誤差參數(shù)估計(jì)和校正。利用實(shí)車實(shí)驗(yàn)分別對(duì)Kalman濾波、1次迭代濾波、2次迭代濾波進(jìn)行對(duì)比分析，結(jié)果表明2次迭代濾波精度最高，但相對(duì)于1次迭代濾波精度提高效果不夠明顯。從濾波精度和計(jì)算量上綜合考慮，SINS/OD組合導(dǎo)航中1次迭代濾波效果最好。

故障檢測(cè)；后向平滑；里程計(jì)；組合導(dǎo)航

0 引 言

戰(zhàn)車導(dǎo)航中，捷聯(lián)慣導(dǎo)（SINS）和里程計(jì)（OD）組合導(dǎo)航是一種完全自主、精度高、安全性好的方案。由于OD不能提供載體絕對(duì)位置和速度信息，定位誤差隨著時(shí)間增長(zhǎng)而增大，需對(duì)模型建立、故障檢測(cè)、濾波方案等展開更深研究。

SINS/OD組合導(dǎo)航中，需對(duì)SINS誤差和OD安裝誤差、系數(shù)誤差等進(jìn)行估計(jì)和補(bǔ)償[1]。速度量測(cè)中OD輸出微分造成噪聲放大，影響濾波精度[2,3]；位置量測(cè)中可觀測(cè)度低，計(jì)算量大，濾波速度慢[4]；導(dǎo)航系下單位時(shí)間內(nèi)SINS和OD位移增量之差的量測(cè)方案[4,5]，能夠取得不錯(cuò)的效果。

狀態(tài)估計(jì)根據(jù)所用量測(cè)信息的情況，可分為預(yù)測(cè)、濾波和平滑[9~11]。平滑是一種離線處理算法，估計(jì)精度優(yōu)于濾波。曹意[12]設(shè)計(jì)“前向?yàn)V波―后向平滑―前向?yàn)V波”的實(shí)時(shí)濾波框架，獲得了精度高于常規(guī)濾波的實(shí)時(shí)濾波算法。該框架可衍生為多階窗口平滑和多次迭代2種算法，2種方案性能難以直接比較，濾波精度與窗口階數(shù)和迭代次數(shù)無(wú)明顯關(guān)系，需進(jìn)行試湊。

本文推導(dǎo)了載體系下單位時(shí)間內(nèi)位移增量之差的量測(cè)方程，在載體系下進(jìn)行2層故障量測(cè)。設(shè)計(jì)了前后向迭代濾波方案，構(gòu)成新的SINS/OD組合導(dǎo)航算法，基于Kalman濾波、1次迭代濾波、2次迭代濾波進(jìn)行了對(duì)比實(shí)驗(yàn)。

1 SINS/OD組合導(dǎo)航系統(tǒng)模型

1.1 SINS、OD誤差模型

記：地心慣性坐標(biāo)系為i系；地球系為e系；導(dǎo)航系n系為東北天坐標(biāo)系；載體系為b系；里程計(jì)坐標(biāo)系為m系。b系和m系為右前上坐標(biāo)系。

（2）

1.2 載體系下位移增量之差

SINS/OD組合導(dǎo)航中，基于濾波周期內(nèi)的位移增量之差的量測(cè)方案可避免速度或位置方案的缺陷[4]。為了便于OD輸出的故障定位及隔離，本文利用載體系的位移增量代替導(dǎo)航系的位移增量，此時(shí)需推導(dǎo)新的量測(cè)方程。

（7）

（9）

（11）

該方案充分利用OD信息，以積分代替微分，有利于提高信噪比；無(wú)需進(jìn)行真正的航位推算，無(wú)需考慮OD故障的累積影響。OD故障時(shí)，只需拋棄該時(shí)刻的量測(cè)數(shù)據(jù)即可，處理方案較為簡(jiǎn)單。

1.3 組合導(dǎo)航系統(tǒng)設(shè)計(jì)

2 系統(tǒng)觀測(cè)的故障檢測(cè)及隔離

SINS/OD組合導(dǎo)航中，當(dāng)發(fā)生車體側(cè)滑、打滑、顛簸、跳躍等現(xiàn)象時(shí)，OD輸出不能反映車輛運(yùn)動(dòng)情況，系統(tǒng)觀測(cè)可能出現(xiàn)故障。如果不對(duì)故障進(jìn)行有效隔離，可能導(dǎo)致更差的導(dǎo)航結(jié)果。為了實(shí)時(shí)檢測(cè)OD信息的有效性，通常采用殘差檢測(cè)法。

2.2 里程計(jì)故障分類及隔離

判定觀測(cè)有效時(shí)，Kalman濾波進(jìn)行量測(cè)更新，系統(tǒng)使用SINS/OD組合導(dǎo)航模式；判定觀測(cè)無(wú)效時(shí)，Kalman濾波不進(jìn)行量測(cè)更新，拋棄里程計(jì)信息，系統(tǒng)使用純慣性導(dǎo)航模式。該方案較為簡(jiǎn)單，便于操作，但是沒(méi)有充分利用系統(tǒng)觀測(cè)信息。對(duì)OD故障信息進(jìn)一步分類和隔離，引入車輛運(yùn)動(dòng)約束、零速修正等方式可提高導(dǎo)航精度[6,8]。

履帶式戰(zhàn)車運(yùn)動(dòng)中，觀測(cè)異常的主要原因有：OD無(wú)輸出、滑行、打滑、側(cè)滑、快速轉(zhuǎn)向、原地轉(zhuǎn)向、顛簸、跳躍等。OD無(wú)輸出意味著車輛行進(jìn)中OD輸出為零；滑行造成OD輸出小于車輛行駛距離；打滑造成OD輸出大于車輛行駛距離，這3種情況影響前向速度測(cè)量，稱為第1類故障。側(cè)滑、快速轉(zhuǎn)向、原地轉(zhuǎn)向等導(dǎo)致車輛產(chǎn)生橫向運(yùn)動(dòng)，稱為第2類故障。顛簸、跳躍等導(dǎo)致車輛產(chǎn)生高低向運(yùn)動(dòng)，稱為第3類故障。根據(jù)車輛運(yùn)動(dòng)學(xué)約束模型，車輛正常行駛條件下，只有前向速度，沒(méi)有橫向和高低向速度。

3 前后向迭代的實(shí)時(shí)濾波

組合導(dǎo)航系統(tǒng)實(shí)時(shí)對(duì)誤差參數(shù)進(jìn)行估計(jì)和校正。根據(jù)估計(jì)時(shí)刻所用觀測(cè)信息的不同，估計(jì)可分為預(yù)測(cè)、濾波和平滑3類。

3.1 R-T-S平滑算法

濾波利用當(dāng)前及以前的觀測(cè)信息對(duì)當(dāng)前狀態(tài)進(jìn)行估計(jì)，平滑在此基礎(chǔ)上利用以后的觀測(cè)信息進(jìn)行估計(jì)，精度優(yōu)于濾波。平滑可分為R-T-S（Rauch-Tung-Striebel）平滑[9,10]和雙濾波器平滑[11]2類。前者采用遞推形式逆向求解最優(yōu)估計(jì)，原理簡(jiǎn)單，易于實(shí)現(xiàn)，但數(shù)值穩(wěn)定性稍差。后者基于正向和逆向模型分別進(jìn)行濾波和數(shù)據(jù)融合，要求實(shí)際系統(tǒng)存在逆向模型，計(jì)算量較大。由于數(shù)值穩(wěn)定性可通過(guò)平方根濾波、奇異值分解（Singular Value Decomposition，SVD）、UD分解等解決，本文采用R-T-S平滑。

R-T-S平滑可分為前向?yàn)V波和后向迭代2個(gè)過(guò)程。前向?yàn)V波采用卡爾曼濾波，按的順序計(jì)算并存儲(chǔ)狀態(tài)估計(jì)，，誤差協(xié)方差陣，，狀態(tài)轉(zhuǎn)移陣。

R-T-S平滑作為一種離線算法，多用于組合導(dǎo)航數(shù)據(jù)后處理和精度測(cè)評(píng)?？紤]到計(jì)算機(jī)處理速度快速發(fā)展，R-T-S平滑也可應(yīng)用到實(shí)時(shí)濾波中。

3.2 基于R-T-S平滑的實(shí)時(shí)濾波

將實(shí)時(shí)濾波算法和事后平滑算法結(jié)合起來(lái)，基于“前向?yàn)V波―后向平滑―前向?yàn)V波”的框架，可形成一種實(shí)時(shí)濾波算法[12]。

圖1 濾波算法的基本框架

為保證濾波的實(shí)時(shí)性（即需在一個(gè)濾波周期里面完成所有計(jì)算），需合理設(shè)置窗口階數(shù)和迭代次數(shù)。階數(shù)相同的窗口算法和迭代算法的濾波精度難以直接比較，迭代算法需存儲(chǔ)的量較少，本文采用迭代算法。迭代次數(shù)的選擇沒(méi)有有效的理論依據(jù)，只能采用試湊法，本文分別考慮1次迭代和2次迭代2種方案。

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

實(shí)驗(yàn)室將某型光纖陀螺SINS安裝在某戰(zhàn)車上，里程計(jì)安裝在某側(cè)履帶主動(dòng)輪上。陀螺零偏穩(wěn)定性小于0.02 (°)/h，加速度計(jì)偏值重復(fù)性小于0.05 m。行駛過(guò)程中使用高精度GPS導(dǎo)航芯片進(jìn)行實(shí)時(shí)位置測(cè)量。戰(zhàn)車在某場(chǎng)地行駛1 h左右，行駛距離約為18 km，行駛路線如圖2所示，進(jìn)行2次獨(dú)立實(shí)驗(yàn)。行駛過(guò)程包括加速、勻速、轉(zhuǎn)彎、上下坡、顛簸路面等。

圖2 試驗(yàn)跑車路線

實(shí)驗(yàn)過(guò)程中，以載體系下1 s內(nèi)位置增量之差作為系統(tǒng)量測(cè)，利用殘差統(tǒng)計(jì)量進(jìn)行2層故障檢測(cè)。為了驗(yàn)證前后向迭代濾波的有效性，分別設(shè)計(jì)Kalman濾波和1次、2次迭代算法的對(duì)比實(shí)驗(yàn)。

圖3 第1次跑車中位置誤差

圖4 第2次跑車中位置誤差

圖3、圖4給出了2次跑車過(guò)程中，緯度誤差和經(jīng)度誤差，其中參考位置由高精度GPS給出，高度方向由于沒(méi)有好的參考值不做比較。=0，1，2表示迭代次數(shù)，其中，=0代表Kalman濾波。短時(shí)間里，3種算法的估計(jì)結(jié)果相差不大，隨著時(shí)間的增長(zhǎng)，各算法誤差逐步體現(xiàn)出來(lái)。Kalman濾波的誤差大于1次和2次迭代濾波，表明引入前后向迭代能夠有效提高估計(jì)精度；1次迭代和2次迭代結(jié)果相差不大，表明經(jīng)過(guò)1次迭代后，再次迭代對(duì)濾波精度的提高有限。

1次前后向迭代濾波包括平滑和濾波2個(gè)過(guò)程，而平滑和濾波的計(jì)算量相當(dāng)，因此次迭代濾波的計(jì)算量相當(dāng)于Kalman濾波的（2+1）倍。從濾波精度和計(jì)算量上綜合考慮，SINS/OD組合導(dǎo)航中，1次前后向迭代濾波效果較好。

5 結(jié)束語(yǔ)

為了提高SINS/OD的導(dǎo)航精度，在導(dǎo)航過(guò)程中對(duì)慣性器件參數(shù)進(jìn)行實(shí)時(shí)估計(jì)和反饋。以載體系下單位時(shí)間內(nèi)的位置增量之差作為系統(tǒng)量測(cè)，引入2層故障量測(cè)方案，有效對(duì)里程計(jì)故障進(jìn)行定位和隔離。重點(diǎn)對(duì)比分析了前后向迭代次數(shù)對(duì)導(dǎo)航精度的影響，結(jié)果表明1次迭代修正效果最為明顯，2次迭代提高精度有限。

[1] 張小躍, 楊功流, 張春熹. 捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航方法算法[J]. 北京航空航天大學(xué)學(xué)報(bào), 2013, 39(7): 922-926.

[2] Wu Y X, Wu M P, Hu X P, et al. Self-calibration for land navigation using inertial sensors and odometer: observability analysis[C]. Chicago Illinois: AIAA Conference of Guidance, Navigation and Control, 2009: 1-10.

[3] Santana D D S, Furukawa C M, Maruyama N. Sensor fusion with low-grade inertial sensors and odometer to estimate geodetic coordinates in environments without GPS signal[J]. IEEE Latin America Transactions, 2013, 11(4): 1015-1021.

[4] 付強(qiáng)文, 秦永元, 周琪. 改進(jìn)量測(cè)的車載捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航算法[J]. 測(cè)控技術(shù), 2013, 32(7): 134-137.

[5] 肖煊, 王清哲, 程遠(yuǎn), 等. 捷聯(lián)慣導(dǎo)系統(tǒng)/里程計(jì)高精度緊組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2012, 33(4): 395-400.

[6] 翁浚, 成研, 秦永元, 等. 車輛運(yùn)動(dòng)約束在 SINS/ OD系統(tǒng)故障檢測(cè)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(3): 406-410.

[7] 吳有龍, 王曉鳴, 曹鵬. 抗差估計(jì)及Allan方差在車載組合導(dǎo)航系統(tǒng)中的應(yīng)用研究[J]. 兵工學(xué)報(bào), 2013, 34(7): 889-895.

[8] 趙洪松, 繆玲娟, 沈軍. 捷聯(lián)慣導(dǎo)/里程計(jì)高精度組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2014, 35(4): 433-440.

[9] Simo S. Unscented Rauch-Tung-Striebel smoother[J]. IEEE Trans. on Automatic Control, 2008, 53(3): 845-849.

[10] Wang X X, Pan Q, Liang Y, et al. Application of unscented transformation for nonlinear state smoothing[J]. Acta Automatica Sinica, 2012, 38(7): 1107-1112.

[11] 劉帥, 孫付平, 李海峰, 等. 前后向平滑算法在精密單點(diǎn)定位/INS緊組合數(shù)據(jù)后處理中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(1): 85-91.

[12] 曹意, 茅旭初. 基于非線性濾波和平滑相迭代的GPS定位估計(jì)算法[J]. 上海交通大學(xué)學(xué)報(bào), 2009, 43(7): 1108-1112.

Application of Forward-backward Iterative Filter inSINS/OD Integrated Navigation

Huang Xiang-yuan, Tang Xia-qing, Wu Meng, Lin Bo

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing, 100072)

To improve the navigation precision of SINS/OD integrated navigation system, the observation model of the deference between displacement increments per unit time in the body coordinate system is derived. The paper conducts the two-layer fault detection and isolation for the OD’s signal using residualdetection, and estimates and corrects of the error parameters by using forward-backward iterative filter. Vehicle test is conducted to comparatively analyze Kalman filter, one-iteration and two-iteration filters. The results show that the filtering precision of two-iteration filter is the best, which doesn’t have an obvious improvement compared with one-iteration filter. For the comprehensive consideration of filtering precision and computational complexity, the filtering effect of one-iteration filer in SINS/OD integrated navigation system is the best.

Failure detection; Backward smoothing; Odometer; Integrated navigation

1004-7182(2016)03-0063-05

10.7654/j.issn.1004-7182.20160315

U666.1

A

2015-06-24；

2015-10-19

軍隊(duì)計(jì)劃項(xiàng)目資助課題

黃湘遠(yuǎn)（1988-），男，博士研究生，主要從事非線性濾波、慣性導(dǎo)航的研究