基于改進(jìn)卡爾曼濾波的RoboCup3D定位研究

楊 帆，王 浩

（1.黃山學(xué)院 國(guó)資處，安徽 黃山245021；2.合肥工業(yè)大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 合肥230009）

1前 言

場(chǎng)上目標(biāo)的定位是機(jī)器人基本技能之一。通過(guò)對(duì)目標(biāo)的定位，才能使機(jī)器人清楚自己位置、目標(biāo)位置和對(duì)手位置。[1，2]在此我們的球員主要是通過(guò)對(duì)標(biāo)桿或目標(biāo)的視覺(jué)信息來(lái)完成自身與目標(biāo)的定位。[3]受限條件下視覺(jué)信息不能同時(shí)看到所有的標(biāo)桿，我們需要考慮各種情況下的球員的自定位。

2 看不到標(biāo)桿時(shí)自定位

通?？床坏綐?biāo)桿的情形很少有，因?yàn)榭梢岳棉D(zhuǎn)頭來(lái)獲取標(biāo)桿信息，但這種情形是可能的。此時(shí)可以使用正運(yùn)動(dòng)學(xué)計(jì)算確定自身位置。

機(jī)器人的定位技術(shù)可以分為兩類(lèi)，即相對(duì)定位和絕對(duì)定位。相對(duì)定位是根據(jù)機(jī)器人前一時(shí)刻的位姿和正運(yùn)動(dòng)學(xué)計(jì)算得出的。絕對(duì)定位是通過(guò)視覺(jué)信息得出。在此使用了相對(duì)定位的方法，相對(duì)定位由于會(huì)累計(jì)誤差，時(shí)間越長(zhǎng)精度越差，因此只能在比較短的路程精確定位。下面介紹具體使用方法。

機(jī)器人在正常情形中至少有一只腳著地（這里暫不考慮倒下情形），將上一周期計(jì)算出的機(jī)器人的坐標(biāo)的x、y當(dāng)成陀螺儀的x、y坐標(biāo)分別計(jì)算兩只腳的中心的坐標(biāo)（此時(shí)腳的z值為0），以著地腳為起點(diǎn)使用正運(yùn)動(dòng)學(xué)計(jì)算到本周期的陀螺儀的向量，上一個(gè)周期的著地腳的坐標(biāo)加上此向量便是本周期的陀螺儀坐標(biāo)。這里沒(méi)使用攝像頭是因?yàn)橥勇輧x與攝像頭的x、y坐標(biāo)幾乎一樣，而且減少了很多計(jì)算。機(jī)器人各腿部關(guān)節(jié)的滾動(dòng)角（roll），俯仰角（pitch），偏擺角（yaw），由機(jī)器人運(yùn)動(dòng)學(xué)知其旋轉(zhuǎn)矩陣。

由鏈乘法則可求其各關(guān)節(jié)的位姿，這就完成了相對(duì)定位，實(shí)驗(yàn)效果如圖1所示。

圖1 看不到標(biāo)桿情形下的相對(duì)定位

從圖1可以看出，剛開(kāi)始是誤差還比較小，隨著時(shí)間的推移，積累的誤差將越來(lái)越大，這種方法適合于緊急情況下的短時(shí)定位，此時(shí)還未考慮智能體倒下情形，不適用長(zhǎng)時(shí)間定位。

3 看到一個(gè)標(biāo)桿時(shí)自定位

這里便是要完成相對(duì)定位和絕對(duì)定位的結(jié)合。此時(shí)的相對(duì)定位是利用上一周期的面向角和陀螺儀的角速度來(lái)計(jì)算本周期的面向角。

本周期的面向角等于上一周期的面向角加前3個(gè)周期的陀螺儀的角速度分別乘0.02再相加。然后利用公式 （x，y，z）=（d*cosφ*cosθ，d*cosφ*sinθ，d*sinφ），d、θ、φ分別表示 t時(shí)刻球員或球與所選標(biāo)志桿之間的距離、水平方向的角度和垂直方向的角度。是將人的面向角加上頭的轉(zhuǎn)角再加上視覺(jué)信息中的水平偏角。

接著用公式進(jìn)行計(jì)算：P=PF-P'，其中PF為標(biāo)志的坐標(biāo)，P'是由極坐標(biāo)轉(zhuǎn)化后的直角坐標(biāo)。但是這種方法無(wú)法處理誤差所帶來(lái)的干擾，實(shí)驗(yàn)效果如圖2所示。

圖2 看到一個(gè)標(biāo)桿的情形下的自定位

從圖2可以看出，在短時(shí)定位其精度不如無(wú)標(biāo)桿的情形，但不會(huì)隨著時(shí)間的推移產(chǎn)生積累誤差。這便孕育出想將兩種方法結(jié)合的想法，事實(shí)上是可以實(shí)現(xiàn)的。

4 看到多個(gè)標(biāo)桿時(shí)自定位

多個(gè)標(biāo)桿的自定位方法較多，但都存在著誤差。最容易想的是每個(gè)標(biāo)桿都計(jì)算后取平均。若是兩個(gè)標(biāo)桿可結(jié)合自身高度和到標(biāo)桿的距離來(lái)計(jì)算（角度的偏差較大一般不采用角度），因?yàn)樵谝粋€(gè)平面上到空間兩點(diǎn)的距離可以唯一地確定一點(diǎn)（兩點(diǎn)的連線(xiàn)不垂直于此面）。

國(guó)際比賽中視覺(jué)噪聲的設(shè)置是與距離有關(guān)，距離、水平角和高度角誤差均是以0為期望的正態(tài)分布，標(biāo)準(zhǔn)差分別為0.0965，0.1225和0.1480。實(shí)測(cè)結(jié)果是誤差標(biāo)準(zhǔn)差近似為距離的線(xiàn)性函數(shù)。對(duì)于隨機(jī)線(xiàn)性系統(tǒng)，當(dāng)模型準(zhǔn)確且系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲為高斯白噪聲序列，方差已知時(shí)，卡爾曼濾波有近似完美的效果。因此我們選用卡爾曼濾波的方法來(lái)減小誤差。

卡爾曼濾波是一種最優(yōu)化自回歸數(shù)據(jù)處理的算法。可以理解成是相對(duì)定位和絕對(duì)定位的信息中更相信誰(shuí)相信多少的一種算法。這將引出一個(gè)概念卡爾曼增益K，可理解為K和上周期的最優(yōu)解里儲(chǔ)存了應(yīng)回歸的數(shù)據(jù)。而K是通過(guò)協(xié)方差來(lái)進(jìn)行更新的。相對(duì)定位即預(yù)測(cè)值我們有兩種方案：

1.無(wú)標(biāo)桿下的定位使用的方法即正運(yùn)動(dòng)學(xué)的計(jì)算。

2.是利用陀螺儀的相關(guān)信息來(lái)計(jì)算。

在距離上利用ACC中上周期的加速度和計(jì)算出的速度來(lái)計(jì)算本周到標(biāo)桿的距離。St=S0+（v*t+0.5*a*t*t）*，a為加速度在人與標(biāo)桿連線(xiàn)上的分加速度，θ為面向與人與標(biāo)桿連線(xiàn)的夾角。水平方向的角度的相對(duì)定位與一個(gè)標(biāo)桿的情形下的自定位一樣。方案1的計(jì)算量較大，但比較準(zhǔn)確。方案2的計(jì)算量較小，由于假設(shè)模擬了控制量，便有過(guò)程誤差，降低的準(zhǔn)確度?？柭鼮V波的流程為：

1.預(yù)估計(jì)：X(k)^=F(k，k-1)·X(k-1)

2.計(jì)算預(yù)估計(jì)協(xié)方差矩陣：

3.計(jì)算卡爾曼增益矩陣：

5.計(jì)算更新后估計(jì)協(xié)方差矩陣：

在使用卡爾曼濾波時(shí)也有兩種算法：

1.使用含8個(gè)元素的數(shù)組，這樣避免各個(gè)標(biāo)桿有干擾，每個(gè)元素分別使用卡爾曼濾波，每3個(gè)周期更新一次。得到經(jīng)濾波后的視覺(jué)信息后在計(jì)算定位。此方案在能看到兩個(gè)或以上的標(biāo)桿時(shí)不需角度便能定位，角度標(biāo)準(zhǔn)差較大，避免使用了角度。其協(xié)方差為0.00931225?？芍苯哟胧褂茫挥每紤]轉(zhuǎn)移矩陣和過(guò)程噪音。

2.首先利用最近標(biāo)桿的視覺(jué)信息作觀測(cè)值和上個(gè)周期的預(yù)測(cè)（相對(duì)定位的方法）來(lái)得到一個(gè)最優(yōu)解。再用最優(yōu)解作預(yù)測(cè)值，和較遠(yuǎn)的標(biāo)桿視覺(jué)信息再使用卡爾曼濾波，這樣可以大大的提高精確度。只是這樣K會(huì)很相信預(yù)測(cè)值，如在機(jī)器人倒下后回歸速度會(huì)慢些。所以在爬起時(shí)的幾個(gè)周期內(nèi)不要使用濾波。若是使用正運(yùn)動(dòng)學(xué)的計(jì)算來(lái)預(yù)測(cè)值可令狀態(tài)轉(zhuǎn)移矩陣為E，預(yù)測(cè)使用的是直角坐標(biāo)在預(yù)測(cè)后應(yīng)將其化為極坐標(biāo)，在計(jì)算最優(yōu)解和協(xié)方差的更新時(shí)都使用極坐標(biāo)，距離、水平角和高度角的標(biāo)準(zhǔn)差在直角坐標(biāo)中不能直接用，如圖3所示。

圖3 使用卡爾曼濾波定位

由圖3可知方案2的線(xiàn)性效果比較好。同時(shí)，在實(shí)際應(yīng)用中發(fā)現(xiàn)，機(jī)器人雖然能夠很好地定位，但是花費(fèi)時(shí)間較長(zhǎng)，停頓時(shí)間較長(zhǎng)，我們?cè)囍阉惴ㄖ械目柭鲆鍷值經(jīng)過(guò)實(shí)驗(yàn)后得到一個(gè)近似值，用近似值取代算法的第3步中的更新計(jì)算，這樣算法得到了優(yōu)化，定位速度明顯加快，誤差略微增加，此時(shí)需要兼顧定位準(zhǔn)確和定位速度這兩個(gè)方面，后期可以繼續(xù)使用幾何地圖和概率推理的定位方法等，不斷提高定位準(zhǔn)確度。

5 總結(jié)與展望

雖然移動(dòng)機(jī)器人定位問(wèn)題已經(jīng)研究多年，但是現(xiàn)有的各種方法都仍然存在種種不足，在解決定位問(wèn)題中遇到的不確定性問(wèn)題上各種方法各有優(yōu)缺點(diǎn)。當(dāng)前移動(dòng)機(jī)器人的研究非常強(qiáng)調(diào)自主能力的提高，定位系統(tǒng)越來(lái)越重視算法的魯棒性、主動(dòng)性和安全性，研究重點(diǎn)從局部、靜態(tài)、被動(dòng)的方式向全局、動(dòng)態(tài)、主動(dòng)定位方法發(fā)展，并且強(qiáng)調(diào)不能孤立地設(shè)計(jì)定位系統(tǒng)，主張將路徑規(guī)劃、避障控制、地圖構(gòu)造、對(duì)象識(shí)別等都集成到定位系統(tǒng)中，控制結(jié)構(gòu)及位姿估計(jì)算法越來(lái)越復(fù)雜，其中一些快速、精確的定位算法均以激光雷達(dá)數(shù)據(jù)為基礎(chǔ)。

[1]孫海夢(mèng).基于視覺(jué)噪聲的粒子濾波定位法在RoboCup中的應(yīng)用[J].計(jì)算機(jī)工程與科學(xué)，2008，30(12):41-42.

[2]陳思，易益敏，等.RoboCup3D基于αβγ濾波的足球定位方法[C].2009年中國(guó)智能自動(dòng)化會(huì)議，2009.

[3]方正，佟國(guó)峰，等.基于貝葉斯濾波理論的自主機(jī)器人自定位方法研究[J].控制與決策，2006，(08):841-847.