本发明属于机器人控制领域，具体涉及仿人足球机器人的运动规划。
背景技术：
机器人足球比赛的研究重点是机器人的机器视觉，全局定位和导航，多机器人协作，双足步态和稳定性研究以及行为和策略等等，在机器人足球比赛中所有参赛队伍采用标准机器人参赛，国际和国内比赛中采用的技术挑战赛，包括自由项目、传球、避障射门等项目，比赛时操作员根据主裁判的决定通过无线网络向机器人发出对应的指令，使机器人进行状态切换，完成相应的比赛。
开展机器人足球世界杯结合当前流行的体育运动来推动机器人技术、人工智能和相关技术的共同发展，如果能够实现机器人带球、传球和踢球等动作，必然会实现相关领域技术的突破。
仿人机器人队伍要进行一场流畅地足球赛，需要一些基本的底层动作的支持，包括行走、踢球、起身等。其中，稳定地动态行走是最基础的，同时也是影响比赛的关键因素。比赛时外界环境随着赛程的推进而实时变化，这对机器人的行走提出更高要求，机器人需能随时改变自身的朝向、灵活地转身和快速地前进。
技术实现要素：
本发明的目的在于解决上述研究中存在的问题，提供仿人足球机器人的运动规划，达到精准定位的效果，并且以此为基础实现带预测控制的全向行走。
本发明解决其技术问题所采用的技术方案是：仿人足球机器人的运动规划，包括目标识别模块、目标定位模块和全向行走方法，其特征在于：
所述目标识别模块，具体步骤包括读取模板图片像素信息、模板特征点读取、实时图像匹配和识别物体；
所述目标定位模块，具体步骤包括粒子集初始化、预测更新、感知更新、可定位性估计、预测协方差估计、动态修正和扩展重采样；
其中，可定位性估计是基于参数估计理论中的fisher信息矩阵和crb定理；
所述全向行走方法，具体为一种基于cmac闭环控制的步态规划方法，其具体为，
步骤1，足部规划器在分析外界不同干扰因素的环境下得出可行的足部落脚点并计算出其zmp值；
步骤2，利用带预测控制的双线性倒立摆模型（d-lip）由zmp的值推出机器人躯干的运动轨迹；
步骤3，由足部落脚点使用三次样条插值法，规划出每两个落脚点之间在三维空间中的运行轨迹；
步骤4，利用机器人躯干和足部的位姿结合逆运动学计算出各个关节的角度；
步骤5，由nao机器人的陀螺仪传感器值可获得其躯干位姿，并与倒立摆形成反馈控制，从而使整个系统变为闭环控制。
进一步地，所述仿人足球机器人的运动规划应用于nao机器人，配合nao机器人自身硬件实现目标功能。
进一步地，所述目标识别模块中检测特征点时主要使用的算法为高斯滤波、canny边缘检测和harris角点检测。
进一步地，所述可定位性估计基于fisher信息矩阵和crb定理，是将移动机器人位姿作为待估参数，通过计算概率栅格地图下的观测模型，得到位姿的fisher信息矩阵中的各个参数，从而得到机器人的可定位。
进一步地，所述全向行走方法步骤2中的基于zmp预测控制的双线性倒立摆，是由足部规划器得出机器人的零力矩点zmp的值：
进一步地，所述全向行走方法步骤4中的逆运动学计算，其过程为：1.用一个齐次变换矩阵表示机器人自身局部坐标系下的足部位置及其朝向；2.再由旋转和平移两个分量得到相对于臀部滚动关节坐标系下的足部位姿；3.由于机器人在行走的过程中膝盖始终处于弯曲状态，可以假设机器人大腿和小腿之间形成一个三角形，在三角形模型下计算出膝关节角和踝关节角。
本发明采用的技术方案与现有技术相比，具有以下技术效果：
本发明提出了仿人足球机器人的运动规划，目标识别模块通过对机器人所采集的图像进行分析与处理，提取出有效目标，采用可定位性矩阵以及预测模型协方差矩阵，通过对这两种指标的在线估计判断两种数据的可信度，对感知更新后的粒子群进行动态位姿修正以提高动态环境下的定位精度及鲁棒性。本发明介绍了一种以机器人精准定位为前提，基于cmac闭环控制的步态规划方法，该方法可以实现带预测控制的全向行走方式。
附图说明
图1是本发明实施例的目标识别过程。
图2是本发明实施例的动态环境中机器人自定位方法。
图3是本发明实施例的闭环控制的全向行走框图。
图4是本发明实施例的落脚点规划。
图5是本发明实施例的双线性倒立摆模型。
图6是本发明实施例的nao的关节示意图。
图7是本发明实施例的腿部的逆运动学计算。
具体实施方式
为使本发明解决的技术问题、采用的技术方案阐述更清晰，下面将结合附图对本发明做进一步描述。
nao机器人在进行足球比赛时，主要是利用其视觉系统来识别目标物体同时感知球场环境。机器人头部上下垂直排列着两个摄像头，可提供分辨率为640*480的yuv422图像，并且每秒30帧的帧速可以确保图像的实时性，如图1所示。该定位方法中，目标识别模块通过对机器人所采集的图像进行分析与处理，提取出有效目标，再结合粒子滤波目标定位模块，精准定位出有效目标在球场上的位置。
相机的成像过程是将真实世界中的三维信息以投影变换的方式映射到二维平面图像的过程，原理是物体反射的光线通过摄像机光轴中心后投射到成像平面形成物体的像。
先拍摄一张足球的图片作为模板，检测出所有的特征点，所用的主要算法为：高斯滤波、canny边缘检测和harris角点检测。在机器人行走的过程中，对实时拍摄的图片进行分析处理之后和模板图片进行匹配，以确定机器人识别到目标物体。具体目标的识别过程如图1所示。
动态环境中由于人的存在往往给读数带来干扰，这会影响激光数据与已知地图的匹配效果，进而影响机器人的定位精度。为解决经典粒子滤波的退化和限制问题，采用可定位性矩阵以及预测模型协方差矩阵，通过对这两种指标的在线估计判断两种数据的可信度，对感知更新后的粒子群进行动态位姿修正以提高动态环境下的定位精度及鲁棒性。其流程如图2所示。
1）可定位性估计。
可定位性估计是基于参数估计理论中的fisher信息矩阵和crb定理。
假设可以交换微分和积分的顺序，即
文献又根据该假设条件推导出了fisher信息的另外两种表示方法：
则
这是一个n重积分，在假设可以交换微分和积分顺序的条件下可以得到
由此可以看出蕴含在一组n个随机采样中的fisher信息是单个观测值的n倍。
2）预测模型协方差估计
以里程计作为预测模型的定位过程中会产生两种误差：系统误差和非系统误差。非系统误差主要有光电编码器的读书误差，车轮与地面摩擦打滑造成的误差。
设t时刻里程计的输入为：
根据已经得到机器人的运动学模型，对可能引入的噪声进行分析，进行里程计误差建模，通过里程计协方差矩阵计算预测模型的不确定性。根据运动学模型可知：
首先对每个周期的里程计协方差矩阵进行求解，具体形式为：
其中：
经典粒子滤波定位算法使用基于里程计的预测模型和基于外部传感器的观测信息对粒子集进行权重的更新来确定机器人在环境中的位姿。由于该方法很大程度上依赖于外部感知的传感器，当机器人处于动态环境中时人体干扰会影响粒子集的权重更新，可能会导致粒子滤波定位失效。而基于里程计的预测模型避免了动态环境中观测信息对机器人定位产生的干扰。
仿人机器人队伍要进行一场流畅地足球赛，需要一些基本的底层动作的支持，包括行走、踢球、起身等。其中，稳定地动态行走是最基础的，同时也是影响比赛的关键因素。比赛时外界环境随着赛程的推进而实时变化，这对机器人的行走提出更高要求，机器人需能随时改变自身的朝向、灵活地转身和快速地前进。本专利介绍了一种以机器人精准定位为前提，基于cmac闭环控制的步态规划方法，该方法可以实现带预测控制的全向行走方式。
基于cmac闭环控制的全向行走方法如图3所示。
步骤1，首先足部规划器在分析外界不同干扰因素的环境下得出可行的足部落脚点并计算出其zmp值。
步骤2，利用带预测控制的双线性倒立摆模型（d-lip）由zmp的值推出机器人躯干的运动轨迹。
步骤3，由足部落脚点使用三次样条插值法，规划出每两个落脚点之间在三维空间中的运行轨迹。
步骤4，利用机器人躯干和足部的位姿结合逆运动学计算出各个关节的角度。
步骤5，由nao的陀螺仪传感器值可获得其躯干位姿，并与倒立摆形成反馈控制，从而使整个系统变为闭环的。
①足部规划器
②基于zmp预测控制的双线性倒立摆。
由足部规划器可以得出机器人的零力矩点zmp的值：
由于本专利建立的倒立摆模型是由一个点质量和一个可伸缩的无质量的腿构成，则机器人的主要质量是集中在躯干上，所以可假定com=trunk。
③足部轨迹。
④逆运动学
nao模型在机械构造方面的特殊之处：
b.机器人的两条腿是由一个臀部偏摆关节连接控制的，即由同一个伺服系统控制的。
nao仿人机器人定义了三个坐标系：躯干坐标系、世界坐标系、机器人坐标系。躯干坐标系随着机器人的运动轨迹变化坐标系会发生相应的变化；机器人的起始位置即为世界的坐标系，在后续机器人的运动过程当中不会发生任何变化；机器人坐标系定义了nao机器人两脚中间位置作为坐标系的原点，垂直方向作为z轴，x轴永远朝向机器人前进的方向。为了便于获取机器人的姿态和运动参数，在nao机器人行走方面我们选用机器人坐标系。
建立机器人姿态和运动参数之间的联系，所用的媒介就是运动学。运动学由两部分组成：正运动学和逆运动学。区别在于正运动学是已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿；而逆运动学是根据已给定的满足工作要求的末端执行器相对参考坐标系的位置和姿态，求各关节的运动参数。两者的求解方向相反。
对于机器人行走问题，显而易见，逆运动学更符合实际需求。只要设定机器人需要达到的目标要求轨迹，这个目标要求轨迹即由倒立摆模型和足部轨迹模块推出的机器人自身局部坐标系下的足部位置及朝向。然后再通过逆运动学求解非线性方程得到腿部各个关节的角度从而实现对机器人的实时控制。
逆运动学简单算法过程如下：
1.用一个齐次变换矩阵表示机器人自身局部坐标系下的足部位置及其朝向。
2.再由旋转和平移两个分量得到相对于臀部滚动关节坐标系下的足部位姿。
3.由于机器人在行走的过程中膝盖始终处于弯曲状态，可以假设机器人大腿
和小腿之间形成一个三角形，在三角形模型下计算出膝关节角和踝关节角。
模型如图7所示。
⑤反馈控制。
通过粒子滤波算法和全向行走控制，实现了nao机器人实时定位并跟踪红球（以红球为例），实现过程如下。
首先建立机器人坐标系，为了便于对红球的检测，使用nao机器人的下摄像头对图像进行采集。预先设定红色的rgb阈值为[255,0,0]~[255,147,2]，并且设定特征对象的直径为5cm，即实验所选取的对象是直径为5cm的红球。
确定对象之后，采用粒子滤波算法对现场环境中直径为5cm的红球进行跟踪定位，通过粒子滤波算法可以实时追踪到红球的位置。
追踪到红球之后，nao机器人、地面和红球三者之间构成一个三角形。通过单目测距原理和三角函数计算可以得到机器人距离红球的距离z及角度。
机器人单目测距追踪可得到与目标红球之间的距离，但这种方式是静态的，结合粒子滤波算法可以动态的追踪目标红球，并且通过调整机器人头部角度可以使得红球始终处于机器人视野的中央。
最后由nao机器人全向行走姿态设计方法，使机器人可以从各个方向走向目标红球。nao机器人在找球的过程中转动头部电机，直至找到目标红球，定位到目标红球后实时计算与球的角度及距离，利用机器人全向行走行走到红球位置。
为了防止机器人踢到红球，预先设定机器人离红球的位置为80mm时机器人停止行走。机器人在不断接近目标红球的过程中，调用内存监视器实时查看离目标红球的距离，得到相应实验数据，结果为机器人在1cm/s的速度下，在距离红球80.635mm时停止行走。