CN103345258A - 一种足球机器人目标追踪方法及系统 - Google Patents

Abstract

本发明公开了一种足球机器人目标追踪方法及系统，其方法包括步骤：一、建立足球机器人的运动模型，二、对足球机器人的运动模型进行离散化，得到足球机器人的状态方程，三、建立足球机器人的测量模型，四、建立k-1时刻足球机器人的状态变量x_k-1的滤波概率分布模型，五、测量更新，六、预测更新，七、估计出k+1时刻足球机器人的状态变量x_k+1的最优估计值，八、目标追踪；其系统包括数据处理器、数据存储卡、串口通信模块、无线通信模块、全向视觉模块、智能功率模块、运动控制卡、电机驱动电路模块、电机、电机编码器和足球机器人教练机。本发明设计合理，使用操作简便，实现方便，实现了快速、准确的足球机器人目标追踪，实用性强，使用效果好，推广应用价值高。

Description

一种足球机器人目标追踪方法及系统

技术领域

本发明属于机器人视觉技术领域，尤其是涉及一种足球机器人目标追踪方法及系统。 

背景技术

足球机器人系统作为一个典型的多智能体系统，它为智能理论研究及多种技术的集成应用提供了良好的实验平台，对足球机器人在运动过程中能够根据周围环境的变化而自主的采取相应措施的能力要求也越来越强，这涉及到机器人定位、路径规划、协调控制、目标追踪及决策等一系列研究课题。 

机器人目标追踪的主要任务是根据上层决策系统提供的目标对象和控制对象状态，包括目标对象和控制对象的位置、朝向，在具有障碍物的环境中，考虑到系统延时的情况，准确快速的估计出运动中的目标对象和控制对象（机器人）的状态，实现机器人的目标追踪和精确控制。 

在目标追踪方面，研究者们提出过很多算法，如蚁群算法、势场法、遗传算法等，这些算法在理论上实现了在特定模型中机器人的目标追踪，但并未考虑到现实环境中外界的干扰及机器人自身的延时、通信等条件的限制，距实际应用还有一定的距离。例如，申请号为201120313058.3的中国专利公开了一种室内移动机器人双目视觉导航系统，采用全局红外视觉定位方式，结合传感器信息，实现室内足球机器人高精度的定位与导航的室内足球机器人双目视觉导航，但它仅适用于障碍物固定且环境较稳定、单一机器人操作的情况；申请号为201220043504.8的中国专利公开了一种网络摄像头的移动机器人目标追踪平台，该平台为低成本、可移植性强、有足够计算能力和一定数量传感器的QuickCam网络摄像头的移动 机器人目标追踪平台；申请号为200810119587.2的中国专利公开了一种基于可行通道的机器人目标追踪方法，该方法使机器人通过自身视觉、超声、红外传感器和码盘提供的信息实现静态障碍物环境下对目标的无碰追踪；申请号为200810184912.3的中国专利公开了一种焊接机器人，提供了一种焊接机器人在双丝焊接焊缝追踪的方法；申请号为200710060125.3的中国专利公开了一种室内环境下机器人自主搜寻气味源的方法。但是从现有的专利看，大部分是固定环境或障碍物条件下，单一机器人的运动控制或目标追踪，未见到可应用于足球机器人目标追踪的方法及系统。 

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种步骤简单、实现方便、能够实现快速、准确的足球机器人目标追踪的足球机器人目标追踪方法。 

为解决上述技术问题，本发明采用的技术方案是：一种足球机器人目标追踪方法，所述足球机器人为由三个全向轮构成运动机构的三轮全向足球机器人，其特征在于该方法包括以下步骤： 

步骤一、采用数据处理器建立足球机器人在足球场场地坐标下的运动模型： 

其中，S_x为足球机器人在足球场场地上的X轴方向的位移，S_y为足球机器人在足球场场地上的Y轴方向的位移，Φ为足球机器人在足球场场地上的转动角度，(S_x,S_y,Φ)为足球机器人的位姿信息；R为全向轮的半径，θ为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与足球场场地坐标X轴之间的夹角，α为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与直线d之间的夹角，所述直线d为落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L为三个全向轮中任意一个 的质心与足球机器人的质心之间的距离，且三个全向轮的质心与足球机器人质心之间的距离相等；分别为三个全向轮转动的角度；所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y轴正方向的前提下，以符合右手坐标系的方向为X轴正方向建立的；所述足球机器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方向的前提下，以符合右手坐标系的方向为x轴正方向建立的； 

步骤二、采用数据处理器对步骤一中建立的足球机器人在足球场场地坐标下的运动模型进行离散化，得到足球机器人在足球场场地坐标下的状态方程： 

x_k=f(x_k-1,u_k-1)+η_k-1

其中，x_k为k时刻足球机器人在足球场场地坐标下的状态变量，x_k-1为k-1时刻足球机器人在足球场场地坐标下的状态变量，u_k-1=[v_k-1,ω_k-1]^T为k-1时刻对足球机器人的控制向量，η_k-1为过程噪声向量；v_k-1为k-1时刻足球机器人的前进速度且v_k-1=-v_xsinθ+v_ycosθ+Rω_k-1，ω_k-1为k-1时刻足球机器人的旋转速度； 

步骤三、采用数据处理器建立足球机器人在足球场场地坐标下的测量模型： 

$z_k=\left[\begin{array}{c}{d}_k\\ {\mathrm{\β}}_k\end{array}\right]+{{\mathrm{\η}}_k}^{\′}$

d_k ²=[P_xk-x_k-Acosθ_k']²+[P_yk-y_k-Asinθ_k']²

${\mathrm{\β}}_k=\mathrm{\π}-\arctan \[\frac{P_{\mathrm{yk}}-y_k-A\sin {{\mathrm{\θ}}_k}^{\′}}{P_{\mathrm{xk}}-x_k-A\cos {{\mathrm{\θ}}_k}^{\′}}\]-{{\mathrm{\θ}}_k}^{\′}$

其中，z_k为k时刻足球机器人位姿的测量量，d_k为k时刻足球机器人与目标之间的距离，β_k为k时刻足球机器人前进方向与目标行进方向之间的夹角，η_k'为测量噪声向量，P_xk为k时刻目标与足球场场地坐标X轴之间的距离，x_k为k时刻足球机器人在足球场场地坐标下的横坐标，A为足球机器人的底座直径，θ_k'为k时刻足球机器人的前进速度v_k与足球场场地坐标X 轴之间的夹角，P_yk为k时刻目标与足球场场地坐标Y轴之间的距离，y_k为k时刻足球机器人在足球场场地坐标下的纵坐标； 

步骤四、所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法建立k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的滤波概率分布模型： 

$p\left(x_{k-1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k-1)i}N(x_{k-1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k-1)i},{\stackrel{\‾}{\mathrm{\Σ}}}_{(k-1)i})$

其中，z_0:k={z₀,z₁,…,z_k}，G为并行的高斯和粒子滤波器的总数，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻状态变量样本点集的权值，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的高斯分布，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集的滤波均值，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集 的协方差； 

步骤五、测量更新：采用全向视觉模块对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器，数据处理器对其接收到的环境信息图像进行分析处理，得到k时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k时刻的足球机器人位姿的测量量z_k进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)； 

步骤六、预测更新：所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)； 

步骤七、所述数据处理器根据k时刻足球机器人位姿的测量量z_k和步骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)，估计出k+1时刻足球机器人在足球场场 地坐标下的状态变量x_k+1的高斯分布

并估计出k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值； 

步骤八、目标追踪：首先，所述数据处理器将步骤七中估计出的k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值作为运动目标，输出运动目标参数并通过串口通信模块输出给智能功率模块，再通过智能功率模块输出相应的控制信号给运动控制卡，再通过运动控制卡输出相应的控制信号给电机驱动电路模块，通过电机驱动电路模块驱动与电机驱动电路模块相接的电机旋转一定的角度，电机进而带动与电机相接的全向轮运动到运动目标处；接着，采用全向视觉模块对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器，数据处理器对其接收到的环境信息图像进行分析处理，得到k+1时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并将k+1时刻足球机器人在足球场场地上的位姿与目标在场地上的位姿进行比对，计算k+1时刻足球机器人在场地坐标系下的坐标与目标在场地坐标系下的坐标之间的差值，当差值小于预先设定的误差阈值时，判断为k+1时刻足球机器人已追上了目标，此时，数据处理器先将判断结果通过串口通信模块输出给智能功率模块，再通过智能功率模块控制足球机器人进行传球、带球或射门的动作，然后返回步骤五；否则，当差值大于预先设定的误差阈值时，判断为k+1时刻足球机器人未追上目标，此时返回步骤五。 

上述的一种足球机器人目标追踪方法，其特征在于：步骤五中所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k-1时刻的足球机器人位姿的测量量z_k-1进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)的具体过程如下： 

步骤501、所述数据处理器根据公式： 

$p\left(x_k\right|z_{0:k})=C_k{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}}p\left(z_k\right|x_k)N(x_k;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})$

计算得到k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波 概率分布p(x_k|z_0:k)；其中，C_k为标准化常数且C_k=(∫p(x_k|z_0:k-1)p(z_k|x_k)dx_k)^-1，

为根据k-1时刻状态变量的概率分布预测得到的k时刻状态变量样本点集的权值，p(x_k|z_0:k-1)为k-1时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布，p(z_k|x_k)为得到k时刻真实状态变量x_k后足球机器人位姿的测量量z_k的贝叶斯分布；为根据k-1时刻状态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布， 

为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集

的滤波均值，为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集 

的协方差； 

步骤502、首先，选取重要性函数q(·)=p(x_k|z_0:k)，并对重要性函数q(·)=p(x_k|z_0:k)进行抽样，得到k时刻的样本点集

其中，j为样本数且j=1,2,...,M，M为样本总数，i为并行的高斯和粒子滤波器的数量且i=1,2,...,G；然后，所述数据处理器根据公式 

$w_{\mathrm{ki}}^j=p\left(z_k\right|x_{\mathrm{ki}}^j)N(x_{\mathrm{ki}}^j;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})/q\left(x_{\mathrm{ki}}^j\right|z_{0:k})$

计算得到k时刻的样本点集

的权值

其中，

为为抽样得到k时刻的样本点集

后足球机器人位姿的测量量z_k的贝叶斯分布， 

为根据k-1时刻状态变量的概率分布预测得到的k时刻的样本点集

的高斯分布；为对应于抽样得到的k时刻的样本点集

的滤波概率分布

的重要性函数； 

步骤503、所述数据处理器模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k时刻机器人样本点集

的滤波均值μ_ki和协方差Σ_ki分别为： 

${\mathrm{\μ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j{x}_{\mathrm{ki}}^j\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

${\mathrm{\Σ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}}){(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}})}^T\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

步骤504、对步骤502中计算得到的k时刻的样本点集

的权值

进行更新，得到测量更新后的k时刻的样本点集

的权值 

${\stackrel{\‾}{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{(k-1)i}{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j/{\mathrm{\Σ}}_{i=1}^G{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

其中，

为测量更新后的k-1时刻的样本点集

的权值； 

步骤505、对步骤504中得到的测量更新后的k时刻的样本点集

的权值

进行标准化处理，得到测量更新后标准化的k时刻的样本点集的权值 ${\tilde{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{\mathrm{ki}}/{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}};$

步骤506、所述数据处理器根据公式： 

$p\left(x_k\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\tilde{w}}_{\mathrm{ki}}N(x_k;{\mathrm{\μ}}_{\mathrm{ki}},{\mathrm{\Σ}}_{\mathrm{ki}})$

计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；其中，N(x_k;μ_ki,Σ_ki)为测量更新得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布，μ_ki为测量更新得到的k时刻样本点集的滤波均值，Σ_ki为测量更新得到的k时刻样本点集 

的协方差。 

上述的一种足球机器人目标追踪方法，其特征在于：步骤六中所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)的具体过程如下： 

步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)抽样得到k+1时刻的状态粒子x_(k+1)i； 

步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集

的权值

进行预测更新，得到预测更新后的k+1时刻状态粒子x_(k+1)i的权值 ${\stackrel{\‾}{w}}_{(k+1)i}={\tilde{w}}_{\mathrm{ki}};$

步骤603、所述数据处理器模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k+1时刻的状态粒子x_(k+1)i的滤波均值

和协方差 

分别为： 

${\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M{x}_{(k+1)i}$

${\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i}){({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i})}^H$

步骤604、所述数据处理器根据公式： 

$p\left(x_{k+1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k+1)i}N(x_{k+1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i},{\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i})$

计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)；其中，

为根据k时刻状态变量的概率分布预测得到的k+1时刻状态粒子x_(k+1)i的高斯分布。 

上述的一种足球机器人目标追踪方法，其特征在于：步骤八中所述预先设定的误差阈值包括横坐标差值阈值和纵坐标差值阈值，所述横坐标差值阈值为20mm，所述纵坐标差值阈值为30mm。 

本发明还提供了一种电路结构简单、设计合理、接线方便、安装布设方便且投入成本低的实现上述足球机器人目标追踪方法的系统，其特征在于：包括数据处理器以及与数据处理器相接的数据存储卡、串口通信模块和无线通信模块，所述数据处理器的输入端接有全向视觉模块，所述串口通信模块上接有智能功率模块，所述智能功率模块的输出端接有三个运动控制卡，三个所述运动控制卡的输出端各接有一个电机驱动电路模块，三个所述电机驱动电路模块的输出端各接有一个电机，三个所述全向轮分别对应于三个所述电机输出轴连接，三个所述电机上各接有一个用于对所述电机的转速和位置进行实时检测的电机编码器，三个所述电机编码器的输出端均与所述智能功率模块的输入端相接，足球机器人踢球机构与所述智能功率模块的输出端相接，所述无线通信模块上接有足球机器人教练机。 

上述的系统，其特征在于：所述全向视觉模块包括设置在足球机器人机体上的全向视觉摄像头、设置在足球机器人机体上且位于所述全向视觉摄像头上方的全景视觉反射镜和与所述全向视觉摄像头相接的视频采集编解码卡，所述视频采集编解码卡与所述数据处理器的输入端相接。 

上述的系统，其特征在于：所述数据处理器为DSP数字信号处理器。 

上述的系统，其特征在于：所述数据存储卡为PCMCIA卡。 

本发明与现有技术相比具有以下优点： 

1、本发明足球机器人目标追踪系统的电路结构简单，设计合理，接线方便，安装布设方便且投入成本低。 

2、本发明足球机器人目标追踪方法的步骤简单，是基于高斯和粒子滤波算法（Gaussian Sum Particle Filter，GSPF）而实现的足球机器人目标追踪算法，将改进的粒子滤波算法（Particle Filter，PF）与高斯和滤波算法（Gaussian Sum Filter，GSF）相融合，采用“加权的高斯密度和”替代了“PF算法使用粒子集”来近似后验概率，降低粒子数，节约了计算时间；在计算滤波概率分布时，全向视觉模块对足球机器人所在场地上的环境信息图像进行实时采集，能够得到实时的测量量，能够使得状态分布更加接近真实的后验概率，实现了快速、准确的足球机器人目标追踪。 

3、本发明能够成功实现足球机器人对抗过程中本方足球机器人追踪目标，减少了足球机器人寻球、传球、带球和射门等这些动作所花费的时间和路径，提高了进球数目。 

4、本发明的实用性强，使用效果好，推广应用价值高。 

综上所述，本发明设计合理，使用操作简便，实现方便，实现了快速、准确的足球机器人目标追踪，实用性强，使用效果好，推广应用价值高。 

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。 

附图说明

图1为本发明足球机器人目标追踪方法的方法流程图。 

图2为本发明足球机器人目标追踪系统的电路原理框图。 

图3为本发明足球机器人在足球场场地坐标下的运动模型。 

图4为本发明足球机器人在足球场场地坐标下的测量模型。 

图5为本发明目标直线行驶时足球机器人目标追踪预测轨迹比对图。 

图6A为本发明目标直线行驶时足球机器人目标追踪在足球场场地坐标下X轴方向的预测轨迹误差对比图。 

图6B为本发明目标直线行驶时足球机器人目标追踪在足球场场地坐标下Y轴方向的预测轨迹误差对比图。 

图6C为本发明目标直线行驶时足球机器人目标追踪足球机器人前进方向与目标行进方向之间的夹角β的预测轨迹误差对比图。 

图7为本发明目标曲线行驶时足球机器人目标追踪预测轨迹比对图。 

图8A为本发明目标曲线行驶时足球机器人目标追踪在足球场场地坐标下X轴方向的预测轨迹误差对比图。 

图8B为本发明目标曲线行驶时足球机器人目标追踪在足球场场地坐标下Y轴方向的预测轨迹误差对比图。 

图8C为本发明目标曲线行驶时足球机器人目标追踪足球机器人前进方向与目标行进方向之间的夹角β的预测轨迹误差对比图。 

附图标记说明: 

1—数据处理器；        2—数据存储卡；             3—串口通信模块； 

4—无线通信模块；      5—全向视觉模块；           5-1—全向视觉摄像头； 

5-2—全景视觉反射镜；  5-3—视频采集编解码卡；     6—智能功率模块； 

7—运动控制卡；        8—电机驱动电路模块；       9—电机； 

10—电机编码器；       11—足球机器人踢球机构； 

12—足球机器人教练机。 

具体实施方式

如图1所示，本发明所述的足球机器人目标追踪方法，所述足球机器人为由三个全向轮构成运动机构的三轮全向足球机器人，该方法包括以下步骤： 

步骤一、采用数据处理器1建立足球机器人在足球场场地坐标下的运动模型： 

其中，S_x为足球机器人在足球场场地上的X轴方向的位移，S_y为足球机器人在足球场场地上的Y轴方向的位移，Φ为足球机器人在足球场场地上的转动角度，(S_x,S_y,Φ)为足球机器人的位姿信息；R为全向轮的半径，θ为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与足球场场地坐标X轴之间的夹角，α为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与直线d之间的夹角，所述直线d为落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L为三个全向轮中任意一个的质心与足球机器人的质心之间的距离，且三个全向轮的质心与足球机器人质心之间的距离相等；分别为三个全向轮转动的角度；所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y轴正方向的前提下，以符合右手坐标系的方向为X轴正方向建立的；所述足球机器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方向的前提下，以符合右手坐标系的方向为x轴正方向建立的；建立的运动模型如图3所示；其中，三个全向轮为结构、形状和大小完全相同的全向轮，三个全向轮以120°均匀分布，使得足球机器人在各个方向上的速度几乎相等；目标为足球； 

步骤二、采用数据处理器1对步骤一中建立的足球机器人在足球场场地坐标下的运动模型进行离散化，得到足球机器人在足球场场地坐标下的状态方程： 

x_k=f(x_k-1,u_k-1)+η_k-1

其中，x_k为k时刻足球机器人在足球场场地坐标下的状态变量，x_k-1为k-1时刻足球机器人在足球场场地坐标下的状态变量，u_k-1=[v_k-1,ω_k-1]^T为k-1时刻对足球机器人的控制向量，η_k-1为过程噪声向量；v_k-1为k-1时刻足球机器人的前进速度且v_k-1=-v_xsinθ+v_ycosθ+Rω_k-1，ω_k-1为k-1时刻足球机器人的旋转速度； 

步骤三、采用数据处理器1建立足球机器人在足球场场地坐标下的测量模型： 

$z_k=\left[\begin{array}{c}{d}_k\\ {\mathrm{\β}}_k\end{array}\right]+{{\mathrm{\η}}_k}^{\′}$

d_k ²=[P_xk-x_k-Acosθ_k']²+[P_yk-y_k-Asinθ_k']²

${\mathrm{\β}}_k=\mathrm{\π}-\arctan \[\frac{P_{\mathrm{yk}}-y_k-A\sin {{\mathrm{\θ}}_k}^{\′}}{P_{\mathrm{xk}}-x_k-A\cos {{\mathrm{\θ}}_k}^{\′}}\]-{{\mathrm{\θ}}_k}^{\′}$

其中，z_k为k时刻足球机器人位姿的测量量，d_k为k时刻足球机器人与目标之间的距离，β_k为k时刻足球机器人前进方向与目标行进方向之间的夹角，η_k'为测量噪声向量，P_xk为k时刻目标与足球场场地坐标X轴之间的距离，x_k为k时刻足球机器人在足球场场地坐标下的横坐标，A为足球机器人的底座直径，θ_k'为k时刻足球机器人的前进速度v_k与足球场场地坐标X轴之间的夹角，P_yk为k时刻目标与足球场场地坐标Y轴之间的距离，y_k为k时刻足球机器人在足球场场地坐标下的纵坐标；建立的测量模型如图4所示； 

步骤四、所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法建立k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的滤波概率分布模型： 

$p\left(x_{k-1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k-1)i}N(x_{k-1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k-1)i},{\stackrel{\‾}{\mathrm{\Σ}}}_{(k-1)i})$

其中，z_0:k={z₀,z₁,…,z_k}，G为并行的高斯和粒子滤波器的总数，为根据k-2时刻状态变量的概率分布预测得到的k-1时刻状态变量样本点集的权值，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的高斯分布，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集

的滤波均值，为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集 

的协方差； 

步骤五、测量更新：采用全向视觉模块5对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器1，数据处理器1对其接收到的环境信息图像进行分析处理，得到k时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并调 用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k时刻的足球机器人位姿的测量量z_k进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)； 

本实施例中，步骤五中所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k-1时刻的足球机器人位姿的测量量z_k-1进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)的具体过程如下： 

步骤501、所述数据处理器1根据公式： 

$p\left(x_k\right|z_{0:k})=C_k{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}}p\left(z_k\right|x_k)N(x_k;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})$

计算得到k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；其中，C_k为标准化常数且C_k=(∫p(x_k|z_0:k-1)p(z_k|x_k)dx_k)^-1，

为根据k-1时刻状态变量的概率分布预测得到的k时刻状态变量样本点集的权值，p(x_k|z_0:k-1)为k-1时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布，p(z_k|x_k)为得到k时刻真实状态变量x_k后足球机器人位姿的测量量z_k的贝叶斯分布；

为根据k-1时刻状态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布， 

为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集

的滤波均值，

为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集 

的协方差； 

步骤502、首先，选取重要性函数q(·)=p(x_k|z_0:k)，并对重要性函数q(·)=p(x_k|z_0:k)进行抽样，得到k时刻的样本点集

其中，j为样本数且j=1,2,...,M，M为样本总数，i为并行的高斯和粒子滤波器的数量且i=1,2,...,G；然后，所述数据处理器1根据公式 

$w_{\mathrm{ki}}^j=p\left(z_k\right|x_{\mathrm{ki}}^j)N(x_{\mathrm{ki}}^j;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})/q\left(x_{\mathrm{ki}}^j\right|z_{0:k})$

计算得到k时刻的样本点集

的权值

其中，

为为抽样得到k时刻的样本点集

后足球机器人位姿的测量量z_k的贝叶斯分布， 

为根据k-1时刻状态变量的概率分布预测得到的k时刻的样本点 集

的高斯分布；为对应于抽样得到的k时刻的样本点集

的滤波概率分布

的重要性函数； 

步骤503、所述数据处理器1模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k时刻机器人样本点集

的滤波均值μ_ki和协方差Σ_ki分别为： 

${\mathrm{\μ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j{x}_{\mathrm{ki}}^j\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

${\mathrm{\Σ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}}){(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}})}^T\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

步骤504、对步骤502中计算得到的k时刻的样本点集

的权值

进行更新，得到测量更新后的k时刻的样本点集

的权值 

${\stackrel{\‾}{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{(k-1)i}{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j/{\mathrm{\Σ}}_{i=1}^G{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

其中，

为测量更新后的k-1时刻的样本点集

的权值； 

步骤505、对步骤504中得到的测量更新后的k时刻的样本点集

的权值

进行标准化处理，得到测量更新后标准化的k时刻的样本点集

的权值 ${\tilde{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{\mathrm{ki}}/{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}};$

步骤506、所述数据处理器1根据公式： 

$p\left(x_k\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\tilde{w}}_{\mathrm{ki}}N(x_k;{\mathrm{\μ}}_{\mathrm{ki}},{\mathrm{\Σ}}_{\mathrm{ki}})$

计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；其中，N(x_k;μ_ki,Σ_ki)为测量更新得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布，μ_ki为测量更新得到的k时刻样本点集

的滤波均值，Σ_ki为测量更新得到的k时刻样本点集 

的协方差。 

步骤六、预测更新：所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)； 

本实施例中，步骤六中所述数据处理器1调用高斯和粒子滤波处理模 块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)的具体过程如下： 

步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)抽样得到k+1时刻的状态粒子x_(k+1)i； 

步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集的权值

进行预测更新，得到预测更新后的k+1时刻状态粒子x_(k+1)i的权值 ${\stackrel{\‾}{w}}_{(k+1)i}={\tilde{w}}_{\mathrm{ki}};$

步骤603、所述数据处理器1模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k+1时刻的状态粒子x_(k+1)i的滤波均值

和协方差 

分别为： 

${\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M{x}_{(k+1)i}$

${\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i}){({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i})}^H$

步骤604、所述数据处理器1根据公式： 

$p\left(x_{k+1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k+1)i}N(x_{k+1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}{\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i})$

计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)；其中，

为根据k时刻状态变量的概率分布预测得到的k+1时刻状态粒子x_(k+1)i的高斯分布。 

步骤七、所述数据处理器1根据k时刻足球机器人位姿的测量量z_k和步骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)，估计出k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的高斯分布

，并估计出k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值；具体而言，x_k+1的最优估计值是在估计出

的基础上，结合步骤603中计算得到的 k+1时刻的状态粒子x_(k+1)i的滤波均值

和协方差估计得到的； 

步骤八、目标追踪：首先，所述数据处理器1将步骤七中估计出的k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值作为运动目标，输出运动目标参数并通过串口通信模块3输出给智能功率模块6，再通过智能功率模块6输出相应的控制信号给运动控制卡7，再通过运动控制卡7输出相应的控制信号给电机驱动电路模块8，通过电机驱动电路模块8驱动与电机驱动电路模块8相接的电机9旋转一定的角度，电机9进而带动与电机9相接的全向轮运动到运动目标处；接着，采用全向视觉模块5对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器1，数据处理器1对其接收到的环境信息图像进行分析处理，得到k+1时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并将k+1时刻足球机器人在足球场场地上的位姿与目标在场地上的位姿进行比对，计算k+1时刻足球机器人在场地坐标系下的坐标与目标在场地坐标系下的坐标之间的差值，当差值小于预先设定的误差阈值时，判断为k+1时刻足球机器人已追上了目标，此时，数据处理器1先将判断结果通过串口通信模块3输出给智能功率模块6，再通过智能功率模块6控制足球机器人进行传球、带球或射门的动作，然后返回步骤五；否则，当差值大于预先设定的误差阈值时，判断为k+1时刻足球机器人未追上目标，此时返回步骤五。 

本实施例中，步骤八中所述预先设定的误差阈值包括横坐标差值阈值和纵坐标差值阈值，所述横坐标差值阈值为20mm，所述纵坐标差值阈值为30mm。 

如图2所示，本发明所述的足球机器人目标追踪系统，包括数据处理器1以及与数据处理器1相接的数据存储卡2、串口通信模块3和无线通信模块4，所述数据处理器1的输入端接有全向视觉模块5，所述串口通信模块3上接有智能功率模块6，所述智能功率模块6的输出端接有三个运动控制卡7，三个所述运动控制卡7的输出端各接有一个电机驱动电路 模块8，三个所述电机驱动电路模块8的输出端各接有一个电机9，三个所述全向轮分别对应于三个所述电机9输出轴连接，三个所述电机9上各接有一个用于对所述电机9的转速和位置进行实时检测的电机编码器10，三个所述电机编码器10的输出端均与所述智能功率模块6的输入端相接，足球机器人踢球机构11与所述智能功率模块6的输出端相接，所述无线通信模块4上接有足球机器人教练机12。 

本实施例中，所述全向视觉模块5包括设置在足球机器人机体上的全向视觉摄像头5-1、设置在足球机器人机体上且位于所述全向视觉摄像头5-1上方的全景视觉反射镜5-2和与所述全向视觉摄像头5-1相接的视频采集编解码卡5-3，所述视频采集编解码卡5-3与所述数据处理器1的输入端相接。全向视觉模块5是足球机器人的眼睛。足球机器人在移动过程中，总能感应到移动中的目标的位置，并通过不断检测自身与目标的相对位置来修正足球机器人运行轨迹。 

本实施例中，所述数据处理器1为DSP数字信号处理器。所述数据存储卡2为PCMCIA卡。 

为了对本发明足球机器人目标追踪方法的追踪速度及准确性进行验证，将在同样的足球场场地环境下，采用粒子滤波算法（PF算法）的足球机器人目标追踪预测轨迹与采用发明所述高斯和粒子滤波算法（GSPF算法）的足球机器人目标追踪预测轨迹相比对，具体如下： 

选取的足球机器人全向轮的半径R=52mm，底座直径A=520mm，三个全向轮中任意一个的质心与足球机器人的质心之间的距离L=250mm，足球机器人的前进速度为1m/s，目标的初始位置为（1，0），测量噪声和过程噪声均为随机噪声，采样周期为0.1s；分目标直线行驶和曲线行驶两种情况进行比对。 

目标直线行驶时，足球机器人的初始位置为（1.2，-2），得到的足球机器人目标追踪预测轨迹比对图如图5所示，足球机器人目标追踪在足球场场地坐标下X轴方向的预测轨迹误差对比图如图6A所示，足球机器 人目标追踪在足球场场地坐标下Y轴方向的预测轨迹误差对比图如图6B所示，足球机器人目标追踪足球机器人前进方向与目标行进方向之间的夹角β的预测轨迹误差对比图如图6C所示。从图5以及图6A、图6B和图6C可以看出，当目标直线行驶时，GSPF算法能够比PF算法更快的接近目标，并且误差较小。GSPF算法与PF算法在各时刻与目标在足球场场地坐标下X轴方向的误差及其两种算法的距离差值数据如表1所示。 

表1直线行驶时X轴方向两种算法的误差比较表 

GSPF算法与PF算法在各时刻与目标在足球场场地坐标下Y轴方向的误差及其两种算法的距离差值数据如表2所示。 

表2直线行驶时Y轴方向两种算法的误差比较表 

目标曲线行驶时，足球机器人的初始位置为（0.5，-0.6），得到的足球机器人目标追踪预测轨迹比对图如图7所示，足球机器人目标追踪在足球场场地坐标下X轴方向的预测轨迹误差对比图如图8A所示，足球机器人目标追踪在足球场场地坐标下Y轴方向的预测轨迹误差对比图如图8B所示，足球机器人目标追踪足球机器人前进方向与目标行进方向之间的夹角β的预测轨迹误差对比图如图8C所示。从图7以及图8A、图8B和图8C可以看出，当目标曲线行驶时，GSPF算法可以较小误差追踪到目标，特别当目标拐弯的时候的跟踪波动比PF算法平稳，并且稳定性比PF算法 稳定性好；GSPF算法在足球场场地坐标下X轴方向和Y轴方向，以及足球机器人目标追踪足球机器人前进方向与目标行进方向之间的夹角β的追踪误差均小于PF算法的追踪误差。GSPF算法与PF算法在各时刻与目标在足球场场地坐标下X轴方向的误差及其两种算法的距离差值数据如表3所示。 

表3曲线行驶时X轴方向两种算法的误差比较表 

GSPF算法与PF算法在各时刻与目标在足球场场地坐标下Y轴方向的误差及其两种算法的距离差值数据如表4所示。 

表4曲线行驶时Y轴方向两种算法的误差比较表 

综上所述，当目标直线运行时，GSPF算法与PF算法都能够在0.3s内以较小的误差追踪到目标物体，GSPF算法比PF算法在跟踪过程中的平均误差减小1.1%。当目标曲线行驶时，PF算法在0.5s后可将x轴误差减小到3.6%以内，但之后误差波动较大，基本维持在6.5%以内，在0.5s后可将y轴误差减小到4.6%以内，并在之后将y轴误差维持在4.6%以内。GSPF算法在0.5s后可将x轴误差减小到1.9%，在之后将误差维持在3.4%以内；在0.5s后可将y轴误差减小到1.0%以内，并在之后将y轴误差维持在2.1%以内。 

以上所述，仅是本发明的较佳实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。 

Claims (8)

1.一种足球机器人目标追踪方法，所述足球机器人为由三个全向轮构成运动机构的三轮全向足球机器人，其特征在于该方法包括以下步骤：

步骤一、采用数据处理器（1）建立足球机器人在足球场场地坐标下的运动模型：

其中，S_x为足球机器人在足球场场地上的X轴方向的位移，S_y为足球机器人在足球场场地上的Y轴方向的位移，Φ为足球机器人在足球场场地上的转动角度，(S_x,S_y,Φ)为足球机器人的位姿信息；R为全向轮的半径，θ为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与足球场场地坐标X轴之间的夹角，α为足球机器人沿足球机器人坐标x轴方向的运动速度v_x与直线d之间的夹角，所述直线d为落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L为三个全向轮中任意一个的质心与足球机器人的质心之间的距离，且三个全向轮的质心与足球机器人质心之间的距离相等；

分别为三个全向轮转动的角度；所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y轴正方向的前提下，以符合右手坐标系的方向为X轴正方向建立的；所述足球机器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方向的前提下，以符合右手坐标系的方向为x轴正方向建立的；

步骤二、采用数据处理器（1）对步骤一中建立的足球机器人在足球场场地坐标下的运动模型进行离散化，得到足球机器人在足球场场地坐标下的状态方程：

x_k=f(x_k-1,u_k-1)+η_k-1

其中，x_k为k时刻足球机器人在足球场场地坐标下的状态变量，x_k-1为k-1时刻足球机器人在足球场场地坐标下的状态变量，u_k-1=[v_k-1,ω_k-1]^T为k-1时刻对足球机器人的控制向量，η_k-1为过程噪声向量；v_k-1为k-1时刻足球机器人的前进速度且v_k-1=-v_xsinθ+v_ycosθ+Rω_k-1，ω_k-1为k-1时刻足球机器人的旋转速度；

步骤三、采用数据处理器（1）建立足球机器人在足球场场地坐标下的测量模型：

$z_k=\left[\begin{array}{c}{d}_k\\ {\mathrm{\β}}_k\end{array}\right]+{{\mathrm{\η}}_k}^{\′}$

d_k ²=[P_xk-x_k-Acosθ_k']²+[P_yk-y_k-Asinθ_k']²

${\mathrm{\β}}_k=\mathrm{\π}-\arctan \[\frac{P_{\mathrm{yk}}-y_k-A\sin {{\mathrm{\θ}}_k}^{\′}}{P_{\mathrm{xk}}-x_k-A\cos {{\mathrm{\θ}}_k}^{\′}}\]-{{\mathrm{\θ}}_k}^{\′}$

其中，z_k为k时刻足球机器人位姿的测量量，d_k为k时刻足球机器人与目标之间的距离，β_k为k时刻足球机器人前进方向与目标行进方向之间的夹角，η_k'为测量噪声向量，P_xk为k时刻目标与足球场场地坐标X轴之间的距离，x_k为k时刻足球机器人在足球场场地坐标下的横坐标，A为足球机器人的底座直径，θ_k'为k时刻足球机器人的前进速度v_k与足球场场地坐标X轴之间的夹角，P_yk为k时刻目标与足球场场地坐标Y轴之间的距离，y_k为k时刻足球机器人在足球场场地坐标下的纵坐标；

步骤四、所述数据处理器（1）调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法建立k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的滤波概率分布模型：

$p\left(x_{k-1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k-1)i}N(x_{k-1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k-1)i},{\stackrel{\‾}{\mathrm{\Σ}}}_{(k-1)i})$

其中，z_0:k={z₀,z₁,…,z_k}，G为并行的高斯和粒子滤波器的总数，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻状态变量样本点集的权值，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量x_k-1的高斯分布，为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集

的滤波均值，

为根据k-2时刻状态变量的概率分布预测得到的k-1时刻样本点集的协方差；

步骤五、测量更新：采用全向视觉模块（5）对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器（1），数据处理器（1）对其接收到的环境信息图像进行分析处理，得到k时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k时刻的足球机器人位姿的测量量z_k进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；

步骤六、预测更新：所述数据处理器（1）调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)；

步骤七、所述数据处理器（1）根据k时刻足球机器人位姿的测量量z_k和步骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)，估计出k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的高斯分布并估计出k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值；

步骤八、目标追踪：首先，所述数据处理器（1）将步骤七中估计出的k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的最优估计值作为运动目标，输出运动目标参数并通过串口通信模块（3）输出给智能功率模块（6），再通过智能功率模块（6）输出相应的控制信号给运动控制卡（7），再通过运动控制卡（7）输出相应的控制信号给电机驱动电路模块（8），通过电机驱动电路模块（8）驱动与电机驱动电路模块（8）相接的电机（9）旋转一定的角度，电机（9）进而带动与电机（9）相接的全向轮运动到运动目标处；接着，采用全向视觉模块（5）对足球机器人所在场地上的环境信息图像进行实时采集，并将所采集到的环境信息图像同步传送至数据处理器（1），数据处理器（1）对其接收到的环境信息图像进行分析处理，得到k+1时刻足球机器人在足球场场地上的位姿和目标在场地上的位姿，并将k+1时刻足球机器人在足球场场地上的位姿与目标在场地上的位姿进行比对，计算k+1时刻足球机器人在场地坐标系下的坐标与目标在场地坐标系下的坐标之间的差值，当差值小于预先设定的误差阈值时，判断为k+1时刻足球机器人已追上了目标，此时，数据处理器（1）先将判断结果通过串口通信模块（3）输出给智能功率模块（6），再通过智能功率模块（6）控制足球机器人进行传球、带球或射门的动作，然后返回步骤五；否则，当差值大于预先设定的误差阈值时，判断为k+1时刻足球机器人未追上目标，此时返回步骤五。

2.按照权利要求1所述的一种足球机器人目标追踪方法，其特征在于：步骤五中所述数据处理器（1）调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k-1时刻的足球机器人位姿的测量量z_k-1进行测量更新，得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)的具体过程如下：

步骤501、所述数据处理器（1）根据公式：

$p\left(x_k\right|z_{0:k})=C_k{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}}p\left(z_k\right|x_k)N(x_k;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})$

计算得到k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；其中，C_k为标准化常数且C_k=(∫p(x_k|z_0:k-1)p(z_k|x_k)dx_k)^-1，为根据k-1时刻状态变量的概率分布预测得到的k时刻状态变量样本点集的权值，p(x_k|z_0:k-1)为k-1时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布，p(z_k|x_k)为得到k时刻真实状态变量x_k后足球机器人位姿的测量量z_k的贝叶斯分布；

为根据k-1时刻状态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布，

为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集的滤波均值，

为根据k-1时刻状态变量的概率分布预测得到的k时刻样本点集的协方差；

步骤502、首先，选取重要性函数q(·)=p(x_k|z_0:k)，并对重要性函数q(·)=p(x_k|z_0:k)进行抽样，得到k时刻的样本点集

其中，j为样本数且j=1,2,...,M，M为样本总数，i为并行的高斯和粒子滤波器的数量且i=1,2,...,G；然后，所述数据处理器（1）根据公式

$w_{\mathrm{ki}}^j=p\left(z_k\right|x_{\mathrm{ki}}^j)N(x_{\mathrm{ki}}^j;{\stackrel{\‾}{\mathrm{\μ}}}_{\mathrm{ki}},{\stackrel{\‾}{\mathrm{\Σ}}}_{\mathrm{ki}})/q\left(x_{\mathrm{ki}}^j\right|z_{0:k})$

计算得到k时刻的样本点集

的权值

其中，

为为抽样得到k时刻的样本点集

后足球机器人位姿的测量量z_k的贝叶斯分布，

为根据k-1时刻状态变量的概率分布预测得到的k时刻的样本点集

的高斯分布；

为对应于抽样得到的k时刻的样本点集

的滤波概率分布

的重要性函数；

步骤503、所述数据处理器（1）模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k时刻机器人样本点集的滤波均值μ_ki和协方差Σ_ki分别为：

${\mathrm{\μ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j{x}_{\mathrm{ki}}^j\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

${\mathrm{\Σ}}_{\mathrm{ki}}=\left({\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}}){(x_{\mathrm{ki}}^j-{\mathrm{\μ}}_{\mathrm{ki}})}^T\right)/{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

步骤504、对步骤502中计算得到的k时刻的样本点集

的权值

进行更新，得到测量更新后的k时刻的样本点集

的权值

${\stackrel{\‾}{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{(k-1)i}{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j/{\mathrm{\Σ}}_{i=1}^G{\mathrm{\Σ}}_{j=1}^M{w}_{\mathrm{ki}}^j$

其中，

为测量更新后的k-1时刻的样本点集

的权值；

步骤505、对步骤504中得到的测量更新后的k时刻的样本点集

的权值进行标准化处理，得到测量更新后标准化的k时刻的样本点集

的权值 ${\tilde{w}}_{\mathrm{ki}}={\stackrel{\‾}{w}}_{\mathrm{ki}}/{\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{\mathrm{ki}};$

步骤506、所述数据处理器（1）根据公式：

$p\left(x_k\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\tilde{w}}_{\mathrm{ki}}N(x_k;{\mathrm{\μ}}_{\mathrm{ki}},{\mathrm{\Σ}}_{\mathrm{ki}})$

计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)；其中，N(x_k;μ_ki,Σ_ki)为测量更新得到的k时刻足球机器人在足球场场地坐标下的状态变量x_k的高斯分布，μ_ki为测量更新得到的k时刻样本点集

的滤波均值，Σ_ki为测量更新得到的k时刻样本点集的协方差。

3.按照权利要求2所述的一种足球机器人目标追踪方法，其特征在于：步骤六中所述数据处理器（1）调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)进行预测更新，得到k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)的具体过程如下：

步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下的状态变量x_k的滤波概率分布p(x_k|z_0:k)抽样得到k+1时刻的状态粒子x_(k+1)i；

步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集

的权值

进行预测更新，得到预测更新后的k+1时刻状态粒子x_(k+1)i的权值 ${\stackrel{\‾}{w}}_{(k+1)i}={\tilde{w}}_{\mathrm{ki}};$

步骤603、所述数据处理器（1）模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计算得到k+1时刻的状态粒子x_(k+1)i的滤波均值

和协方差

分别为：

${\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M{x}_{(k+1)i}$

${\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i}=\frac{1}{M}{\mathrm{\Σ}}_{i=1}^M({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i}){({\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}-x_{(k+1)i})}^H$

步骤604、所述数据处理器（1）根据公式：

$p\left(x_{k+1}\right|z_{0:k})={\mathrm{\Σ}}_{i=1}^G{\stackrel{\‾}{w}}_{(k+1)i}N(x_{k+1};{\stackrel{\‾}{\mathrm{\μ}}}_{(k+1)i}{\stackrel{\‾}{\mathrm{\Σ}}}_{(k+1)i})$

计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量x_k+1的预测概率分布p(x_k+1|z_0:k)；其中，

为根据k时刻状态变量的概率分布预测得到的k+1时刻状态粒子x_(k+1)i的高斯分布。

4.按照权利要求1所述的一种足球机器人目标追踪方法，其特征在于：步骤八中所述预先设定的误差阈值包括横坐标差值阈值和纵坐标差值阈值，所述横坐标差值阈值为20mm，所述纵坐标差值阈值为30mm。

5.一种实现如权利要求1所述足球机器人目标追踪方法的系统，其特征在于：包括数据处理器（1）以及与数据处理器（1）相接的数据存储卡（2）、串口通信模块（3）和无线通信模块（4），所述数据处理器（1）的输入端接有全向视觉模块（5），所述串口通信模块（3）上接有智能功率模块（6），所述智能功率模块（6）的输出端接有三个运动控制卡（7），三个所述运动控制卡（7）的输出端各接有一个电机驱动电路模块（8），三个所述电机驱动电路模块（8）的输出端各接有一个电机（9），三个所述全向轮分别对应于三个所述电机（9）输出轴连接，三个所述电机（9）上各接有一个用于对所述电机（9）的转速和位置进行实时检测的电机编码器（10），三个所述电机编码器（10）的输出端均与所述智能功率模块（6）的输入端相接，足球机器人踢球机构（11）与所述智能功率模块（6）的输出端相接，所述无线通信模块（4）上接有足球机器人教练机（12）。

6.按照权利要求5所述的系统，其特征在于：所述全向视觉模块（5）包括设置在足球机器人机体上的全向视觉摄像头（5-1）、设置在足球机器人机体上且位于所述全向视觉摄像头（5-1）上方的全景视觉反射镜（5-2）和与所述全向视觉摄像头（5-1）相接的视频采集编解码卡（5-3），所述视频采集编解码卡（5-3）与所述数据处理器（1）的输入端相接。

7.按照权利要求5所述的系统，其特征在于：所述数据处理器（1）为DSP数字信号处理器。

8.按照权利要求5所述的系统，其特征在于：所述数据存储卡（2）为PCMCIA卡。

Images