基于運動學非完整約束的里程計參數在線辨識

宋金龍，石志勇，王律化，王海亮

（陸軍工程大學石家莊校區，石家莊 050003）

捷聯慣導系統（SINS）具有輸出導航參數多、完全自主、隱蔽性強等突出優點，因此被廣泛應用于陸基導彈、武器發射車等陸地導航系統中。但是其誤差隨時間積累[1-2]，不利于長時間的高精度導航，因此組合導航技術得到了國內外學者的廣泛關注。

全球衛星定位系統（GPS）導航范圍廣、精度高，成為組合導航技術的首選，但是其容易發生信號遮擋，具有暴露位置等缺點，而里程計（OD）通過輸出位置增量，借助航位推算（DR）方法能夠實現自主導航，并且與INS組合導航誤差比慣導系統導航誤差小，所以 SINS/DR組合導航技術在車輛導航領域有著重要意義[3-5]。然而里程計與捷聯慣導安裝在車體的不同位置，存在安裝偏差角，并且里程計的標度因數受到車輛載荷、輪胎磨損、氣壓和溫度等因素的影響[6]，因此對里程計的安裝偏差角以及標度因數進行標定有利于提高SINS/DR組合導航的精度。目前針對里程計的標定方法主要有離線標定和在線標定[7]。離線標定的方法是通過確定車輛行駛直線路徑的起點終點進行里程計的參數辨識，該方法對車輛行駛路徑要求較高。文獻[8]采用零速修正的慣導信息進行里程計標定，但是需要間隔一定時間停車一次，降低了車輛的機動性。文獻[9]基于車輛的運動約束，通過卡爾曼濾波進行里程計標度因數誤差的最優估計，但是文章沒有考慮里程計與捷聯慣性組的安裝誤差角。文獻[10-11]通過建立航位推算的誤差模型，利用卡爾曼濾波方法進行里程計參數的在線辨識，取得了較好的效果。文獻[12]采用EKF進行里程計參數辨識，提高了系統的魯棒性。文獻[13]對里程計標度因數誤差、桿臂誤差進行了建模補償，提高了INS/OD的組合導航精度。本文提出利用運動學非完整約束算法進行里程計參數的在線辨識。

1 SINS/DR組合導航系統誤差分析

選取東北天為導航坐標系，記為n系；里程計坐標系與車體坐標系重合，記為m系，y軸沿車體縱軸指向正前方，x軸沿車體橫軸方向指向右方，z軸與x軸、y軸構成右手直角坐標系；慣導坐標系記為b系。通常在安裝慣性組件時會盡可能使b系與m系重合，但是受到安裝工藝的限制，m系與b系之間存在著安裝誤差角，即為里程計安裝偏差角依次為航向安裝偏差角誤差、橫滾安裝偏差角誤差和俯仰安裝偏差角誤差。

1.1 里程計標度因數誤差分析

里程計的輸出是脈沖信號，車輪每旋轉一周，里程計輸出c個脈沖，此時車輛行駛距離為L，則里程計輸出脈沖與車輛行駛距離之間存在如下關系：

由式(1)可以看出，里程計推算距離的精度很大程度取決于標度因數KD的穩定性。然而標度因數受到車輪充氣程度、車輪磨損、車輛載荷、行駛中輪胎打滑等因素的影響，為了能夠獲得車輛行駛的精確里程或速度，必須要對里程計的標度因數進行標定，從而減小標度因數誤差對導航精度的影響。基于里程計的工作原理，在不考慮車輛發生側滑和跳躍的情況下通過記錄行駛距離為L的時間，即可求解車輛的行駛速度里程計輸出速度在車體坐標系的投影為：

車輛真實速度在導航坐標系的投影為：

1.2 航位推算誤差模型

由于慣導坐標系與里程計坐標系之間存在安裝偏差角，則慣導坐標系到里程計坐標系的轉換矩陣可以表示為：

則里程計輸出在慣導坐標系的投影為：

由式(7)可以看出，由于里程計坐標系與慣導坐標系存在小角度安裝偏差，導致里程計的輸出在慣導坐標系上的投影存在x、z軸的分量，并且與橫滾安裝偏差無關。假設方位安裝角與俯仰安裝角的誤差分別為均滿足小角度要求，所以同理。忽略二階小量，則里程計的實際輸出在慣導坐標系的投影為：

則式(8)進一步化簡為：

因為M1中含有小量，所以忽略二階小量得：

由于a為小角度，所以公式(11)中M1可簡化為

綜上，里程計方位安裝角誤差以及俯仰安裝角誤差對速度解算存在較大影響，因此需要考慮安裝誤差，以提高SINS/DR組合導航系統的導航精度。

由于里程計輸出速度在導航系的投影為

對式(12)求導，并結合式(11)得：

1.3 捷聯慣導誤差模型

按照文獻[9-11]列寫捷聯慣導的誤差方程如下：

2 濾波算法

2.1 狀態方程

陀螺、加計的誤差由零偏以及隨機誤差組成，其中零偏為隨機常數，隨機誤差為均值為0的白噪聲，并認為里程計與慣性元件之間的安裝誤差經過上電后保持為一常數不變，則有如下關系：根據式(14)～(18)可以得到系統狀態方程為：

式中，F為狀態轉移矩陣，G為噪聲驅動矩陣，W為系統噪聲矩陣。

2.2 基于車輛運動學非完整約束單點算法的觀測量設計

利用里程計的輸出求解車輛速度信息并將其轉化為導航系的速度矢量，與捷聯慣導輸出的速度矢量進行比較，兩者的差值作為系統的量測量之一，如下：

車輛在低速轉向運動時，車體的橫向加速度較小，輪胎不會發生側滑，該時刻車輛的轉向運動可用阿克曼轉向模型來描述。根據阿克曼轉向原理，車輛轉向時，存在一點使得四個車輪均繞其做圓周運動。如圖1所示，假設車輛在很短的時間內由A點移動到B點，移動距離dS可以通過里程計獲得，行駛方向變化的角度為為車輛的旋轉半徑，當車輛直線運動時，可以認為

圖1 車輛轉彎運動分析Fig.1 Analysis of vehicle turning motion

向心加速度以及車輛旋轉角速度的觀測值可以通過慣導輸出獲得，將其換算到載體坐標系如下：

由于車輛線運動相對于地球的旋轉角速度相對較小，所以忽略其影響，得到：

根據式(25)(26)可得向心加速度差值的觀測值為：

向心加速度差值誤差量的誤差模型推導如式(31)～(35)所示。

對式(24)進行全微分可得：

由式(13)及(32)～(36)可以得到量測矩陣為：

式(36)中，V表示量測噪聲矩陣。按照如上設計的狀態方程、量測方程進行卡爾曼濾波，完成車載里程計參數的在線標定。

3 仿真試驗與車載試驗驗證

為了驗證所提方法的可行性，本文按照真實車輛的行駛規律，設計了車輛的加速、勻速、左轉、右轉、爬升、下降、最后減速為零的運動形式，模擬了車輛的行駛環境，進行仿真分析。車輛的具體行駛路徑為：①靜止100 s；②沿正北方向以1 m/s2的加速度行駛10 s；③勻速直線行駛110 s；④以2 (°)/s的角速度左轉90°（45 s），之后勻速直線行駛100 s；⑤以9 (°)/s的角速度右轉450°（50 s），勻速直線行駛100 s；⑥以2 (°)/s的俯仰角速度爬升10 s，之后勻速行駛50 s，之后再以?2 (°)/s的俯仰角速度行駛10 s，完成車輛的爬坡運動，車輛恢復水平；⑦勻速直線運行100 s；⑧以-2 (°)/s的俯仰角速度行駛10 s，并勻速行駛50 s，之后以2 (°)/s的俯仰角速度行駛10 s，完成車輛的下坡運動，從而車輛再次恢復水平；⑨勻速直線行駛100 s；⑩以?2 m/s2的加速度做減速運動5 s，車輛恢復靜止100 s。

設定車輛行駛的初始位置：經度為108.909°E，緯度為34.246°N，海拔高為380 m，車輛的初始速度為0。得到車輛的行駛軌跡如圖2所示，車輛行駛至東經108.904°附近繞行450°，旋轉一周后向北行駛。

圖2 車輛的行駛軌跡Fig.2 Track of vehicle

圖3 車輛的行駛軌跡二維平面圖Fig.3 Two-dimensional map of vehicle track

車輛行駛軌跡的二維平面圖如圖3所示。設置慣性元件的參數如下：陀螺儀零漂為0.01 (°)/h，陀螺隨機游走為加速度計常值漂移為加計隨機游走為設置車輛的初始速度為零，方位角、橫滾角、俯仰角均為零；姿態誤差角為為了方便從仿真結果看出應用算法的效果，這里將里程計與慣導之間的安裝偏差角設置得稍大一些，航向安裝偏差角為150￠，俯仰安裝偏差為100￠；里程計的刻度因數誤差為 1%。當單純使用航位推算進行車輛軌跡求解時，得到如圖4所示行駛路徑，可見單純使用航位推算會產生較大的導航誤差。

利用SINS/DR組合導航方式進行車輛導航，并按照以速度差作為觀測量，通過卡爾曼濾波的方式進行里程計的標定（記為濾波1）；按照基于運動學非完整約束算法設計的觀測量進行濾波，標定里程計參數（記為濾波2）。圖5給出了利用加速度計與里程計得到的向心加速度的對比結果。

車輛左轉、右轉時的行駛速度為10 m/s，左轉時角速度為因此左轉時車輛的向心加速度為同理，車輛右轉時角速度為9 (°)/s，因此右轉時車輛的向心加速度為，以上計算結果與圖5中車輛向心加速度仿真結果基本吻合，證明了仿真結果的正確性。

圖4 航位推算系統軌跡與真實軌跡的對比Fig.4 Comparison between the real track and the track of dead reckoning system

圖6～7給出了兩種濾波方式下，里程計航向安裝偏差角誤差以及標度因數誤差的估計殘差的對比結果。從圖中仿真結果可見，第二種濾波方式能夠實現里程計航向安裝偏差角誤差以及標度因數誤差的最優估計。

圖6 兩種濾波方式下里程計安裝偏差角的估計殘差Fig.6 Comparison on residuals of installation deviation angle error estimation between the two filters

圖7 兩種濾波方式下里程計刻度因數的估計殘差Fig.7 Comparison on res iduals of scalefactor estimatio n between the two filters

對于車載導航系統，由式(11)可知里程計的航向安裝偏差角以及標度因數誤差對車輛水平軸向速度影響較大，即對導航精度影響較大。由于濾波2相比濾波1對里程計的航向安裝偏差角誤差以及標度因數誤差進行了最優估計，因此，由圖8可見第二種濾波方式下經緯度誤差明顯較小，實現了較高精度的導航。當車輛行駛960s，對兩次濾波進行組合導航誤差分析，采用濾波2的組合導航方式造成的經度誤差為?1.098×10-4(°)，采用濾波1的組合導航方式造成的經度誤差為?3.231×10-4(°)，即東向位置誤差提高了19.6 m。同理采用濾波2的組合導航方式造成的緯度誤差為?1.257×10-4(°)，采用濾波1的組合導航方式造成的緯度誤差為?2.531×10-4(°)，即北向位置精度提高14.2m。與采用濾波1進行組合導航相比，由于濾波2進行組合導航時，標度因數估計精度有所提高，所以高度誤差也稍有減小，但兩種組合導航方式對車輛的高度精度整體相差不大。

圖8 兩種濾波方式下組合導航系統的位置誤差對比結果Fig.8 Comparison o f position error estimation between the two filters

為了驗證所提方法的可行性，在車輛的航向角速度中加入均值為零的白噪聲，模擬車輛的隨機輕微轉向運動，使車輛的模擬路徑接近真實路徑，并利用兩種濾波方式進行里程計的在線標定，得到兩種濾波方式下組合導航的行駛軌跡對比結果，如圖9所示。可見該方法能夠較高精度的辨識里程計參數，實現較高精度的組合導航。

圖9 兩種濾波方式在模擬真實路徑下導航軌跡對比結果Fig .9 Navigatioon path comparriison results betw een the two filteers in simulatedd real trajectory

為了驗證本文所述方法的可行性，設計跑車試驗，試驗中所用設備的相關參數如下：陀螺零偏穩定性≤0.02(°)/h，隨機游走系數加速度計零偏；里程計標度因數誤差≤0.5%；跑車利用高精度 GGPS位置信息作為航位推算的位置參考，GPS定位精度≤10 m (1σ)。實車試驗裝置如圖10所示。里程計安裝于非轉向輪的輪軸上。高精度慣導以及GPS安裝如圖10所示。

利用已知車輛行駛平直路徑起點終點位置的傳統里程計標定方式，對里程計參數進行標定，得到里程計刻度因數為0.0885m/pulse，里程計的定位精度為行駛里程）；航向安裝偏差角為 0.20°，俯仰安裝偏差角為 0.41°。試驗中，車輛的行駛路徑如圖11所示。

圖10 試驗裝置Fig .10 Experimental device

在實車試驗中，初始裝訂里程計的標度因數為0.085m/pulse，航向安裝偏差角以及俯仰安裝偏差角均為0。按照上述里程計參數辨識方法進行里程計參數的在線辨識，得到車載試驗的誤差參數如圖12所示。從圖12可見，里程計的標度因數誤差約為5‰，所提方法的標定結果與里程計傳統標定結果基本吻合。利用本文方法得到航向安裝偏差角為 0.1964°，俯仰安裝偏差角為 0.4134°，從而驗證了里程計參數在線辨識方法的可行性。

圖11 車載試驗跑車路徑Fig.11 Path graph of vehicular experiment

圖12 車載試驗在線標定結果Fig.12 Online calibration results of vehicular experiment

4 結 論

針對里程計標度因數誤差以及安裝偏差角誤差對SINS/DR組合導航精度影響較大的問題，本文提出了一種基于運動學非完整約束的里程計參數在線辨識方法，通過分析安裝偏差以及標度因數誤差對里程計輸出的影響，建立航位推算的誤差模型。將里程計安裝偏差以及標度因數誤差作為狀態量，建立卡爾曼濾波狀態方程，通過里程計輸出速度解算得到車輛的向心加速度與捷聯慣導加速度計的輸出計算得到的向心加速度做差作為量測之一；將捷聯慣導與里程計輸出速度做差作為量測之二，建立卡爾曼濾波量測方程，進行里程計參數在線辨識；最后通過理想路徑和模擬真實路徑的仿真以及實車試驗，發現該方法能夠提高里程計航向安裝偏差角以及標度因數誤差的估計精度，從而實現較高精度的車輛導航。