里程計(jì)輔助的高精度車載GNSS/INS組合導(dǎo)航系統(tǒng)

劉鵬飛

(1.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，河北 石家莊 050081;2.中國電子科技集團(tuán)公司 第五十四研究所，河北 石家莊 050081)

1 引 言

衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System,GNSS)具有全球性、全天候、精度高等優(yōu)點(diǎn)，已廣泛應(yīng)用于車載組合導(dǎo)航系統(tǒng)中，但GNSS信號(hào)容易被遮擋和干擾，動(dòng)態(tài)性能較差；捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strap-down Inertial Navigation System,SINS)不依賴于外界環(huán)境，可以完全自主地提供全面的導(dǎo)航信息，動(dòng)態(tài)性能好、短時(shí)間內(nèi)導(dǎo)航精度高，但其由于慣性測(cè)量單元(Inertial Measurement Unit,IMU)本身存在測(cè)量誤差，導(dǎo)致導(dǎo)航系統(tǒng)的誤差隨時(shí)間累積，不利于長(zhǎng)時(shí)間長(zhǎng)距離的高精度導(dǎo)航[1-2]；將GNSS和INS進(jìn)行組合構(gòu)成組合導(dǎo)航系統(tǒng)，可實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，提高導(dǎo)航系統(tǒng)的精度和可靠性[3]。但是在城市隧道、高架橋下等長(zhǎng)時(shí)間無GNSS信號(hào)的場(chǎng)景下，車載組合導(dǎo)航系統(tǒng)的精度會(huì)急劇下降，抗干擾能力和可靠性也無法得到保障。

因此，在沒有GNSS信號(hào)的場(chǎng)景下，可以采用其他的導(dǎo)航傳感器來輔助慣性導(dǎo)航系統(tǒng)，而易于車輛安裝的里程計(jì)(Odometer,OD)則通過輸出位置增量信息，利用航位推算(Dead-Reckon,DR)算法可實(shí)現(xiàn)完全自主導(dǎo)航，進(jìn)一步抑制慣導(dǎo)系統(tǒng)長(zhǎng)時(shí)間無GNSS信號(hào)場(chǎng)景下的誤差發(fā)散，但是里程計(jì)本身受車輛安裝位置、輪胎氣壓、輪胎磨損程度以及車輛載荷等因素影響，會(huì)直接導(dǎo)致標(biāo)度因數(shù)誤差和車輛安裝誤差等發(fā)生變化[4]，所以在里程計(jì)輔助的車載GNSS/INS組合導(dǎo)航系統(tǒng)里程計(jì)的初次安裝使用前，必須對(duì)里程計(jì)的誤差項(xiàng)進(jìn)行標(biāo)定和補(bǔ)償[5-7]。文獻(xiàn)[8]對(duì)設(shè)定已知路標(biāo)點(diǎn)的傳統(tǒng)里程計(jì)標(biāo)定方法進(jìn)行了詳細(xì)介紹；文獻(xiàn)[9]提出了一種基于運(yùn)動(dòng)學(xué)非完整約束的里程計(jì)參數(shù)在線辨識(shí)方法，取得了較好的測(cè)試結(jié)果。

航位推算的位置誤差隨著車輛行駛里程的增加而逐漸變大，所以在GNSS信號(hào)質(zhì)量較好的路段，車載導(dǎo)航系統(tǒng)的組合模式應(yīng)由DR/INS組合模式切換回GNSS/INS組合模式，進(jìn)一步抑制由航位推算帶來的位置誤差積累。文獻(xiàn)[10]提出了GPS/SINS/OD組合導(dǎo)航定位系統(tǒng)，采用聯(lián)邦卡爾曼濾波對(duì)SINS進(jìn)行誤差估計(jì)并作反饋校正，通過仿真驗(yàn)證了該算法可有效抑制組合導(dǎo)航系統(tǒng)誤差的發(fā)散，但該文獻(xiàn)并沒有在實(shí)際的動(dòng)態(tài)跑車測(cè)試環(huán)境下進(jìn)行算法的驗(yàn)證，也沒有驗(yàn)證GNSS/INS模式和DR/INS模式自適應(yīng)切換場(chǎng)景下的算法性能；文獻(xiàn)[11]將里程計(jì)輸出位置信息與慣導(dǎo)輸出位置信息做差作為觀測(cè)，將估計(jì)出的誤差信息進(jìn)行反饋校正，構(gòu)成了緊組合導(dǎo)航系統(tǒng)，但是該系統(tǒng)在長(zhǎng)時(shí)間導(dǎo)航下還是會(huì)存在航位推算誤差累積的問題；文獻(xiàn)[12-13]則是從GNSS/INS組合的角度對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行了介紹，并沒有考慮當(dāng)GNSS信號(hào)長(zhǎng)時(shí)間失鎖組合導(dǎo)航精度下降的問題。

本文提出一種里程計(jì)輔助的高精度車載GNSS/INS組合導(dǎo)航方法，組合濾波模式根據(jù)載車環(huán)境變化可在GNSS/INS組合模式和DR/INS組合模式間實(shí)現(xiàn)自適應(yīng)切換，保證車載GNSS/INS組合導(dǎo)航系統(tǒng)在復(fù)雜路況下的高精度可靠定位。

2 組合導(dǎo)航系統(tǒng)設(shè)計(jì)

組合導(dǎo)航系統(tǒng)原理圖如圖1所示，在GNSS信號(hào)質(zhì)量良好，衛(wèi)導(dǎo)接收機(jī)可提供可靠導(dǎo)航信息時(shí)，自適應(yīng)組合濾波器則利用衛(wèi)導(dǎo)提供的位置、速度信息作為外部觀測(cè)量對(duì)組合濾波器進(jìn)行量測(cè)更新，此時(shí)的航位推算模塊不工作，自適應(yīng)組合濾波器將更新后的誤差估計(jì)值反饋回慣性導(dǎo)航系統(tǒng)，對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行誤差修正，同時(shí)組合導(dǎo)航濾波器輸出最優(yōu)的組合導(dǎo)航信息；當(dāng)復(fù)雜路況下衛(wèi)導(dǎo)信號(hào)不可用時(shí)，航位推算模塊根據(jù)上一時(shí)刻慣導(dǎo)提供的導(dǎo)航信息進(jìn)行航位推算，自適應(yīng)組合濾波器則根據(jù)航位推算提供的位置信息作為外部觀測(cè)量對(duì)組合濾波器進(jìn)行量測(cè)更新，自適應(yīng)組合濾波器將更新后的位置誤差估計(jì)值反饋回航位推算模塊中，對(duì)航位推算模塊進(jìn)行位置修正，同時(shí)組合導(dǎo)航濾波器輸出最優(yōu)的組合導(dǎo)航信息；組合導(dǎo)航濾波器只需根據(jù)衛(wèi)導(dǎo)是否定位，即可在GNSS/INS和DR/INS組合模式間快速切換，保證組合導(dǎo)航系統(tǒng)高可靠連續(xù)性的高精度定位。

圖1 里程計(jì)輔助的高精度車載GNSS/INS組合導(dǎo)航系統(tǒng)原理圖Fig.1 Block diagram of high-precision vehicle GNSS/INS integrated navigation system aided by odometer

3 組合導(dǎo)航系統(tǒng)誤差分析

選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，記為n系；慣性坐標(biāo)系記為i系；假設(shè)里程計(jì)坐標(biāo)系與車體坐標(biāo)系重合，記為m系，ox軸沿車體橫軸方向指向右方，oy軸沿車體縱軸指向正前方，oz軸垂直于地面向上，和ox軸、oy軸構(gòu)成右手直角坐標(biāo)系，即構(gòu)成一個(gè)右-前-上坐標(biāo)系；載體坐標(biāo)系，即慣導(dǎo)坐標(biāo)系記為b系，坐標(biāo)軸向定義與車體坐標(biāo)系一致；文中涉及到的導(dǎo)航坐標(biāo)系示意圖圖2所示。

圖2 導(dǎo)航坐標(biāo)系示意圖Fig.2 Diagram of Navigation coordinate system

3.1 捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)誤差分析

參照文獻(xiàn)[8]列出SINS的位置誤差方程、速度誤差方程和姿態(tài)誤差方程如式(1)：

(1)

3.2 航位推算(DR)位置誤差分析

首先考慮里程計(jì)標(biāo)度因數(shù)誤差δKD，給出DR速度誤差方程：

(2)

在不考慮里程計(jì)安裝誤差角的情況下，里程計(jì)航位推算位置誤差方程與捷聯(lián)慣導(dǎo)系統(tǒng)位置誤差方程一致，如式(3)所示：

(3)

其中：D表示航位推算系統(tǒng)誤差。

將式(2)代入到式(3)可得含里程計(jì)標(biāo)度因數(shù)誤差的位置誤差方程：

(4)

然后在考慮里程計(jì)安裝誤差角的情況下推導(dǎo)位置誤差方程。

(5)

航位推算誤差源主要包括：里程計(jì)平臺(tái)失準(zhǔn)角φD、里程計(jì)安裝偏差角αθ和αψ中未能完全補(bǔ)償?shù)氖Ｓ喔┭霭惭b誤差角δαθ和方位安裝誤差角δαψ以及里程計(jì)標(biāo)度因數(shù)誤差δKD，安裝誤差角和安裝偏差角間的關(guān)系如式(6)所示：

(6)

(7)

對(duì)式(7)進(jìn)一步處理，忽略各誤差項(xiàng)二階和高階小量，可得：

(8)

(9)

(10)

矩陣Ma可進(jìn)一步簡(jiǎn)化為常值矩陣：

(11)

最終得到的位置誤差方程：

(12)

至此，包括里程計(jì)標(biāo)度因數(shù)誤差、俯仰安裝誤差角以及方位安裝誤差角的航位推算位置誤差方程已得到。

4 組合導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)

本節(jié)首先給出里程計(jì)輔助的GNSS/INS組合導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)的具體過程，并簡(jiǎn)單描述了經(jīng)典Kalman濾波的更新過程，然后對(duì)航位推算算法的位置更新算法和姿態(tài)更新算法進(jìn)行介紹。

4.1 組合導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)

狀態(tài)方程中，選取捷聯(lián)慣導(dǎo)系統(tǒng)誤差中的姿態(tài)誤差φ、速度誤差δv、位置誤差δp、陀螺儀漂移εb、加速度計(jì)常值偏置b以及航位推算誤差中航位推算位置誤差δpD、里程計(jì)標(biāo)度因數(shù)誤差δK、俯仰安裝誤差角δαθ以及方位安裝誤差角δαψ作為濾波器的狀態(tài)量，共計(jì)21維，具體表示如式(13)：

X=

(13)

(14)

其中：F為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲分配矩陣，W為系統(tǒng)狀態(tài)噪聲陣；結(jié)合式(1)、式(12)，狀態(tài)轉(zhuǎn)移矩陣F的表達(dá)式如式(15)所示：

(15)

由GNSS接收機(jī)構(gòu)成的觀測(cè)方程如式(16)所示：

(16)

針對(duì)里程計(jì)航位推算模塊，將捷聯(lián)慣導(dǎo)解算的位置與里程計(jì)航位推算的位置之差作為系統(tǒng)觀測(cè)量，則由里程計(jì)航位推算模塊構(gòu)成的觀測(cè)方程如式(17)所示：

δp-δpD=HDX+VD

HD=[03×6I3×303×6-I3×303×3]T.

(17)

至此，由里程計(jì)輔助的GNSS/INS組合導(dǎo)航系統(tǒng)濾波器的狀態(tài)方程和量測(cè)方程推導(dǎo)完成。

進(jìn)一步將組合導(dǎo)航系統(tǒng)狀態(tài)方程(14)和觀測(cè)方程(16)、方程(17)進(jìn)行離散化為式(18)：

Xk=Φk,k-1Xk-1+Γk,k-1Wk-1

Zk=HkXk+Vk,

(18)

其中：Φk,k-1為系統(tǒng)離散化后的狀態(tài)轉(zhuǎn)移矩陣，Γk,k-1為離散化后的噪聲分配矩陣。Xk與Xk-1依次代表系統(tǒng)在k時(shí)刻和k-1時(shí)刻的狀態(tài)量，Wk-1代表系統(tǒng)的狀態(tài)噪聲，Vk代表系統(tǒng)的觀測(cè)噪聲。

式(18)的具體實(shí)現(xiàn)過程可使用經(jīng)典Kalman濾波的更新過程表示，如式(19)～式(25)所示：

狀態(tài)一步預(yù)測(cè)方程：

(19)

狀態(tài)估計(jì)：

(20)

一步預(yù)測(cè)誤差方差陣：

(21)

濾波增益矩陣:

(22)

估計(jì)誤差方差陣：

(23)

濾波增益矩陣又可以進(jìn)一步寫成：

(24)

估計(jì)誤差方差陣又可以進(jìn)一步寫成：

Pk=[I-KkHk]Pk,k-1.

(25)

4.2 航位推算算法

航位推算過程主要是利用里程信息、姿態(tài)以及航向信息來推算載車相對(duì)于初始點(diǎn)的相對(duì)位置，航位推算過程主要包括位置更新和姿態(tài)更新。

與捷聯(lián)慣導(dǎo)解算的位置更新微分方程類似，可以列出航位推算的位置更新微分方程：

(26)

根據(jù)式(26)即可得到航位推算的數(shù)字更新算法：

(27)

5 車載試驗(yàn)驗(yàn)證

車載試驗(yàn)裝置由高精度光纖GNSS/INS組合導(dǎo)航系統(tǒng)、里程計(jì)、流動(dòng)站GNSS接收機(jī)、基準(zhǔn)站GNSS接收機(jī)、GNSS接收天線以及GNSS高精度后處理軟件等組成，其中各模塊的技術(shù)指標(biāo)如表1所示。

表1 車載組合導(dǎo)航系統(tǒng)技術(shù)指標(biāo)列表

Tab.1 Technical indicators for vehicle integrated navigation system

技術(shù)指標(biāo)動(dòng)態(tài)范圍光纖陀螺儀零偏穩(wěn)定性:≤0.01 (°)/h±300 (°)/s石英加速度計(jì)偏置穩(wěn)定性:≤50 μg±6 gGNSS流動(dòng)站/基準(zhǔn)站接收機(jī)定位精度:10 m速度限制:≤515 m/s里程計(jì)標(biāo)度因數(shù)誤差:≤0.2%無限制GNSS高精度后處理軟件定位精度:≤5 cm無限制

光纖慣組和GNSS組合導(dǎo)航接收機(jī)均固連在車輛行李架上置于車頂，里程計(jì)安裝于測(cè)試車輛右后輪的轉(zhuǎn)軸上。測(cè)試開始前，在一大約4 km的平直路段上使用傳統(tǒng)里程計(jì)標(biāo)定方法對(duì)里程計(jì)誤差進(jìn)行標(biāo)定，標(biāo)定出里程計(jì)標(biāo)度因數(shù)誤差為0.15%，俯仰安裝誤差角為2.14°，方位安裝誤差角為1.06°，將標(biāo)定出的里程計(jì)誤差參數(shù)裝訂到車載組合導(dǎo)航系統(tǒng)中。

基準(zhǔn)站GNSS接收機(jī)天線架設(shè)在單位四樓樓頂?shù)臉?biāo)準(zhǔn)已知點(diǎn)上，使用GNSS后處理的高精度位置信息作為測(cè)試的位置參考信息。圖3為里程計(jì)在車輛的安裝圖、慣組和組合導(dǎo)航接收機(jī)在車輛行李架的安裝圖以及基準(zhǔn)站接收機(jī)和基準(zhǔn)站天線在樓頂?shù)陌惭b圖。測(cè)試時(shí)使用功分器將車頂?shù)腉NSS接收天線分別和流動(dòng)站GNSS接收機(jī)和GNSS/INS組合導(dǎo)航接收機(jī)連接。

圖3 車載組合導(dǎo)航系統(tǒng)安裝圖Fig.3 Installation diagram of vehicle GNSS/INS integrated navigation system

為進(jìn)一步模擬車輛在隧道等遮擋環(huán)境下的組合導(dǎo)航系統(tǒng)定位精度，在車輛行進(jìn)過程中切斷GNSS/INS組合導(dǎo)航接收機(jī)的天線信號(hào)，時(shí)間大約為7 min，行駛里程大約為7 km，分兩個(gè)路段切斷組合導(dǎo)航接收機(jī)天線信號(hào)，時(shí)刻大約從405～800 s和1 010～1 400 s，為進(jìn)一步評(píng)估有里程計(jì)輔助下的組合導(dǎo)航系統(tǒng)位置精度，分別進(jìn)行兩次重復(fù)試驗(yàn)，試驗(yàn)1為有里程計(jì)輔助下的車載跑車試驗(yàn)，試驗(yàn)2為無里程計(jì)輔助下的車載跑車試驗(yàn)，兩次重復(fù)試驗(yàn)車輛的行駛軌跡和參考基準(zhǔn)軌跡分別如圖4、圖5所示，位置誤差如圖6和圖8所示，速度誤差如圖7、圖9所示。

圖4 試驗(yàn)1-車輛行駛軌跡Fig.4 Test 1-Vehicle trajectory of integrated navigation system

由圖6、圖8可以看出，在無衛(wèi)導(dǎo)信號(hào)的兩次GNSS信號(hào)中斷時(shí)刻，有里程計(jì)輔助的組合導(dǎo)航系統(tǒng)，單個(gè)方向位置誤差最大值也未超過8 m，同時(shí)可以看出當(dāng)衛(wèi)導(dǎo)信號(hào)恢復(fù)后，位置誤差能夠很快收斂到正常水平；而無里程計(jì)輔助的組合導(dǎo)航系統(tǒng)，經(jīng)度誤差最大值將近160 m；由圖7、圖9可以看出，在無衛(wèi)導(dǎo)信號(hào)的兩次GNSS信號(hào)中斷時(shí)刻，有里程計(jì)輔助的組合導(dǎo)航系統(tǒng)，單方向的速度誤差最大值也未超過0.1 m/s；而無里程計(jì)輔助的組合導(dǎo)航系統(tǒng)，天向速度誤差最大，最大值達(dá)到了0.5 m/s。明顯可以看出有里程計(jì)輔助下組合導(dǎo)航系統(tǒng)精度要優(yōu)于無里程計(jì)輔助下的組合導(dǎo)航系統(tǒng)精度。

圖5 試驗(yàn)2-車輛行駛軌跡Fig.5 Test 2-Vehicle trajectory of integrated navigation system

圖6 有里程計(jì)輔助下的位置誤差Fig.6 Position error aided by odometer

圖7 有里程計(jì)輔助下的速度誤差Fig.7 Velocity error aided by odometer

圖8 無里程計(jì)輔助下的位置誤差Fig.8 Position error without odometer assistance

圖9 無里程計(jì)輔助下的速度誤差Fig.9 Velocity error without odometer assistance

從表2中也可以看出，有里程計(jì)輔助的組合導(dǎo)航系統(tǒng)位置誤差在3 m以內(nèi)；而無里程計(jì)輔助的組合導(dǎo)航系統(tǒng)位置誤差達(dá)到了42.4 m，遠(yuǎn)大于有里程計(jì)輔助的定位誤差。

表2 位置誤差統(tǒng)計(jì)表

圖10 自主導(dǎo)航系統(tǒng)水平位置誤差Fig.10 Autonomous navigation system horizontal position error

試驗(yàn)最后給出了高精度光纖慣性導(dǎo)航系統(tǒng)只在里程計(jì)輔助下的自主導(dǎo)航系統(tǒng)精度，試驗(yàn)路段同試驗(yàn)1、試驗(yàn)2的測(cè)試路段，整個(gè)測(cè)試路段共計(jì)31.2 km，水平位置誤差如圖10所示，可以看出整個(gè)測(cè)試過程中單方向的位置誤差最大值不超過20 m，精度統(tǒng)計(jì)值約為0.08×D(D為車輛行駛里程)，高于0.01 °/h的光纖慣導(dǎo)系統(tǒng)自主導(dǎo)航系統(tǒng)0.1%D的常規(guī)指標(biāo)要求。

6 結(jié) 論

針對(duì)城市隧道、偏遠(yuǎn)山區(qū)等復(fù)雜路況，GNSS信號(hào)遮擋較嚴(yán)重或長(zhǎng)時(shí)間無GNSS信號(hào)的場(chǎng)景下，車載GNSS/INS組合導(dǎo)航系統(tǒng)精度下降的問題，提出一種里程計(jì)輔助的高精度車載GNSS/INS組合導(dǎo)航方法，在考慮里程計(jì)標(biāo)度因數(shù)誤差、里程計(jì)安裝誤差角的情況下，將航位推算位置誤差、里程計(jì)標(biāo)度因數(shù)誤差、安裝誤差角列入傳統(tǒng)的組合導(dǎo)航濾波器的狀態(tài)量中，建立了組合導(dǎo)航濾波器的狀態(tài)方程和量測(cè)方程，實(shí)現(xiàn)了GNSS/INS組合模式和DR/INS組合模式間的自動(dòng)切換，在DR/INS組合模式下，使用慣導(dǎo)解算的位置與航位推算的位置之差作為濾波器的觀測(cè)值，進(jìn)行濾波器的量測(cè)更新。整個(gè)跑車過程中位置誤差在3 m以內(nèi)，進(jìn)一步驗(yàn)證里程計(jì)輔助下的車載組合導(dǎo)航系統(tǒng)的正確性。