CN102375416B - 基于快速搜索树的人型机器人踢球动作信息处理方法 - Google Patents

Abstract

本发明涉及一种基于快速搜索树的人型机器人踢球动作信息处理方法，包括以下步骤：1)机器人在不确定环境下根据关节位置传感器、加速度传感器和陀螺仪采集的信息，并通过正向运动模型得到机器人当前状态；并通过正向运动模型得到机器人当前状态；2)机器人根据当前状态和踢球的终止状态，通过快速随机搜索树计算出所需的机器人各关节运动轨迹；3)通过动作平滑滤波器对步骤2)中生成的关节运动轨迹进行平滑处理，并动力学滤波器进行修正得到稳定的最终踢球运动轨迹。与现有技术相比，本发明具有保证机器人在不确定环境下踢球动作可靠执行等优点。

Description

基于快速搜索树的人型机器人踢球动作信息处理方法

技术领域

本发明涉及一种机器人踢球动作信息处理方法，尤其是涉及一种基于快速搜索树的人型机器人踢球动作信息处理方法。

背景技术

近年来机器人足球赛越来越受世界各国高等院校的青睐，而在类人机器人足球赛中，鲁棒、有效的踢球动作这是十分重要的。目前大多数的类人机器人踢球动作都是通过人工离线的编排一系列固定的关节位置来实现的，这种方法不仅需要大量繁琐枯燥的人力劳动，而且受实时环境的干扰非常大，地面的微小变化都会导致踢球动作无法完成。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种保证机器人在不确定环境下踢球动作可靠执行的基于快速搜索树的人型机器人踢球动作信息处理方法。

本发明的目的可以通过以下技术方案来实现：

一种基于快速搜索树的人型机器人踢球动作信息处理方法，其特征在于，包括以下步骤：

1)机器人在不确定环境下根据关节位置传感器、加速度传感器和陀螺仪采集的信息，并通过正向运动模型得到机器人当前状态；

2)机器人根据当前状态和踢球的终止状态，通过快速随机搜索树计算出所需的机器人各关节运动轨迹；

3)通过动作平滑滤波器对步骤2)中生成的关节运动轨迹进行平滑处理，并动力学滤波器进行修正得到稳定的最终踢球运动轨，并将该轨迹发送给机器人执行机构。

所述的步骤1)中的通过正向运动模型得到机器人当前状态的具体步骤如下：

1)利用加速度传感器的静态特性和陀螺仪的动态特性，通过补偿滤波器获得机器人身体的全局旋转矩阵R_bw；

2)利用正向运动模型由身体开始向各子连杆递归计算各子连杆全局位置和旋转矩阵：

p_j＝p_i+R_ib_j

R_j＝R_iR(q_j)

其中p_j为待求第j连杆全局位置，第i连杆为第j连杆的母连杆，R_i为第i连杆的全局旋转矩阵，b_j为第j连杆关节在其母连杆坐标系下的位置向量；R_j为待求第j连杆的全局旋转矩阵，R(q_j)是由第j连杆关节旋转q_j所造成的旋转效应。

所述的步骤2)通过快速随机搜索树计算出所需的机器人各关节运动轨迹的具体步骤如下：

1)根据踢球动作的起始位置初始化搜索树算法的起始状态；

2)在满足无连杆碰撞和所设定支撑模式的约束条件下，使用RRT-Extend方法随机生成扩展状态并同时采用RRT-Connect生成连接状态；

3)重复步骤2)，直到由起始状态和终止状态生成的两棵搜索树实现连接为止。

所述的动作平滑滤波器通过以下步骤来实现关节运动轨迹的平滑处理：

1)采用计算机图形学中的三阶贝塞尔曲线进行平滑处理，减小关节值的震荡；

2)采用直线切割的方法去除不必要的关节运动。

所述的动力学滤波器通过以下步骤来实现关节运动轨迹的修正处理：

设C为机器人质心，是机器人关节角θ的函数，即C＝X(θ)，则质心的雅可比矩阵表示为

亦即

对于微小量的变化，近似的有ΔC＝J(θ)Δθ(t)，滤波目标为保证质心在地面的投影位置不变，该约束条件可表示为：

$\mathrm{\Δ}{C}_x=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{x,i}\mathrm{\Δ}{\mathrm{\θ}}_i\left(t\right)=0,$ $\mathrm{\Δ}{C}_y=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{y,i}\mathrm{\Δ}{\mathrm{\θ}}_i\left(t\right)=0$

其中Δθ_i＝Δθ_cmdi+Δθ_compi，Δθ_i表示实际角度变化值，Δθ_cmdi表示输入命令角度变化值，Δθ_compi表示滤波补偿值；

为了保证对原始姿态的动作改变最小，设计一个评价函数

将问题转化为一个最优化问题，其表述如下：

求取

约束条件ΔC_x＝0与ΔC_y＝0。

与现有技术相比，本发明具有采用机载传感器计算机器人的当前状态，自动规划的方法对机器人踢球动作进行实时生成，提高了踢球动作的鲁棒性，保证了机器人在不确定环境下踢球动作的可靠执行。

附图说明

图1为本发明的流程图；

图2为本发明的硬件结构示意图；

图3为本发明的机器人二叉树结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1、图2所示，一种基于快速搜索树的人型机器人踢球动作信息处理方法，包括以下步骤：

1)机器人在不确定环境下根据关节位置传感器1、加速度传感器2和陀螺仪3采集的信息，并通过正向运动模型得到机器人当前状态；

2)机器人的处理器4根据当前状态和踢球的终止状态，通过快速随机搜索树计算出所需的机器人各关节运动轨迹；

3)通过动作平滑滤波器对步骤2)中生成的关节运动轨迹进行平滑处理，并动力学滤波器进行修正得到稳定的最终踢球运动轨迹，并将该轨迹发送给机器人执行机构。

机器人位姿的计算是动作规划的基础之一，主要包含正运动学和逆运动学计算两部分。为了方便对机器人进行运动学的计算，需要将机器人抽象为一系列连杆和关节构成的模型。同时，由于在机器人动作规划过程中还要满足其与自身或环境无碰撞的条件。还需要将机器人抽象为一系列三维物体构成的模型。人形机器人是由很多连杆和关节构成的复杂机械系统，对其进行运动学分析的第一步是对其连杆和关节进行分解。划分好关节后，需要对连杆连接关系进行描述，这里采用二叉树结构(如图3)，左子树表示子结点，右子树表示姊妹结点，结构明确固定，利于编程。

人形机器人腿部股关节处有两个在YZ平面上与Z轴呈45度和135度的关节。在建模时均假设这两个关节的关节轴相对世界坐标系的朝向沿Z轴正方向。因此计算旋转矩阵时须分别作特殊处理如下：(q_j表示该关节的角度值)

$R_{j1}=\mathrm{RotX}(-\frac{\mathrm{\π}}{4})\·\mathrm{RotZ}\left(q_j\right)\·\mathrm{RotX}\left(\frac{\mathrm{\π}}{4}\right),$ $R_{j2}=\mathrm{RotX}(-\frac{3\mathrm{\π}}{4})\·\mathrm{RotZ}\left(q_j\right)\·\mathrm{RotX}\left(\frac{3\mathrm{\π}}{4}\right)$

在单脚支撑模式下(双脚支撑模式也以单脚支撑模式的计算为基础)，脚底(Foot)坐标系被认为是机器人的世界坐标系。而一般采用的递归方法计算得到的是以身体(Body)坐标系为世界坐标系下的机器人位姿，这样计算出的位姿不能用于做碰撞检测，也不易直观的观察机器人动作的变化。因此需要再做一次坐标系的转换，将参考坐标系由Body坐标系转变为Foot坐标系。转变方法比较直观，即在第一次正运动学计算后将Body的位置和姿态转换为相对Foot坐标系的值，然后再做一次正运动学计算，此时所有连杆和关节的位姿的参考坐标系就变成了Foot坐标系。

质心即各个连杆质心位置按照其质量的加权平均。针对每一个连杆，其质心可由如下公式计算：COM_j＝p_j+R_j*c_j，其中p_j，R_j分别为连杆在世界坐标系中的位置和旋转矩阵，c_j为质心在连杆坐标系中的位置。使用类似正运动学的递归方法，即可以计算各质心位置的加权平均制。

动作搜索过程中除需要考虑正、逆运动学及重心计算外，还必须要对随机生成的动作做碰撞检测。本文采用北卡罗来纳大学计算机系提供的V-Collide开源软件进行碰撞检测。通过该软件实现碰撞检测需要构建机器人的3D模型，并输入机器人3D模型外表面三角化后的三角形参数。

为简便起见，本文根据NAO机器人的外形抽象出了圆柱体、三角体、圆台、立方体四种立体模型以分别表示头、躯干、四肢及脚。可以看到简化3D模型在肘、膝盖、脚踝等关节间均留出了一定的间隙，这是为了保证关节运动时同一个关节连接的两个连杆之间不发生碰撞而采取的一种简化处理方法。采用这种方法在一些特殊的情况下可能导致实际发生碰撞但却未被程序检测到的情况。但考虑到它的简单性和在绝大多数情况下的有效性，最终在程序中采用了这种3D模型的构建方法。

快速随机搜索树(RRT)是一种有效的在同时包含代数约束(由障碍物引起)和微分方程约束(由动力学和非完全可控性引起)的高维空间下进行快速搜索的数据结构和采样方法。其核心的思想是让搜索方向朝未被探索的空间偏移。

一个基本的RRT构建算法通过在每一步执行一个简单的循环将RRT不断朝随机生成的状态方向扩展。依靠基本的RRT算法进行路径搜索将会消耗较多的时间，因为RRT每一次仅向随机生成的方向扩展一个较小的距离ε，且每一部扩展都须搜索最近点并进行碰撞检测。针对这个问题，可以采用RRT-Connect算法。RRT-Connect是专门为没有微分方程约束的路径规划问题设计的算法。其在搜索过程中采用根节点分别为初始状态和目标状态的Ta，Tb两棵树展开双向搜索。当两棵树相互连接时就意味着解路径被找到。

采用上节阐述的RRT算法，令每个RRT节点包含人形机器人的22个关节值(机器人的位姿信息)，本方法们即可采用完全相同于RRT-Connect核心思想的搜索算法进行人形机器人的动作路径规划。但对于机器人的动作搜索，还需要对算法中的随机状态生成模块进行扩展。本方法将在这一节具体阐述随机状态生成的方法。

本方法们所讨论的动作规划算法对随机生成的状态有如下三个要求：1)各关节值在机器人所能达到的关节值范围内，2)机器人与自身和环境中的物体无碰撞，3)机器人静态稳定。

对于要求1)，只需建立一个数组存储机器人各关节值的取值范围，在生成随机数后对其进行线性变换映射到取值范围的空间内即可。对于要求2)，结合机器人3D模型与V-Collide冲撞检测开源软件即可实现，V-Collide源代码即使用方法参见。

对于要求3)，首先需要定义静态稳定性的概念。所谓静态稳定，即机器人重心位置在地面的投影在机器人的支撑平面内。本方法们将人形机器人的脚掌抽象为一个矩形。所以这个支撑平面在单脚支撑的状态下即为支撑脚与地面接触的部分，即脚掌所构成的矩形；在双脚支撑状态下，这个支撑平面为两个脚掌与地面接触点构成的凸包。下面本方法们分两种对静态稳定状态的生成进行讨论。

单脚支撑下的情况比较简单。根据随机生成的关节值，获取单脚支撑下的机器人状态，再按照计算机器人重心，判断重心的x，y坐标值是否在支撑脚的矩形范围内，是则满足静态稳定条件，否则不满足。

双脚支撑下的情况相对复杂。由于双脚支撑状态下机器人两个脚的相对位姿始终不变。因此如果假设两脚之间存在一个连杆，本方法们可以发现机器人股关节以下(包括股关节)的部分实际构成了一个闭环的系统。

对于人形机器人双脚支撑下的随机状态生成，可首先仍按照单脚支撑状态的限制条件，这里本方法们假设为右脚支撑，生成一个随机状态。得到该随机状态后由左右脚的固定相对位姿计算出左脚的位姿。根据左脚的位姿和身体的位姿可以通过逆运动学方法计算出左腿6个关节的取值。由此即可生成一个双脚支撑下的机器人状态。

接下来需要对其静态稳定性进行判断。质心的计算没有任何不同，但支撑平面的计算要相对复杂。单脚支撑下支撑脚与地面相接触的矩形即为支撑平面，而双脚支撑下则为两支撑脚与地面所有接触点所构成的凸包(本方法们假设脚掌构成的矩形完全与地面接触，则所有接触点凸包的计算可以简化为代表双脚的两个矩形所构成的凸包的计算)。凸包的计算可以采用了格雷厄姆方法。找到凸包后判断质心投影是否在凸包内即可判断机器人的静态稳定性。

路径平滑所要解决的问题为：1)减小关节值的震荡，2)去除不必要的关节运动。对于前者，本方法借鉴了计算机图形学中的三阶贝塞尔曲线进行平滑；对于后者，本方法简单采用了直线切割的方法。

在采用RRT进行随机搜索时，主要有两种策略：EXTEND与CONNECT。通过CONNECT策略生成的路径在各自维度内均按直线方向生成，因此无需对其平滑。但通过EXTEND生成的路径每次均朝随机生成的方向进行扩展。这种搜索策略必然很容易导致震荡的产生。为了减轻这种震荡对机器人执行动作时的影响，本方法采用三阶贝塞尔曲线对路径进行平滑。

为方便工程实用，可通过矩阵形式计算贝塞尔曲线平滑后的点值。表述如下：

令

其中MBez为贝塞尔曲线

参数矩阵，u为参数值，即贝塞尔曲线参数形式中的t，P为三阶贝塞尔曲线的四个控制点。构建上述矩阵后，经过贝塞尔曲线滤波的点集可表示为：

p_new＝u·MBez·P

实际处理中的平滑原则为：若为连续的CONNECT点则不进行平滑；一旦出现EXTEND点即开始记录状态点的个数n，所出现的最大值max和最小值min，直到下一次出现两个连续的CONNECT点(只出现一个CONNECT点时其实质仍为EXTEND)为止。矩阵u的规模由n确定，u₁到u_n分别为[0，1]区间(n-1)等分后各端点上的值。P矩阵中的四个控制点分别为第一个EXTEND点之前的CONNECT点，最大值max，最小值min(按其在点集中出现的位置排列)以及连续两个CONNECT点中的前一个。按矩阵形式计算出新的点值p_new后，将其赋回原点集中。

动力学滤波器的方法如下：

设C为机器人质心，是机器人关节角θ的函数，即C＝X(θ)。则质心的雅可比矩阵可以表示为

亦即对于微小量的变化，近似的有ΔC＝J(θ)Δθ(t)。滤波目标为保证质心在地面的投影位置不变，该约束条件可表示为：

$\mathrm{\Δ}{C}_x=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{x,i}\mathrm{\Δ}{\mathrm{\θ}}_i\left(t\right)=0,$ $\mathrm{\Δ}{C}_y=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{y,i}\mathrm{\Δ}{\mathrm{\θ}}_i\left(t\right)=0$

其中Δθ_i＝Δθ_cmdi+Δθ_compi，三个变量分别表示实际角度变化值，输入命令角度变化值(动作路径以增量形式输入)和滤波补偿值。

为了保证对原始姿态的动作改变最小，可设计一个评价函数

将问题转化为一个最优化问题，其表述如下：

求取

约束条件ΔC_x＝0与ΔC_y＝0。

在双脚支撑模式下雅可比矩阵的计算也略有不同。前文已述，双足支撑时的处理仍认为右腿为支撑腿，在确定右腿的状态后根据逆运动学计算左腿的位姿。由于左腿处于从属关系，因此计算雅可比矩阵时应认为其对各连杆质心位置的影响为0。但左腿位置的改变实际上影响了左腿各连杆质量的质心位置，因此需要对其作一定特殊处理。通过分析不难发现，由于左右脚掌的相对位置和转向始终是固定的，在机器人可达到的姿态限制下左右腿的相对位置实际上也是始终固定的(尽管关节转向可能不同)，因而右腿关节的转动对左右腿连杆质心位置带来的影响可以认为是完全相同的。因此在计算右腿关节对右腿连杆质心的影响时将其加倍，即可将左腿连杆质心位置的变化包含在内。

在计算出每一连杆质心位置的雅可比矩阵后，对其按各连杆质量进行加权平均，即可得机器人质心的雅可比矩阵。公式为：

Claims (4)

1.一种基于快速搜索树的人型机器人踢球动作信息处理方法，其特征在于，包括以下步骤：

1）机器人在不确定环境下根据关节位置传感器、加速度传感器和陀螺仪采集的信息，并通过正向运动模型得到机器人当前状态；

2）机器人根据当前状态和踢球的终止状态，通过快速随机搜索树计算出所需的机器人各关节运动轨迹；

3）通过动作平滑滤波器对步骤2）中生成的关节运动轨迹进行平滑处理，并通过动力学滤波器进行修正得到稳定的最终踢球运动轨迹，并将该轨迹发送给机器人执行机构；

所述的动力学滤波器通过以下步骤来实现关节运动轨迹的修正处理：

设C为机器人质心，是机器人关节角θ的函数，即C＝X(θ)，则质心的雅可比矩阵表示为

亦即对于微小量的变化，近似的有ΔC＝J(θ)Δθ(t)，滤波目标为保证质心在地面的投影位置不变，约束条件可表示为：

$\mathrm{\Δ}{C}_x=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{x,i}{\mathrm{\Δ\θ}}_i\left(t\right)=0,$ $\mathrm{\Δ}{C}_y=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{J}_{y,i}{\mathrm{\Δ\θ}}_i\left(t\right)=0$

其中Δθ_i＝Δθ_cmdi+Δθ_compi，Δθ_i表示实际角度变化值，Δθ_cmdi表示输入命令角度变化值，Δθ_compi表示滤波补偿值；

为了保证对原始姿态的动作改变最小，设计一个评价函数

将问题转化为一个最优化问题，其表述如下：

求取 $\mathrm{MIN}(E\left({\mathrm{\Δ\θ}}_{\mathrm{compi}}\right)=\underset{i=1}{\overset{22}{\mathrm{\Σ}}}{\mathrm{\ω}}_i{\left({\mathrm{\Δ\θ}}_{\mathrm{compi}}\right)}^2),$ 约束条件ΔC_x＝0与ΔC_y＝0。

2.根据权利要求1所述的一种基于快速搜索树的人型机器人踢球动作信息处理方法，其特征在于，所述的步骤1）中的通过正向运动模型得到机器人当前状态的具体步骤如下：

1)利用加速度传感器的静态特性和陀螺仪的动态特性，通过补偿滤波器获得机器人身体的全局旋转矩阵R_bw；

2)利用正向运动模型由身体开始向各子连杆递归计算各子连杆全局位置和旋转矩阵：

p_j＝p_i+R_ib_j

^R _j＝R_iR(q_j)

其中p_j为待求第j连杆全局位置，第i连杆为第j连杆的母连杆，R_i为第i连杆的全局旋转矩阵，b_j为第j连杆关节在其母连杆坐标系下的位置向量;R_j为待求第j连杆的全局旋转矩阵，R(q_j)是由第j连杆关节旋转q_j所造成的旋转效应。

3.根据权利要求1所述的一种基于快速搜索树的人型机器人踢球动作信息处理方法，其特征在于，所述的步骤2）通过快速随机搜索树计算出所需的机器人各关节运动轨迹的具体步骤如下：

1)根据踢球动作的起始位置初始化搜索树算法的起始状态；

2)在满足无连杆碰撞和所设定支撑模式的约束条件下，使用RRT-Extend方法随机生成扩展状态并同时采用RRT-Connect生成连接状态；

3)重复步骤2），直到由起始状态和终止状态生成的两棵搜索树实现连接为止。

4.根据权利要求1所述的一种基于快速搜索树的人型机器人踢球动作信息处理方法，其特征在于，所述的动作平滑滤波器通过以下步骤来实现关节运动轨迹的平滑处理：

1）采用计算机图形学中的三阶贝塞尔曲线进行平滑处理，减小关节值的震荡；

2）采用直线切割的方法去除不必要的关节运动。

Images