前后向迭代濾波在SINS/OD組合導航中的應用

黃湘遠，湯霞清，武 萌，藺 博

?

前后向迭代濾波在SINS/OD組合導航中的應用

黃湘遠，湯霞清，武 萌，藺 博

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

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

故障檢測；后向平滑；里程計；組合導航

0 引 言

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

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

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

本文推導了載體系下單位時間內位移增量之差的量測方程，在載體系下進行2層故障量測。設計了前后向迭代濾波方案，構成新的SINS/OD組合導航算法，基于Kalman濾波、1次迭代濾波、2次迭代濾波進行了對比實驗。

1 SINS/OD組合導航系統模型

1.1 SINS、OD誤差模型

記：地心慣性坐標系為i系；地球系為e系；導航系n系為東北天坐標系；載體系為b系；里程計坐標系為m系。b系和m系為右前上坐標系。

（2）

1.2 載體系下位移增量之差

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

（7）

（9）

（11）

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

1.3 組合導航系統設計

2 系統觀測的故障檢測及隔離

SINS/OD組合導航中，當發生車體側滑、打滑、顛簸、跳躍等現象時，OD輸出不能反映車輛運動情況，系統觀測可能出現故障。如果不對故障進行有效隔離，可能導致更差的導航結果。為了實時檢測OD信息的有效性，通常采用殘差檢測法。

2.2 里程計故障分類及隔離

判定觀測有效時，Kalman濾波進行量測更新，系統使用SINS/OD組合導航模式；判定觀測無效時，Kalman濾波不進行量測更新，拋棄里程計信息，系統使用純慣性導航模式。該方案較為簡單，便于操作，但是沒有充分利用系統觀測信息。對OD故障信息進一步分類和隔離，引入車輛運動約束、零速修正等方式可提高導航精度[6,8]。

履帶式戰車運動中，觀測異常的主要原因有：OD無輸出、滑行、打滑、側滑、快速轉向、原地轉向、顛簸、跳躍等。OD無輸出意味著車輛行進中OD輸出為零；滑行造成OD輸出小于車輛行駛距離；打滑造成OD輸出大于車輛行駛距離，這3種情況影響前向速度測量，稱為第1類故障。側滑、快速轉向、原地轉向等導致車輛產生橫向運動，稱為第2類故障。顛簸、跳躍等導致車輛產生高低向運動，稱為第3類故障。根據車輛運動學約束模型，車輛正常行駛條件下，只有前向速度，沒有橫向和高低向速度。

3 前后向迭代的實時濾波

組合導航系統實時對誤差參數進行估計和校正。根據估計時刻所用觀測信息的不同，估計可分為預測、濾波和平滑3類。

3.1 R-T-S平滑算法

濾波利用當前及以前的觀測信息對當前狀態進行估計，平滑在此基礎上利用以后的觀測信息進行估計，精度優于濾波。平滑可分為R-T-S（Rauch-Tung-Striebel）平滑[9,10]和雙濾波器平滑[11]2類。前者采用遞推形式逆向求解最優估計，原理簡單，易于實現，但數值穩定性稍差。后者基于正向和逆向模型分別進行濾波和數據融合，要求實際系統存在逆向模型，計算量較大。由于數值穩定性可通過平方根濾波、奇異值分解（Singular Value Decomposition，SVD）、UD分解等解決，本文采用R-T-S平滑。

R-T-S平滑可分為前向濾波和后向迭代2個過程。前向濾波采用卡爾曼濾波，按的順序計算并存儲狀態估計，，誤差協方差陣，，狀態轉移陣。

R-T-S平滑作為一種離線算法，多用于組合導航數據后處理和精度測評。考慮到計算機處理速度快速發展，R-T-S平滑也可應用到實時濾波中。

3.2 基于R-T-S平滑的實時濾波

將實時濾波算法和事后平滑算法結合起來，基于“前向濾波―后向平滑―前向濾波”的框架，可形成一種實時濾波算法[12]。

圖1 濾波算法的基本框架

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

4 實驗驗證

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

圖2 試驗跑車路線

實驗過程中，以載體系下1 s內位置增量之差作為系統量測，利用殘差統計量進行2層故障檢測。為了驗證前后向迭代濾波的有效性，分別設計Kalman濾波和1次、2次迭代算法的對比實驗。

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

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

圖3、圖4給出了2次跑車過程中，緯度誤差和經度誤差，其中參考位置由高精度GPS給出，高度方向由于沒有好的參考值不做比較。=0，1，2表示迭代次數，其中，=0代表Kalman濾波。短時間里，3種算法的估計結果相差不大，隨著時間的增長，各算法誤差逐步體現出來。Kalman濾波的誤差大于1次和2次迭代濾波，表明引入前后向迭代能夠有效提高估計精度；1次迭代和2次迭代結果相差不大，表明經過1次迭代后，再次迭代對濾波精度的提高有限。

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

5 結束語

為了提高SINS/OD的導航精度，在導航過程中對慣性器件參數進行實時估計和反饋。以載體系下單位時間內的位置增量之差作為系統量測，引入2層故障量測方案，有效對里程計故障進行定位和隔離。重點對比分析了前后向迭代次數對導航精度的影響，結果表明1次迭代修正效果最為明顯，2次迭代提高精度有限。

[1] 張小躍, 楊功流, 張春熹. 捷聯慣導/里程計組合導航方法算法[J]. 北京航空航天大學學報, 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] 付強文, 秦永元, 周琪. 改進量測的車載捷聯慣導/里程計組合導航算法[J]. 測控技術, 2013, 32(7): 134-137.

[5] 肖煊, 王清哲, 程遠, 等. 捷聯慣導系統/里程計高精度緊組合導航算法[J]. 兵工學報, 2012, 33(4): 395-400.

[6] 翁浚, 成研, 秦永元, 等. 車輛運動約束在 SINS/ OD系統故障檢測中的應用[J]. 中國慣性技術學報, 2013, 21(3): 406-410.

[7] 吳有龍, 王曉鳴, 曹鵬. 抗差估計及Allan方差在車載組合導航系統中的應用研究[J]. 兵工學報, 2013, 34(7): 889-895.

[8] 趙洪松, 繆玲娟, 沈軍. 捷聯慣導/里程計高精度組合導航算法[J]. 兵工學報, 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] 劉帥, 孫付平, 李海峰, 等. 前后向平滑算法在精密單點定位/INS緊組合數據后處理中的應用[J]. 中國慣性技術學報, 2015, 23(1): 85-91.

[12] 曹意, 茅旭初. 基于非線性濾波和平滑相迭代的GPS定位估計算法[J]. 上海交通大學學報, 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

軍隊計劃項目資助課題

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