捷聯慣導/里程計高精度組合導航算法

趙洪松，繆玲娟，沈軍

(1. 北京理工大學 自動化學院，北京100081;2.94032 部隊，甘肅 武威733003)

0 引言

在陸用車輛導航應用中，相比GPS 信號易受干擾的特點，里程計(OD)是一種完全自主地測量車輛行駛路程/速度的裝置［1-2］。捷聯慣導系統(SINS)與OD 組合能夠有效解決在GPS 失效的情況下車輛導航系統誤差快速發散的問題［3-11］，但是由于OD不能直接提供載體的絕對位置信息，因此其定位誤差仍然隨著時間的增長而發散［1，9-10］。

對于SINS/OD 組合導航系統誤差建模方面，目前的研究主要集中在OD 刻度系數誤差和SINS 相對于機體系的安裝誤差2 個方面，方法主要有離線標定［2，9］和在線估計［1，10，12-13］。GPS/INS 緊組合的特點是GPS 接收機和慣性導航系統相互輔助［14］，其中，文獻［10］借鑒了此概念，提出了SINS/OD 高精度緊組合導航算法。由于車輛運動的特殊性，零速修正以及車輛運動學約束技術被應用于車輛慣性導航技術中抑制慣導誤差的發散［12，15-17］。文獻［18］提出了附加速度先驗信息的車載GPS/INS/OD 組合導航算法，分析在INS/OD 組合導航時只有速度是直接可測的，其他參數都是間接可測的，提出了改進的位置修正法，用修正后的速度推算位置，提高了位置精度。

本文綜合考慮了OD 刻度系數誤差及SINS 安裝誤差角對導航系統的影響，重新推導建立了系統的量測方程，構成新的SINS/OD 組合導航算法，其中組合導航位置更新算法采用文獻［18］中的方法，即用修正后的速度推算位置。該導航算法能夠有效地檢測由于車輛打滑、滑行、側滑以及跳躍等引起的量測故障，并且能夠及時隔離量測故障信息，采用容錯導航模式。車輛打滑、滑行、側滑描述的是車輛3 種不同的運動狀態。車輛打滑指的是由于地面摩擦力不夠，不能夠給車輛提供足夠的前行動力，導致車輪空轉而車輛沒有啟動或者緩慢行駛，典型的表現是車輪速度大于車輛行駛速度。滑行與打滑正好相反，多發生在行駛車輛做剎車減速的情況下，典型的表現是車輪速度小于車輛行駛速度。正常行駛的車輛橫向是沒有速度的，側滑特別指由于路面較滑等情況導致車輛產生橫向運動的一種特殊運動狀態。本文的導航模式與容錯導航模式由一套算法實現，這樣不僅避免了備份一套用于容錯導航模式的算法，而且也避免了2 套算法之間的切換調用。仿真結果表明，該算法能夠有效地提高SINS/OD 組合導航的精度，并及時準確地檢測出量測故障，保證在發生量測故障時的導航系統精度。

1 SINS/OD 組合導航系統誤差分析

選取東北天地理坐標系為導航坐標系，記為n 系;記SINS 解算得到的計算導航坐標系為n'系;假設OD 坐標系與車輛載體坐標系重合，記為m 系，y 軸沿機體縱軸指向正前方，x 軸沿機體橫軸指向右側，z 軸垂直于x 軸和y 軸并構成右手直角坐標系;SINS 坐標系記為b 系，安裝SINS 應該使得b 系盡量與m 系重合，但由于安裝工藝水平限制，使其相對m 系存在一定的安裝誤差角。

1.1 OD 刻度系數誤差的影響

車輛運動學約束是指車輛在道路上正常行駛時，在不發生側滑和跳躍的情況下，車輛在x 軸和z 軸方向的速度為0［12，16-17］. OD 測量的只是車輛y 軸速度，在SINS/OD 組合導航系統中實際上同時利用了OD 量測輸出與車輛運動學約束條件作為量測量。即在m 系下OD 測量載體速度的理想輸出記為

式中:VOD為OD 測量的載體速度值。

在實際中，OD 測量的載體速度值是有誤差的，主要是由刻度系數誤差引起的。雖然在很多情況下使用OD 之前，其刻度系數已經經過標校，基本能夠消除刻度系數誤差;但是在車輛行進過程中，經常會由于路面條件不同、環境溫度變化、輪胎磨損情況以及車輛載重變化等原因引起OD 刻度系數的變化［2，10］，從而造成量測誤差。所以，OD 實際輸出一般為

1.2 SINS 安裝誤差角的影響

文獻［10，12］分別討論了安裝誤差角對SINS/OD 組合、車輛運動學約束輔助慣導系統誤差的影響。一般情況下，b 系偏離m 系的安裝誤差角為小角度;若不滿足小角度時，只需首次標定，即可使得剩余安裝誤差角為小角度［12］。假設b 系偏離m 系的安裝誤差角δΦ 為小角度，且δΦ=［δθ δγ δψ］，其中:δθ 為俯仰安裝誤差角，δγ 為橫滾安裝誤差角，δψ 為方位安裝誤差角。若b 系與m 系之間的轉換矩陣為，且δΦ 滿足小角度要求，則有

式中:I 為單位陣;(δΦ×)為δΦ 的反對稱矩陣。

文獻［10，12］已經指出，在3 個安裝誤差角中只有δθ 和δψ 對OD 在b 系下的速度分量產生影響，而δγ 沒有影響。因此，SINS/OD 組合導航在利用SINS 輸出和OD 輸出作量測量時必須要考慮安裝誤差角δθ 和δψ 的影響，否則將造成一定的誤差。

2 SINS/OD 組合誤差模型

2.1 狀態方程

選擇SINS 的誤差狀態為速度誤差、姿態誤差、位置誤差、加速度計常值零偏、陀螺常值漂移，共15 維。考慮到OD 刻度系數誤差和SINS 安裝誤差角的影響，將這3 個擴充為狀態變量，則SINS/OD 組合導航模型的狀態變量為18 維:

通常將OD 刻度系數誤差和SINS 安裝誤差角認為是隨機常數，即

SINS/OD 組合導航系統狀態方程為

式中:F 為系統狀態矩陣;G 為噪聲輸入矩陣;W 為系統狀態噪聲;FSINS為SINS 的狀態矩陣;GSINS為SINS 的噪聲輸入矩陣。

2.2 量測方程

車輛在行駛過程中可能發生打滑、滑行、側滑以及跳躍等情況從而導致量測輸出故障，其中將打滑和滑行導致的OD 輸出信息錯誤稱為OD 故障。車載導航系統必須具備處理這些故障的能力，否則量測輸出的錯誤信息就會污染導航系統。考慮到系統故障檢測以及容錯的需要，重新推導并建立了以SINS 和OD 輸出的速度之差為量測量的量測方程。

在b 系下SINS 的速度實際輸出值為

式中:φ 為n'系偏離n 系的失準角。

由(3)式和(4)式可得

由(11)式即可得

由(2)式、(10)式和(12)式可得:

(13)式即為SINS/OD 高精度組合的量測方程，寫成矩陣形式即為

式中:

由(1)式和(4)式可得

根據(18)式和(19)式，(13)式等號右邊最后兩項可以合并寫為

由(21)式可看出，本文量測矩陣H 與文獻［10］不同點在于，這里安裝誤差角的量測系數是SINS 解算的b 系下的速度分量，而文獻［10］為OD 輸出的n'系下的速度值。當OD 發生故障時，本文安裝誤差角的量測系數不會受到OD 故障信息的影響，只需隔離掉OD 的量測輸出，即可轉入車輛運動學約束輔助導航模式，從而實現容錯組合導航。

3 SINS/OD 組合導航濾波算法

3.1 濾波算法

將組合導航的狀態方程(7)式和量測方程(14)式離散化，可得

這里采用經典的Kalman 濾波算法作為SINS/OD組合導航濾波算法，取SINS 和OD 輸出的速度差值為量測量，對各誤差狀態進行估計，并對關鍵參數進行修正，以有效地減小導航誤差的累積。

當OD 發生故障時，為防止這種錯誤信息對導航精度的影響，應當及時隔離這種信息。采用的方法如下:當檢測到OD 發生故障的時候，量測量Z(2)對應的量測噪聲方差設置為無窮大，從而使得增益矩陣中相對應的增益為0，這樣在狀態估計中就僅依靠另外2 個量測量Z(1)和Z(3)進行狀態更新，完全隔離OD 的錯誤信息。此時即等效為文獻［12］車輛運動學約束輔助導航模式，實現OD 故障時的容錯導航模式。

3.2 故障診斷

車輛可能出現打滑、滑行狀況，從而導致OD 故障，輸出數據無效;另外當車輛快速轉向、側滑和跳躍的時候，車輛不滿足車輛運動學的約束條件［9］。這就要求系統能夠及時準確的判斷故障狀態。

根據Kalman 濾波中的新息，可以對故障狀態進行判斷。根據文獻［19］，新息符合如下正態分布:

式中:i=1，2，3;Mk(i，i)為矩陣Mk的第i 個對角元素。

根據對故障漏檢率和誤檢率的要求，綜合考慮車輛運行的實際環境，設置相應的置信度水平β 對應的檢驗閾值εi，則檢驗準則如下:

當

時，判斷對應的新息正常，無故障發生;

當

時，判斷對應的新息異常，有故障發生。

分析新息的構成，由Zk和 二部分構成，故障信息對新息造成異常只能通過量測量Zk和量測矩陣Hk. 其中，量測量，可見OD 故障影響量測量Zk(2);車輛側滑或者快速轉向時影響量測量Zk(1);車輛發生跳躍時影響量測量Zk(3). 由(20)式可知，OD 發生故障時影響量測矩陣Hk的系數，但是通過只影響新息rk(2)，對另外2 個新息量不產生影響。綜上可得表1 的故障判斷準則。

表1 故障判斷準則Tab.1 Fault criteria

這樣就可以依據表1 的故障判斷準則，選擇恰當的導航模式，以提高導航系統對環境的魯棒性和適應性。

4 仿真驗證

仿真初始條件設置如下:

陀螺隨機常值漂移0.02°/h，噪聲0.02°/h(1σ);加速度計隨機常值零偏1×10-4g，噪聲1×10-4g(1σ);OD 刻度系數誤差為0.2%;SINS 安裝誤差角均為0.5°;SINS 初始對準水平姿態誤差角1'、航向誤差角10';初始經緯度和高度誤差均為10 m.

仿真時間1 h，車輛首先靜止100 s，然后開始運動，軌跡如圖1 所示，其中車輛機動主要是航向轉彎、水平加速及爬坡和下坡機動過程，最大速度20 m/s，總路程約69.9 km.

圖1 車輛運動軌跡Fig.1 Vehicle trajectory

4.1 SINS/OD 組合導航算法驗證

為了檢驗SINS/OD 組合導航算法的有效性，分別對比了單純SINS 導航算法和SINS/OD 組合導航算法。

如圖1 所示，單純SINS 導航位置誤差隨著時間的增長而快速發散，而SINS/OD 組合導航能夠很好地抑制位置誤差的發散。由圖2 可知，SINS/OD 組合導航位置誤差也是緩慢發散的，這是因為OD 只提供速度量測信息，不能提供絕對的位置信息，因此SINS/OD 組合導航位置誤差會隨車輛行駛路程不斷累積［9］。圖3 為OD 刻度系數誤差和SINS 安裝誤差角的估計曲線。由圖3 可以看出，SINS/OD 組合導航算法能夠正確地估計出OD 刻度系數誤差和SINS 安裝誤差角。

圖2 SINS/OD 組合導航位置誤差Fig.2 Position errors of SINS/OD integrated navigation

4.2 SINS/OD 組合導航算法的容錯性能驗證

為了驗證SINS/OD 組合導航算法的故障診斷以及容錯性能，設置如表2 所示的故障狀態。

表2 故障狀態Tab.2 Fault condition

圖3 δk、δθ 和δψ 的估計值Fig.3 Estimates of δk，δθ and δψ

車輛運動軌跡與圖1 基本相同，只是由于車輛發生側滑以及跳躍等導致略有差異，在三維軌跡圖中差異不明顯，故未給出其三維軌跡圖。

此處分別仿真驗證了未進行量測故障檢測和隔離的SINS/OD 組合導航算法和文中所設計的SINS/OD 組合導航算法。如圖4 所示，對于未進行故障檢測和隔離的SINS/OD 組合導航系統，由于量測故障信息的污染，使得位置誤差迅速發散，并且從位置誤差發散的速度可以看出打滑和滑行故障的影響要遠大于側滑和跳躍。如圖5 所示，在未發生故障之前，OD 刻度系數誤差和SINS 安裝誤差角的估計已經穩定，當發生故障時，由于未隔離故障信息，導致OD 刻度系數誤差和SINS 安裝誤差角的估計完全偏離了真實值，同樣也可以從參數估計失真的程度可以看出打滑和滑行故障的影響要遠大于側滑和跳躍。對于文中的SINS/OD 組合導航算法，如圖6 與圖2 相比，誤差發散趨勢基本一致，說明該算法能夠有效對量測故障進行容錯，提高了發生故障時組合導航系統的精度。又如圖7 所示，在發生故障時，文中算法依然保持了對OD 刻度系數誤差和SINS 安裝誤差角的估計精度。圖8 為本文算法對量測故障的檢測結果，“0”表示無故障，“1”表示有故障，由故障檢測結果可知，該算法能夠準確及時地判斷出量測故障。

圖4 未隔離量測故障的SINS/OD 組合導航位置誤差Fig.4 Position errors of SINS/ODintegrated navigation without measuring fault isolation

圖5 未隔離量測故障的SINS/OD 組合導航算法對δk、δθ 和δψ 的估計Fig.5 Estimation of δk，δθ and δψ by SINS/OD integrated navigation algorithm without measuring fault isolation

圖6 量測故障時本文SINS/OD 組合導航位置誤差Fig. 6 Position errors of SINS/OD integrated navigation proposed in the paper in the case of measuring fault

圖7 量測故障時本文SINS/OD 組合導航算法對δk、δθ 和δψ 的估計Fig.7 Estimation of δk，δθ and δψ by SINS/OD integrated navigation algorithm proposed in the paper in the case of measuring fault

圖8 本文SINS/OD 組合導航對量測故障的檢測結果Fig.8 Measuring fault detection results of SINS/OD integrated navigation algorithm proposed in the paper

5 結論

本文綜合考慮了OD 刻度系數誤差和SINS 安裝誤差角的影響，以及量測故障時系統容錯性能的要求，重新推導了組合導航誤差模型，建立了SINS/OD 高精度組合導航算法。該算法能夠有效地檢測量測故障，并且能夠實現容錯導航模式。這樣就避免了為容錯導航模式另外備份一套算法，減少了算法復雜度和計算量，節省了程序存儲空間，具有重要的工程實際意義。仿真結果表明，本文SINS/OD 組合導航算法定位精度高，故障診斷及時準確，采用的容錯方案有效地避免了量測故障信息對系統的影響。

References)

［1］丁文娟，李歲勞，熊偉. 捷聯慣導系統/里程計自主式車載組合導航系統研究［J］. 計測技術，2006，26(1):14 -16.DING Wen-juan，LI Sui-lao，XIONG Wei. Research on SINS/OD independently integrated navigation system for land vehicles［J］.Metrology ＆ Measurement Technology，2006，26(1):14 - 16.(in Chinese)

［2］朱立彬，王瑋. 基于滑動模型的車輛里程儀標度因數標定方法［J］. 系統工程與電子技術，2012，34(4):778 -781.ZHU Li-bin，WANG Wei. Calibration of odometer’s scale factor based on sliding model［J］. Systems Engineering and Electronics，2012，34(4):778 -781. (in Chinese)

［3］Skog I，H?ndel P. In-car positioning and navigation technologies-a survey［J］. IEEE Transactions on Intelligent Transportation Systems，2009，10(1):4 -21.

［4］Georgy J，Karamat T，Iabal U，et al. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter［J］. GPS Solutions，2011，15(3):239 -252.

［5］Georgy J，Noureldin A，Korenberg M J，et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation［J］. IEEE Transactions on Intelligent Transportation Systems，2010，11(4):856 -872.

［6］Duong T T，Huang Y W，Chiang K W. Improving the accuracy of mems IMU/GPS POS systems for land-based mobile mapping system by using tightly coupled integration and auxiliary odometer［C］∥31st Asian Conference on Remote Sensing. Tokyo，Japan:Asian Association on Remote Sensing，2010:515 -520.

［7］Wang W，Wang D. Land vehicle navigation using odometry/INS/vision integrated system［C］∥2008 IEEE International Conference on Cybernetics and Intelligent Systems. Chengdu:IEEE Computer Society，2008:754 -759.

［8］徐田來，崔平遠，崔祜濤. 車載多傳感器組合導航系統設計與實現［J］. 系統工程與電子技術，2008，30(4):686 -691.XU Tian-lai， CUI Ping-yuan， CUI Hu-tao. Design and implementation of multi-sensor integrated navigation system of land vehicle［J］. Systems Engineering and Electronics，2008，30(4):686 -691. (in Chinese)

［9］嚴恭敏. 車載自主定位定向系統研究［D］. 西安:西北工業大學，2006.YAN Gong-min. Research on autonomous position and azimuth determining systems for land vehicles［D］. Xi’an:Northwestern Polytechnical University，2006. (in Chinese)

［10］肖煊，王清哲，程遠，等. 捷聯慣導系統/里程計高精度緊組合導航算法［J］. 兵工學報，2012，33(4):395 -400.XIAO Xuan，WANG Qing-zhe，CHENG Yuan，et al. High accuracy navigation algorithm for tightly coupled INS/odometer［J］. Acta Armamentarii，2012，33 (4):395 - 400. (in Chinese)

［11］Kim S B，Bazin J C，Lee H K，et al. Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system，global positioning system，odometer and vision data［J］.IET Radar，Sonar ＆ Navigation，2011，5(8):814 -823.

［12］付強文，秦永元，李四海，等. 車輛運動學約束輔助的慣性導航算法［J］. 中國慣性技術學報，2012，20(6):640 -643.FU Qiang-wen，QIN Yong-yuan，LI Si-hai，et al. Inertial navigation algorithm aided by motion constraints of vehicle［J］.Journal of Chinese Inertial Technology，2012，20(6):640 -643. (in Chinese)

［13］蘭春云，繆玲娟，沈軍. 陸用捷聯慣導系統中里程計刻度因子的在線辨識［J］. 北京理工大學學報，2003，23(2):198 -201.LAN Chun-yun， MIAO Ling-juan， SHEN Jun. Onlineidentification of odometer’s scale factor in land-used strapdown inertial navigation system［J］. Transactions of Beijing Institute of Technology，2003，23(2):198 -201. (in Chinese)

［14］劉建業，曾慶化，趙偉，等. 導航系統理論與應用［M］. 西安:西北工業大學出版社，2010:336 -337.LIU Jian-ye，ZENG Qing-hua，ZHAO Wei，et al. Navigation system theory and application［M］. Xi’an:Northwestern Polytechnical University Press，2010:336 -337. (in Chinese)

［15］方靖，顧啟泰，丁天懷. 車載慣性導航的動態零速修正技術［J］. 中國慣性技術學報，2008，16(3):265 -268.FANG Jing，GU Qi-tai，DING Tian-huai. Dynamic zero velocity update for vehicle inertial navigation system［J］. Journal of Chinese Inertial Technology，2008，16(3):265 - 268. (in Chinese)

［16］Godha S，Cannon M E. GPS/MEMS INS integrated system for navigation in urban areas［J］. GPS Solutions，2007，11(3):193-203.

［17］Dissanayake G，Sukkarieh S，Nebot E，et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications［J］. IEEE Transactions on Robotics and Automation，2001，17(5):731 -747.

［18］吳富梅，楊元喜. 附加速度先驗信息的車載GPS/INS/Odometer 組合導航算法［J］. 宇航學報，2010，31(10):2314-2320.WU Fu-mei，YANG Yuan-xi. GPS/INS/odometer integrated navigation algorithm with prior velocity in land vehicle system［J］. Journal of Astronautics，2010，31(10):2314 -2320. (in Chinese)

［19］石靜，繆玲娟，倪茂林. 一種抗野值自適應濾波算法及在MEMS-SINS/GPS 中應用［J］. 宇航學報，2010，32(12):2711 -2716.SHI Jing，MIAO Ling-juan，NI Mao-lin. An outlier rejecting and adaptive filter algorithm applied in MEMS-SINS/GPS［J］.Journal of Astronautics，2010，32(12):2711 - 2716. (in Chinese)