吳 耀王田苗 王曉剛 劉 淼

（北京航空航天大學(xué)機械工程及自動化學(xué)院 北京 100191）

基于自適應(yīng)卡爾曼濾波的側(cè)滑移動機器人運動模型估計

吳 耀*王田苗 王曉剛 劉 淼

（北京航空航天大學(xué)機械工程及自動化學(xué)院 北京 100191）

精確實時在線的運動模型對于側(cè)滑移動機器人的運動控制和軌跡規(guī)劃至關(guān)重要，相比于離線模型估計，該文在基于速度瞬心（ICRs）的側(cè)滑移動機器人運動學(xué)模型基礎(chǔ)上，采用擴展卡爾曼濾波（EKF），在同一特定地形下在線準(zhǔn)確得到ICRs的參數(shù)值；并針對不同的地形情況，采用k-近鄰法對地形進行分類，實時判別機器人當(dāng)前運行的路面，采用自適應(yīng)的卡爾曼濾波器（AKF）調(diào)整濾波器參數(shù)。仿真和實驗對比表明，該方法在同一地形和變化地形下均能快速估計出側(cè)滑移動機器人的運動學(xué)模型，收斂時間均為3 s以內(nèi)，可以滿足實際使用的需要。

移動機器人；側(cè)滑移動；自適應(yīng)卡爾曼濾波；速度瞬心；k-近鄰法

1 引言

側(cè)滑轉(zhuǎn)向（skid-steering）機制廣泛應(yīng)用在移動機器人和車輛上［1］，它是通過改變左右輪子或履帶的速度來控制車輛方向的，而不是通過獨立的機械轉(zhuǎn)向機構(gòu)實現(xiàn)轉(zhuǎn)向。因此，這種轉(zhuǎn)向機制簡單有效并且具有很好的魯棒性，能夠?qū)崿F(xiàn)零-半徑轉(zhuǎn)向，特別適合全地形操作的機器人或者車輛。但是，側(cè)滑轉(zhuǎn)向操作固有的特點 打滑，帶來了復(fù)雜的輪-地接觸力關(guān)系，相比于轉(zhuǎn)向車輛（比如ackman轉(zhuǎn)向）或者兩輪車輛，其運動模型的準(zhǔn)確建立更加困難［2-6］。而側(cè)滑移動機器人高性能的運動控制和軌跡規(guī)劃，需要相對準(zhǔn)確的車輛運動模型［4］。在過去十年來，關(guān)于準(zhǔn)確建立其運動模型的的研究主要分為兩大部分。

一部分研究從動力學(xué)的角度，建立輪胎的剛度或履帶力學(xué)的模型［2，3］，這個領(lǐng)域的研究常常集中在地形與運動系統(tǒng)的作用力上，打滑模型為輪胎或者牽引力之間的函數(shù)關(guān)系。其中，文獻［7，8］通過數(shù)值和實驗的方法，分析和驗證了不同的打滑率作用下牽拉、牽引力系數(shù)和驅(qū)動力矩與輪子的半徑和寬度等參數(shù)的關(guān)系，并使用遺傳算法得到了優(yōu)化的輪子設(shè)計參數(shù)和最佳的牽引力參數(shù)。但是該方法是在特定地形條件下離線測量的，沒有進行實時在線的測量和辨識。文獻［9］采用了統(tǒng)一輪胎模型建立了高速輪式移動機器人在一般運動狀態(tài)和漂移狀態(tài)下的動力學(xué)模型，并在此基礎(chǔ)上設(shè)計了漂移運動控制器，但是仿真和實驗的結(jié)果存在一定差距。在文獻［10］中，針對側(cè)滑輪式車輛的緯線動力學(xué)，提出了二自由度的線性緯線動力學(xué)模型，研究其轉(zhuǎn)向過度，轉(zhuǎn)向不足和中性轉(zhuǎn)向情況下的轉(zhuǎn)向性能；文獻［11］利用文獻［10］提出的模型，發(fā)現(xiàn)在線性條件下穩(wěn)態(tài)和瞬態(tài)特性具有很好的結(jié)果，但是在側(cè)滑移動機器人存在很多非線性運動的情況，因此該模型的使用受到一定的限制。在實際應(yīng)用中，由于動力學(xué)模型相對復(fù)雜，計算量大，在實時控制的場合下運算代價大，所以一般動力學(xué)模型用于機器人的運動仿真［2，3］。

另一部分研究使用運動學(xué)模型來估計打滑［12］。側(cè)滑移動的機器人打滑用運動不滿足非完整約束的關(guān)系，難以建立準(zhǔn)確的運動學(xué)模型。文獻［4］使用擴展卡爾曼濾波（Extended Kalman Filter， EKF）融合慣性測量單元（Inertial Measurement Unit， IMU）和虛擬速度來估計運動打滑。實驗結(jié)果表明，當(dāng)機器人在復(fù)雜路面上行駛約40 m時，位置的估計偏差小于25 cm。文獻［13］使用了一個延時狀態(tài)EKF估計打滑模型，其實質(zhì)是采用了一種積分預(yù)測誤差最小化方法模型。文獻［14］通過實驗手段描述打滑模型，他們用指數(shù)函數(shù)來描述打滑系數(shù)與轉(zhuǎn)彎半徑之間的關(guān)系。實驗結(jié)果顯示，基于該模型的側(cè)滑移動車輛具有很好的控制性能。在文獻［15］的工作中，用到了本文中將會用到的基于速度瞬心（Instantaneous Centers of Rotation， ICRs）的運動學(xué)模型，但是ICRs位置是利用遺傳算法處理記錄下來的實驗數(shù)據(jù)，然后通過離線方法得到的。文獻［16］分析了四輪側(cè)滑移動機器人同側(cè)速度不一致的情況，仿真研究表明了該分析結(jié)果的有效性，但是缺少實驗的進一步驗證。

文獻［12］的研究也采用了文獻［15］中的模型，并且采用基于激光雷達定位的方法，得到了 ICRs與軌跡半徑和速度的函數(shù)關(guān)系。實驗結(jié)果表明，在同一地形路面下，ICRs確實在一個較小的范圍內(nèi)變化（1.4～1.5），這與文獻［15］的研究相吻合。實驗結(jié)果顯示，文獻［12］得到的經(jīng)驗公式模型顯著提高了航跡推算精度。但是同文獻［17］的研究一樣，該實驗方法只能通過離線方式得到 ICRs值與軌跡半徑和運動速度的函數(shù)關(guān)系。文獻［17］提出了一種基于擴展卡爾曼濾波（EKF）在線實時學(xué)習(xí)獲得ICRs值的方法，通過對機器人的給定速度、偏航角和位置的測量，可直接得到機器人的運動模型。但是該方法在變化地形條件下，EKF收斂時間較長，甚至達到100 s～150 s，導(dǎo)致在實際應(yīng)用中受到很大限制。對于擴展卡爾曼濾波的方法，文獻［18］提出了一種基于神經(jīng)網(wǎng)絡(luò)的自適應(yīng)卡爾曼濾波方法，應(yīng)用在圖像處理中，提高了處理的精度并有效的抑制了濾波器的發(fā)散，但是離線的神經(jīng)網(wǎng)絡(luò)訓(xùn)練方法不適合實時的應(yīng)用；文獻［19］提出了一種基于變分貝葉斯學(xué)習(xí)的自適應(yīng)卡爾曼濾波方法，仿真結(jié)果表明，該方法對時變的噪聲具有較好的跟蹤效果，相對經(jīng)典卡爾曼濾波有著較高的濾波精度。而在該文中，濾波器的噪聲主要隨著地形的變化而發(fā)生突變，因此，結(jié)合機器人作業(yè)地形的分類（Robotic Terrain Classification，RTC），建立相應(yīng)地形條件下的模型估計算法，是處理側(cè)滑移動機器人運動模型實時辨識問題的一個途徑。

關(guān)于機器人對不同地形條件的判別方法，前人已進行了非常豐富的研究［20-23］，這些研究主要是通過激光雷達，視覺或振動測量實現(xiàn)地形分類。其中基于振動的地形分類方法通過分析移動機器人車輪與地面的振動信號，采用統(tǒng)計特征［20］，快速傅里葉變換（Fast Fourier Transformation， FFT），或功率譜分解（Power Spectral Density， PSD）［22］的方法提取信號的特征向量，采用 k-近鄰（k-Nearest Neighbors， kNN）方法、概率神經(jīng)網(wǎng)絡(luò)（Probabilistic Neural Network， PNN）、支持向量機（Support Vector Machine， SVM）實現(xiàn)地形分類。

結(jié)合已有的研究工作［12，17］，本文提出了一種基于擴展卡爾曼濾波（EKF）學(xué)習(xí)輪子軌跡（輪跡）相對于地面的瞬時旋轉(zhuǎn)中心ICRs的方法，通過ICRs建立的機器人運動學(xué)模型來預(yù)測其運動。針對該方法在變換地形條件下EKF收斂時間較長的問題，直接應(yīng)用IMU測量的z軸加速度信號，基于功率譜密度（PSD）的方法提取特征，采用k-近鄰地形分類方法，實時判別當(dāng)前機器人運行的路面地形。同時在地形發(fā)生變化時自適應(yīng)地調(diào)整 EKF濾波器誤差協(xié)方差矩陣初值，使EKF快速進入收斂狀態(tài)。仿真和實驗結(jié)果表明，本文方法和傳統(tǒng)的遺傳算法相比，在保證收斂精度的情況下，極大地縮短了收斂時間。

2 側(cè)滑轉(zhuǎn)向（skid-steering）的ICRs運動模型

對于輪式側(cè)滑移動機器人，我們考慮下面的模型假設(shè)：（1）輪式機器人同一側(cè)所有輪子的轉(zhuǎn)速相同；（2）機器人行駛在堅固路面，并且所有的輪子都與地面處于接觸狀態(tài)；（3）只考慮2D平面運動的情況。在圖1中，定義慣性（全局）坐標(biāo)系（X ，Y ）和機器人本體坐標(biāo)系（x， y）。機器人本體坐標(biāo)系下的運動可以表示為，在慣性坐標(biāo)系下的運動可以表示為根據(jù)文獻［1］有

圖1 側(cè)滑移動機器人運動學(xué)原理圖

根據(jù)假設(shè)，令左右兩邊輪速分別為 vl，vr。當(dāng)機器人運動時，左右的輪子印跡和車體的速度瞬心分別為ICRl， ICRr和ICRG，在x-y坐標(biāo)系中，定義其坐標(biāo)分別為（xl，yl），（xr，yr）和（xG，yG）。據(jù)文獻［15］有

從而得到車輛的運動學(xué)模型如下：

由式（3）-式（5）可知，如果已知 yl，yr，xG，就可得到側(cè)滑移動機器人的運動學(xué)模型。全部動力學(xué)的影響，包括輪-地接觸力，重心分布，運動速度等，都體現(xiàn)在這3個運動學(xué)參數(shù)上。通過這組集總參數(shù)，可描述系統(tǒng)相對復(fù)雜的運動狀態(tài)，在此基礎(chǔ)上，使用基于EKF的方法，可以在線實時得到 yl，yr，xG的值。

3 EKF方法估計ICRs位置

3.1 同一特定地形下EKF估計ICRs值

本節(jié)將推導(dǎo)使用 EKF方法來估計 ICRs的位置。根據(jù)式（3）-式（5），定義狀態(tài)變量： X=［X Y θ ylyrxG］，這里X為向量，以下采用同樣的記法。系統(tǒng)模型則可由以下非線性微分方程表示。

這里u為控制輸入向量，由左右兩側(cè)的速度值組成；w為系統(tǒng)的噪聲，分別包括位姿噪聲和速度瞬心的運動參數(shù)噪聲?？紤]圖1坐標(biāo)系下側(cè)滑移動機器人運動，式（6）可以具體寫成

其中 wX，wG是均值為零的高斯白噪聲。根據(jù)參考文獻［12，17］，在特定的均勻地形條件下，當(dāng)機器人以較低的速度（小于 1 m/s）行駛在平坦硬質(zhì)路面時，ICRs 的位置在一個小的范圍內(nèi)變化。因此，可以把這里的ICRs的值看作為在常值上疊加隨機噪聲，隨后仿真結(jié)果也將證實在此處所作的假設(shè)的合理性。把式（7）離散化，設(shè)濾波器采樣時間間隔為 Δt ?？紤]EKF濾波器，預(yù)測估計協(xié)方差為

其中的Q為狀態(tài)噪聲協(xié)方差矩陣， F（k -1）和L （k -1）為Jacobi矩陣，定義如下：

式（9）中， In和 On，n∈?，分別表示n × n矩陣和零矩陣，以下相同， Δt 為濾波器采樣時間間隔，矩陣中的其他項為

式（11）中，c θ= cosθ，s θ= sin θ。而噪聲雅可比矩陣為

在全局坐標(biāo)系下傳感器測量的位置和偏航角為

假設(shè)測量噪聲 V （k）是方差為R的高斯白噪聲，H （k ）為測量矩陣，在此處為

計算卡爾曼增益為

最后，狀態(tài)和協(xié)方差估計更新分別為

3.2 不同地形條件下EKF估計ICRs值

在系統(tǒng)狀態(tài)方程式（7）中，我們假設(shè)ICRs 是在常值的基礎(chǔ)上疊加隨機噪聲來得到的。根據(jù)參考文獻［17］，當(dāng)側(cè)滑移動機器人行駛在同一特定的地形條件下時，ICRs不會發(fā)生很大的變化，但是如果機器人穿梭在不同地形環(huán)境中時，比如環(huán)境中有瓷磚路面、混凝土路面、沙土路面和草地等，輪-地接觸參數(shù)發(fā)生了變化，由此也會帶來 ICRs值的變化，此時“ICRs不會發(fā)生很大的變化”這個假設(shè)條件不再成立。此時，3.1節(jié)描述的算法是否能夠收斂，以及收斂的速度是否滿足要求，需要重新加以分析驗證。事實上，從 4.2節(jié)的仿真結(jié)果可以看到，在這種情況下EKF收斂時間很長，甚至在實際中幾乎無法使用。

為了保證收斂的速度，同時不降低精度，需要對前述的EKF方法做一些調(diào)整。分析發(fā)現(xiàn)，機器人在地形A時，EKF濾波器已經(jīng)達到穩(wěn)定，誤差協(xié)方差矩陣 P （k）也趨于穩(wěn)定；而機器人從地形A運行到地形B時，由于ICRs的變化，誤差協(xié)方差矩陣 P（k）有一個較大的初始偏差值。為此，使用現(xiàn)有的測量值和運動模型，基于機器人地形分類的方法，提出一種快速判斷地形是否發(fā)生變化（或者說ICRs發(fā)生突變）的方法。當(dāng)機器人從地形A運行到地形B時，如果檢測到 ICRs發(fā)生顯著變化，此時自適應(yīng)的調(diào)整EKF的誤差協(xié)方差矩陣 P （k）的初值，這樣可在保證精度不變的前提下，大大縮短收斂時間。

在該文中使用慣性測量單元IMU，可以輸出偏航角和X，Y，Z3軸的加速度值。我們采用基于振動的地形分類方法，利用Z軸加速度信息，提取加速度的頻率特征，利用分類算法實現(xiàn)地形的分類。IMU的輸出頻率為100 Hz，因此可以把Z軸加速度記為1× 100的向量，也就是機器人運行1 s的采樣數(shù)據(jù)。同時，對該向量標(biāo)記相應(yīng)的地形類型，并通過對該向量做功率譜變換（PSD），把每個加速度變?yōu)轶w現(xiàn)地形特征的特征向量，生成1 ～ 50 Hz下的功率譜，產(chǎn)生了1× 50的特征向量。采用k-近鄰法（kNN）對地形分類，選擇距離測試樣本最近的已知地形分類的前若干個樣本作為訓(xùn)練樣本，用它們的類別投票來決定測試樣本的地形類別。

已知地形的訓(xùn)練樣本集（在每種相應(yīng)的地形上運行并采集數(shù)據(jù)），并記為

其中 xi和 ci為分別第i個樣本的特征向量和對應(yīng)的地形類別；設(shè)有m個類，即有并采用歐氏距離 xi-xj定義兩個樣本間的距離為d（xi，xj），xi為1 × 50的特征向量，記為 xi=（xi（1），xi（2），… ， xi（50）），則有

當(dāng)機器人運行在特定的未知地形路面時，設(shè)此路面為待測樣本x，為了決定機器人的運行地形，考察待測樣本x在N個訓(xùn)練樣本中的前k個近鄰，設(shè)有 ki個屬于 ci類， ci類的判別函數(shù)和決策規(guī)則分別為

通過實時的測量，采用k-近鄰法得到當(dāng)前機器人運行路面的類型，如果機器人此時運行的路面類型和上一時刻的路面類型不同，并且連續(xù)2次采樣后的判別結(jié)果都是保持在新的路面時，此時認(rèn)為機器人運行時發(fā)生了路面變化，則調(diào)整誤差協(xié)方差矩陣P為事先設(shè)定的較大初值 PB，保證濾波器快速收斂。

4 仿真和實驗驗證

本文使用MATLAB仿真測試上述的EKF方法，并研究其收斂的精度、速度和對于噪聲的敏感性，并由實驗驗證文中提出的算法。仿真和實驗中，使用Pioneer P3-AT側(cè)滑移動機器人，其參數(shù)如表1所示。仿真時，同一地形條件下位置和偏航角的測量值通過式（3）-式（5）運動學(xué)模型輸出疊加均值為0的高斯白噪聲產(chǎn)生，其中位置標(biāo)準(zhǔn)差設(shè)為0.01 m，角度標(biāo)準(zhǔn)差設(shè)為1.0°。這樣的偏差值接近在實際中使用的差分GPS系統(tǒng)和IMU測量模塊。而在4.2節(jié)不同地形條件下基于地形分類的EKF估計ICRs值的仿真中，使用的是IMU實際輸出的z軸加速度值。整個實驗系統(tǒng)組成如圖2所示，采用NovAtel的差分GPS系統(tǒng)測量移動機器人的位置和方向，位置精度為0.01 m，雙星測向精度為0.03°，數(shù)據(jù)更新速率為20 Hz，同時采用iFly公司的G2慣性測量單元IMU測量加速度信息。在實驗中，機器人分別行駛在瓷磚，草地，混凝土路面上，通過車載的PC完成控制和數(shù)據(jù)的采集。在仿真和實驗中，比較了本文所提算法得出的側(cè)滑移動機器人運動學(xué)模型和文獻［15］中提出的基于遺傳算法（Genetic Algorithm，GA）的運動學(xué)模型，并對比不考慮打滑（No Slip， NS）的機器人運動模型，進行里程計計算，并與真實值比較，以此驗證本文提出算法的有效性。

表1 基于Pioneer P3-AT的仿真平臺參數(shù)

圖2 P3-AT側(cè)滑移動機器人實驗系統(tǒng)

4.1 同一特定地形下EKF估計ICRs仿真

在仿真中，設(shè)定機器人運行在同一特定路面（比如混凝土路面），假設(shè)在此路面下ICRs值分別為：yl=0.3 m， yr=-0 .5 m， xG=-0 .1 m 。設(shè)定狀態(tài)噪聲協(xié)方差矩陣Q和測量噪聲協(xié)方差矩陣R分別為

對ICRs賦任意初值： yl=1.0 m，yr=-1.0 m，xG= 1.0 m ，圖3所示的是不同的里程計與真實軌跡的仿真結(jié)果比較，其中ICR是本文采用EKF算法得到的運動學(xué)模型，GA和NS分別是遺傳算法和不考慮打滑的模型所計算的里程計軌跡。從圖 3（a）中可以看出ICR和GA模型所估計的軌跡和真實的軌跡非常的吻合，而NS模型偏離真值較大。圖3（b）顯示的是上述3種不同里程計估計值和運動軌跡真實值的偏差，可以發(fā)現(xiàn)ICR和GA模型的偏差均為100 mm以內(nèi)，遠(yuǎn)小于NS模型的偏差。

圖4表示在此同一特定路面下ICRs真值和本文采用的EKF算法估計值的對比，發(fā)現(xiàn)EKF算法能夠很快的辨識出 ICRs真值： yl=0.3 m，yr= -0 .5 m，xG=- 0.1 m 。而通過遺傳算法得到的ICRs值為： yl=0.30 m，yr=-0 .51 m，xG=-0 .11 m 。根據(jù)文獻［24］，定義EKF算法收斂時間為ICRs估計

值從 10%～90%最終值所經(jīng)歷的時間。從圖中可以看出，在此特定路面下，收斂時間大約為0.5 s。這也證實了在3.1節(jié)中所使用的假設(shè)“ICRs 的位置在一個小的范圍內(nèi)變化”，所以把ICRs值作為在常值上疊加隨機噪聲信號的假設(shè)是合理的。

4.2 不同地形條件下基于地形分類的 EKF估計ICRs值

當(dāng)?shù)匦巫兓瘯r，如圖5和圖6所示，在0～50 s內(nèi)，機器人運行在地形A混凝土路面上，此時ICRs值為：在50 s時刻，機器人運動到地形B瓷磚路面上（假定不考慮過濾時間，以下相同），此時 ICRs值為： yl=在150 s時刻，機器人運動到地形 C草地上，此時 ICRs值為：yl=0.4 m，yr=- 0.6 m，xG=-0 .2 m 。

由圖5，可知這種情況下ICR法和GA模型的偏差顯著增大，相對于 NS模型偏差提高得很少；事實上根據(jù)圖6，如果未考慮地形變化，雖然ICRs值達到了較高的收斂精度，但是收斂時間延長到100 s以上。

圖3 同一地形條件下不同里程計和真實值軌跡基于EKF算法的仿真結(jié)果比較

圖4 同一地形下基于EKF算法的ICRs值仿真結(jié)果

圖5 不同地形條件下不同里程計和真實值軌跡基于EKF算法的仿真結(jié)果比較

圖6 不同地形條件下基于EKF算法的ICRs值仿真結(jié)果

為此，采用k-近鄰分類的算法對上述3種路面A， B和C分類，判斷當(dāng)前機器人運行路面的類型。在實驗中測試的這 3種地形：A 混凝土路面（Concrete）， B瓷磚路面（Tile）， C草地（Grass），每種地形的訓(xùn)練樣本為15組，一共45組。為使用自適應(yīng)卡爾曼濾波（AKF）算法，設(shè)定如下的 PB值，

同樣地，當(dāng)機器人進行與圖5和圖6相同的地形轉(zhuǎn)換時，結(jié)合k-近鄰地形分類算法，分別采用基于地形分類的自適應(yīng)卡爾曼濾波（AKF）算法和無地形分類的EKF估計ICRs值，結(jié)果如圖7和圖8所示。仿真結(jié)果顯示，當(dāng)?shù)匦伟l(fā)生轉(zhuǎn)換時，即圖中t=50 s和t=200 s時刻，ICRs參數(shù) yl，yr和 xG的值都發(fā)生了突變（圖 8中實線所示）。采用基于地形分類的自適應(yīng)卡爾曼濾波（AKF）算法，ICRs值在3 s內(nèi)收斂到真實值，與未采用地形分類的EKF算法相比，調(diào)節(jié)時間大大縮短，而對應(yīng)在圖7中，基于AKF的里程計偏差明顯的小于其他里程計偏差。

4.3 實驗驗證

在同一地形的測試中，側(cè)滑移動機器人按照一定的速度和半徑行駛在瓷磚路面上，圖9和圖10分別是機器人在此實驗過程中，不同里程計和真實值軌跡比較以及該地形下的ICRs估計值。由圖9可以看出，不考慮打滑的 NS模型的里程計偏差超過了1200 mm，而基于EKF的ICR模型和基于遺傳算法GA模型的里程計偏差比較接近，均為300 mm以內(nèi)，比仿真中的100 mm偏差大，這可能是因為實際情況下噪聲并非理想的高斯白噪聲，和仿真假設(shè)的理想情況有差別導(dǎo)致的。而在圖10中，在保證一樣估計精度的情況下，相比于只能離線運算的GA算法，EKF算法得到的ICRs值在3 s以內(nèi)收斂到常數(shù)值： yl=0.37 m，yr=-0 .25 m，xG= 0.03 m ，滿足實時應(yīng)用的要求。

圖7 不同地形條件下不同里程計和真實值軌跡基于AKF算法和EKF算法的仿真結(jié)果比較

圖8 不同地形條件下基于EKF算法和AKF算法的ICRs值仿真結(jié)果

圖9 同一地形條件下不同里程計和真實值軌跡基于EKF算法的實驗結(jié)果比較

圖10 同一地形下基于EKF算法的ICRs實驗結(jié)果

在不同地形的測試中，側(cè)滑移動機器人首先行駛在混凝土路面，之后行駛到瓷磚路面，然后又運行到草地上。圖11和圖12分別是側(cè)滑移動機器人在此變化地形的實驗過程中，不同里程計和真實值軌跡比較以及該地形下的 ICRs估計值，真實值使用差分GPS測量的數(shù)據(jù)表示，為方便比較，將基于EKF算法的結(jié)果也繪制在圖中。從圖11中可以發(fā)現(xiàn)，不考慮打滑的NS模型的里程計偏差超過了5000 mm， ICR和GA模型的里程計偏差為400 mm左右，而AKF算法里程計偏差為150 mm以內(nèi)，說明自適應(yīng)卡爾曼濾波方法得到的運動模型更加準(zhǔn)確。而從圖12中的ICRs估計值可以更清楚的看到：當(dāng)機器人行駛在不同地形上時，基于AKF算法的ICRs估計值有3個明顯的變化階段，每一個階段都能很快的收斂，而基于EKF的ICRs估計值由于收斂較慢，并沒有明顯的變化階段，得到的估計模型精度也較低。

圖11 不同地形條件下不同里程計和真實值軌跡基于AKF算法和EKF算法的實驗結(jié)果比較

圖12 不同地形條件下基于AKF算法和EKF算法的ICRs值實驗結(jié)果

5 結(jié)束語

本文采用的 EKF算法可以在線實時地估計側(cè)滑移動機器人ICRs的模型，基于ICRs的運動模型通過輸入的速度作為模型控制量，以測量位置和方向信息更新狀態(tài)和學(xué)習(xí) ICRs的位置。仿真實驗表明，該算法可以在3 s內(nèi)收斂，并準(zhǔn)確地得到ICRs的值。但是當(dāng)?shù)匦伟l(fā)生變化時，因為建立方程所用的 ICRs值在很小范圍內(nèi)變化的假設(shè)不成立，使得收斂時間長達 100 s以上。此時直接應(yīng)用本文中已有的IMU測量的Z軸加速度信號，基于功率譜密度（PSD）的方法提取特征，采用k-近鄰地形分類方法，實時判別當(dāng)前機器人運行的路面類型， 并根據(jù)地形變化采用自適應(yīng)的 EKF濾波器（AKF），通過調(diào)整EKF濾波器的初始誤差協(xié)方差矩陣，可使收斂時間縮短至3 s以內(nèi)，相比于離線運算的GA算法，采用改進的AKF算法不僅提高了模型精度，而且能快速的收斂到真實值，可以滿足實際使用的需要。

本文中機器人主要是在瓷磚，混凝土和草地路面上實驗驗證，后續(xù)工作將進一步研究該算法在更多地形條件下的適用性，并改進地形判別算法，使其適應(yīng)高速運動的移動機器人。

［1］ Kozlowski K and Pazderski D. Modeling and control of a 4-wheel skid-steering mobile robot［J］. International Journal of Applied Mathematics and Computer Science， 2004， 12（4）: 477-496.

［2］ Yu W， Chuy O， Collins E G， et al.. Dynamic modeling of a skid-steered wheeled vehicle with experimental verification［C］. IEEE/RSJ International Conference on Intelligent Robots and Systems， St. Louis， MO， USA， 2009: 4211-4219.

［3］ Yu W， Chuy O， Collins E G， et al.. Analysis and experimental verification for dynamic modeling of a skid-steered wheeled vehicle［J］. IEEE Transactions on Robotics， 2010， 26（2）: 340-353.

［4］ Yi J， Zhang J， Song D， et al.. IMU-based localization and slip estimation for skid-steered mobile robots［C］. IEEE/RSJInternational Conference on Intelligent Robots and Systems，San Diego， CA， USA， 2007: 2845-2850.

［5］ Wong J and Chiang C. A general theory for skid steering of tracked vehicles on firm ground［J］. Proceedings of the Institution of Mechanical Engineers， Part D: Journal of Automobile Engineering， 2001， 215（3）: 343-355.

［6］ Le A， Rye D， and Durrant-Whyte H. Estimation of track-soil interactions for autonomous tracked vehicles［C］. IEEE International Conference on Robotics & Automation，Albuquerque， NM， USA， 1997: 1388-1393.

［7］ Ani O A， Xu He， Xue Kai， et al.. Analytical modeling and multi?objective optimization （MOO） of slippage for wheeled mobile robot （WMR） in rough terrain［J］. Journal of Central South University， 2012， 19（9）: 2458-2467.

［8］ Ani O A， Xu He， Shen Yi-ping， et al.. Modeling and multiobjective optimization of traction performance for autonomous wheeled mobile robot in rough terrain［J］. Journal of Zhejiang University， 2013， 14（1）: 11-29.

［9］ 趙磊， 王鴻鵬， 董良， 等. 一種基于動力學(xué)模型的高速輪式移動機器人漂移運動控制方法［J］. 機器人， 2014， 36（2）: 137-146. Zhao Lei， Wang Hong-peng， Dong Liang， et al.. A drift control method for high-speed wheeled mobile robot based on dynamic model［J］. ROBOT， 2014， 36（2）: 137-146.

［10］ Zhang Yu， Hu Ji-bin， Li Xue-yuan， et al.. A linear lateral dynamic model of skid steered wheeled vehicle［C］. IEEE Intelligent Vehicles Symposium， Gold Coast， Australia， 2013: 964-969.

［11］ Ni Jun and Hu Ji-bin. The research of steady-state and transient-state response of skid steering wheeled vehicle［C］. IEEE Transportation Electrification Conference & Expo，Beijing， China， 2014: 1-6.

［12］ Wu Yao， Wang Tian-miao， Liang Jian-hong， et al.. Experimental kinematics modeling estimation for wheeled skid-steering mobile robots［C］. IEEE International Conference on Robotics and Biomimetics， Shenzhen， China，2014: 268-273.

［13］ Rogers-Marcovitz F， George M， Seegmiller N， et al.. Aiding off-road inertial navigation with high performance models of wheel slip［C］. IEEE/RSJ International Conference on Intelligent Robots and Systems， Vilamoura， Portugal， 2012: 215-222.

［14］ Moosavian S A A and Kalantari A. Experimental slip estimation for exact kinematics modeling and control of a tracked mobile robot［C］. IEEE/RSJ International Conference on Intelligent Robots and Systems， Nice， France， 2008: 95-100.

［15］ Martínez J， Mandow A， Morales J， et al.. Approximating kinematics for tracked mobile robots［J］. The International Journal of Robotics Research， 2005， 24（10）: 867-878.

［16］ 楊云， 王鴻鵬， 劉景泰， 等. 四輪獨立驅(qū)動式移動機器人的運動學(xué)分析與仿真［C］. 第 30屆中國控制會議， 煙臺， 2011: 3958-3963. Yang Yun， Wang Hong-peng， Liu Jing-tai， et al.. The kinematic analysis and simulation for four-wheel independent drive mobile robot［C］. Proceedings of the 30th Chinese Control Conference， Yantai， China， 2011: 3958-3963.

［17］ Pentzer J， Brennan S， and Reichard K. Model-based prediction of skid-steer robot kinematics using online estimation of track instantaneous centers of rotation［J］. Journal of Field Robotics， 2014， 31（3）: 455-476.

［18］ 蔣恩松， 李孟超， 孫劉杰. 一種基于神經(jīng)網(wǎng)絡(luò)的卡爾曼濾波改進方法［J］. 電子與信息學(xué)報， 2007， 29（9）: 2073-2076. Jiang En-song， Li Meng-chao， and Sun Liu-jie. An improved method of kalman filter based on neural network［J］. Journal of Electronics & Information Technology， 2007， 29（9）: 2073-2076.

［19］ 徐定杰， 沈忱， 沈鋒. 時變有色觀測噪聲下基于變分貝葉斯學(xué)習(xí)的自適應(yīng)卡爾曼濾波［J］. 電子與信息學(xué)報， 2013， 35（7）: 1593-1598. Xu Ding-jie， Shen Chen， and Shen Feng. Adaptive kalman filtering with time-varying colored measurement noise by variational bayesian learning ［J］. Journal of Electronics & Information Technology， 2013， 35（7）: 1593-1598.

［20］ 李強. 基于振動信號的輪式移動機器人地面分類方法研究［D］.［博士論文］， 哈爾濱工程大學(xué)， 2013. Li Qiang. Research on terrain classification methods for wheeled robots based on vibration signals［D］. ［Ph.D. dissertation］， Harbin Engineering University， 2013.

［21］ Reinstein M， Kubelka V， and Zimmermann K. Terrain adaptive odometry for mobile skid-steer robots［C］. IEEE International Conference on Robotics & Automation，Karlsruhe， Germany， 2013: 4691-4696.

［22］ Weiss C， Fechner N， Stark M， et al.. Comparison of different approaches to vibration-based terrain classification［C］. The European Conference on Mobile Robots， Freiburg， Germany，2007.

［23］ Tick D， Rahman T， Busso C， et al.. Indoor robotic terrain classification via angular velocity based hierarchical classifier selection［C］. IEEE International Conference on Robotics & Automation， River Centre， Saint Paul， Minnesota， USA， 2012: 3594-3600.

［24］ Dorf R C and Bishop R H. Modern Control System［M］. 12th Edition. Upper Saddle River， NJ， US， Prentice Hall， Inc.，2010: 234-235.

吳 耀： 男，1984年生，博士生，研究方向為地面移動機器人建模、辨識和控制.

王田苗： 男，1960年生，教授，主要研究方向為地面移動機器人、空中機器人和水下機器人.

王曉剛： 男，1989年生，碩士生，研究方向為地面移動機器人導(dǎo)航和控制.

Kinematics Model Prediction of Skid-steering Robot Using Adaptive Kalman Filter Estimation

Wu Yao Wang Tian-miao Wang Xiao-gang Liu Miao
（School of Mechanical Engineering & Automation， Beihang University， Beijing 100191， China）

Exact and real-time kinematics model plays a very important role in the mobile robot motion control and path planning. Compared to the off-line model estimation， based on an Instantaneous Centers of Rotation （ICRs）based kinematic model of skid-steering， an Extend Kalman Filter （EKF） method is used to estimate ICRs values on specific terrain on line. Terrains are identified by introducing k-Nearest Neighbors （kNN） algorithm when the robot moves on different terrains. Based on terrain classification， an Adaptive Kalman Filter （AKF） is used to adjust the filter parameters. The simulation and experiment results show that this method can converge very fast and estimate the ICRs value accurately with 3 seconds.

Mobile robot； Skid-steering； Adaptive Kalman Filter （AKF）； Instantaneous Centers of Rotation（ICRs）； k-Nearest Neighbors （kNN）

The National 863 Program of China （2011AA 040202）

TP242

A

1009-5896（2015）12-3016-09

10.11999/JEIT150289

2015-03-09；改回日期：2015-09-09；網(wǎng)絡(luò)出版：2015-10-13

*通信作者：吳耀 wuyaongyao@126.com

國家863計劃（2011AA040202）