卡尔曼滤波在足球机器人定位中的应用

《装备制造技术》２００８年第２期导航系统是移动机器人的一个关键子系统。

在很多应用中，导航系统最关心的一个要素是机器人的自我定位［１］，也就是自身的位置和方向。

机器人只有得知自身位姿，才能进行路径规划和决策，完成一系列复杂的任务。

在多机器人团队中，机器人通过交换位姿信息来确定相关的有组织的行为。

在足球机器人中，机器人定位也是一个关键技术。

得到位姿信息，机器人才能移动到目标点，团队之间才能进行传球等一系列配合。

机器人的定位技术可以分为两类：相对定位和绝对定位。

相对定位是根据机器人前一时刻的位姿和由编码器、里程计等传感器得到此刻的位姿。

绝对定位是通过激光超声波、视觉系统、或ＧＰＳ等传感器，利用已知位置的标志物进行定位，需要对环境的先验知识。

如根据路标的几何关系定位；提取直线然后霍夫变换［６￣７］，在参数坐标系定位。

相对定位由于会累计误差，时间越长精度越差，因此只能在比较短的路程精确定位。

绝对定位虽然误差相互独立，不会随着时间累计，但容易受到干扰，短时间内波动很大。

由此也出现了结合相对定位和绝对定位的方法，融合多种传感器信息，对机器人载体进行定位，如卡尔曼滤波［２］和蒙特卡罗方法［４￣５］。

由于在足球机器人中存在误差和噪声，包括运动系统和测量系统。

如果直接利用这些数据将会对机器人定位造成偏差，并且会累计误差使得定位失败。

卡尔曼滤波对于含噪声的动态系统是一种很好的处理方法。

卡尔曼滤波采用物理意义较为直观的时间域语言，仅需要有限时间内的观测数据，具有计算量小、存储量低，实时性高的优点，和维纳滤波相比较，更适合处理动态系统。

卡尔曼滤波具有很好的鲁棒性，在导航中有广泛的应用。

笔者将把卡尔曼滤波应用到足球机器人定位中。

１卡尔曼滤波卡尔曼滤波由Ｒ．Ｅ．Ｋａｌｍａｎ［１１］于１９６０年提出，是一种递归数据处理算法，可估计带噪声的线型动态系统的状态。

卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。

因此卡尔曼滤波器可分为两个部分：预估阶段和测量更新阶段。

设一动态系统描述如下：ｘｋ＝Ａｘｋ－１＋Ｂｕｋ－１＋Ｗｋ－１（１）ｚｋ＝Ｈｘｋ＋ｖｋ（２）式中ｘ为系统状态变量，ｚ为观测变量，ｕ为外部输入变量。

随机信号ｗ和ｖ分别表示系统噪声和观测噪声。

假设它们为相互独立的高斯白噪声：ｐ（ｗ）：Ｎ（０，Ｑ）（３）ｐ（ｖ）：Ｎ（０，Ｒ）（４）则卡尔曼滤波方程可用以下递推方程表述：由式可知，当观测噪声协方差Ｒ越小，增益Ｋｋ越大。

特别地，当Ｒ趋向于零时，有：ｌｉｍＲ→０Ｋｋ＝Ｈ－１（５）而当先验估计误差协方差Ｐ－ｋ越小，增益Ｋｋ越大。

特别地，当Ｐ－ｋ趋向于零时，有：ｌｉｍＰ－ｋ→０Ｋｋ＝Ｈ－１（６）由此可以看出，增益Ｋｋ表示在更新阶段测量信息的权值。

如果在预估阶段误差越小，则测量变量ｚｋ的权值越小，而ｚｋ的收稿日期：２００７－１１－３０作者简介：董晓杰（１９８２—），男，浙江宁波人，同济大学控制科学与工程系硕士研究生，研究方向：机器人，定位；陈启军，男，同济大学控制科学与工程系教授，博士生导师。

卡尔曼滤波在足球机器人定位中的应用董晓杰，陈启军（同济大学控制科学与工程系，上海２０１８０４）摘要：在动态环境中的自我定位是移动机器人的一个关键问题。

机器人要完成复杂的任务，首先要感知周围环境，得到机器人的位姿，才能完成后续的决策及规划。

卡尔曼滤波是一种最优化回归数据处理算法，已广泛应用于机器人导航、传感器数据融合、导弹追踪。

本文将卡尔曼滤波应用于足球机器人定位中。

关键词：卡尔曼滤波；导航；定位；建模中图分类号：ＴＰ２４２文献标识码：Ａ文章编号：１６７２－５４５Ｘ（２００８）０２－００４５－０４ｘ!ｋ＝Ａｘ"ｋ－１＋Ｂｕｋ－１Ｐ－ｋ＝ＡｋＰ＋ｋ－１＋ＡＴ＋ＱＰｋ＝Ｐ－ｋ＋ＨＴ（ＨＰ－ｋＨＴ＋Ｒ）－１ｘ#ｋ＝ｘ$ｋ＋Ｋｋ（ｚｋ－Ｈｘ%ｋ）Ｐｋ＝（Ｉ－ｋｋＨ）Ｐ－ｋ４５ＥｑｕｉｐｍｅｎｔＭａｎｕｆａｃｔｒｉｎｇＴｅｃｈｎｏｌｏｇｙＮｏ．２，２００８预测Ｈｘ!ｋ的权值越大。

相反，如果测量噪声越小，则测量变量ｚｋ的权值越大，而ｚｋ的预测Ｈｘ"ｋ的权值越小。

卡尔曼滤波算法过程可归结如下：（１）设置系统状态初始值ｘ０及其误差协方差Ｐ０，以及噪声协方差Ｑ０，Ｒ０。

（２）根据预估阶段方程，计算预估状态ｘ#ｋ和预估协方差Ｐ－ｋ。

（３）根据更新阶段方程，计算卡尔曼滤波增益和更新后的状态估计和协方差Ｐ－ｋ。

（４）重复（２）、（３）步骤。

２系统模型根据卡尔曼滤波预估和更新两个阶段，分别建立机器人的运动模型和测量模型。

２．１运动模型移动机器人具有驱动系统，通常是轮子。

电机控制轮子转速从而使机器人有不同的速度和移动方向。

通过读取安装在电机上的编码器，可以得到轮子的转速以及圈数，计算出每个轮子的位移，从而转换成机器人的位移。

运动模型是根据机器人前一时刻的位姿以及位移量来确定机器人此刻的位姿，即相对定位，不同的运动结构得到不同的运动模型，但通用的表达式可用下式表示：ｐｏｓｅｎｅｗ＝ｐｏｓｅｏｌｄ＋Δｏｄｏｍｅｔｒｙ＋Δｎｏｉｓｅ笔者采用三个全向轮组合，全向运动方式是一种可以在平面内获得任意运动方向的运动方式，可以完全控制机器人在平面运动的三个自由度（两个水平运动分量和一个自身姿态旋转分量）。

具有全向运动能力的运动系统使机器人可以向任意方向做直线运动，而之前不需要做旋转运动，并且这种轮系可以满足一边做直线运动一边旋转的要求，达到终状态所需要的任意姿态角，相比较两轮机器人具有更大的灵活性。

设Ｘｗ－Ｙｗ为全局坐标系，ＸＹ－ＹＹ为机器人坐标系，１，２，３为三个全向轮，分别成１２０°夹角，ｘ＝ｘ［ｘ］ｘ［ｙ］ｘ［$］!"为机器人全局坐标，如图１所示。

图１机器人全局坐标分析机器人运动学，可得运动方程：%’１)+２-/３#$$$$$$$$$$$%&’’’’’’’’’’’(＝１ｒ－３)２１２Ｌ０－１Ｌ３)２１２Ｌ#$$$$$$$$$$$$$%&’’’’’’’’’’’’’(ｘ1［ｘ］ｘ2［ｙ］ｘ3［4］#$$$$$$$$$$$%&’’’’’’’’’’’(，即56＝１ｒＷｘ7（７）式中8１，:２，<３，为机器人在Δｔ时间内各轮子转过的弧度，ｒ表示轮子半径，Ｌ表示机器人中心到轮子的距离。

要得到机器人在Δｔ时间内的相对位移，可由公式推出：ｕ＝ｒ*Ｗ－１>?ｄｔ（８）即ｕ＝ｕ［ｘ］ｕ［ｙ］ｕ［@］#$$$$$$$$%&’’’’’’’’(＝ｒ*－３)３０３)３１３－２３１３１３Ｌ１３Ｌ１３Ｌ#$$$$$$$$$$$$$$$%&’’’’’’’’’’’’’’’(AC １E G ２I K ３#$$$$$$$$$$$%&’’’’’’’’’’’(ｄｔ，（９）ｕ为其在机器人坐标系的相对位移，进一步简化：ｕ＝ｕ［ｘ］ｕ［ｙ］ｕ［M ］#$$$$$$$$%&’’’’’’’’(＝ｒ－３)３N １＋３)３P ３１３R １－２３T ２＋１３V ３１３Ｌ（X １＋Z ２＋\３）#$$$$$$$$$$$$$$$%&’’’’’’’’’’’’’’’(（１０）引入旋转矩阵ｃｏｓｘ［^］－ｓｉｎｘ［_］ｓｉｎｘ［‘］ｃｏｓｘ［a ］!"，可得机器人在全局坐标系下的位移：Δｘ＝Δｘ［ｘ］Δｘ［ｙ］Δｘ［b ］#$$$$$$$$%&’’’’’’’’(＝ｃｏｓｘ［c ］－ｓｉｎｘ［d ］ｓｉｎｘ［e ］ｃｏｓｘ［f ］!"・ｕ［ｘ］ｕ［ｙ］!"ｕ［g ］#$$$$$$$%&’’’’’’’(（１１）从而机器人的位姿可表示为：ｘｋ＝ｆ（ｘｋ－１，ｕｋ－１）＝ｘ［ｘ］，ｋ－１＋（ｕ［ｘ］，ｋ－１・ｃｏｓ（ｘ［h ］，ｋ－１）－（ｕ［ｙ］，ｋ－１・ｓｉｎ（ｘ［i ］，ｋ－１）ｘ［ｙ］，ｋ－１＋（ｕ［ｘ］，ｋ－１・ｓｉｎ（ｘ［j ］，ｋ－１）＋（ｕ［ｙ］，ｋ－１・ｃｏｓ（ｘ［k ］，ｋ－１）ｘ［l ］，ｋ－１＋ｕ［m ］，ｋ－１#$$$$$$$$%&’’’’’’’’(（１２）２．２观测模型通常机器人上会有不同的传感器感知周围环境，如超声波，摄像头。

不同的传感器对周围环境的感知能力不同，得到的模型也会有所区别。

本文采用摄像头和全向反射镜组合的全景视觉系统，具有３６０°的水平视角和一定角度的垂直视角，使机器人能感知周围全局环境，克服了摄像头视角的问题。

在足球机器人比赛中，场地环境是已知的，设定全局坐标系，球门和角柱的坐标也就惟一的确定下来了，因此可利用球门或角柱作为路标进行机器人定位。

设某一路标在全局坐标系的坐标为ｌ，在机器人坐标系中的坐标为ｚ，而机器人在全局坐标系中坐标为ｘ。

如图２所示。

（ｘ［ｘ］，ｘ［ｙ］）ｘ［n］Ｘｒ１ＸｗＹｒＹｗ２３图２机器人坐标系与全局坐标系（ｘ［ｘ］，ｘ［ｙ］）ＸｒＸｗＹｒＹｗ（ｌ［ｘ］，ｌ［ｙ］）ｚ［ｙ］ｚ［ｘ］ｘ［o］ｘp ｋ４６《装备制造技术》２００８年第２期由几何关系，可以得到ｌ，ｚ和ｘ的关系式，即：ｚ＝ｈ（ｘ，ｌ）ｈｘ（ｘ，ｌ）ｈｙ（ｘ，ｌ!"）（１３）我们可以通过坐标系的平移和旋转使得路标的全局坐标转换为机器人坐标系，即：ｚ＝Ｒ（－ｘ［!］）ｌ′（１４）式中，ｌ′表示路标的平移，即：ｌ′＝ｌ［ｘ］ｌ［ｙ］!"－ｘ［ｘ］ｘ［ｘ］!"＝ｌ［ｘ］－ｘ［ｘ］ｌ［ｙ］－ｘ［ｙ］!"（１５）而Ｒ（－ｘ［"］）表示路标的旋转，即：Ｒ（－ｘ［#］）＝ｃｏｓ（－ｘ［$］）－ｓｉｎ（－ｘ［%］）ｓｉｎ（－ｘ［&］）ｃｏｓ（－ｘ［’］!"）（１６）由此，我们可以得到视觉模型：ｚｋ＝ｈ（ｘｋ，ｌｋ）＝ｈｘ（ｘｋ，ｌｋ）ｈｙ（ｘｋ，ｌｋ!"）＝（ｌ［ｘ］，ｋ－ｘ［ｘ］，ｋ）ｃｏｓ（ｘ［(］，ｋ）＋（ｌ［ｙ］，ｋ－ｘ［ｙ］，ｋ）ｓｉｎ（ｘ［)］，ｋ）（ｘ［ｘ］，ｋ－ｌ［ｘ］，ｋ）ｓｉｎ（ｘ［*］，ｋ）＋（ｌ［ｙ］，ｋ－ｘ［ｙ］，ｋ）ｃｏｓ（ｘ［+］，ｋ!"）（１７）２．３噪声分析上述建立的是理想模型，而在实际模型中，必然含有各种噪声和误差。

在运动模型中，轮子的半径ｒ和轮子与机器人质心的距离Ｌ存在着误差，以及轮子打滑使得编码器读数不能正确反映机器人位移，这些因素都使得机器人位移存在偏差，在卡尔曼滤波方程中用协方差矩阵Ｑ表示其噪声；在视觉模型中，由于采用全向反射镜，使得图像有一定程度的失真，需要进行摄像头标定和距离标定，然后利用插值得到图像中像素点之间的距离与实际距离的映射关系，这其中必然存在着误差。

在卡尔曼滤波中用Ｒ表示。

系统内部以及外部环境噪声的存在，使得机器人定位具有不确定性，影响了定位的精确度，因此必须把噪声也考虑进来。