楊 帆，王 浩

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

1前 言

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

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

通?？床坏綐?biāo)桿的情形很少有，因為可以利用轉(zhuǎn)頭來獲取標(biāo)桿信息，但這種情形是可能的。此時可以使用正運動學(xué)計算確定自身位置。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.無標(biāo)桿下的定位使用的方法即正運動學(xué)的計算。

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

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

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

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

3.計算卡爾曼增益矩陣：

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

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

1.使用含8個元素的數(shù)組，這樣避免各個標(biāo)桿有干擾，每個元素分別使用卡爾曼濾波，每3個周期更新一次。得到經(jīng)濾波后的視覺信息后在計算定位。此方案在能看到兩個或以上的標(biāo)桿時不需角度便能定位，角度標(biāo)準(zhǔn)差較大，避免使用了角度。其協(xié)方差為0.00931225。可直接代入使用，不用考慮轉(zhuǎn)移矩陣和過程噪音。

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

圖3 使用卡爾曼濾波定位

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

5 總結(jié)與展望

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

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

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

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