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

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

（1.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室，石家莊 050081；2.中國電子科技集團公司第五十四研究所，石家莊 050081；3.92493 部隊，葫蘆島 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)的定位定向誤差隨時間累積，不利于長時間長距離的高精度導(dǎo)航[1-2]。

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

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

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

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

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

技術(shù)研究

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

1.2 航位推算位置誤差分析

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

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

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

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

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

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

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

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

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

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

對式（12）進一步處理，忽略誤差項的二階和高階小量，可得：

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

矩陣Ma可進一步簡化為常值矩陣：

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

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

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

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

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

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

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

自主導(dǎo)航系統(tǒng)上電啟動后認為陀螺儀漂移、加速度計偏置、里程計安裝以及標度因數(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)解算的位置與里程計航位推算的位置之差作為系統(tǒng)觀測量，即：

3 車載試驗驗證

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

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

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

圖2 里程計在線標定估計的誤差曲線

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

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

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

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

4 結(jié)束語

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