[发明专利]一种足球机器人目标追踪方法及系统

权利要求书

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）建立足球机器人在足球场场地坐标下的测量模型：

zk=dkβk+ηk′]]>

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

βk=π-arctan[Pyk-yk-Asinθk′Pxk-xk-Acosθk′]-θ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(xk-1|z0:k)=Σi=1Gw‾(k-1)iN(xk-1;μ‾(k-1)i,Σ‾(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时刻足球机器人未追上目标，此时返回步骤五。