基于改進卡爾曼濾波的RoboCup3D定位研究

楊 帆，王 浩

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

1前 言

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

2 看不到標桿時自定位

通常看不到標桿的情形很少有，因為可以利用轉頭來獲取標桿信息，但這種情形是可能的。此時可以使用正運動學計算確定自身位置。

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

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

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

圖1 看不到標桿情形下的相對定位

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

3 看到一個標桿時自定位

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

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

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

圖2 看到一個標桿的情形下的自定位

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

4 看到多個標桿時自定位

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

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

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

1.無標桿下的定位使用的方法即正運動學的計算。

2.是利用陀螺儀的相關信息來計算。

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

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

2.計算預估計協方差矩陣：

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

5.計算更新后估計協方差矩陣：

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

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

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

圖3 使用卡爾曼濾波定位

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

5 總結與展望

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

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

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

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