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

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

0 引言

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

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

1 里程計導(dǎo)航定位原理與誤差分析

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

1.1 里程計定位原理

(1)

1.2 里程計誤差模型

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

(2)

(3)

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

1.3 里程計故障檢測

當車輛在行駛過程中出現(xiàn)打滑、滑行等情況時，里程計輸出的速度會與實際速度產(chǎn)生偏差。因此，當里程計出現(xiàn)故障時，需對里程計進行故障隔離?？衫脷埐瞀?檢驗對系統(tǒng)的故障進行檢測和隔離[9]。殘差χ2檢驗原理如下：

(4)

則第k時刻系統(tǒng)量測的預(yù)測值為

(5)

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

其中，k時刻的殘差為：

(6)

k時刻的方差為：

(7)

3)計算rk的統(tǒng)計特性，統(tǒng)計量構(gòu)造如下：

(8)

4)判斷系統(tǒng)是否發(fā)生故障

當系統(tǒng)正常工作時，λk服從自由度為x的χ2分布，x為Zk的維數(shù)，當系統(tǒng)出現(xiàn)故障時，λk不再服從自由度為x的χ2分布。

因此故障判定準則為

(9)

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

2 零速修正

2.1 狀態(tài)方程

車載慣導(dǎo)導(dǎo)航系統(tǒng)選取SINS系統(tǒng)誤差量、里程計刻度系數(shù)、航向安裝誤差角及俯仰安裝誤差角作為系統(tǒng)狀態(tài)向量，

(10)

(11)

則SINS系統(tǒng)誤差方程可以表示如下：

2.2 量測方程

(12)

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

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

(13)

3 組合導(dǎo)航模型

3.1 狀態(tài)方程

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

3.2 量測方程

1) 傳統(tǒng)SINS_ODO_ZUPT量測方程

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

(14)

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

(15)

2) 本文量測方程

(16)

(17)

Hp=[01×8101×9]

(18)

3.3 估計誤差修正

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

(19)

4 跑車試驗與仿真分析

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

圖1 跑車實驗現(xiàn)場圖

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

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

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

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

3)系統(tǒng)上電完成后，同時采集慣導(dǎo)系統(tǒng)、里程計以及高精度基準導(dǎo)航系統(tǒng)數(shù)據(jù)并準備開始跑車；

4)跑車實驗過程中，觀察慣導(dǎo)系統(tǒng)、里程計以及高精度基準導(dǎo)航系統(tǒng)數(shù)據(jù)是否異常并記錄；

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

檢查數(shù)據(jù)無誤，完成仿真運算后，列出相關(guān)計算結(jié)果。圖2為緯度、經(jīng)度軌跡對比。

圖2 軌跡對比圖

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

圖4 位置誤差對比

圖5 速度誤差對比

兩種方法對應(yīng)的6min內(nèi)RMSE誤差結(jié)果如表1所示

表1 兩種算法誤差對比

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

5 結(jié)論

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