王春榮，蔡 勇，蔣 剛

（西南科技大學(xué) 制造科學(xué)與工程學(xué)院，四川 綿陽(yáng)621010）

對(duì)于在動(dòng)態(tài)環(huán)境中的移動(dòng)機(jī)器人來(lái)說(shuō)，定位技術(shù)是一項(xiàng)最基本、最重要的功能。因此,研究移動(dòng)機(jī)器人的自主定位具有很重要的意義。

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

1 系統(tǒng)定位模型的構(gòu)建

差動(dòng)驅(qū)動(dòng)式移動(dòng)機(jī)器人的幾何模型如圖1所示。

根據(jù)圖1所示則有：

其中，NL是編碼器的線數(shù)，ML和MR分別為Δt時(shí)間內(nèi)左右兩個(gè)編碼器輸出的脈沖數(shù)，L為左右兩個(gè)驅(qū)動(dòng)輪之間的距離，r為驅(qū)動(dòng)輪的半徑，VL為機(jī)器人左邊兩輪的線速度，VR為機(jī)器人右邊兩輪的線速度，w為里程計(jì)測(cè)得的機(jī)器人航向變化的角速率，θk為機(jī)器人的航向角。

一般差動(dòng)機(jī)器人的位置 p=[x，y，θ]T可以從一個(gè)已知的位置開始，然后對(duì)其運(yùn)動(dòng)積分來(lái)估計(jì)下一時(shí)刻的位置。若采樣周期為Δt的離散系統(tǒng)，則位置增量為：

系統(tǒng)的定位模型為：

2 定位算法

2.1 卡爾曼濾波器

卡爾曼濾波[3，4]是由 KALMAN R E于 1960年提出的一種有噪聲線性動(dòng)態(tài)系統(tǒng)狀態(tài)預(yù)估的遞歸算法，它是一個(gè)不斷預(yù)測(cè)與校正的過(guò)程。

用卡爾曼濾波器來(lái)融合碼盤和陀螺儀數(shù)據(jù)，設(shè)狀態(tài)變量為：

式中，ω為陀螺儀測(cè)得的角速度，則系統(tǒng)的狀態(tài)方程為：

式中，ΦK+1/K為 tK時(shí)刻到tK+1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，本系統(tǒng)中為：

WK是協(xié)方差為Q的系統(tǒng)噪聲矩陣；ΓK是系統(tǒng)噪聲矩陣的驅(qū)動(dòng)矩陣，本系統(tǒng)中為I。

系統(tǒng)的量測(cè)方程為：

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

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

2.2 最小二乘支持向量機(jī)

最小二乘支持向量機(jī)[5,6]與標(biāo)準(zhǔn)支持向量機(jī)的最大不同之處是前者把不等式約束變成等式約束，而且把經(jīng)驗(yàn)風(fēng)險(xiǎn)由偏差的一次方改為二次方。給定訓(xùn)練集：

通過(guò)高維特征空間中的線性函數(shù)

來(lái)擬合樣本集，因此在最小二乘支持向量機(jī)中，回歸問(wèn)題可表述為約束優(yōu)化問(wèn)題，即：

要求解式（8）的優(yōu)化問(wèn)題，把約束優(yōu)化問(wèn)題轉(zhuǎn)變成無(wú)約束優(yōu)化問(wèn)題，從而建立Lagrange函數(shù)：

根據(jù)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即支持向量機(jī)的輸出為：

因此，最小二乘支持向量機(jī)的訓(xùn)練問(wèn)題不像標(biāo)準(zhǔn)支持向量機(jī)那樣要求解一個(gè)二次型規(guī)劃問(wèn)題，而是歸結(jié)為一個(gè)線性方程組的求解問(wèn)題，這樣就變得更為簡(jiǎn)單快速。

因此定位算法步驟如下：

(1)設(shè)置系統(tǒng)狀態(tài)變量的初始值ξ0，及其誤差協(xié)方差P0，系統(tǒng)噪聲協(xié)方差Q0，以及觀測(cè)噪聲協(xié)方差 R0；

(2)Kalman濾波完成一步預(yù)測(cè)計(jì)算預(yù)估狀態(tài)ξ^-以及計(jì)算先驗(yàn)誤差協(xié)方差；

(3)Kalman濾波進(jìn)行測(cè)量更新，分別計(jì)算增益K、更新后的狀態(tài)估計(jì)和更新后的誤差協(xié)方差；

3 實(shí)驗(yàn)及分析

本系統(tǒng)用Matlab對(duì)機(jī)器人的行走的航向角和路徑進(jìn)行仿真實(shí)驗(yàn)。首先假設(shè)系統(tǒng)噪聲是相互獨(dú)立而且是零均值的高斯白噪聲，因此協(xié)方差矩陣Q為對(duì)角矩陣。狀態(tài)變量初始值取所采集數(shù)據(jù)濾波前的均值，則：

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

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

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

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

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

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

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

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

[6]周松斌,劉桂雄.基于LS_SVR的無(wú)線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)定位算法[J].制造業(yè)自動(dòng)化,2008,30(9):1-5.