一種改進(jìn)的SINS_ODO_ZUPT組合導(dǎo)航算法*

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

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

0 引言

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

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

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

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

1.1 里程計(jì)定位原理

(1)

1.2 里程計(jì)誤差模型

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

(2)

(3)

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

1.3 里程計(jì)故障檢測(cè)

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

(4)

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

(5)

2)計(jì)算第k時(shí)刻的Kalman濾波器的殘差及方差

其中，k時(shí)刻的殘差為：

(6)

k時(shí)刻的方差為：

(7)

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

(8)

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

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

因此故障判定準(zhǔn)則為

(9)

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

2 零速修正

2.1 狀態(tài)方程

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

(10)

(11)

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

2.2 量測(cè)方程

(12)

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

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

(13)

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

3.1 狀態(tài)方程

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

3.2 量測(cè)方程

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

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

(14)

式中，H0為觀測(cè)矩陣，V0為觀測(cè)噪聲。

(15)

2) 本文量測(cè)方程

(16)

(17)

Hp=[01×8101×9]

(18)

3.3 估計(jì)誤差修正

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

(19)

4 跑車試驗(yàn)與仿真分析

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

圖1 跑車實(shí)驗(yàn)現(xiàn)場(chǎng)圖

車載試驗(yàn)系統(tǒng)主要由MEMS捷聯(lián)慣導(dǎo)系統(tǒng)、里程計(jì)和高精度組合導(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加速度計(jì)的量程為±50g，零偏穩(wěn)定性≤0.5mg。高精度組合導(dǎo)航系統(tǒng)由耐威公司的POS320組成，數(shù)據(jù)輸出頻率為20Hz，此導(dǎo)航系統(tǒng)為高精度基準(zhǔn)系統(tǒng)。通過測(cè)量POS320組合導(dǎo)航系統(tǒng)可以獲得初始對(duì)準(zhǔn)點(diǎn)位置。

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

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

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

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

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

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

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

圖2 軌跡對(duì)比圖

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

圖4 位置誤差對(duì)比

圖5 速度誤差對(duì)比

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

表1 兩種算法誤差對(duì)比

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

5 結(jié)論

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