基于自適應卡爾曼濾波的側滑移動機器人運動模型估計

吳 耀王田苗 王曉剛 劉 淼

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

基于自適應卡爾曼濾波的側滑移動機器人運動模型估計

吳 耀*王田苗 王曉剛 劉 淼

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

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

移動機器人；側滑移動；自適應卡爾曼濾波；速度瞬心；k-近鄰法

1 引言

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

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

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

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

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

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

2 側滑轉向（skid-steering）的ICRs運動模型

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

圖1 側滑移動機器人運動學原理圖

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

從而得到車輛的運動學模型如下：

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

3 EKF方法估計ICRs位置

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

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

這里u為控制輸入向量，由左右兩側的速度值組成；w為系統的噪聲，分別包括位姿噪聲和速度瞬心的運動參數噪聲。考慮圖1坐標系下側滑移動機器人運動，式（6）可以具體寫成

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

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

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

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

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

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

計算卡爾曼增益為

最后，狀態和協方差估計更新分別為

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

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

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

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

已知地形的訓練樣本集（在每種相應的地形上運行并采集數據），并記為

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

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

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

4 仿真和實驗驗證

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

表1 基于Pioneer P3-AT的仿真平臺參數

圖2 P3-AT側滑移動機器人實驗系統

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

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

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

圖4表示在此同一特定路面下ICRs真值和本文采用的EKF算法估計值的對比，發現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 。根據文獻［24］，定義EKF算法收斂時間為ICRs估計

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

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

當地形變化時，如圖5和圖6所示，在0～50 s內，機器人運行在地形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模型偏差提高得很少；事實上根據圖6，如果未考慮地形變化，雖然ICRs值達到了較高的收斂精度，但是收斂時間延長到100 s以上。

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

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

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

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

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

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

4.3 實驗驗證

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

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

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

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

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

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

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

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

5 結束語

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

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

［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］ 趙磊， 王鴻鵬， 董良， 等. 一種基于動力學模型的高速輪式移動機器人漂移運動控制方法［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］ 楊云， 王鴻鵬， 劉景泰， 等. 四輪獨立驅動式移動機器人的運動學分析與仿真［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］. 電子與信息學報， 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］ 徐定杰， 沈忱， 沈鋒. 時變有色觀測噪聲下基于變分貝葉斯學習的自適應卡爾曼濾波［J］. 電子與信息學報， 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］.［博士論文］， 哈爾濱工程大學， 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年生，碩士生，研究方向為地面移動機器人導航和控制.

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；網絡出版：2015-10-13

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

國家863計劃（2011AA040202）