基于卡爾曼與改進SVM移動機器人定位研究*

王春榮，蔡 勇，蔣 剛

（西南科技大學 制造科學與工程學院，四川 綿陽621010）

對于在動態環境中的移動機器人來說，定位技術是一項最基本、最重要的功能。因此,研究移動機器人的自主定位具有很重要的意義。

機器人的定位[1]技術分為兩類：相對定位和絕對定位。相對定位是由編碼器、里程計等傳感器與機器人前一時刻的位姿來推出下一時刻的位姿。絕對定位是采用攝像頭和激光超聲波等傳感器,然后根據已知位置的標志物進行定位,需要對環境的先驗知識。由于單傳感器都有局限性，因此采用多個傳感器進行融合以達到較準確的定位技術。本文將講述利用卡爾曼濾波算法將編碼器測得的機器人航向角和陀螺儀測得的機器人航向角進行濾波融合[2]，修正因輪子打滑等引起的編碼器測角誤差，再經過LS_SVR建立回歸曲線，提高定位精度。

1 系統定位模型的構建

差動驅動式移動機器人的幾何模型如圖1所示。

根據圖1所示則有：

其中，NL是編碼器的線數，ML和MR分別為Δt時間內左右兩個編碼器輸出的脈沖數，L為左右兩個驅動輪之間的距離，r為驅動輪的半徑，VL為機器人左邊兩輪的線速度，VR為機器人右邊兩輪的線速度，w為里程計測得的機器人航向變化的角速率，θk為機器人的航向角。

一般差動機器人的位置 p=[x，y，θ]T可以從一個已知的位置開始，然后對其運動積分來估計下一時刻的位置。若采樣周期為Δt的離散系統，則位置增量為：

系統的定位模型為：

2 定位算法

2.1 卡爾曼濾波器

卡爾曼濾波[3，4]是由 KALMAN R E于 1960年提出的一種有噪聲線性動態系統狀態預估的遞歸算法，它是一個不斷預測與校正的過程。

用卡爾曼濾波器來融合碼盤和陀螺儀數據，設狀態變量為：

式中，ω為陀螺儀測得的角速度，則系統的狀態方程為：

式中，ΦK+1/K為 tK時刻到tK+1時刻的狀態轉移矩陣，本系統中為：

WK是協方差為Q的系統噪聲矩陣；ΓK是系統噪聲矩陣的驅動矩陣，本系統中為I。

系統的量測方程為：

式中，P為誤差協方差矩陣，K為卡爾曼增益矩陣。

通過式(7)可以計算出系統變量的估計值，將估計值代入公式（3）中，即可得到融合后的碼盤推算值。

2.2 最小二乘支持向量機

最小二乘支持向量機[5,6]與標準支持向量機的最大不同之處是前者把不等式約束變成等式約束，而且把經驗風險由偏差的一次方改為二次方。給定訓練集：

通過高維特征空間中的線性函數

來擬合樣本集，因此在最小二乘支持向量機中，回歸問題可表述為約束優化問題，即：

要求解式（8）的優化問題，把約束優化問題轉變成無約束優化問題，從而建立Lagrange函數：

根據KKT條件有：

在方程（11）中消去 ei、w，即得到：

其 中 ：y=(y1，y2， … ，yl)T，∧=(1，1， … ，1)T，a=(a1，a2，…，al)T，Qij=[φ(xi)·φ(xj)]=K(xi，xj)，i，j=1，2，…，l即支持向量機的輸出為：

因此，最小二乘支持向量機的訓練問題不像標準支持向量機那樣要求解一個二次型規劃問題，而是歸結為一個線性方程組的求解問題，這樣就變得更為簡單快速。

因此定位算法步驟如下：

(1)設置系統狀態變量的初始值ξ0，及其誤差協方差P0，系統噪聲協方差Q0，以及觀測噪聲協方差 R0；

(2)Kalman濾波完成一步預測計算預估狀態ξ^-以及計算先驗誤差協方差；

(3)Kalman濾波進行測量更新，分別計算增益K、更新后的狀態估計和更新后的誤差協方差；

3 實驗及分析

本系統用Matlab對機器人的行走的航向角和路徑進行仿真實驗。首先假設系統噪聲是相互獨立而且是零均值的高斯白噪聲，因此協方差矩陣Q為對角矩陣。狀態變量初始值取所采集數據濾波前的均值，則：

估計誤差初值取濾波前的均方差和4%的均方差，則：

實驗方法：以鋪著鵝卵石的水泥地為實驗平臺，設定好全局坐標及將機器人的起始航向角記為0，然后通過編碼器實現機器人的直線和轉彎行走。圖2所示為機器人直線行走時航向角的變化，可以看出經過卡爾曼濾波和最小二乘支持向量機回歸之后的航向角誤差在1°以內。圖3所示為機器人的軌跡圖，可明顯看出處理完之后的軌跡很接近于理論的軌跡。

本文將卡爾曼濾波和最小二乘支持向量機相結合實現了對機器人的定位?？柭鼮V波是一種遞推估計算法，主要思想是利用前一時刻的估計值和現時刻的觀測值來更新對狀態變量的估計，求出現時刻的估計值,經實驗證明有較好的效果。但是,在本文中所采用的定位方法只是相對定位方法,隨著時間的增加就會有誤差的累積，因而可將激光超聲波和攝像頭等傳感器加入機器人定位系統,采用相對定位和絕對定位相結合,來消除經長時間運行而產生的累積誤差。

[1]歷茂海,洪炳熔.一種魯棒的室內移動機器人定位方法[J].計算機工程與應用,2005，42(4)：1-4.

[2]王軍,蘇劍波,席裕庚.多傳感器集成與融合概述[J].機器人,2001,23(2)：183－192.

[3]陳旻.基與DSP的擴展卡爾曼濾波直流無刷電機控制[J].單片機與嵌入式系統應用，2003,3(11)：1-4.

[4]段凱宇,張力軍.基于到達角 Kalman濾波的 TDOA/AOA定位算法[J].電子與信息學報，2006，28(9)：1-4.

[5]辛菁,劉丁,徐慶坤.基于 LS_SVR的機器人空間4DOF無標定視覺定位[J].控制理論與應用,2010,27(1):1-9.

[6]周松斌,劉桂雄.基于LS_SVR的無線傳感器網絡節點定位算法[J].制造業自動化,2008,30(9):1-5.