基于動(dòng)態(tài)零速修正的SINS/ODO組合導(dǎo)航算法研究*

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

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

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

本文提出動(dòng)態(tài)零速修正輔助SINS/ODO進(jìn)行組合導(dǎo)航的方法，來提高系統(tǒng)的導(dǎo)航定位精度。并將里程計(jì)刻度系數(shù)誤差和IMU安裝誤差角考慮在內(nèi)，給出了SINS/ODO松組合的數(shù)學(xué)模型。跑車試驗(yàn)結(jié)果表明在GNSS信號短時(shí)拒止時(shí)，該算法能夠有效抑制SINS的誤差發(fā)散，導(dǎo)航精度得到明顯提升。

1 動(dòng)態(tài)零速修正

定義車體坐標(biāo)系(v系)如圖1所示，坐標(biāo)系中原點(diǎn)是車體重心位置，Y指向車頭方向，X軸指向車體右側(cè)，Z軸指向車頂方向，X軸、Y軸和Z軸構(gòu)成右手坐標(biāo)系。

圖1 車體坐標(biāo)系

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

(1)

根據(jù)實(shí)際情況，觀測噪聲可以考慮為零均值高斯白噪聲。動(dòng)態(tài)零速修正構(gòu)成X軸和Z軸方向上的速度觀測量，里程計(jì)輸出車輛Y軸方向上的速度值，從而構(gòu)成三維速度量測，建立速度觀測方程，進(jìn)行Kalman濾波，修正MEMS慣導(dǎo)的漂移。

2 里程計(jì)

2.1 里程計(jì)刻度系數(shù)誤差

(2)

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

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

(3)

進(jìn)一步，里程計(jì)傳感器輸出的真實(shí)載體前向速度為：

(4)

2.2 里程計(jì)故障判斷

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

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

(5)

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

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

當(dāng)里程計(jì)出現(xiàn)故障時(shí)，應(yīng)停止MINS/ODO組合濾波，改由SINS子系統(tǒng)單獨(dú)工作，直到里程計(jì)輸出再次正常時(shí)，方可再進(jìn)行組合濾波。

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

3.1 SINS安裝角誤差

(6)

將上式展開，得

(7)

(8)

3.2 狀態(tài)方程

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

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

(9)

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

(10)

SINS系統(tǒng)狀態(tài)方程表述如下：

(11)

其中，F(xiàn)(t)為18×18的SINS系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；G(t)為18×6的SINS系統(tǒng)噪聲驅(qū)動(dòng)矩陣；W(t)為6×1的SINS系統(tǒng)噪聲矩陣。具體形式如下：

SINS系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣F(t)：

(12)

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

(13)

(14)

其中，Tgx，Tgy和Tgz為陀螺儀3個(gè)軸的相關(guān)時(shí)間，Tax，Tay和Taz為加速度計(jì)的相關(guān)時(shí)間。

SINS系統(tǒng)噪聲驅(qū)動(dòng)矩陣G(t)：

(15)

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

W(t)=[wgxwgywgzwaxwaywaz]T

(16)

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

3.3 量測方程

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

(17)

將ODO子系統(tǒng)輸出的載體速度分解到導(dǎo)航坐標(biāo)系，并與SINS子系統(tǒng)解算的速度進(jìn)行對比，取兩者差值作為觀測量[8]，則系統(tǒng)觀測方程為：

(18)

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

(19)

本文采用反饋校正來修正SINS的誤差。反饋校正是將慣性導(dǎo)航系統(tǒng)導(dǎo)航參數(shù)誤差的估值反饋到慣性系統(tǒng)中，對誤差狀態(tài)進(jìn)行校準(zhǔn)。反饋校正如圖2所示。

圖2 反饋校正

由于里程計(jì)只輸出速度值，故本文采用反饋校正的方法對SINS系統(tǒng)的速度進(jìn)行校正。

(20)

4 跑車試驗(yàn)與性能分析

為了驗(yàn)證設(shè)計(jì)的相關(guān)算法以及SINS/ODO組合導(dǎo)航系統(tǒng)的性能，進(jìn)行了跑車試驗(yàn)。跑車試驗(yàn)選擇在南京市麒麟科技園空曠路段進(jìn)行，跑車試驗(yàn)裝車效果如圖3所示，具體試驗(yàn)路線如圖4所示。

圖3 跑車試驗(yàn)裝車效果圖

圖4 跑車試驗(yàn)路線圖

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

表1 MSI3200G性能指標(biāo)

本次試驗(yàn)初始緯度32.022071°，初始經(jīng)度118.884072°，初始高度11.46m；尋北結(jié)果-143.65°。

地面跑車試驗(yàn)的具體試驗(yàn)方法如下：

1)跑車試驗(yàn)前，需要在實(shí)驗(yàn)室內(nèi)對試驗(yàn)設(shè)備和相關(guān)軟件進(jìn)行性能檢查，檢查無問題后進(jìn)行試驗(yàn)設(shè)備的安裝；

2)保證電源設(shè)置和系統(tǒng)線路連接正確后，給組合導(dǎo)航系統(tǒng)和高精度基準(zhǔn)導(dǎo)航系統(tǒng)(由高精度GNSS接收機(jī)與高精度光纖慣導(dǎo)等部分組成)上電；

3)由POS320提供初始航向角并將初始航向角信息通過監(jiān)控軟件上傳給導(dǎo)航板卡。待MSI3200G完成自對準(zhǔn)后，然后開始SINS/GNSS組合導(dǎo)航，保存組合導(dǎo)航系統(tǒng)和高精度基準(zhǔn)導(dǎo)航系統(tǒng)的數(shù)據(jù)，然后開始跑車；

4)通過拔掉衛(wèi)星導(dǎo)航的天線饋線獲得衛(wèi)星導(dǎo)航失效狀態(tài)，之后進(jìn)行SINS/ODO組合導(dǎo)航；

5)跑車試驗(yàn)的過程中，時(shí)刻對組合導(dǎo)航系統(tǒng)監(jiān)控軟件界面顯示的導(dǎo)航數(shù)據(jù)和高精度基準(zhǔn)導(dǎo)航系統(tǒng)監(jiān)控軟件界面顯示的導(dǎo)航數(shù)據(jù)進(jìn)行監(jiān)測，觀察數(shù)據(jù)是否異常；

6)跑車試驗(yàn)結(jié)束后，將組合導(dǎo)航系統(tǒng)輸出的數(shù)據(jù)和高精度基準(zhǔn)導(dǎo)航系統(tǒng)的數(shù)據(jù)進(jìn)行時(shí)標(biāo)對齊，然后進(jìn)行誤差比對分析，檢驗(yàn)系統(tǒng)的可靠性和組合導(dǎo)航算法的性能。

組合導(dǎo)航系統(tǒng)相對高精度基準(zhǔn)導(dǎo)航系統(tǒng)的試驗(yàn)結(jié)果如圖5～6所示。

圖5 位置誤差

圖6 速度誤差

從圖5和6可以看出，動(dòng)態(tài)零速修正輔助的SINS/ODO組合導(dǎo)航系統(tǒng)的三維位置誤差穩(wěn)定，且誤差?。蝗S速度誤差穩(wěn)定，且誤差小。無衛(wèi)星導(dǎo)航90s時(shí)統(tǒng)計(jì)值為：緯度誤差1.72m，經(jīng)度誤差-1.36m，高度誤差-4.38m，東向速度誤差0.02m/s，北向速度誤差-0.04m/s，天向速度誤差-0.23m/s。結(jié)果表明，本文設(shè)計(jì)的基于動(dòng)態(tài)零速修正的SINS/ODO組合導(dǎo)航在實(shí)際動(dòng)態(tài)環(huán)境下對純慣性導(dǎo)航誤差有較好的約束效果。

5 結(jié)論

提出了在衛(wèi)星信號據(jù)止情況下的基于動(dòng)態(tài)零速修正的SINS/ODO組合導(dǎo)航算法，通過跑車試驗(yàn)進(jìn)行驗(yàn)證。結(jié)果表明，在衛(wèi)星信號拒止時(shí)，該算法有較高的導(dǎo)航精度，能夠有效抑制MEMS慣導(dǎo)的漂移，可以作為備份導(dǎo)航方案。