admin管理员组文章数量:1565779
2024年6月21日发(作者:)
(19)中华人民共和国国家知识产权局
(12)发明专利说明书
(21)申请号 CN2.1
(22)申请日 2013.06.16
(71)申请人 西安科技大学
地址 710054 陕西省西安市雁塔中路58号
(72)发明人 侯媛彬 康倩 王磊 高阳东
(74)专利代理机构 西安创知专利事务所
代理人 谭文琰
(51)
G05D1/12
权利要求说明书 说明书 幅图
(10)申请公布号 CN 103345258 A
(43)申请公布日 2013.10.09
(54)发明名称
统
(57)摘要
本发明公开了一种足球机器人目标
一种足球机器人目标追踪方法及系
追踪方法及系统,其方法包括步骤:一、
建立足球机器人的运动模型,二、对足球
机器人的运动模型进行离散化,得到足球
机器人的状态方程,三、建立足球机器人
的测量模型,四、建立k-1时刻足球机器
人的状态变量x
法律状态
法律状态公告日
法律状态信息
法律状态
权 利 要 求 说 明 书
1.一种足球机器人目标追踪方法,所述足球机器人为由三个全向轮构成运动机构的
三轮全向足球机器人,其特征在于该方法包括以下步骤:
步骤一、采用数据处理器(1)建立足球机器人在足球场场地坐标下的运动模型:
其中,Sx为足球机器人在足球场场地上的X轴方向的位移,
Sy为足球机器人在足球场场地上的Y轴方向的位移,Φ为足球机器人
在足球场场地上的转动角度,(Sx,Sy,Φ)为足球机器人的
位姿信息;R为全向轮的半径,θ为足球机器人沿足球机器人坐标x轴方向的运动
速度vx与足球场场地坐标X轴之间的夹角,α为足球机器人沿足球机
器人坐标x轴方向的运动速度vx与直线d之间的夹角,所述直线d为
落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L
为三个全向轮中任意一个的质心与足球机器人的质心之间的距离,且三个全向轮的
质心与足球机器人质心之间的距离相等;
所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y
轴正方向的前提下,以符合右手坐标系的方向为X轴正方向建立的;所述足球机
器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方
向的前提下,以符合右手坐标系的方向为x轴正方向建立的;
步骤二、采用数据处理器(1)对步骤一中建立的足球机器人在足球场场地坐标下
的运动模型进行离散化,得到足球机器人在足球场场地坐标下的状态方程:
xk=f(xk-1,uk-1)+ηk-1
其中,xk为k时刻足球机器人在足球场场地坐标下的状态变量,
xk-1为k-1时刻足球机器人在足球场场地坐标下的状态变量,
uk-1=[vk-1,ωk-1]T为k-1时刻
对足球机器人的控制向量,ηk-1为过程噪声向量;vk-1为
k-1时刻足球机器人的前进速度且vk-1=-
vxsinθ+vycosθ+Rωk-1,ωk-1为
k-1时刻足球机器人的旋转速度;
步骤三、采用数据处理器(1)建立足球机器人在足球场场地坐标下的测量模型:
mfenced>
mfenced>
ime;>
dk2=[Pxk-xk-
Acosθk']2+[Pyk-yk-
Asinθk']2
mo>
k
i>
b>
sup>> 其中,zk为k时刻足球机器人位姿的测量量,dk为k时 刻足球机器人与目标之间的距离,βk为k时刻足球机器人前进方向与 目标行进方向之间的夹角,ηk'为测量噪声向量,Pxk为k 时刻目标与足球场场地坐标X轴之间的距离,xk为k时刻足球机器人 在足球场场地坐标下的横坐标,A为足球机器人的底座直径,θk'为k 时刻足球机器人的前进速度vk与足球场场地坐标X轴之间的夹角, Pyk为k时刻目标与足球场场地坐标Y轴之间的距离,yk 为k时刻足球机器人在足球场场地坐标下的纵坐标; 步骤四、所述数据处理器(1)调用高斯和粒子滤波处理模块且采用高斯和粒子滤 波算法建立k-1时刻足球机器人在足球场场地坐标下的状态变量xk-1 的滤波概率分布模型: mrow> msubsup> > mrow> mo>‾ msub> mo>( mrow>> 其中,z0:k={z0,z1,…,zk},G 为并行的高斯和粒子滤波器的总数, 预测得到的k-1时刻状态变量样本点集的权值, 的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量 xk-1的高斯分布, 到的k-1时刻样本点集 率分布预测得到的k-1时刻样本点集 步骤五、测量更新:采用全向视觉模块(5)对足球机器人所在场地上的环境信息 图像进行实时采集,并将所采集到的环境信息图像同步传送至数据处理器(1), 数据处理器(1)对其接收到的环境信息图像进行分析处理,得到k时刻足球机器 人在足球场场地上的位姿和目标在场地上的位姿,并调用高斯和粒子滤波处理模块 且采用高斯和粒子滤波算法对k时刻的足球机器人位姿的测量量zk进 行测量更新,得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k); 步骤六、预测更新:所述数据处理器(1)调用高斯和粒子滤波处理模块且采用高 斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k)进行预测更新, 得到k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的预测 概率分布p(xk+1|z0:k); 步骤七、所述数据处理器(1)根据k时刻足球机器人位姿的测量量zk 和步骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变 量xk+1的预测概率分布p(xk+1|z0:k),估计 出k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的高斯分 布 xk+1的最优估计值; 步骤八、目标追踪:首先,所述数据处理器(1)将步骤七中估计出的k+1时刻足 球机器人在足球场场地坐标下的状态变量xk+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时刻的足球机器人位姿的测量量zk-1进行测量更新,得到测量更新 后k时刻足球机器人在足球场场地坐标下的状态变量xk的滤波概率分 布p(xk|z0:k)的具体过程如下: 步骤501、所述数据处理器(1)根据公式: mi>k > > > OverBar; b> ub> k ‾ >> 计算得到k时刻足球机器人在足球场场地坐标下的状态变量xk的滤波 概率分布p(xk|z0:k);其中,Ck为标准化常 数且Ck=(∫p(xk|z0:k- 1)p(zk|xk)dxk)-1, 的权值,p(xk|z0:k-1)为k-1时刻足球机器人在足球场场地 坐标下的状态变量xk的滤波概率分布, p(zk|xk)为得到k时刻真实状态变量xk后足 球机器人位姿的测量量zk的贝叶斯分布; 态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量 xk的高斯分布, 到的k时刻样本点集 分布预测得到的k时刻样本点集 步骤502、首先,选取重要性函数q(·)=p(xk|z0:k),并对 重要性函数q(·)=p(xk|z0:k)进行抽样,得到k时刻的样本 点集 子滤波器的数量且i=1,2,...,G;然后,所述数据处理器(1)根据公式 ar; mi> / mi> mi>k 计算得到k时刻的样本点集 时刻的样本点集 的高斯分布; 分布 步骤503、所述数据处理器(1)模块调用蒙特卡罗估计模块且按照蒙特卡罗估计 方法计算得到k时刻机器人样本点集 Σki分别为: mo> mo> mi> p> mi> mi> > row> > mi>j i>w 步骤504、对步骤502中计算得到的k时刻的样本点集 更新,得到测量更新后的k时刻的样本点集 ; >Σ > /mo> mi> 其中, 步骤505、对步骤504中得到的测量更新后的k时刻的样本点集 权值 mover> >i w>> 步骤506、所述数据处理器(1)根据公式: mi>k > Sigma; subsup> >N msub> 计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k);其中, N(xk;μki,Σki)为测量更新得到的k时刻足球 机器人在足球场场地坐标下的状态变量xk的高斯分布, μki为测量更新得到的k时刻样本点集 Σki为测量更新得到的k时刻样本点集 3.按照权利要求2所述的一种足球机器人目标追踪方法,其特征在于:步骤六中所 述数据处理器(1)调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测 量更新后k时刻足球机器人在足球场场地坐标下的状态变量xk的滤波 概率分布p(xk|z0:k)进行预测更新,得到k+1时刻足球机 器人在足球场场地坐标下的状态变量xk+1的预测概率分布 p(xk+1|z0:k)的具体过程如下: 步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下 的状态变量xk的滤波概率分布p(xk|z0:k)抽 样得到k+1时刻的状态粒子x(k+1)i; 步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集 权值 x(k+1)i的权值 > 步骤603、所述数据处理器(1)模块调用蒙特卡罗估计模块且按照蒙特卡罗估计 方法计算得到k+1时刻的状态粒子x(k+1)i的滤波均值 差 /mo> 1 w>> mo> o>) mi>M mo>- n>1 msup> mover> mo> n>1 mi>H
步骤604、所述数据处理器(1)根据公式:
mrow>
mi>z o> mo> >‾ mn>1 mo>( w> ver>
erBar;
/mn>
>>
计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量
xk+1的预测概率分布p(xk+1|z0: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卡。
说 明 书
技术领域
本发明属于机器人视觉技术领域,尤其是涉及一种足球机器人目标追踪方法及系
统。
背景技术
足球机器人系统作为一个典型的多智能体系统,它为智能理论研究及多种技术的集
成应用提供了良好的实验平台,对足球机器人在运动过程中能够根据周围环境的变
化而自主的采取相应措施的能力要求也越来越强,这涉及到机器人定位、路径规划、
协调控制、目标追踪及决策等一系列研究课题。
机器人目标追踪的主要任务是根据上层决策系统提供的目标对象和控制对象状态,
包括目标对象和控制对象的位置、朝向,在具有障碍物的环境中,考虑到系统延时
的情况,准确快速的估计出运动中的目标对象和控制对象(机器人)的状态,实现
机器人的目标追踪和精确控制。
在目标追踪方面,研究者们提出过很多算法,如蚁群算法、势场法、遗传算法等,
这些算法在理论上实现了在特定模型中机器人的目标追踪,但并未考虑到现实环境
中外界的干扰及机器人自身的延时、通信等条件的限制,距实际应用还有一定的距
离。例如,申请号为2.3的中国专利公开了一种室内移动机器人双目
视觉导航系统,采用全局红外视觉定位方式,结合传感器信息,实现室内足球机器
人高精度的定位与导航的室内足球机器人双目视觉导航,但它仅适用于障碍物固定
且环境较稳定、单一机器人操作的情况;申请号为2.8的中国专利公
开了一种网络摄像头的移动机器人目标追踪平台,该平台为低成本、可移植性强、
有足够计算能力和一定数量传感器的QuickCam网络摄像头的移动 机器人目标追
踪平台;申请号为2.2的中国专利公开了一种基于可行通道的机器人
目标追踪方法,该方法使机器人通过自身视觉、超声、红外传感器和码盘提供的信
息实现静态障碍物环境下对目标的无碰追踪;申请号为2.3的中国专
利公开了一种焊接机器人,提供了一种焊接机器人在双丝焊接焊缝追踪的方法;申
请号为2.3的中国专利公开了一种室内环境下机器人自主搜寻气味源
的方法。但是从现有的专利看,大部分是固定环境或障碍物条件下,单一机器人的
运动控制或目标追踪,未见到可应用于足球机器人目标追踪的方法及系统。
发明内容
本发明所要解决的技术问题在于针对上述现有技术中的不足,提供一种步骤简单、
实现方便、能够实现快速、准确的足球机器人目标追踪的足球机器人目标追踪方
法。
为解决上述技术问题,本发明采用的技术方案是:一种足球机器人目标追踪方法,
所述足球机器人为由三个全向轮构成运动机构的三轮全向足球机器人,其特征在于
该方法包括以下步骤:
步骤一、采用数据处理器建立足球机器人在足球场场地坐标下的运动模型:
其中,Sx为足球机器人在足球场场地上的X轴方向的位移,
Sy为足球机器人在足球场场地上的Y轴方向的位移,Φ为足球机器人
在足球场场地上的转动角度,(Sx,Sy,Φ)为足球机器人的
位姿信息;R为全向轮的半径,θ为足球机器人沿足球机器人坐标x轴方向的运动
速度vx与足球场场地坐标X轴之间的夹角,α为足球机器人沿足球机
器人坐标x轴方向的运动速度vx与直线d之间的夹角,所述直线d为
落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L
为三个全向轮中任意一个 的质心与足球机器人的质心之间的距离,且三个全向轮
的质心与足球机器人质心之间的距离相等;
所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y
轴正方向的前提下,以符合右手坐标系的方向为X轴正方向建立的;所述足球机
器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方
向的前提下,以符合右手坐标系的方向为x轴正方向建立的;
步骤二、采用数据处理器对步骤一中建立的足球机器人在足球场场地坐标下的运动
模型进行离散化,得到足球机器人在足球场场地坐标下的状态方程:
xk=f(xk-1,uk-1)+ηk-1
其中,xk为k时刻足球机器人在足球场场地坐标下的状态变量,
xk-1为k-1时刻足球机器人在足球场场地坐标下的状态变量,
uk-1=[vk-1,ωk-1]T为k-1时刻
对足球机器人的控制向量,ηk-1为过程噪声向量;vk-1为
k-1时刻足球机器人的前进速度且vk-1=-
vxsinθ+vycosθ+Rωk-1,ωk-1为
k-1时刻足球机器人的旋转速度;
步骤三、采用数据处理器建立足球机器人在足球场场地坐标下的测量模型:
mfenced>
mfenced>
ime;>
dk2=[Pxk-xk-
Acosθk']2+[Pyk-yk-
Asinθk']2
mo>
k
i>
b>
sup>> 其中,zk为k时刻足球机器人位姿的测量量,dk为k时 刻足球机器人与目标之间的距离,βk为k时刻足球机器人前进方向与 目标行进方向之间的夹角,ηk'为测量噪声向量,Pxk为k 时刻目标与足球场场地坐标X轴之间的距离,xk为k时刻足球机器人 在足球场场地坐标下的横坐标,A为足球机器人的底座直径,θk'为k 时刻足球机器人的前进速度vk与足球场场地坐标X 轴之间的夹角, Pyk为k时刻目标与足球场场地坐标Y轴之间的距离,yk 为k时刻足球机器人在足球场场地坐标下的纵坐标; 步骤四、所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算法 建立k-1时刻足球机器人在足球场场地坐标下的状态变量xk-1的滤波 概率分布模型: mrow> msubsup> > mrow> mo>‾ msub> mo>( mrow>> 其中,z0:k={z0,z1,…,zk},G 为并行的高斯和粒子滤波器的总数, 预测得到的k-1时刻状态变量样本点集的权值, 的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量 xk-1的高斯分布, 到的k-1时刻样本点集 率分布预测得到的k-1时刻样本点集 步骤五、测量更新:采用全向视觉模块对足球机器人所在场地上的环境信息图像进 行实时采集,并将所采集到的环境信息图像同步传送至数据处理器,数据处理器对 其接收到的环境信息图像进行分析处理,得到k时刻足球机器人在足球场场地上的 位姿和目标在场地上的位姿,并调用高斯和粒子滤波处理模块且采用高斯和粒子滤 波算法对k时刻的足球机器人位姿的测量量zk进行测量更新,得到测 量更新后k时刻足球机器人在足球场场地坐标下的状态变量xk的滤波 概率分布p(xk|z0:k); 步骤六、预测更新:所述数据处理器调用高斯和粒子滤波处理模块且采用高斯和粒 子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k)进行预测更新, 得到k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的预测 概率分布p(xk+1|z0:k); 步骤七、所述数据处理器根据k时刻足球机器人位姿的测量量zk和步 骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量 xk+1的预测概率分布p(xk+1|z0:k),估计出 k+1时刻足球机器人在足球场场 地坐标下的状态变量xk+1的高斯分布 xk+1的最优估计值; 步骤八、目标追踪:首先,所述数据处理器将步骤七中估计出的k+1时刻足球机 器人在足球场场地坐标下的状态变量xk+1的最优估计值作为运动目标, 输出运动目标参数并通过串口通信模块输出给智能功率模块,再通过智能功率模块 输出相应的控制信号给运动控制卡,再通过运动控制卡输出相应的控制信号给电机 驱动电路模块,通过电机驱动电路模块驱动与电机驱动电路模块相接的电机旋转一 定的角度,电机进而带动与电机相接的全向轮运动到运动目标处;接着,采用全向 视觉模块对足球机器人所在场地上的环境信息图像进行实时采集,并将所采集到的 环境信息图像同步传送至数据处理器,数据处理器对其接收到的环境信息图像进行 分析处理,得到k+1时刻足球机器人在足球场场地上的位姿和目标在场地上的位 姿,并将k+1时刻足球机器人在足球场场地上的位姿与目标在场地上的位姿进行 比对,计算k+1时刻足球机器人在场地坐标系下的坐标与目标在场地坐标系下的 坐标之间的差值,当差值小于预先设定的误差阈值时,判断为k+1时刻足球机器 人已追上了目标,此时,数据处理器先将判断结果通过串口通信模块输出给智能功 率模块,再通过智能功率模块控制足球机器人进行传球、带球或射门的动作,然后 返回步骤五;否则,当差值大于预先设定的误差阈值时,判断为k+1时刻足球机 器人未追上目标,此时返回步骤五。 上述的一种足球机器人目标追踪方法,其特征在于:步骤五中所述数据处理器调用 高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对k-1时刻的足球机器人位姿 的测量量zk-1进行测量更新,得到测量更新后k时刻足球机器人在足 球场场地坐标下的状态变量xk的滤波概率分布 p(xk|z0:k)的具体过程如下: 步骤501、所述数据处理器根据公式: mi>k > > > OverBar; b> ub> k ‾ >> 计算得到k时刻足球机器人在足球场场地坐标下的状态变量xk的滤 波 概率分布p(xk|z0:k);其中,Ck为标准化 常数且Ck=(∫p(xk|z0:k- 1)p(zk|xk)dxk)-1, 的权值,p(xk|z0:k-1)为k-1时刻足球机器人在足球场场地 坐标下的状态变量xk的滤波概率分布, p(zk|xk)为得到k时刻真实状态变量xk后足 球机器人位姿的测量量zk的贝叶斯分布; 态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量 xk的高斯分布, 到的k时刻样本点集 分布预测得到的k时刻样本点集 步骤502、首先,选取重要性函数q(·)=p(xk|z0:k),并对 重要性函数q(·)=p(xk|z0:k)进行抽样,得到k时刻的样本 点集 子滤波器的数量且i=1,2,...,G;然后,所述数据处理器根据公式 ar; mi> / mi> mi>k 计算得到k时刻的样本点集 时刻的样本点集 布, 波概率分布 步骤503、所述数据处理器模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计 算得到k时刻机器人样本点集 Σki分别为: mo> mo> mi> p> mi> mi> > row> > mi>j i>w 步骤504、对步骤502中计算得到的k时刻的样本点集 更新,得到测量更新后的k时刻的样本点集 ; >Σ > /mo> mi> 其中, 步骤505、对步骤504中得到的测量更新后的k时刻的样本点集 权值 mover> >i w>> 步骤506、所述数据处理器根据公式: mi>k > Sigma; subsup> >N msub> 计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k);其中, N(xk;μki,Σki)为测量更新得到的k时刻足球 机器人在足球场场地坐标下的状态变量xk的高斯分布, μki为测量更新得到的k时刻样本点集 Σki为测量更新得到的k时刻样本点集 上述的一种足球机器人目标追踪方法,其特征在于:步骤六中所述数据处理器调用 高斯和粒子滤波处理模块且采用高斯和粒子滤波算法对测量更新后k时刻足球机器 人在足球场场地坐标下的状态变量xk的滤波概率分布 p(xk|z0:k)进行预测更新,得到k+1时刻足球机器人在足 球场场地坐标下的状态变量xk+1的预测概率分布 p(xk+1|z0:k)的具体过程如下: 步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下 的状态变量xk的滤波概率分布p(xk|z0:k)抽 样得到k+1时刻的状态粒子x(k+1)i; 步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集 权值 x(k+1)i的权值 > 步骤603、所述数据处理器模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法计 算得到k+1时刻的状态粒子x(k+1)i的滤波均值 差 /mo> 1 w>> mo> o>) mi>M mo>- n>1 msup> mover> mo> n>1 mi>H
步骤604、所述数据处理器根据公式:
mrow>
mi>z o> mo> >‾ mn>1 mo>( w> ver> >
mo>
row>>
计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量
xk+1的预测概率分布p(xk+1|z0:k);其中,
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建立足球机器人在足球场场地坐标下的运动模型:
其中,Sx为足球机器人在足球场场地上的X轴方向的位移,
Sy为足球机器人在足球场场地上的Y轴方向的位移,Φ为足球机器人
在足球场场地上的转动角度,(Sx,Sy,Φ)为足球机器人的
位姿信息;R为全向轮的半径,θ为足球机器人沿足球机器人坐标x轴方向的运动
速度vx与足球场场地坐标X轴之间的夹角,α为足球机器人沿足球机
器人坐标x轴方向的运动速度vx与直线d之间的夹角,所述直线d为
落于足球机器人坐标第一象限内的全向轮的质心与足球机器人坐标原点的连线;L
为三个全向轮中任意一个的质心与足球机器人的质心之间的距离,且三个全向轮的
质心与足球机器人质心之间的距离相等;
所述足球场场地坐标是以足球场中心点为坐标原点和以足球机器人进攻方向为Y
轴正方向的前提下,以符合右手坐标系的方向为X轴正方向建立的;所述足球机
器人坐标是以足球机器人质心为坐标原点和以足球机器人正前方的方向为y轴正方
向的前提下,以符合右手坐标系的方向为x轴正方向建立的;建立的运动模型如图
3所示;其中,三个全向轮为结构、形状和大小完全相同的全向轮,三个全向轮以
120°均匀分布,使得足球机器人在各个方向上的速度几乎相等;目标为足球;
步骤二、采用数据处理器1对步骤一中建立的足球机器人在足球场场地坐标下的运
动模型进行离散化,得到足球机器人在足球场场地坐标下的状态方程:
xk=f(xk-1,uk-1)+ηk-1
其中,xk为k时刻足球机器人在足球场场地坐标下的状态变量,
xk-1为k-1时刻足球机器人在足球场场地坐标下的状态变量,
uk-1=[vk-1,ωk-1]T为k-1时刻
对足球机器人的控制向量,ηk-1为过程噪声向量;vk-1为
k-1时刻足球机器人的前进速度且vk-1=-
vxsinθ+vycosθ+Rωk-1,ωk-1为
k-1时刻足球机器人的旋转速度;
步骤三、采用数据处理器1建立足球机器人在足球场场地坐标下的测量模型:
mfenced>
mfenced>
ime;>
dk2=[Pxk-xk-
Acosθk']2+[Pyk-yk-
Asinθk']2
mo>
k
i>
b>
sup>> 其中,zk为k时刻足球机器人位姿的测量量,dk为k时 刻足球机器人与目标之间的距离,βk为k时刻足球机器人前进方向与 目标行进方向之间的夹角,ηk'为测量噪声向量,Pxk为k 时刻目标与足球场场地坐标X轴之间的距离,xk为k时刻足球机器人 在足球场场地坐标下的横坐标,A为足球机器人的底座直径,θk'为k 时刻足球机器人的前进速度vk与足球场场地坐标X轴之间的夹角, Pyk为k时刻目标与足球场场地坐标Y轴之间的距离,yk 为k时刻足球机器人在足球场场地坐标下的纵坐标;建立的测量模型如图4所 示; 步骤四、所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯和粒子滤波算 法建立k-1时刻足球机器人在足球场场地坐标下的状态变量xk-1的滤 波概率分布模型: mrow> msubsup> > mrow> mo>‾ msub> mo>( mrow>> 其中,z0:k={z0,z1,…,zk},G 为并行的高斯和粒子滤波器的总数, 预测得到的k-1时刻状态变量样本点集的权值, 的概率分布预测得到的k-1时刻足球机器人在足球场场地坐标下的状态变量 xk-1的高斯分布, 到的k-1时刻样本点集 率分布预测得到的k-1时刻样本点集 步骤五、测量更新:采用全向视觉模块5对足球机器人所在场地上的环境信息图像 进行实时采集,并将所采集到的环境信息图像同步传送至数据处理器1,数据处理 器1对其接收到的环境信息图像进行分析处理,得到k时刻足球机器人在足球场场 地上的位姿和目标在场地上的位姿,并调 用高斯和粒子滤波处理模块且采用高斯 和粒子滤波算法对k时刻的足球机器人位姿的测量量zk进行测量更新, 得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量xk 的滤波概率分布p(xk|z0:k); 本实施例中,步骤五中所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯 和粒子滤波算法对k-1时刻的足球机器人位姿的测量量zk-1进行测量 更新,得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k)的具体过程如 下: 步骤501、所述数据处理器1根据公式: mi>k > > > OverBar; b> ub> k ‾ >> 计算得到k时刻足球机器人在足球场场地坐标下的状态变量xk的滤波 概率分布p(xk|z0:k);其中,Ck为标准化常 数且Ck=(∫p(xk|z0:k- 1)p(zk|xk)dxk)-1, 的权值,p(xk|z0:k-1)为k-1时刻足球机器人在足球场场地 坐标下的状态变量xk的滤波概率分布, p(zk|xk)为得到k时刻真实状态变量xk后足 球机器人位姿的测量量zk的贝叶斯分布; 态变量的概率分布预测得到的k时刻足球机器人在足球场场地坐标下的状态变量 xk的高斯分布, 到的k时刻样本点集 分布预测得到的k时刻样本点集 步骤502、首先,选取重要性函数q(·)=p(xk|z0:k),并对 重要性函数q(·)=p(xk|z0:k)进行抽样,得到k时刻的样本 点集 子滤波器的数量且i=1,2,...,G;然后,所述数据处理器1根据公式 ar; mi> / mi> mi>k 计算得到k时刻的样本点集 时刻的样本点集 布, 波概率分布 步骤503、所述数据处理器1模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法 计算得到k时刻机器人样本点集 Σki分别为: mo> mo> mi> p> mi> mi> > row> > mi>j i>w 步骤504、对步骤502中计算得到的k时刻的样本点集 更新,得到测量更新后的k时刻的样本点集 ; >Σ > /mo> mi> 其中, 步骤505、对步骤504中得到的测量更新后的k时刻的样本点集 权值 mover> >i w>> 步骤506、所述数据处理器1根据公式: mi>k > Sigma; subsup> >N msub> 计算得到测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k);其中, N(xk;μki,Σki)为测量更新得到的k时刻足球 机器人在足球场场地坐标下的状态变量xk的高斯分布, μki为测量更新得到的k时刻样本点集 Σki为测量更新得到的k时刻样本点集 步骤六、预测更新:所述数据处理器1调用高斯和粒子滤波处理模块且采用高斯和 粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k)进行预测更新, 得到k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的预测 概率分布p(xk+1|z0:k); 本实施例中,步骤六中所述数据处理器1调用高斯和粒子滤波处理模 块且采用高 斯和粒子滤波算法对测量更新后k时刻足球机器人在足球场场地坐标下的状态变量 xk的滤波概率分布p(xk|z0:k)进行预测更新, 得到k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的预测 概率分布p(xk+1|z0:k)的具体过程如下: 步骤601、依次从i=1,2,...,G的测量更新后k时刻足球机器人在足球场场地坐标下 的状态变量xk的滤波概率分布p(xk|z0:k)抽 样得到k+1时刻的状态粒子x(k+1)i; 步骤602、对步骤505中得到的测量更新后标准化的k时刻的样本点集 权值 x(k+1)i的权值 > 步骤603、所述数据处理器1模块调用蒙特卡罗估计模块且按照蒙特卡罗估计方法 计算得到k+1时刻的状态粒子x(k+1)i的滤波均值 差 /mo> 1 w>> mo> o>) mi>M mo>- n>1 msup> mover> mo> n>1 mi>H
步骤604、所述数据处理器1根据公式:
mrow>
mi>z o> mo> >‾ mn>1 mo>( w> ver>
erBar;
/mn>
>>
计算得到预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量
xk+1的预测概率分布p(xk+1|z0:k);其中,
x(k+1)i的高斯分布。
步骤七、所述数据处理器1根据k时刻足球机器人位姿的测量量zk和
步骤六中得到的预测更新后k+1时刻足球机器人在足球场场地坐标下的状态变量
xk+1的预测概率分布p(xk+1|z0:k),估计出
k+1时刻足球机器人在足球场场地坐标下的状态变量xk+1的高斯分布
xk+1的最优估计值;具体而言,xk+1的最优估计值是在
估计出
x(k+1)i的滤波均值
步骤八、目标追踪:首先,所述数据处理器1将步骤七中估计出的k+1时刻足球机
器人在足球场场地坐标下的状态变量xk+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%以内。
以上所述,仅是本发明的较佳实施例,并非对本发明作任何限制,凡是根据本发明
技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化,均仍属于本
发明技术方案的保护范围内。
版权声明:本文标题:一种足球机器人目标追踪方法及系统 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/shuma/1718931275a741709.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论