[发明专利]基于双平衡控制机制的仿人足球机器人全向踢球方法

权利要求书

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

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

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

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

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

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

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

                                                      （1）

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

          （2）

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

                                                           （3）

选取式（3）中踢球代价值KickCost为最小值时的踢球点得到最小代价踢球点。

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

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

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

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

在的n个时刻有n个插值点，三次样条插值函数在每个区间是一个三次多项式，而且三次样条插值函数的一阶导数和二阶导数在区间是连续的；

令，则为：        （4）

在式（4）中，未知量由下列方程式（5）的解得出：

              （5）

其中，；

由于初始状态，终止状态，可以得到下列等式：

                                              （6）

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