車載自主導(dǎo)航系統(tǒng)里程計(jì)誤差在線標(biāo)定方法

劉鵬飛，師鵬宇，智奇楠，馬國駒，賈浩男，李梟楠

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

車載自主導(dǎo)航系統(tǒng)被廣泛應(yīng)用于自行火炮、導(dǎo)彈發(fā)射車、前線偵察車等現(xiàn)代陸地作戰(zhàn)車輛上，捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）是自主導(dǎo)航系統(tǒng)定位定向的核心設(shè)備，但其由于慣性測量單元（IMU）本身存在測量誤差，導(dǎo)致慣導(dǎo)系統(tǒng)的定位定向誤差隨時(shí)間累積，不利于長時(shí)間長距離的高精度導(dǎo)航[1-2]。

衛(wèi)星導(dǎo)航系統(tǒng)（GNSS）導(dǎo)航精度高、范圍廣，不受時(shí)間和空間限制，但其信號容易受到干擾和遮擋，在沒有衛(wèi)星導(dǎo)航信號或者信號質(zhì)量較差的路段，無法提供定位服務(wù)或定位精度較差，而車輛安裝的里程計(jì)（OD）通過輸出位置增量信息，利用航位推算（DR）算法可實(shí)現(xiàn)完全自主導(dǎo)航，但是里程計(jì)本身受車輛安裝位置、輪胎氣壓、輪胎磨損程度以及車輛載荷等因素影響，會直接導(dǎo)致標(biāo)度因數(shù)誤差和車輛安裝誤差等發(fā)生變化[3]，所以在SINS/ DR 自主導(dǎo)航解算前，必須對里程計(jì)的誤差項(xiàng)進(jìn)行在線標(biāo)定和補(bǔ)償[4-6]。文獻(xiàn)[7]對設(shè)定已知路標(biāo)點(diǎn)的傳統(tǒng)里程計(jì)標(biāo)定方法進(jìn)行了詳細(xì)介紹，傳統(tǒng)標(biāo)定方法要求車輛盡量沿直線行駛，高程變化盡量小等諸多條件限制，實(shí)際使用過程中存在很多不便；文獻(xiàn)[8]提出了一種基于運(yùn)動(dòng)學(xué)非完整約束的里程計(jì)參數(shù)在線辨識方法，該文獻(xiàn)使用加速度之差和速度之差作為濾波器觀測，但該方法實(shí)際動(dòng)態(tài)測試結(jié)果中里程計(jì)誤差參數(shù)估計(jì)值收斂時(shí)間較慢，難以滿足快速機(jī)動(dòng)的載車要求；文獻(xiàn)[9]將里程輸出速度與慣導(dǎo)輸出速度做差作為觀測，增加故障檢測模塊，對車輪打滑、滑行和側(cè)滑進(jìn)行了故障檢測，從而提高了里程計(jì)誤差辨識的準(zhǔn)確性；文獻(xiàn)[10]將里程計(jì)輸出位置信息與慣導(dǎo)輸出位置信息做差作為觀測，將估計(jì)出的誤差信息進(jìn)行反饋校正，構(gòu)成緊組合導(dǎo)航系統(tǒng)，如果濾波器估計(jì)出的誤差值不夠準(zhǔn)確，反饋到濾波器中反而會污染整個(gè)組合導(dǎo)航系統(tǒng)；文獻(xiàn)[11]-[12]利用車輛運(yùn)動(dòng)約束方法對里程計(jì)標(biāo)度因數(shù)誤差進(jìn)行在線標(biāo)定，但沒有對安裝誤差角進(jìn)行標(biāo)定修正；文獻(xiàn)[13]利用GPS 位置、速度信息對里程計(jì)的誤差進(jìn)行標(biāo)定修正，但是系統(tǒng)引入了衛(wèi)星導(dǎo)航系統(tǒng)，在衛(wèi)星信號質(zhì)量較差或沒有衛(wèi)導(dǎo)信號時(shí)，自主導(dǎo)航系統(tǒng)的使用會受到限制；文獻(xiàn)[14]利用擴(kuò)展卡爾曼濾波器對里程計(jì)誤差進(jìn)行在線估計(jì)和補(bǔ)償，提高了自主導(dǎo)航系統(tǒng)的魯棒性。

本文提出一種車載自主導(dǎo)航系統(tǒng)里程計(jì)在線標(biāo)定方法。將里程計(jì)航位推算的位置信息與慣性導(dǎo)航系統(tǒng)解算的位置信息之差作為組合導(dǎo)航濾波器的量測更新數(shù)據(jù)，同時(shí)將里程計(jì)的航位推算位置誤差、安裝誤差角、標(biāo)度因數(shù)誤差列入傳統(tǒng)的組合導(dǎo)航濾波器狀態(tài)量中并進(jìn)行實(shí)時(shí)估計(jì)，實(shí)現(xiàn)對里程計(jì)誤差的在線標(biāo)定。

1 SINS/DR 自主導(dǎo)航系統(tǒng)誤差分析

選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，記為n 系；假設(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)系一致；

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

技術(shù)研究

參照文獻(xiàn)[7]列出捷聯(lián)慣導(dǎo)系統(tǒng)的位置誤差方程、速度誤差方程和姿態(tài)誤差方程如下：

1.2 航位推算位置誤差分析

首先在不考慮里程計(jì)安裝誤差角的情況下推導(dǎo)航位推算位置誤差。

假設(shè)里程計(jì)的測量誤差主要是標(biāo)度因數(shù)誤差，記為δKD，則里程計(jì)測量的實(shí)際速度大小和理論速度大小的關(guān)系在里程計(jì)坐標(biāo)系m 下可用式（2）表示如下：

將式（4）、（2）代入到式（3）中可得：

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

式中，下標(biāo)D 表示航位推算系統(tǒng)誤差，將式（6）代入到式（7）可得含里程計(jì)標(biāo)度因數(shù)誤差的位置誤差方程：

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

通常在安裝慣組時(shí)會盡可能使慣導(dǎo)系統(tǒng)與車體坐標(biāo)系重合，即要求慣導(dǎo)坐標(biāo)系與里程計(jì)坐標(biāo)系重合，但受到安裝工藝的限制，慣導(dǎo)坐標(biāo)系和里程計(jì)坐標(biāo)系間存在安裝偏差角，即里程計(jì)安裝偏差角，分別記為俯仰安裝偏差角、橫滾安裝偏差角和方位安裝偏差角，慣導(dǎo)坐標(biāo)系到里程計(jì)坐標(biāo)系的轉(zhuǎn)換矩陣可表示為：

由式（10）可以看出，由于里程計(jì)安裝偏差角的存在，里程計(jì)位置增量輸出在慣導(dǎo)坐標(biāo)系上存在x、z 軸的分量輸出誤差，并且輸出誤差只跟俯仰安裝偏差角和方位偏差誤差角有關(guān)。

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

里程計(jì)位置增量輸出在導(dǎo)航坐標(biāo)系下的投影為：

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

通常意義上，里程計(jì)安裝偏差角均為小角度，車輛行駛過程中慣組的俯仰角和橫滾角也為小角度值，所以慣組坐標(biāo)系到導(dǎo)航坐標(biāo)系可進(jìn)一步簡化為只與方位角相關(guān)的轉(zhuǎn)換矩陣：

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

將式（15）、（16）代入到式（14）等式右邊的第二項(xiàng)中，可得：

為進(jìn)一步提取出俯仰安裝誤差角δαθ和方位安裝誤差角δαψ，考慮到：

結(jié)合式（18）、式（17）最終可表示成：

將式（19）代入到式（14）可得：

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

2 S INS/DR 自主導(dǎo)航系統(tǒng)濾波器設(shè)計(jì)

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

自主導(dǎo)航系統(tǒng)上電啟動(dòng)后認(rèn)為陀螺儀漂移、加速度計(jì)偏置、里程計(jì)安裝以及標(biāo)度因數(shù)誤差保持為一固定常數(shù)不變，即：

再結(jié)合式（1）、（20）可得自主導(dǎo)航系統(tǒng)狀態(tài)方程：

式中，F(xiàn) 為狀態(tài)轉(zhuǎn)移矩陣；G 為噪聲分配矩陣；W 為系統(tǒng)狀態(tài)噪聲陣。

將捷聯(lián)慣導(dǎo)解算的位置與里程計(jì)航位推算的位置之差作為系統(tǒng)觀測量，即：

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

車載試驗(yàn)中自主導(dǎo)航系統(tǒng)由高精度光纖捷聯(lián)慣性導(dǎo)航系統(tǒng)與里程計(jì)組成，其中高精度光纖陀螺儀隨機(jī)漂移小于0.01/ h（1σ），量程為±300°/s，加速度計(jì)偏置穩(wěn)定性小于50ug（1σ），量程為±6g，里程計(jì)標(biāo)度因數(shù)誤差小于0.2%。為保證設(shè)定已知路標(biāo)點(diǎn)的傳統(tǒng)里程計(jì)標(biāo)定方法的標(biāo)度結(jié)果準(zhǔn)確可靠，選取一較為平直約3km 的開闊路段進(jìn)行標(biāo)定測試試驗(yàn)，這一路段的高度差在5m以內(nèi)；里程計(jì)安裝于測試車輛右后輪的轉(zhuǎn)軸上，光纖慣組固連在車輛行李架上置于車頂，如下圖所示，測試過程中使用高精度商用GNSS 接收機(jī)提供的位置信息作為自主導(dǎo)航系統(tǒng)的位置參考信息，GNSS 接收機(jī)在開闊路段的定位精度優(yōu)于3m（RMS）。

圖1 車載自主導(dǎo)航系統(tǒng)設(shè)備安裝圖

使用傳統(tǒng)里程計(jì)標(biāo)定方法得到的里程計(jì)標(biāo)度因數(shù)誤差為-0.16%，俯仰安裝誤差角為-1.0587°，方位安裝誤差角為1.86°則使用上述里程計(jì)在線標(biāo)定方法得到的里程計(jì)誤差曲線如下圖所示：由圖2 可知，最終估算的標(biāo)度因數(shù)誤差為-0.12%，俯仰安裝誤差角為-1.064°，方位安裝誤差角為1.866°，標(biāo)定結(jié)果與傳統(tǒng)標(biāo)定方法得到的結(jié)果是一致的。

圖2 里程計(jì)在線標(biāo)定估計(jì)的誤差曲線

將上述里程計(jì)標(biāo)定的里程計(jì)誤差參數(shù)裝訂到自主導(dǎo)航系統(tǒng)中進(jìn)行動(dòng)態(tài)測試，動(dòng)態(tài)測試路線如圖3所示，車輛行駛總里程約40km，測試總時(shí)間約3，400s。

自主導(dǎo)航系統(tǒng)位置誤差如圖4所示，由圖中可知，在大約2，000s 處，車輛回到初始點(diǎn)，此時(shí)的水平位置誤差最小，均在5m以內(nèi)，隨后當(dāng)車輛再次駛離初始點(diǎn)后，水平位置誤差又開始逐漸增大，整個(gè)過程中水平位置誤差不大于30m，定位精度滿足0.1%D的陸用導(dǎo)航精度要求，整個(gè)過程中高度誤差都是在逐漸增大的，可見高度誤差在自主導(dǎo)航系統(tǒng)是不容易控制和約束的。

圖3 動(dòng)態(tài)測試路線圖

圖4 自主導(dǎo)航系統(tǒng)位置誤差曲線

4 結(jié)束語

針對車載自主導(dǎo)航系統(tǒng)存在的里程計(jì)安裝誤差和標(biāo)度因數(shù)誤差影響其導(dǎo)航精度的問題，本文提出了一種車載自主導(dǎo)航系統(tǒng)里程計(jì)誤差在線標(biāo)定方法，在考慮里程計(jì)標(biāo)度因數(shù)誤差、里程計(jì)安裝誤差角的情況下，推導(dǎo)了航位推算位置誤差方程，將航位推算位置誤差、里程計(jì)標(biāo)度因數(shù)誤差、安裝誤差角列入傳統(tǒng)的組合導(dǎo)航濾波器的狀態(tài)量中，使用捷聯(lián)慣導(dǎo)系統(tǒng)解算的位置與航位推算的位置之差作為濾波器的觀測值，進(jìn)行濾波器的量測更新，與傳統(tǒng)里程計(jì)標(biāo)定方法相比，驗(yàn)證了這種里程計(jì)誤差在線標(biāo)定方法的正確性，通過實(shí)際的動(dòng)態(tài)跑車測試，進(jìn)一步驗(yàn)證了里程計(jì)誤差參數(shù)標(biāo)校的正確性和可靠性。