機(jī)器人足球中的卡爾曼慮波的應(yīng)用

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

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

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

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

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

6、-1+ K (k)Cx klk -1后驗(yàn)預(yù)測(cè)誤差方差矩陣P (k I k) = I 一 K (k) H (k) P (k I k 一 1)其中時(shí)間更新是預(yù)測(cè)過(guò)程，狀態(tài)更新是矯正過(guò)程。3程序上慮波的實(shí)現(xiàn)3.1卡爾曼慮波程序?qū)崿F(xiàn)設(shè)置初值x , Ex , xT = P 0000先驗(yàn)預(yù)測(cè)方程xk| k-1 = A (k I k 一 1)xk-1|k一先驗(yàn)預(yù)測(cè)誤差方差 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后驗(yàn)預(yù)測(cè)方程xkIk = xkIk-1 + K (k

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

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