机器人足球中的卡尔曼虑波的应用

1、足球机器人中卡尔曼虑波的应用1引言在足球机器人中，对于对方球员行动的预测、小球位置的预测是主导比赛走向的关键。 如果不能对于上述两个方面进行准确的预测，任何策略上的研究都不可能很好的实现。 这里提供一种基于Kalman滤波的算法，他可以克服由观测方程得到的，含有大量噪 声的数据，从而准确的预报出若十个周期后目标的速度、加速度以及位置信息。由于 卡尔曼虑波是一种基于动力学模型的状态空间中的滤波算法，故本章首先需要推导机 器人的动力学模型。然后对于模型进行实时仿真。并得出相关结论。离散时间的卡尔曼 滤波可以广泛的应用于通信、检测、跟踪、预报等领域，并且有非常优越的收敛性能， 也就是说只要线性系统可

2、以保持能观能控，那么卡尔曼虑波就是一定可以保持收敛的， 无论预测初值选取是否无偏。最后，无论是在模拟仿真还是实践中都可以证明卡尔曼 虑波算法的优越性。2动力学模型与观测方程的构建 2.1离散时间的状态方程状态方程：xk+1 =也+七观测方程：；=CxkL v2.2动力学模型的分析k k足球机器人比赛中，小球（后者其他目标）的运动整体来看是非线性的，但是由于十毫秒以至微妙级的采样时间内，可以将其运动看作为线性的。我们将机器人的位置信息用&来表示，那么我们可以得到速度以及加速的地模型分别为v = &,，a = &。那么在某一瞬时，目标的位置为（，&），速度（，& ），加速度Q ,&）x yx yx

3、 y定义状态变量：&yXk&xXk&x&y -由运动学可知在t时刻有:& = & + vt + at& = v + at& = a这样我们可以得到状态转移矩阵: TOC o 1-5 h z 1 T- T 22A =01T0 01这样我们可以得到:1T 22T由于环境为仿真组11人足球，所以不存在传感器数量以及位置的变化那么，观测方程为一阶线形的模型。另外需要注意的是我们对于反馈信息的选取 由于速度加速度信息属于二阶变量的测量，误差方差较大，不宜所谓观测变量。 故这里选用位置信息。那么令：C = 1 0 0* = 1 0 0七 + 匕2.3噪音模型的分析无论任何系统都存在噪声的干扰，早卡尔曼虑波

4、方程中包含着三种干扰。1 随机干扰，作用于状态方程，属于系统噪音。2观测干扰，实际中是来自摄像头 的采样时间、以及外界环境的干扰。3数据丢失以及延迟的影响。在仿真组的比赛 中，第三项是显然不存在的。其中第一项作用偏小，而主要是第二项的噪声影响预测。 通常情况下，我们人来系统的噪声，以及来自外界的干扰为平稳过程的白噪声序列。 下面我们说明一下两个噪音变量的性质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采样时间与噪声是相关的，也就是说在将连续系统离散化后，其噪声 的方差也将随之变化。通常我们认为：2.4卡尔曼虑波卡尔曼虑波的实现可以分为两个部分：时间更新与状态更新。如下所示：时间更新：先验预测误差方差矩阵：P(k I k -1) = AP(k -11 k -1) AT + Q先验预测x = Axk I k -1k - ilk -1状态更新：后验预测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)其中时间更新是预测过程，状态更新是矫正过程。3程序上虑波的实现3.1卡尔曼虑波程序实现设置初值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模拟在离散时间的线性系统下应用卡尔曼虑波的跟踪性能(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由此可以得到一定的验证，当系统为稳定系统时，无论初值选取如何，卡尔曼虑波都将保持收:(3)在(2)的基础上将p 0 70Time stepobsevationstrue valuewith kalman filtering estimate valueTime stepobsevationstrue valuewith kalman filtering estimate value由此可以得到，线性系统的卡尔曼虑波，初始参数对于虑波算法时候收敛没有影响， 而是对虑波收敛时间有一定的影响SXM u-lo省 U 一POQO参考书目：1 Yaakov Bar-Shalom X.Rong Li