機器人足球中的卡爾曼慮波的應用

1、足球機器人中卡爾曼慮波的應用1引言在足球機器人中，對于對方球員行動的預測、小球位置的預測是主導比賽走向的關鍵。 如果不能對于上述兩個方面進行準確的預測，任何策略上的研究都不可能很好的實現(xiàn)。 這里提供一種基于Kalman濾波的算法，他可以克服由觀測方程得到的，含有大量噪 聲的數(shù)據(jù)，從而準確的預報出若十個周期后目標的速度、加速度以及位置信息。由于 卡爾曼慮波是一種基于動力學模型的狀態(tài)空間中的濾波算法，故本章首先需要推導機 器人的動力學模型。然后對于模型進行實時仿真。并得出相關結論。離散時間的卡爾曼 濾波可以廣泛的應用于通信、檢測、跟蹤、預報等領域，并且有非常優(yōu)越的收斂性能， 也就是說只要線性系統(tǒng)可

2、以保持能觀能控，那么卡爾曼慮波就是一定可以保持收斂的， 無論預測初值選取是否無偏。最后，無論是在模擬仿真還是實踐中都可以證明卡爾曼 慮波算法的優(yōu)越性。2動力學模型與觀測方程的構建 2.1離散時間的狀態(tài)方程狀態(tài)方程：xk+1 =也+七觀測方程：；=CxkL v2.2動力學模型的分析k k足球機器人比賽中，小球（后者其他目標）的運動整體來看是非線性的，但是由于十毫秒以至微妙級的采樣時間內(nèi)，可以將其運動看作為線性的。我們將機器人的位置信息用&來表示，那么我們可以得到速度以及加速的地模型分別為v = &,，a = &。那么在某一瞬時，目標的位置為（，&），速度（，& ），加速度Q ,&）x yx yx

3、 y定義狀態(tài)變量：&yXk&xXk&x&y -由運動學可知在t時刻有:& = & + vt + at& = v + at& = a這樣我們可以得到狀態(tài)轉移矩陣: TOC o 1-5 h z 1 T- T 22A =01T0 01這樣我們可以得到:1T 22T由于環(huán)境為仿真組11人足球，所以不存在傳感器數(shù)量以及位置的變化那么，觀測方程為一階線形的模型。另外需要注意的是我們對于反饋信息的選取 由于速度加速度信息屬于二階變量的測量，誤差方差較大，不宜所謂觀測變量。 故這里選用位置信息。那么令：C = 1 0 0* = 1 0 0七 + 匕2.3噪音模型的分析無論任何系統(tǒng)都存在噪聲的干擾，早卡爾曼慮波

4、方程中包含著三種干擾。1 隨機干擾，作用于狀態(tài)方程，屬于系統(tǒng)噪音。2觀測干擾，實際中是來自攝像頭 的采樣時間、以及外界環(huán)境的干擾。3數(shù)據(jù)丟失以及延遲的影響。在仿真組的比賽 中，第三項是顯然不存在的。其中第一項作用偏小，而主要是第二項的噪聲影響預測。 通常情況下，我們?nèi)藖硐到y(tǒng)的噪聲，以及來自外界的干擾為平穩(wěn)過程的白噪聲序列。 下面我們說明一下兩個噪音變量的性質(zhì)w(k),kT ,(v(k),k ET 均為高斯序列，x (0)為高斯變量w (k) = v (k) = 0cov w(k), w( j) = Q (k )8其中kjk = j時8谷=0k更j時8 k = 1cov v(k), v(j) =

5、 R (k )8 kj其中k = j 時 8 = 0k j 時 8 = 1var z(0) = Pz (0) = P0cov w(k), v(j) = cov w(k), z(0) = cov v(k), z(0) = 0采樣時間與噪聲是相關的，也就是說在將連續(xù)系統(tǒng)離散化后，其噪聲 的方差也將隨之變化。通常我們認為：2.4卡爾曼慮波卡爾曼慮波的實現(xiàn)可以分為兩個部分：時間更新與狀態(tài)更新。如下所示：時間更新：先驗預測誤差方差矩陣：P(k I k -1) = AP(k -11 k -1) AT + Q先驗預測x = Axk I k -1k - ilk -1狀態(tài)更新：后驗預測x klk =x klk

6、-1+ K (k)Cx klk -1后驗預測誤差方差矩陣P (k I k) = I 一 K (k) H (k) P (k I k 一 1)其中時間更新是預測過程，狀態(tài)更新是矯正過程。3程序上慮波的實現(xiàn)3.1卡爾曼慮波程序?qū)崿F(xiàn)設置初值x , Ex , xT = P 0000先驗預測方程xk| k-1 = A (k I k 一 1)xk-1|k一先驗預測誤差方差 1 1P( klk -1) = AP (k - 1I k -1) AT + Q卡爾曼慮波增益矩陣K (k) = P(k I k - 1)CT CP (k I k - 1)CT + R-1后驗預測方程xkIk = xkIk-1 + K (k

7、)CxkI k-1后驗誤差方差P (k I k) = I - K (k) H (k) P (k I k - 1)3.2程序流程圖3.3 MATLAB上的模擬先面利用MATLAB模擬在離散時間的線性系統(tǒng)下應用卡爾曼慮波的跟蹤性能(1)觀測噪聲方差R = 30預測初值電=0 6 10時刻真值為均值為零，方差為10的高斯隨機變量。y軸方向的預測情況。200obsevationstrue value200obsevationstrue valuewith kalman filtering estimate valueSXM u-lo省 U 一POQO(2)在(1)的基礎上，令X = 0 10 1X =

8、 0 1101得到：0600500600500400300200100-10001002003004005006007008009001000Time step由此可以得到一定的驗證，當系統(tǒng)為穩(wěn)定系統(tǒng)時，無論初值選取如何，卡爾曼慮波都將保持收:(3)在(2)的基礎上將p 0 70Time stepobsevationstrue valuewith kalman filtering estimate valueTime stepobsevationstrue valuewith kalman filtering estimate value由此可以得到，線性系統(tǒng)的卡爾曼慮波，初始參數(shù)對于慮波算法時候收斂沒有影響， 而是對慮波收斂時間有一定的影響SXM u-lo省 U 一POQO參考書目：1 Yaakov Bar-Shalom X.Rong Li