基于運(yùn)動(dòng)學(xué)非完整約束的里程計(jì)參數(shù)在線辨識(shí)

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

（陸軍工程大學(xué)石家莊校區(qū)，石家莊 050003）

捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）具有輸出導(dǎo)航參數(shù)多、完全自主、隱蔽性強(qiáng)等突出優(yōu)點(diǎn)，因此被廣泛應(yīng)用于陸基導(dǎo)彈、武器發(fā)射車等陸地導(dǎo)航系統(tǒng)中。但是其誤差隨時(shí)間積累[1-2]，不利于長(zhǎng)時(shí)間的高精度導(dǎo)航，因此組合導(dǎo)航技術(shù)得到了國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。

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

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

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

1.1 里程計(jì)標(biāo)度因數(shù)誤差分析

里程計(jì)的輸出是脈沖信號(hào)，車輪每旋轉(zhuǎn)一周，里程計(jì)輸出c個(gè)脈沖，此時(shí)車輛行駛距離為L(zhǎng)，則里程計(jì)輸出脈沖與車輛行駛距離之間存在如下關(guān)系：

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

車輛真實(shí)速度在導(dǎo)航坐標(biāo)系的投影為：

1.2 航位推算誤差模型

由于慣導(dǎo)坐標(biāo)系與里程計(jì)坐標(biāo)系之間存在安裝偏差角，則慣導(dǎo)坐標(biāo)系到里程計(jì)坐標(biāo)系的轉(zhuǎn)換矩陣可以表示為：

則里程計(jì)輸出在慣導(dǎo)坐標(biāo)系的投影為：

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

則式(8)進(jìn)一步化簡(jiǎn)為：

因?yàn)镸1中含有小量，所以忽略二階小量得：

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

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

由于里程計(jì)輸出速度在導(dǎo)航系的投影為

對(duì)式(12)求導(dǎo)，并結(jié)合式(11)得：

1.3 捷聯(lián)慣導(dǎo)誤差模型

按照文獻(xiàn)[9-11]列寫捷聯(lián)慣導(dǎo)的誤差方程如下：

2 濾波算法

2.1 狀態(tài)方程

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

式中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲驅(qū)動(dòng)矩陣，W為系統(tǒng)噪聲矩陣。

2.2 基于車輛運(yùn)動(dòng)學(xué)非完整約束單點(diǎn)算法的觀測(cè)量設(shè)計(jì)

利用里程計(jì)的輸出求解車輛速度信息并將其轉(zhuǎn)化為導(dǎo)航系的速度矢量，與捷聯(lián)慣導(dǎo)輸出的速度矢量進(jìn)行比較，兩者的差值作為系統(tǒng)的量測(cè)量之一，如下：

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

圖1 車輛轉(zhuǎn)彎運(yùn)動(dòng)分析Fig.1 Analysis of vehicle turning motion

向心加速度以及車輛旋轉(zhuǎn)角速度的觀測(cè)值可以通過慣導(dǎo)輸出獲得，將其換算到載體坐標(biāo)系如下：

由于車輛線運(yùn)動(dòng)相對(duì)于地球的旋轉(zhuǎn)角速度相對(duì)較小，所以忽略其影響，得到：

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

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

對(duì)式(24)進(jìn)行全微分可得：

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

式(36)中，V表示量測(cè)噪聲矩陣。按照如上設(shè)計(jì)的狀態(tài)方程、量測(cè)方程進(jìn)行卡爾曼濾波，完成車載里程計(jì)參數(shù)的在線標(biāo)定。

3 仿真試驗(yàn)與車載試驗(yàn)驗(yàn)證

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

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

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

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

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

利用SINS/DR組合導(dǎo)航方式進(jìn)行車輛導(dǎo)航，并按照以速度差作為觀測(cè)量，通過卡爾曼濾波的方式進(jìn)行里程計(jì)的標(biāo)定（記為濾波1）；按照基于運(yùn)動(dòng)學(xué)非完整約束算法設(shè)計(jì)的觀測(cè)量進(jìn)行濾波，標(biāo)定里程計(jì)參數(shù)（記為濾波2）。圖5給出了利用加速度計(jì)與里程計(jì)得到的向心加速度的對(duì)比結(jié)果。

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

圖4 航位推算系統(tǒng)軌跡與真實(shí)軌跡的對(duì)比Fig.4 Comparison between the real track and the track of dead reckoning system

圖6～7給出了兩種濾波方式下，里程計(jì)航向安裝偏差角誤差以及標(biāo)度因數(shù)誤差的估計(jì)殘差的對(duì)比結(jié)果。從圖中仿真結(jié)果可見，第二種濾波方式能夠?qū)崿F(xiàn)里程計(jì)航向安裝偏差角誤差以及標(biāo)度因數(shù)誤差的最優(yōu)估計(jì)。

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

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

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

圖8 兩種濾波方式下組合導(dǎo)航系統(tǒng)的位置誤差對(duì)比結(jié)果Fig.8 Comparison o f position error estimation between the two filters

為了驗(yàn)證所提方法的可行性，在車輛的航向角速度中加入均值為零的白噪聲，模擬車輛的隨機(jī)輕微轉(zhuǎn)向運(yùn)動(dòng)，使車輛的模擬路徑接近真實(shí)路徑，并利用兩種濾波方式進(jìn)行里程計(jì)的在線標(biāo)定，得到兩種濾波方式下組合導(dǎo)航的行駛軌跡對(duì)比結(jié)果，如圖9所示。可見該方法能夠較高精度的辨識(shí)里程計(jì)參數(shù)，實(shí)現(xiàn)較高精度的組合導(dǎo)航。

圖9 兩種濾波方式在模擬真實(shí)路徑下導(dǎo)航軌跡對(duì)比結(jié)果Fig .9 Navigatioon path comparriison results betw een the two filteers in simulatedd real trajectory

為了驗(yàn)證本文所述方法的可行性，設(shè)計(jì)跑車試驗(yàn)，試驗(yàn)中所用設(shè)備的相關(guān)參數(shù)如下：陀螺零偏穩(wěn)定性≤0.02(°)/h，隨機(jī)游走系數(shù)加速度計(jì)零偏；里程計(jì)標(biāo)度因數(shù)誤差≤0.5%；跑車?yán)酶呔?GGPS位置信息作為航位推算的位置參考，GPS定位精度≤10 m (1σ)。實(shí)車試驗(yàn)裝置如圖10所示。里程計(jì)安裝于非轉(zhuǎn)向輪的輪軸上。高精度慣導(dǎo)以及GPS安裝如圖10所示。

利用已知車輛行駛平直路徑起點(diǎn)終點(diǎn)位置的傳統(tǒng)里程計(jì)標(biāo)定方式，對(duì)里程計(jì)參數(shù)進(jìn)行標(biāo)定，得到里程計(jì)刻度因數(shù)為0.0885m/pulse，里程計(jì)的定位精度為行駛里程）；航向安裝偏差角為 0.20°，俯仰安裝偏差角為 0.41°。試驗(yàn)中，車輛的行駛路徑如圖11所示。

圖10 試驗(yàn)裝置Fig .10 Experimental device

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

圖11 車載試驗(yàn)跑車路徑Fig.11 Path graph of vehicular experiment

圖12 車載試驗(yàn)在線標(biāo)定結(jié)果Fig.12 Online calibration results of vehicular experiment

4 結(jié) 論

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