基于動態零速修正的SINS/ODO組合導航算法研究*

王 琛 陳安升 陳 帥 韓 林 譚聚豪

1.南京理工大學，南京210094 2.北京自動化控制設備研究所，北京100074

目前，SINS/GNSS組合導航系統應用廣泛，但其長時間精度取決于衛星導航。GNSS正常工作時，采用SINS/GNSS組合導航，對SINS誤差進行校正，可以實現準確導航，但是衛星易受遮擋和干擾導致衛星導航失效，使得系統工作在純SINS模式，SINS誤差得不到校正，持續累積發散[1]。MEMS慣性器件更是如此。由于其體積小，功耗低等優勢被廣泛應用到各個領域，但其精度較差也大大制約了導航定位的精度[2]。因此衛星失效時需要額外的導航傳感器輔助，來抑制慣性導航誤差，提高系統的可靠性。里程計結構簡單，成本低，可測量車輛行駛速度，且測量誤差不隨時間積累，自主性和抗干擾能力強，同SINS具有互補性，兩者組合無需停車即可在較長時間內修正SINS系統誤差，提高導航系統的精度[3]。

本文提出動態零速修正輔助SINS/ODO進行組合導航的方法，來提高系統的導航定位精度。并將里程計刻度系數誤差和IMU安裝誤差角考慮在內，給出了SINS/ODO松組合的數學模型。跑車試驗結果表明在GNSS信號短時拒止時，該算法能夠有效抑制SINS的誤差發散，導航精度得到明顯提升。

1 動態零速修正

定義車體坐標系(v系)如圖1所示，坐標系中原點是車體重心位置，Y指向車頭方向，X軸指向車體右側，Z軸指向車頂方向，X軸、Y軸和Z軸構成右手坐標系。

圖1 車體坐標系

車輛在道路上正常行駛時，理想情況下不發生側滑和跳躍，在v系中，車體橫向(X軸)和垂向(Z軸)上的速度近似為0[4-5]，即：

(1)

根據實際情況，觀測噪聲可以考慮為零均值高斯白噪聲。動態零速修正構成X軸和Z軸方向上的速度觀測量，里程計輸出車輛Y軸方向上的速度值，從而構成三維速度量測，建立速度觀測方程，進行Kalman濾波，修正MEMS慣導的漂移。

2 里程計

2.1 里程計刻度系數誤差

(2)

其中，Tk為采樣周期；Pk為采樣周期內檢測到的脈沖數；wodok為采樣噪聲；kodo為里程計的刻度系數(即，載體行駛距離和里程計傳感器檢測到的脈沖數的比值)。

在實際工程環境中，受到車輛行進過程中，路面條件、環境溫度、輪胎胎壓和輪胎磨損程度等的影響，里程計的刻度系數并不是理想值kodo，總是存在一定的誤差Δodo，則刻度系數的真實值為：

(3)

進一步，里程計傳感器輸出的真實載體前向速度為：

(4)

2.2 里程計故障判斷

SINS/ODO組合導航系統中，SINS子系統作為主系統，可靠性高、不易出現故障。而ODO子系統出現故障的主要原因是車輛在行駛過程中出現打滑、側滑或滑行等不可避免的情況，導致里程計輸出的脈沖數和速度與實際不符。其中，打滑會使得脈沖數和速度大于實際值，滑行則小于實際值。

根據SINS子系統短時間內精度較高的特性，在組合濾波進行反饋時速度誤差的校正量一般很小，如果ODO子系統出現打滑、滑行等故障，慣性導航解算的速度和里程計輸出的速度將存在較大的差值，可以通過閾值判斷進行里程計故障檢測。

(5)

其中，Cs2為打滑閾值，取值范圍為(0,∞)；Cs1為滑動閾值，取值范圍為(-VN,0)。

此外，閾值Cs的選取受車輛行駛速度，SINS解算誤差，單一SINS工作時長，ODO觀測噪聲等的綜合影響。因此，多次或者連續檢測到里程計故障時，閾值需要隨故障次數進行調整。

當里程計出現故障時，應停止MINS/ODO組合濾波，改由SINS子系統單獨工作，直到里程計輸出再次正常時，方可再進行組合濾波。

3 組合導航模型

3.1 SINS安裝角誤差

(6)

將上式展開，得

(7)

(8)

3.2 狀態方程

SINS/GNSS松組合導航系統選取SINS系統誤差量、里程計刻度系數、航向安裝誤差角及俯仰安裝誤差角作為系統狀態向量，

X=[φEφNφUδVEδVNδVUδLδλδhεxεyεz▽x▽y▽zδkαψαθ]T

(9)

其中，φE,φN和φU分別為東北天3個方向的姿態誤差角，δVE,δVN和δVU分別為東北天3個方向的速度誤差，δL,δλ和δh分別為緯經高3個方向的位置誤差，εx,εy和εz分別為載體坐標系下陀螺儀三軸常值漂移，▽x,▽y和▽z分別為載體坐標系下加速度計三軸常值偏置，δk表示里程計的刻度誤差系數，αψ,αθ分別表示航向安裝誤差角和俯仰安裝誤差角。里程計的δk及安裝角誤差αψ和αθ均可視作隨機常數，其誤差方程可表示為：

(10)

SINS系統狀態方程表述如下：

(11)

其中，F(t)為18×18的SINS系統狀態轉移矩陣；G(t)為18×6的SINS系統噪聲驅動矩陣；W(t)為6×1的SINS系統噪聲矩陣。具體形式如下：

SINS系統狀態轉移矩陣F(t)：

(12)

其中，FN(t)為SINS系統基本誤差矩陣，矩陣中各元素見參考文獻[7]；FS(t)和FM(t)為慣性器件的誤差矩陣，具體如下：

(13)

(14)

其中，Tgx，Tgy和Tgz為陀螺儀3個軸的相關時間，Tax，Tay和Taz為加速度計的相關時間。

SINS系統噪聲驅動矩陣G(t)：

(15)

SINS系統噪聲矩陣W(t)：

W(t)=[wgxwgywgzwaxwaywaz]T

(16)

其中，wgx，wgy和wgz分別為陀螺儀3個軸向的量測白噪聲，wax，way和waz分別為加速度計三個軸向的量測白噪聲。

3.3 量測方程

里程計輸出的真實載體前向速度見式(4)，寫成三維向量形式，有：

(17)

將ODO子系統輸出的載體速度分解到導航坐標系，并與SINS子系統解算的速度進行對比，取兩者差值作為觀測量[8]，則系統觀測方程為：

(18)

其中，HO為觀測矩陣，具體見下式;VO為觀測噪聲。

(19)

本文采用反饋校正來修正SINS的誤差。反饋校正是將慣性導航系統導航參數誤差的估值反饋到慣性系統中，對誤差狀態進行校準。反饋校正如圖2所示。

圖2 反饋校正

由于里程計只輸出速度值，故本文采用反饋校正的方法對SINS系統的速度進行校正。

(20)

4 跑車試驗與性能分析

為了驗證設計的相關算法以及SINS/ODO組合導航系統的性能，進行了跑車試驗。跑車試驗選擇在南京市麒麟科技園空曠路段進行，跑車試驗裝車效果如圖3所示，具體試驗路線如圖4所示。

圖3 跑車試驗裝車效果圖

圖4 跑車試驗路線圖

本文選取MSI3200G作為MEMS慣性器件，其主要性能指標如表1所示。將MSI3200G、里程計與導航板卡組成組合導航系統固定在車體上，MEMS慣性器件的更新率為200Hz。選用耐威公司的POS320作為高精度基準導航系統。

表1 MSI3200G性能指標

本次試驗初始緯度32.022071°，初始經度118.884072°，初始高度11.46m；尋北結果-143.65°。

地面跑車試驗的具體試驗方法如下：

1)跑車試驗前，需要在實驗室內對試驗設備和相關軟件進行性能檢查，檢查無問題后進行試驗設備的安裝；

2)保證電源設置和系統線路連接正確后，給組合導航系統和高精度基準導航系統(由高精度GNSS接收機與高精度光纖慣導等部分組成)上電；

3)由POS320提供初始航向角并將初始航向角信息通過監控軟件上傳給導航板卡。待MSI3200G完成自對準后，然后開始SINS/GNSS組合導航，保存組合導航系統和高精度基準導航系統的數據，然后開始跑車；

4)通過拔掉衛星導航的天線饋線獲得衛星導航失效狀態，之后進行SINS/ODO組合導航；

5)跑車試驗的過程中，時刻對組合導航系統監控軟件界面顯示的導航數據和高精度基準導航系統監控軟件界面顯示的導航數據進行監測，觀察數據是否異常；

6)跑車試驗結束后，將組合導航系統輸出的數據和高精度基準導航系統的數據進行時標對齊，然后進行誤差比對分析，檢驗系統的可靠性和組合導航算法的性能。

組合導航系統相對高精度基準導航系統的試驗結果如圖5～6所示。

圖5 位置誤差

圖6 速度誤差

從圖5和6可以看出，動態零速修正輔助的SINS/ODO組合導航系統的三維位置誤差穩定，且誤差小；三維速度誤差穩定，且誤差小。無衛星導航90s時統計值為：緯度誤差1.72m，經度誤差-1.36m，高度誤差-4.38m，東向速度誤差0.02m/s，北向速度誤差-0.04m/s，天向速度誤差-0.23m/s。結果表明，本文設計的基于動態零速修正的SINS/ODO組合導航在實際動態環境下對純慣性導航誤差有較好的約束效果。

5 結論

提出了在衛星信號據止情況下的基于動態零速修正的SINS/ODO組合導航算法，通過跑車試驗進行驗證。結果表明，在衛星信號拒止時，該算法有較高的導航精度，能夠有效抑制MEMS慣導的漂移，可以作為備份導航方案。