CN103217903B - 基于双平衡控制机制的仿人足球机器人全向踢球方法 - Google Patents

Abstract

本发明提供一种基于双平衡控制机制的仿人足球机器人全向踢球方法，根据距离代价值和角度代价值确定具有最小代价的踢球点，并控制机器人行走至该踢球点；机器人身体重心的转移，同时利用三次样条插值法进行足部空间轨迹的规划，即可获得机器人每个时刻足部运动的参考位姿；由上述得出的参考位姿利用逆运动学知识计算出机器人各个关节的角度；采用基于质心和陀螺仪反馈的双平衡机制维持机器人在踢球过程中稳定性。本发明一种基于双平衡控制机制的仿人足球机器人全向踢球方法，在仿人机器人踢球过程中，利用质心和陀螺仪反馈双平衡机制实现机器人的全向踢球。相比现有技术，本发明的机器人踢球方法具有稳定性好、执行效率高的优点。

Description

基于双平衡控制机制的仿人足球机器人全向踢球方法

技术领域

本发明属于仿人机器人踢球技术领域，具体涉及一种基于双平衡机制的仿人足球机器人全向踢球方法。

背景技术

仿人足球机器人是仿人机器人技术研究的分支，与一般的仿人机器人相比，仿人足球机器人具有体积小、质量轻、运动灵活等特点。另外由于仿人足球机器人的竞技娱乐性，使得其更容易走进人类的生活。因此仿人足球机器人已经成为烦人机器人技术研究的热点之一。

1996年，RoboCup国际联合会成立，并在日本举行了表演赛。2002年的RoboCup比赛中启动了类人组的比赛，比赛要求仿人足球机器人反应迅速，能进行较为激烈的对抗，因此参赛队伍要综合考虑其仿人机器人的运动能力、平衡等多方面的技术。

在仿人机器人踢球方面，也有其它一些方法，如RuiFerreira的仿人机器人全向踢球的方法，采用贝塞尔曲线规划足部运动轨迹，并结合基于质心的稳定性控制模块，该方法当修改支撑腿的关节位置以更好的维稳时，会同时改变游脚的位置即踢球的脚的运动轨迹会出现偏差；PatrickMacAlpine的踢球引擎执行机制，建立踢球技能文件以动态规划不同踢球方式下的足部运动轨迹，根据足球的位置确定固定的踢球方向。该方法使用CMA-ES优化足部运动的轨迹点，以控制踢球的距离和速度等重要的参数。

由于上述方法都是研究基于特定模块的踢球运动，而在动态对抗和时间因素等方面有一定地缺陷，即无法快速、准确而又稳定的完成踢球动作。上述问题是在仿人足球机器人的踢球过程中应当予以考虑并解决的问题。

发明内容

本发明的目的是提供一种基于双平衡机制的仿人足球机器人全向踢球方法解决现有技术中存在的基于特定模块踢球运动，而在动态对抗和有限的时间内应用效率并不高，即抗外界干扰性不高，无法快速、准确而又稳定的完成踢球动作的问题。

本发明的技术解决方案是：

一种基于双平衡控制机制的仿人足球机器人全向踢球方法，包括踢球点选择机制、足部空间轨迹规划、关节角度计算和双平衡控制机制，

步骤一，踢球点选择机制：以球为圆心、目标偏移TOffsetP为半径确定圆，该圆周上面的点均为可行的踢球点K1、K2……Kn，对其中若干个踢球点的踢球代价值KickCost进行分析；选取踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点，并控制机器人A行走至该最小代价踢球点；

步骤二，足部空间轨迹规划：由机器人A身体重心的转移，同时利用三次样条插值法进行足部三维空间轨迹的规划，以获得机器人A每个时刻足部运动的参考位姿；

步骤三，关节角度计算：由步骤二得出机器人A每个时刻足部运动的参考位姿利用逆运动学知识计算出机器人A的膝盖处的关节角、踝关节的滚动角和俯仰角，以及股关节的偏摆角、滚动角和俯仰角；

步骤四，双平衡控制机制：包括机器人A身体的倾斜角度的控制和机器人A髋关节的滚动和俯仰角速度变化的控制，采用基于质心平衡的方法，维持机器人A身体在X-Y平面上的稳定同时控制机器人A身体的倾斜角度；采用陀螺仪反馈平衡机制对机器人A髋关节的滚动和俯仰角速度变化的控制。

进一步改进在于：所述步骤一的踢球点选择机制中最小代价踢球点的选取包括步骤1a和步骤1b：

步骤1a，踢球点选择机制：以球为圆心、目标偏移TOffsetP为半径确定圆，该圆周上面的点均为可行的踢球点K1、K2……Kn，对其中若干个踢球点的踢球代价值KickCost进行分析；机器人A的当前位置AgentP为机器人A相对于球而言的二维坐标值，目标偏移位置TOffsetP为机器人A的踢球点K1相对于球而言的偏移量，当前位置AgentP与目标偏移位置TOffsetP之差的绝对值除以标准值m得到距离代价值DistCost：

DistCost＝|AgentP-TOffsetP|/m(1)

步骤1b，计算机器人A的转身代价值TurnCost，机器人A的当前朝向与球形成的夹角α，踢球点K1的朝向与球形成的夹角β，夹角α与夹角β之差的绝对值除以360°即是转身代价值TurnCost：

TurnCost＝|AgentOrientation-TargetOrientation|/360°＝|α-β|/360°(2)

步骤1c，距离代价值DistCost和转身代价值TurnCost之和为机器人A在踢球点的踢球代价值KickCost：

KickCost＝DistCost+TurnCost(3)选取式(3)中踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点。

进一步改进在于：所述步骤二的足部空间轨迹规划包括步骤2a和步骤2b：

步骤2a，根据机器人A游脚的运动情况能够将机器人A的一个完整地踢球动作划分为后抬腿、摆动和复位三个阶段，即得到后抬腿、摆动和复位三条运动曲线；

步骤2b，由步骤2a中的后抬腿、摆动和复位三条运动曲线分别利用三次样条插值法保证插值得出低阶分段且光滑的曲线函数，得出机器人A的踝关节每个时刻的参考位姿，以实现对足部踢球运动的控制。所述步骤2b中机器人A的踝关节每个时刻的参考位姿的算法为：

在t₁＜t₂＜…＜t_n的n个时刻有n个插值点S(t_j)＝p_j，j＝1,2,…n，三次样条插值函数S(t)在每个(t_j，t_j+1)区间是一个三次多项式，而且三次样条插值函数S(t)的一阶导数S′(t)和二阶导数S″(t)在区间(t₁，t_n)是连续的；

令I_j＝(t_j，t_j+1)，T_j＝t_j+1-t_j，则S(t)为：

$S\left(t\right)=\frac{G_j}{6T_j}{(t_{j+1}-t)}^3+\frac{G_{j+1}}{6T_j}{(t-t_j)}^3+(p_j-\frac{G_j{T}_j^2}{6})\frac{t_{j+1}-t}{T_j}+(p_{j+1}-\frac{G_j+T_j^2}{6})\frac{t-t_j}{T_j}---\left(4\right)$

在式(4)中，未知量G_j由下列方程式(5)的解得出：

$\left\{\begin{array}{c}2G_j+b_1{G}_2=d_1\\ \frac{T_{j-1}}{6}{G}_{j-1}+\frac{T_j+T_{j-1}}{3}{G}_j+\frac{T_j}{6}{G}_{j+1}=\frac{p_{j+1}-p_j}{T_j}-\frac{p_j-p_{j-1}}{T_{j-1}}\\ a_n{G}_{n-1}+2G_n=d_n\end{array}\right.,j=2,3,...n-1---\left(5\right)$

其中， $a_j=\frac{p_{j-1}}{T_j+T_{j-1}},b_j=1-a_j,c_j=\frac{p_{j+1}-p_j}{T_j},d_j=\frac{6(c_j-c_{j-1})}{T_j+T_{j-1}},j=2,3,...n-1;$ 由于初始状态S′(t₁)＝0，终止状态S″(t_n)＝0，可以得到下列等式：

$\left\{\begin{array}{c}{a}_n=b_1=1,\\ d_1=\frac{6(p_2-p_1)}{T_j},d_n=-\frac{6(p_n-p_{n-1})}{T_{n-1}}\end{array}\right.---\left(6\right)$

从而解出函数S(t)，函数S(t)是一条连续平滑的曲线即是足部踝关节在三维空间的运动轨迹，从而能够得出踝关节每个时刻的参考位姿。

进一步改进在于：所述步骤四的双平衡控制机制包括步骤4a和步骤4b：

步骤4a，机器人A身体的倾斜角度的控制：利用机器人A质心COM的投影必须在支撑多边形内部准则来维持自身稳定性；如果机器人A质心COM的投影不在支撑多边形内，将打开与支撑腿同侧的手臂直到机器人A质心COM的投影在支撑多边形内为止；利用PID控制机器人A身体在X、Y平面上的平衡性，对于机器人A整个身体在三维空间中稳定性，采用毕达哥拉斯定理控制踝关节的旋转角度；

步骤4b，机器人A髋关节的滚动和俯仰角速度变化的控制：利用陀螺仪反馈控制，使用指数平滑迭代法弥补陀螺仪传感器信息的延迟，实现对机器人A髋关节的滚动和俯仰角速度变化的精确控制。

进一步改进在于：所述步骤4a的机器人A身体的倾斜角度的控制的具体算法为：控制器输入的误差值是测量的质心位置值COM_mea与理想的质心位置值COM_des之间的差值，即err＝COM_mea-COM_des，其中理想的质心位置值COM_des是通过未来n个时刻支撑脚的位置值的调和平均数计算得出的，测量的质心位置值COM_mea为：

${\mathrm{COM}}_{mea}=\left(\underset{i=0}{\overset{n}{\Σ}}{\mathrm{part}}_i\×m_i\right)/m_{total}---\left(7\right)$

其中，part_i是机器人A身体中除去腿部的各个部分相对于自身局部坐标系的位置，m_i是机器人A每个连杆部分的质量，m_total是机器人A的总质量；

机器人A身体分别在X、Y平面上的平衡性和

${\mathrm{Balance}}_{x_t}={\mathrm{Kp}}_x\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_y\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(8\right)$

${\mathrm{Balance}}_{y_t}={\mathrm{Kp}}_y\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_x\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(9\right)$

其中，Kp_x和Kp_y是比例系数，Ki是积分系数，Kd_x、Kd_y是微分系数，将机器人A质心COM的误差值err分为x和y平面考虑，但由于未考虑机器人A质心的高度COM_z，故无法控制机器人A身体的倾斜角度；再使用毕达哥拉斯定理实现机器人A踝关节的旋转控制，实现对机器人A身体倾斜角度的控制，其中倾斜角度

${\mathrm{\θ}}_{x_t}=atan2({\mathrm{Balance}}_{x_t},{\mathrm{COM}}_{z_t})---\left(10\right)$

${\mathrm{\θ}}_{y_t}=atan2({\mathrm{Balance}}_{y_t},{\mathrm{COM}}_{z_t})---\left(11\right).$

进一步改进在于：所述步骤4b的机器人A髋关节的滚动和俯仰角速度变化的控制的具体算法为：利用髋关节的滚动角θ_HipRoll和俯仰角θ_HipPitch得出角度变化的目标速度，即目标角速度的计算如下所示：

${\mathrm{Des}}_{x_t}=({\mathrm{\θ}}_{{\mathrm{HipRoll}}_t}-{\mathrm{\θ}}_{{\mathrm{HipRoll}}_{t-1}})/\mathrm{\Δ}t---\left(12\right)$

${\mathrm{Des}}_{y_t}=({\mathrm{\θ}}_{{\mathrm{HipPitch}}_t}-{\mathrm{\θ}}_{{\mathrm{HipPitch}}_{t-1}})/\mathrm{\Δ}t---\left(13\right)$

由于获得机器人A的陀螺仪传感器值有一定地延迟，可以使用指数平滑迭代法预测下一时刻可能的角度变化速度：

${\tilde{v}}_t=\mathrm{\α}\×v_t+(1-\mathrm{\α})\×{\tilde{v}}_{t-1}---\left(14\right)$

其中，α是折扣因子，v_t是t时刻的实际值，是t-1时刻的平滑值，求解出的是t时刻的平滑值即对t+1时刻的预测；

髋关节的滚动角的偏移角度髋关节的俯仰角的偏移角度分别计算如下：

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipRoll}}_t}}=Kp\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{x_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{x_t}\right)}{dt}---\left(15\right)$

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipPitch}}_t}}=Kp\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{y_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(16\right)$

其中，Kp是比例系数，Ki是积分系数，Kd是微分系数， 是实际角速度与目标角速度之间的差值；将髋关节的滚动角的偏移角度和髋关节的俯仰角的偏移角度直接纳入髋关节的滚动和俯仰角的逆运动学计算中以维持平衡。

本发明提供一种基于双平衡控制机制的仿人足球机器人全向踢球方法，根据距离代价值和角度代价值确定具有最小代价的踢球点，并控制机器人行走至该踢球点；机器人身体重心的转移，同时利用三次样条插值法进行足部空间轨迹的规划，即可获得机器人每个时刻足部运动的参考位姿；由上述得出的参考位姿利用逆运动学知识计算出机器人各个关节的角度；采用基于质心和陀螺仪反馈的双平衡机制维持机器人在踢球过程中稳定性。本发明一种基于双平衡控制机制的仿人足球机器人全向踢球方法，在仿人机器人踢球过程中，利用质心和陀螺仪反馈双平衡机制实现机器人的全向踢球。相比现有技术，本发明的机器人踢球方法具有稳定性好、执行效率高的优点。

附图说明

图1是本发明实施例中选择最小代价的踢球点的说明示意图；

图2是本发明实施例一种基于双平衡控制机制的仿人足球机器人全向踢球方法的说明示意图；

具体实施方式

下面结合附图详细说明本发明的优选实施例。

如图2所示，本实施例提供一种基于双平衡控制机制的仿人足球机器人全向踢球方法，包括踢球点选择机制、足部空间轨迹规划、关节角度计算和双平衡控制机制，

步骤一，踢球点选择机制：以球为圆心、目标偏移TOffsetP为半径确定圆，该圆周上面的点均为可行的踢球点K1、K2……Kn，对其中若干个踢球点的踢球代价值KickCost进行分析；选取踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点，并控制机器人A行走至该最小代价踢球点；所述步骤一的踢球点选择机制中最小代价踢球点的选取包括步骤1a和步骤1b：

在步骤1a中，计算机器人A与球的距离代价值DistCost，机器人A的当前位置AgentP为机器人A相对于球而言的二维坐标值，目标偏移位置TOffsetP为机器人A的踢球点K1相对于球而言的偏移量，当前位置AgentP与目标偏移位置TOffsetP之差的绝对值除以标准值m得到距离代价值DistCost：

DistCost＝|AgentP-TOffsetP|/m(1)

在步骤1b中，计算机器人A的转身代价值TurnCost，如图1所示，机器人A的当前朝向与球形成的夹角α，踢球点K1的朝向与球形成的夹角β，夹角α与夹角β之差的绝对值除以360°即是转身代价值TurnCost：

TurnCost＝|AgentOrientation-TargetOrientation|/360°＝|α-β|360°(2)

在步骤1c中，距离代价值DistCost和转身代价值TurnCost之和为机器人A在踢球点的踢球代价值KickCost：

KickCost＝DistCost+TurnCost(3)选取式(3)中踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点。

步骤二，足部空间轨迹规划：由机器人A身体重心的转移，同时利用三次样条插值法进行足部三维空间轨迹的规划，以获得机器人A每个时刻足部运动的参考位姿；所述步骤二的足部空间轨迹规划包括步骤2a和步骤2b：

在步骤2a中，根据机器人A游脚的运动情况能够将机器人A的一个完整地踢球动作划分为后抬腿、摆动和复位三个阶段，即得到后抬腿、摆动和复位三条运动曲线；

在步骤2b中，由步骤2a中的后抬腿、摆动和复位三条运动曲线分别利用三次样条插值法保证插值得出低阶分段且光滑的曲线函数，得出机器人A的踝关节每个时刻的参考位姿，以实现对足部踢球运动的控制。

所述步骤2b中机器人A的踝关节每个时刻的参考位姿的算法为：

在t₁＜t₂＜…＜t_n的n个时刻有n个插值点S(t_j)＝p_j，j＝1,2,…n，三次样条插值函数S(t)在每个(t_j，t_j+1)区间是一个三次多项式，而且三次样条插值函数S(t)的一阶导数S′(t)和二阶导数S″(t)在区间(t₁，t_n)是连续的；

令I_j＝(t_j，t_j+1)，T_j＝t_j+1-t_j，则S(t)为：

$S\left(t\right)=\frac{G_j}{6T_j}{(t_{j+1}-t)}^3+\frac{G_{j+1}}{6T_j}{(t-t_j)}^3+(p_j-\frac{G_j{T}_j^2}{6})\frac{t_{j+1}-t}{T_j}+(p_{j+1}-\frac{G_j+T_j^2}{6})\frac{t-t_j}{T_j}---\left(4\right)$

在式(4)中，未知量G_j由下列方程式(5)的解得出：

$\left\{\begin{array}{c}2G_j+b_1{G}_2=d_1\\ \frac{T_{j-1}}{6}{G}_{j-1}+\frac{T_j+T_{j-1}}{3}{G}_j+\frac{T_j}{6}{G}_{j+1}=\frac{p_{j+1}-p_j}{T_j}-\frac{p_j-p_{j-1}}{T_{j-1}}\\ a_n{G}_{n-1}+2G_n=d_n\end{array}\right.,j=2,3,...n-1---\left(5\right)$

其中， $a_j=\frac{p_{j-1}}{T_j+T_{j-1}},b_j=1-a_j,c_j=\frac{p_{j+1}-p_j}{T_j},d_j=\frac{6(c_j-c_{j-1})}{T_j+T_{j-1}},j=2,3,...n-1;$ 由于初始状态S′(t₁)＝0，终止状态S″(t_n)＝0，可以得到下列等式：

$\left\{\begin{array}{c}{a}_n=b_1=1,\\ d_1=\frac{6(p_2-p_1)}{T_j},d_n=-\frac{6(p_n-p_{n-1})}{T_{n-1}}\end{array}\right.---\left(6\right)$

从而解出函数S(t)，函数S(t)是一条连续平滑的曲线即是足部踝关节在三维空间的运动轨迹，从而能够得出踝关节每个时刻的参考位姿。

步骤三，关节角度计算：由步骤二得出机器人A每个时刻足部运动的参考位姿利用逆运动学知识计算出机器人A的膝盖处的关节角、踝关节的滚动角和俯仰角，以及股关节的偏摆角、滚动角和俯仰角；

步骤四，双平衡控制机制：包括机器人A身体的倾斜角度的控制和机器人A髋关节的滚动和俯仰角速度变化的控制，采用基于质心平衡的方法，维持机器人A身体在的X-Y平面上的稳定同时控制机器人A身体的倾斜角度；采用陀螺仪反馈平衡机制对机器人A髋关节的滚动和俯仰角速度变化的控制。所述步骤四的双平衡控制机制包括步骤4a和步骤4b：

在步骤4a中，机器人A身体的倾斜角度的控制：利用机器人A质心COM的投影必须在支撑多边形内部准则来维持自身稳定性；如果机器人A质心COM的投影不在支撑多边形内，将打开与支撑腿同侧的手臂直到机器人A质心COM的投影在支撑多边形内为止；利用PID控制机器人A身体在X、Y平面上的平衡性，对于机器人A整个身体在三维空间中稳定性，采用毕达哥拉斯定理控制踝关节的旋转角度；所述步骤4a的机器人A身体的倾斜角度的控制的具体算法为：

控制器输入的误差值是测量的质心位置值COM_mea与理想的质心位置值COM_des之间的差值，即err＝COM_mea-COM_des，其中理想的质心位置值COM_des是通过未来n个时刻支撑脚的位置值的调和平均数计算得出的，测量的质心位置值COM_mea为：

${\mathrm{COM}}_{mea}=(\underset{i=0}{\overset{n}{\Σ}}{\mathrm{part}}_i\×m_i)/m_{total}---\left(7\right)$

其中，part_i是机器人A身体中除去腿部的各个部分相对于自身局部坐标系的位置，m_i是机器人A每个连杆部分的质量，m_total是机器人A的总质量；

机器人A身体分别在X、Y平面上的平衡性和

${\mathrm{Balance}}_{x_t}={\mathrm{Kp}}_x\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_y\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(8\right)$

${\mathrm{Balance}}_{y_t}={\mathrm{Kp}}_y\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_x\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(9\right)$

其中，Kp_x和Kp_y是比例系数，Ki是积分系数，Kd_x、Kd_y是微分系数，将机器人A质心COM的误差值err分为x和y平面考虑，但由于未考虑机器人A质心的高度COM_z，故无法控制机器人A身体的倾斜角度；再使用毕达哥拉斯定理实现机器人A踝关节的旋转控制，实现对机器人A身体倾斜角度的控制，其中倾斜角度

${\mathrm{\θ}}_{x_t}=atan2({\mathrm{Balance}}_{x_t},{\mathrm{COM}}_{z_t})---\left(10\right)$

${\mathrm{\θ}}_{y_t}=atan2({\mathrm{Balance}}_{y_t},{\mathrm{COM}}_{z_t})---\left(11\right)$

在步骤4b中，机器人A髋关节的滚动和俯仰角速度变化的控制：利用陀螺仪反馈控制，使用指数平滑迭代法弥补陀螺仪传感器信息的延迟，实现对机器人A髋关节的滚动和俯仰角速度变化的精确控制。所述步骤4b的机器人A髋关节的滚动和俯仰角速度变化的控制的具体算法为：

利用髋关节的滚动角θ_HipRoll和俯仰角θ_HipPitch得出角度变化的目标速度，即目标角速度的计算如下所示：

${\mathrm{Des}}_{x_t}=({\mathrm{\θ}}_{{\mathrm{HipRoll}}_t}-{\mathrm{\θ}}_{{\mathrm{HipRoll}}_{t-1}})/\mathrm{\Δ}t---\left(12\right)$

${\mathrm{Des}}_{y_t}=({\mathrm{\θ}}_{{\mathrm{HipPitch}}_t}-{\mathrm{\θ}}_{{\mathrm{HipPitch}}_{t-1}})/\mathrm{\Δ}t---\left(13\right)$

由于获得机器人A的陀螺仪传感器值有一定地延迟，可以使用指数平滑迭代法预测下一时刻可能的角度变化速度：

${\tilde{v}}_t=\mathrm{\α}\×v_t+(1-\mathrm{\α})\×{\tilde{v}}_{t-1}---\left(14\right)$

其中，α是折扣因子，v_t是t时刻的实际值，是t-1时刻的平滑值，求解出的是t时刻的平滑值即对t+1时刻的预测；

髋关节的滚动角的偏移角度髋关节的俯仰角的偏移角度分别计算如下：

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipRoll}}_t}}=Kp\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{x_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{x_t}\right)}{dt}---\left(15\right)$

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipPitch}}_t}}=Kp\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{y_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(16\right)$

其中，Kp是比例系数，Ki是积分系数，Kd是微分系数， 是实际角速度与目标角速度之间的差值；将髋关节的滚动角的偏移角度和髋关节的俯仰角的偏移角度直接纳入髋关节的滚动和俯仰角的逆运动学计算中以维持平衡。

本实施例一种基于双平衡控制机制的仿人足球机器人全向踢球方法，根据距离代价值和角度代价值确定具有最小代价的踢球点，并控制机器人行走至该踢球点；机器人身体重心的转移，同时利用三次样条插值法进行足部空间轨迹的规划，即可获得机器人每个时刻足部运动的参考位姿；由上述得出的参考位姿利用逆运动学知识计算出机器人各个关节的角度；采用基于质心和陀螺仪反馈的双平衡机制维持机器人在踢球过程中稳定性。本发明一种基于双平衡控制机制的仿人足球机器人全向踢球方法，在仿人机器人踢球过程中，利用质心和陀螺仪反馈双平衡机制实现机器人的全向踢球。相比现有技术，本发明的机器人踢球方法具有稳定性好、执行效率高的优点。

Claims (6)

1.一种基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于：包括踢球点选择机制、足部空间轨迹规划、关节角度计算和双平衡控制机制，

步骤一，踢球点选择机制：以球为圆心、目标偏移TOffsetP为半径确定圆，该圆周上面的点均为可行的踢球点K1、K2……Kn，对其中若干个踢球点的踢球代价值KickCost进行分析；选取踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点，并控制机器人A行走至该最小代价踢球点；

步骤二，足部空间轨迹规划：由机器人A身体重心的转移，同时利用三次样条插值法进行足部三维空间轨迹的规划，以获得机器人A每个时刻足部运动的参考位姿；

步骤三，关节角度计算：由步骤二得出机器人A每个时刻足部运动的参考位姿利用逆运动学知识计算出机器人A的膝盖处的关节角、踝关节的滚动角和俯仰角，以及股关节的偏摆角、滚动角和俯仰角；

步骤四，双平衡控制机制：包括机器人A身体的倾斜角度的控制和机器人A髋关节的滚动和俯仰角速度变化的控制，采用基于质心平衡的方法，维持机器人A身体在X-Y平面上的稳定同时控制机器人A身体的倾斜角度；采用陀螺仪反馈平衡机制对机器人A髋关节的滚动和俯仰角速度变化的控制；

所述步骤四的双平衡控制机制包括以下步骤：

步骤4a，机器人A身体的倾斜角度的控制：利用机器人A质心COM的投影必须在支撑多边形内部准则来维持自身稳定性；如果机器人A质心COM的投影不在支撑多边形内，将打开与支撑腿同侧的手臂直到机器人A质心COM的投影在支撑多边形内为止；利用PID控制机器人A身体在X、Y平面上的平衡性，对于机器人A整个身体在三维空间中稳定性，采用毕达哥拉斯定理控制踝关节的旋转角度；

步骤4b，机器人A髋关节的滚动和俯仰角速度变化的控制：利用陀螺仪反馈控制，使用指数平滑迭代法弥补陀螺仪传感器信息的延迟，实现对机器人A髋关节的滚动和俯仰角速度变化的精确控制。

2.如权利要求1所述的基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于，所述步骤一的踢球点选择机制中最小代价踢球点的选取包括以下步骤：

步骤1a，踢球点选择机制：以球为圆心、目标偏移TOffsetP为半径确定圆，该圆周上面的点均为可行的踢球点K1、K2……Kn，对其中若干个踢球点的踢球代价值KickCost进行分析；机器人A的当前位置AgentP为机器人A相对于球而言的二维坐标值，目标偏移位置TOffsetP为机器人A的踢球点K1相对于球而言的偏移量，当前位置AgentP与目标偏移位置TOffsetP之差的绝对值除以标准值m得到距离代价值DistCost：

DistCost＝|AgentP-TOffsetP|/m(1)

步骤1b，计算机器人A的转身代价值TurnCost，机器人A的当前朝向与球形成有夹角α，踢球点K1的朝向与球形成的夹角β，夹角α与夹角β之差的绝对值除以360°即是转身代价值TurnCost：

TurnCost＝|AgentOrientation-TargetOrientation|/360°＝|α-β|/360°(2)

步骤1c，距离代价值DistCost和转身代价值TurnCost之和为机器人A在踢球点的踢球代价值KickCost：

KickCost＝DistCost+TurnCost(3)选取式(3)中踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点。

3.如权利要求1所述的基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于，所述步骤二的足部空间轨迹规划包括以下步骤：

步骤2a，根据机器人A游脚的运动情况能够将机器人A的一个完整地踢球动作划分为后抬腿、摆动和复位三个阶段，即得到后抬腿、摆动和复位三条运动曲线；

步骤2b，由步骤2a中的后抬腿、摆动和复位三条运动曲线分别利用三次样条插值法保证插值得出低阶分段且光滑的曲线函数，得出机器人A的踝关节每个时刻的参考位姿，以实现对足部踢球运动的控制。

4.如权利要求3所述的基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于，所述步骤2b中机器人A的踝关节每个时刻的参考位姿的算法为：

在t₁＜t₂＜…＜t_n的n个时刻有n个插值点S(t_j)＝p_j，j＝1,2,…n，三次样条插值函数S(t)在每个(t_j，t_j+1)区间是一个三次多项式，而且三次样条插值函数S(t)的一阶导数S′(t)和二阶导数S″(t)在区间(t₁，t_n)是连续的；

令I_j＝(t_j，t_j+1)，T_j＝t_j+1-t_j，则S(t)为：

$S\left(t\right)=\frac{G_j}{6T_j}{\left(t_{j+1}-t\right)}^3+\frac{G_{j+1}}{6T_j}{\left(t-t_j\right)}^3+\left(p_j-\frac{G_j{T}_j^2}{6}\right)\frac{t_{j+1}-t}{T_j}+\left(p_{j+1}-\frac{G_j+T_j^2}{6}\right)\frac{t-t_i}{T_j}---\left(4\right)$

在式(4)中，未知量G_j由下列方程式(5)的解得出：

$\left\{\begin{array}{c}2G_j+b_1{G}_2=d_1\\ \frac{T_{j-1}}{6}{G}_{j-1}+\frac{T_j+T_{j-1}}{3}{G}_j+\frac{T_j}{6}{G}_{j+1}=\frac{p_{j+1}-p_j}{T_j}-\frac{p_j-p_{j-1}}{T_{j-1}}\\ a_n{G}_{n-1}+2G_n=d_n\end{array}\right.,j=2,3,...n-1---\left(5\right)$

其中，b_j＝1-a_j， $c_j=\frac{p_{j+1}-p_j}{T_j},d_j=\frac{6(c_j-c_{j-1})}{T_j+T_{j-1}},$ j＝2,3，…n-1；

由于初始状态S′(t₁)＝0，终止状态S″(t_n)＝0，可以得到下列等式：

$\left\{\begin{array}{c}{a}_n=b_1=1,\\ d_1=\frac{6(p_2-p_1)}{T_j},d_n=-\frac{6(p_n-p_{n-1})}{T_{n-1}}\end{array}\right.---\left(6\right)$

从而解出函数S(t)，函数S(t)是一条连续平滑的曲线即是足部踝关节在三维空间的运动轨迹，从而能够得出踝关节每个时刻的参考位姿。

5.如权利要求1所述的基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于，所述步骤4a的机器人A身体的倾斜角度的控制的具体算法为：

控制器输入的误差值是测量的质心位置值COM_mea与理想的质心位置值COM_des之间的差值，即err＝COM_mea-COM_des，其中理想的质心位置值COM_des是通过未来n个时刻支撑脚的位置值的调和平均数计算得出的，测量的质心位置值COM_mea为：

${\mathrm{COM}}_{mea}=(\underset{i=0}{\overset{n}{\Σ}}{\mathrm{part}}_i\×m_i)/m_{total}---\left(7\right)$

其中，part_i是机器人A身体中除去腿部的各个部分相对于自身局部坐标系的位置，m_i是机器人A每个连杆部分的质量，m_total是机器人A的总质量；

机器人A身体分别在X、Y平面上的平衡性和

${\mathrm{Balance}}_{x_t}={\mathrm{Kp}}_x\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_y\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(8\right)$

${\mathrm{Balance}}_{y_t}={\mathrm{Kp}}_y\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{\mathrm{\τ}}d\mathrm{\τ}+{\mathrm{Kd}}_x\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(9\right)$

其中，Kp_x和Kp_y是比例系数，Ki是积分系数，Kd_x、Kd_y是微分系数，将机器人A质心COM的误差值err分为x和y平面考虑，但由于未考虑机器人A质心的高度COM_z，故无法控制机器人A身体的倾斜角度；再使用毕达哥拉斯定理实现机器人A踝关节的旋转控制，实现对机器人A身体倾斜角度的控制，其中倾斜角度

${\mathrm{\θ}}_{x_t}=a\tan 2({\mathrm{Balance}}_{x_t},{\mathrm{COM}}_{z_t})---\left(10\right)$

${\mathrm{\θ}}_{y_t}=atan2({\mathrm{Balance}}_{y_t},{\mathrm{COM}}_{z_t})---\left(11\right).$

6.如权利要求1所述的基于双平衡控制机制的仿人足球机器人全向踢球方法，其特征在于，所述步骤4b的机器人A髋关节的滚动和俯仰角速度变化的控制的具体算法为：

利用髋关节的滚动角θ_HipRoll和俯仰角θ_HipPitch得出角度变化的目标速度，即目标角速度的计算如下所示：

${\mathrm{Des}}_{x_t}=({\mathrm{\θ}}_{{\mathrm{HipRoll}}_t}-{\mathrm{\θ}}_{{\mathrm{HipRoll}}_{t-1}})/\mathrm{\Δ}t---\left(12\right)$

${\mathrm{Des}}_{y_t}=({\mathrm{\θ}}_{{\mathrm{HipPitch}}_t}-{\mathrm{\θ}}_{{\mathrm{HipPitch}}_{t-1}})/\mathrm{\Δ}t---\left(13\right)$

由于获得机器人A的陀螺仪传感器值有一定地延迟，使用指数平滑迭代法预测下一时刻可能的角度变化速度：

${\tilde{v}}_t=\mathrm{\α}\×v_t+(1-\mathrm{\α})\×{\tilde{v}}_{t-1}---\left(14\right)$

其中，α是折扣因子，v_t是t时刻的实际值，是t-1时刻的平滑值，求解出的是t时刻的平滑值即对t+1时刻的预测；

髋关节的滚动角的偏移角度髋关节的俯仰角的偏移角度分别计算如下：

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipRoll}}_t}}=Kp\×{\mathrm{err}}_{x_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{x_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{x_t}\right)}{dt}---\left(15\right)$

${\mathrm{\θ}}_{{\mathrm{Offset}}_{{\mathrm{HipPitch}}_t}}=Kp\×{\mathrm{err}}_{y_t}+Ki\×{\∫}_0^t{\mathrm{err}}_{y_{\mathrm{\τ}}}d\mathrm{\τ}+Kd\×\frac{d\left({\mathrm{err}}_{y_t}\right)}{dt}---\left(16\right)$

其中，Kp是比例系数，Ki是积分系数，Kd是微分系数， 是实际角速度与目标角速度之间的差值；将髋关节的滚动角的偏移角度和髋关节的俯仰角的偏移角度直接纳入髋关节的滚动和俯仰角的逆运动学计算中以维持平衡。