一種改進的SINS_ODO_ZUPT組合導航算法*

張 敏 陳安升 陳 帥 王 琛 姚曉涵

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

0 引言

目前車載組合定位導航常用SINS/GNSS 組合導航來實現車輛定位導航。然而，GNSS由于受到多徑效應的影響，在城市峽谷地段、橋梁下、隧道內等復雜環境下，其信號會嚴重丟失甚至中斷[1]。由于慣性導航系統自主定位精度會隨時間而降低，基于GNSS_SINS的車載定位導航的估計精度必然會嚴重下降[2]。SINS/ODO組合和零速修正是解決上述問題的2種廉價且有效的導航方法。里程計[3-4]成本低，自主性強，利用里程計提供的速度，推導航位推算算法和航位推算誤差，可以提高慣性導航精度。零速修正技術[5-6]是提高車載慣導系統的一種簡單廉價的方法。在衛星信號拒止情況下，該方法利用車輛停車時慣導系統導航系的速度輸出作為系統速度誤差的觀測量，進而對位置等其它誤差量進行修正，從而有效抑制和補償車載慣性導航誤差。文獻[7]將動態零速修正和SINS/ODO組合方案相結合，利用零速修正輔助SINS/ODO進行組合導航，提高了一定的車載導航精度。但是該方法存在高度發散的問題。

本文提出了一種零速修正輔助SINS_ODO的組合導航算法。將里程計刻度系數誤差和IMU安裝誤差角考慮在內，建立了組合導航誤差狀態模型、量測模型以及估計誤差修正方法。針對傳統的動態零速修正輔助的SINS_ODO的算法存在的高度發散問題，提出了一種新的量測模型，即對天向速度以及天向高度進行約束的量測模型。最后采用仿真計算驗證了本文的組合導航方法，并與傳統的動態零速修正輔助的SINS_ODO方法進行了比較。該算法能夠在衛星拒止時有效抑制SINS的誤差發散，實現高精度自主定位。

1 里程計導航定位原理與誤差分析

定義以下坐標系：慣導坐標系b系為慣導的右前上坐標系，導航坐標系n系為東北天坐標系，載體坐標系m系為車體的右前上坐標系，m系中原點是車體重心，Y軸指向車頭方向，X軸指向車體右側，Z軸指向車頂方向，X軸、Y軸和Z軸構成右手坐標系。

1.1 里程計定位原理

(1)

1.2 里程計誤差模型

里程計的誤差主要包括刻度系數誤差和安裝角誤差。里程計的刻度系數誤差是指在車輛行進過程中，由于環境溫度，輪胎狀況等的影響下，產生的誤差。若安裝誤差角為0，則受里程計刻度系數誤差影響，此時b系的速度為

(2)

(3)

式中，αφ為航向安裝誤差角，αθ為俯仰安裝誤差角。

1.3 里程計故障檢測

當車輛在行駛過程中出現打滑、滑行等情況時，里程計輸出的速度會與實際速度產生偏差。因此，當里程計出現故障時，需對里程計進行故障隔離。可利用殘差χ2檢驗對系統的故障進行檢測和隔離[9]。殘差χ2檢驗原理如下：

(4)

則第k時刻系統量測的預測值為

(5)

2)計算第k時刻的Kalman濾波器的殘差及方差

其中，k時刻的殘差為：

(6)

k時刻的方差為：

(7)

3)計算rk的統計特性，統計量構造如下：

(8)

4)判斷系統是否發生故障

當系統正常工作時，λk服從自由度為x的χ2分布，x為Zk的維數，當系統出現故障時，λk不再服從自由度為x的χ2分布。

因此故障判定準則為

(9)

其中，T是預先設定的門限值。當檢測到里程計故障時，應停止SINS/ODO組合導航，改由 SINS 子系統單獨工作，直到里程計輸出再次正常時，方可再進行組合。

2 零速修正

2.1 狀態方程

車載慣導導航系統選取SINS系統誤差量、里程計刻度系數、航向安裝誤差角及俯仰安裝誤差角作為系統狀態向量，

(10)

(11)

則SINS系統誤差方程可以表示如下：

2.2 量測方程

(12)

式中：V為觀測噪聲，H為觀測矩陣，如下所示

H=[03×3I3×303×12]

(13)

3 組合導航模型

3.1 狀態方程

系統狀態方程同式(10)系統狀態方程一致，此處不再贅述。

3.2 量測方程

1) 傳統SINS_ODO_ZUPT量測方程

利用SINS系統與ODO東北天坐標系下的速度誤差作為觀測量，系統觀測方程表示如下：

(14)

式中，H0為觀測矩陣，V0為觀測噪聲。

(15)

2) 本文量測方程

(16)

(17)

Hp=[01×8101×9]

(18)

3.3 估計誤差修正

采用組合導航過程中估計所得的誤差量對SINS狀態進行反饋校正。導航參數誤差估計量的修正如式(19)所示

(19)

4 跑車試驗與仿真分析

為了驗證本算法對高度約束效果的有效性，利用試驗車進行了跑車試驗。跑車試驗選擇在南京市麒麟科技園某空曠路段進行，行駛時間大約為6min。跑車實驗現場圖和行車路線圖如圖1所示。

圖1 跑車實驗現場圖

車載試驗系統主要由MEMS捷聯慣導系統、里程計和高精度組合導航系統組成。MEMS捷聯慣導系統由MSI3200G構成，數據輸出頻率為200Hz，MEMS陀螺儀的量程為±300(°)/s，零偏穩定性≤5(°)/h；MEMS加速度計的量程為±50g，零偏穩定性≤0.5mg。高精度組合導航系統由耐威公司的POS320組成，數據輸出頻率為20Hz，此導航系統為高精度基準系統。通過測量POS320組合導航系統可以獲得初始對準點位置。

同時記錄慣性測量單元(IMU)數據、里程計數據以及高精度基準數據，離線進行導航算法驗證。由高精度基準數據可以得到本次試驗初始緯度 32.0161°，初始經度118. 8892°。跑車實驗具體步驟如下：

1)跑車實驗前，先檢查軟硬件設備是否有異常，檢查無誤后，將所有硬件裝車并安裝好；

2)連接好系統線路和電源，給慣導系統、里程計以及高精度基準導航系統上電；

3)系統上電完成后，同時采集慣導系統、里程計以及高精度基準導航系統數據并準備開始跑車；

4)跑車實驗過程中，觀察慣導系統、里程計以及高精度基準導航系統數據是否異常并記錄；

5)跑車結束后，將采集到的數據進行仿真分析，驗證算法的有效性。

檢查數據無誤，完成仿真運算后，列出相關計算結果。圖2為緯度、經度軌跡對比。

圖2 軌跡對比圖

通過軌跡對比圖可以發現，本算法和傳統算法在緯度和經度的軌跡基本重合，并與基準軌跡誤差較小。為方便觀察，計算位置誤差如圖3所示，速度誤差如圖4所示。

圖4 位置誤差對比

圖5 速度誤差對比

兩種方法對應的6min內RMSE誤差結果如表1所示

表1 兩種算法誤差對比

從表1可以看出，本文提出的算法在緯度和經度上和傳統SINS_ODO_ZUPT算法基本相當。與傳統SINS_ODO_ZUPT算法相比，本文提出的算法高度誤差降低了73.8%。

5 結論

針對本文提出的SINS_ODO_ZUPT組合導航方法進行了仿真驗證，并與傳統的動態零速修正輔助的SINS_ODO方法進行了對比。仿真結果表明：本文介紹的組合導航方法相對于傳統的動態零速修正的SINS_ODO導航方法具有更高的導航精度，能有效實現自主高精度定位定向。