一种基于贝塞尔曲线插值的仿人足球机器人踢球方法

摘要

本发明涉及一种基于贝塞尔曲线插值的仿人足球机器人踢球方法，基于轨迹规划与逆运动学相互协作技术，采用全新设计架构逻辑关系，基于所设计高精准度踢球动作，获得的精准足部运动轨迹的同时，利用逆运动学技术，获得仿人足球机器人踢球腿在进行踢球动作时，腿部关节的角度变化过程，并以此过程作为控制指令，实现针对仿人足球机器人踢球腿的控制，以实现高精准度的踢球动作，并同时获得更优的仿人足球机器人自身稳定性。

说明书

技术领域

本发明涉及一种基于贝塞尔曲线插值的仿人足球机器人踢球方法，属于仿人足球机器人技术领域。

背景技术

仿人机器人的踢球设计主要包括运动规划和机器人自身稳定性的研究，运动规划中的轨迹规划是为了找到从起点到目标点的可行且无碰撞的最优路径，实现高效踢球。由于三维空间中双足机器人对运动加速度的变化很敏感，运动状态的改变极易影响自身的平衡，引起机器人的倒地，大幅降低队伍的进攻效率；所以踢球过程中如何找出最优的踢球路径是一个难点。

发明内容

本发明所要解决的技术问题是提供一种基于轨迹规划与逆运动学相互协作技术，精确获得足部运动轨迹，在实现高精准度踢球动作的同时，能够有效提高仿人足球机器人自身稳定性的基于贝塞尔曲线插值的仿人足球机器人踢球方法。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种基于贝塞尔曲线插值的仿人足球机器人踢球方法，实现单个仿人足球机器人针对其视野范围内足球的踢球动作，其特征在于，包括如下步骤：

步骤A.获得以足球所在位置为圆心，偏移R为半径的圆环上的各个待测踢球点位置，并获得仿人足球机器人所在位置分别相对各个待测踢球点位置的踢球代价值C_Kick，进而获得最小踢球代价值所对应的待测踢球点位置，作为最佳踢球点位置，然后进入步骤B；

步骤B.获得仿人足球机器人踢球腿上足部踝关节对应踢球动作过程中的至少三个三维空间位置，其中，所获各个足部踝关节三维空间位置至少包括仿人足球机器人双腿站立时踢球腿上足部踝关节三维空间位置、后抬腿至最高点时踢球腿上足部踝关节三维空间位置和踢球腿接触足球时踢球腿上足部踝关节三维空间位置，然后进入步骤C；

步骤C.将所获仿人足球机器人各个踢球腿上足部踝关节三维空间位置作为各个插值点，基于贝塞尔曲线，利用样条插值法，获得仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)，然后进入步骤D；

步骤D.根据踢球腿足部踝关节三维空间运动曲线S(t)，获得臀部关节坐标系下足部位姿矩阵并进而获得臀部滚动关节坐标系下足部位姿矩阵然后进入步骤E；

步骤E.根据臀部滚动关节坐标系下足部位姿矩阵获得膝关节偏移角θ_knee、踝关节偏转角θ_anklePitch、踝关节转动角θ_ankleRoll、髋关节俯仰角θ_hipYaw、髋关节滚动角θ_hipRoll和髋关节偏转角θ_hipPitch，然后进入步骤F；

步骤F.控制仿人足球机器人行走至最佳踢球点位置，并根据所获膝关节、踝关节、髋关节的各个角度，控制仿人足球机器人针对足球执行踢球动作。

作为本发明的一种优选技术方案：所述步骤A具体包括如下步骤：

步骤A01.获得以足球所在位置为圆心，偏移R为半径的圆环上的各个待测踢球点位置，并进入步骤A02；

步骤A02.分别针对各个待测踢球点位置，首先获得仿人足球机器人所在位置与待测踢球点位置之间的连线，再获得仿人足球机器人朝向方向与该连线之间的夹角大小，接着根据该夹角大小与2π的比值，获得仿人足球机器人对应该待测踢球点位置的转身代价值，进而获得仿人足球机器人分别对应各个待测踢球点位置的转身代价值C_turn，然后进入步骤A03；

步骤A03.分别针对各个待测踢球点位置，首先获得以仿人足球机器人所在位置为起点，待测踢球点位置为终点的向量，接着根据该向量的模，获得仿人足球机器人对应该待测踢球点位置的距离代价值，进而获得仿人足球机器人分别对应各个待测踢球点位置的距离代价值C_dist，然后进入步骤A04；

步骤A04.根据预设距离代价值权重ω_d和预设转身代价值权重ω_t，分别针对各个待测踢球点位置，按如下公式(3)执行，

C_Kick＝ω_tC_turn+ω_dC_dist(3)

进而获得仿人足球机器人分别对应各个待测踢球点位置的踢球代价值C_Kick，然后进入步骤A05；

步骤A05.针对各个待测踢球点位置，获得最小踢球代价值所对应的待测踢球点位置，作为最佳踢球点位置，然后进入步骤B。

作为本发明的一种优选技术方案：所述步骤C具体包括如下步骤：

步骤C01.将所获仿人足球机器人各个踢球腿上足部踝关节三维空间位置作为各个插值点，获得各个插值点分别对应于踢球动作过程中的各个时间点t₁、…、t_n，其中，n为插值点的个数，然后进入步骤C02；

步骤C02.根据各个插值点分别所对应足部踝关节各个三维空间位置S(t₁)、…、S(t_n)，构建如下公式(5)：

其中，T_j表示t_j至t_j+1之间的时长，t_j∈{t₁、…、t_n}，t_j+1∈{t₁、…、t_n}，G_j与G_j+1通过如下公式(6)、(7)获得；

将公式(6)和公式(7)所表示的G_j与G_j+1代入到公式(5)，并令初态S'(t)＝0，终态S”(t)＝0，如(8)所示，解得a_n、b₁、d₁、d_n；

由此，根据公式(5)，获得仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)。

作为本发明的一种优选技术方案：所述仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)包括仿人足球机器人双腿站立时踢球腿足部踝关节三维空间位置至后抬腿最高点时足部踝关节三维空间位置之间运动曲线，仿人足球机器人踢球腿后抬腿最高点时足部踝关节三维空间位置至踢球腿接触足球时足部踝关节三维空间位置之间运动曲线，以及仿人足球机器人踢球腿接触足球时足部踝关节三维空间位置至双腿站立时足部踝关节三维空间位置之间运动曲线。

作为本发明的一种优选技术方案：所述步骤D包括如下步骤：

步骤D01.根据踢球腿足部踝关节三维空间运动曲线S(t)，获得臀部关节坐标系下足部位姿矩阵然后进入步骤D02；

步骤D02.根据臀部关节坐标系下足部位姿矩阵按如下公式(9)：

获得臀部滚动关节坐标系下足部位姿矩阵然后进入步骤E，其中，l_d为仿人足球机器人两条腿之间的距离，表示沿着x轴旋转表示沿着y轴平移

作为本发明的一种优选技术方案：所述步骤E包括如下步骤：

步骤E01.根据仿人足球机器人大腿长l_thigh、小腿长l_lowerleg、臀部滚动关节坐标系下足部位姿矩阵的平移矢量，以及仿人足球机器人踢球腿后抬腿时大腿根与足部踝关节之间连线长l_trans，按如下公式(10)、(11)和(12)：

θ_ankleRoll＝atan2(y,z)(12)

计算获得膝关节偏移角θ_knee、踝关节偏移角θ_anklePitch、踝关节转动角θ_ankleRoll，然后进入步骤E02，其中，(x,y,z)表示的平移矢量的分量，atan2(y,x)表示点(x,y)与坐标系原点之间连线和x轴之间的夹角；

步骤E02.构建公式(13)

其中，θ_hipYaw表示髋关节俯仰角，θ_hipRoll表示髋关节滚动角，θ_hipPitch表示髋关节偏转角，S_x代表sinθ_hipRoll，S_y代表sinθ_hipPitch，S_z代表sinθ_hipYaw，C_x代表cosθ_hipRoll，C_y代表cosθ_hipPitch，C_z代表cosθ_hipYaw，用式(14)(15)确定：

根据公式(14)和公式(15)，针对(13)求解，获得髋关节俯仰角θ_hipYaw、髋关节滚动角θ_hipRoll和髋关节偏转角θ_hipPitch。

本发明所述一种基于贝塞尔曲线插值的仿人足球机器人踢球方法采用以上技术方案与现有技术相比，具有以下技术效果：本发明所设计基于贝塞尔曲线插值的仿人足球机器人踢球方法，基于轨迹规划与逆运动学相互协作技术，采用全新设计架构逻辑关系，基于所设计高精准度踢球动作，获得的精准足部运动轨迹的同时，利用逆运动学技术，获得仿人足球机器人踢球腿在进行踢球动作时，腿部关节的角度变化过程，并以此过程作为控制指令，实现针对仿人足球机器人踢球腿的控制，以实现高精准度的踢球动作，并同时获得更优的仿人足球机器人自身稳定性。

附图说明

图1是本发明设计基于贝塞尔曲线插值的仿人足球机器人踢球方法的流程示意图；

图2是本发明设计中最佳踢球点位置的选择示意图；

图3是本发明设计中仿人足球机器人踢球腿足部路径示意图；

图4a是本发明设计中臀部关节坐标系示意图；

图4b是本发明设计中臀部滚动关节坐标系示意图；

图5是本发明设计中踢球仿真效果示意图；

图6a是两种方法下仿人足球机器人选择完踢球点准备后抬腿的动作示意图；

图6b是两种方法下仿人足球机器人进行轨迹规划并做出后抬腿的动作示意图；

图6c是两种方法下仿人足球机器人进行轨迹规划并后抬腿过程中的动作示意图；

图6d是两种方法下仿人足球机器人进行轨迹规划并后抬腿至最高位置的动作示意图；

图7是两种方法下仿人足球机器人踢球动作后足球的运动轨迹示意图。

具体实施方式

下面结合说明书附图对本发明的具体实施方式作进一步详细的说明。

本发明所设计一种基于贝塞尔曲线插值的仿人足球机器人踢球方法，实现单个仿人足球机器人针对其视野范围内足球的踢球动作，其中，仿人足球机器人在接到踢球指令之后，为了在短时间内把球踢向预期位置，就需要选择一个合适的踢球点，踢球点的选择需要考虑机器人本身与球的相对位置，包括足部与球之间的距离、矢量角，以及球到目标之间的矢量，当仿人足球机器人选择完合适的踢球点后，确定游脚并进行身体重心的转移，同时利用贝塞尔函数确定足部踢球的轨迹曲线，本专利将曲线分为n个点，对于每一个点通过以下步骤计算，完成整个踢球过程，在实际应用过程当中，如图1所示，具体包括如下步骤：

步骤A.获得以足球所在位置为圆心，偏移R为半径的圆环上的各个待测踢球点位置K₁、K₂、…，并获得仿人足球机器人所在位置分别相对各个待测踢球点位置的踢球代价值C_Kick，进而获得最小踢球代价值所对应的待测踢球点位置，作为最佳踢球点位置，然后进入步骤B。

上述步骤A具体包括如下步骤：

步骤A01.如图2所示，获得以足球所在位置为圆心，偏移R为半径的圆环上的各个待测踢球点位置K₁、K₂、…，并进入步骤A02。

步骤A02.分别针对各个待测踢球点位置，首先获得仿人足球机器人所在位置与待测踢球点位置之间的连线，再获得仿人足球机器人朝向方向与该连线之间的夹角大小，接着根据该夹角大小与2π的比值，获得仿人足球机器人对应该待测踢球点位置的转身代价值，进而获得仿人足球机器人分别对应各个待测踢球点位置的转身代价值C_turn，然后进入步骤A03。

实际应用中，如图2所示，具体首先获得仿人足球机器人所在位置与足球位置之间的连线，即向量以及获得仿人足球机器人所在位置与待测踢球点位置之间的连线，即向量则仿人足球机器人朝向方向与向量之间的夹角为α，向量与向量之间的夹角为β，进而获得仿人足球机器人朝向方向与该连线之间的夹角大小|α-β|，接着根据该夹角大小|α-β|与2π的比值，获得仿人足球机器人对应该待测踢球点位置的转身代价值，如此获得仿人足球机器人分别对应各个待测踢球点位置的转身代价值C_turn。

步骤A03.分别针对各个待测踢球点位置，首先获得以仿人足球机器人所在位置为起点，待测踢球点位置为终点的向量，接着根据该向量的模，获得仿人足球机器人对应该待测踢球点位置的距离代价值，进而获得仿人足球机器人分别对应各个待测踢球点位置的距离代价值C_dist，然后进入步骤A04。

实际应用中，如图2所示，通过获得仿人足球机器人对应该待测踢球点位置的距离代价值，进而获得仿人足球机器人分别对应各个待测踢球点位置的距离代价值C_dist。

步骤A04.根据预设距离代价值权重ω_d和预设转身代价值权重ω_t，分别针对各个待测踢球点位置，按如下公式(3)执行，

C_Kick＝ω_tC_turn+ω_dC_dist(3)

进而获得仿人足球机器人分别对应各个待测踢球点位置的踢球代价值C_Kick，然后进入步骤A05。

步骤A05.针对各个待测踢球点位置，获得最小踢球代价值所对应的待测踢球点位置，作为最佳踢球点位置，然后进入步骤B。

步骤B.获得仿人足球机器人踢球腿上足部踝关节对应踢球动作过程中的至少三个三维空间位置，其中，如图3所示，所获各个足部踝关节三维空间位置至少包括仿人足球机器人双腿站立时踢球腿上足部踝关节三维空间位置、后抬腿至最高点时踢球腿上足部踝关节三维空间位置和踢球腿接触足球时踢球腿上足部踝关节三维空间位置，然后进入步骤C。

步骤C.将所获仿人足球机器人各个踢球腿上足部踝关节三维空间位置作为各个插值点，基于贝塞尔曲线，利用样条插值法，获得仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)，如图3所示，并保证使用插值法所得低阶分段的曲线函数是光滑的，然后进入步骤D。

上述步骤C具体包括如下步骤：

步骤C01.将所获仿人足球机器人各个踢球腿上足部踝关节三维空间位置作为各个插值点，获得各个插值点分别对应于踢球动作过程中的各个时间点t₁、…、t_n，其中，n为插值点的个数，然后进入步骤C02。

步骤C02.根据各个插值点分别所对应足部踝关节各个三维空间位置S(t₁)、…、S(t_n)，构建如下公式(5)：

其中，T_j表示t_j至t_j+1之间的时长，t_j∈{t₁、…、t_n}，t_j+1∈{t₁、…、t_n}，G_j与G_j+1通过如下公式(6)、(7)获得。

将公式(6)和公式(7)所表示的G_j与G_j+1代入到公式(5)，并令初态S'(t)＝0，终态S”(t)＝0，如(8)所示，解得a_n、b₁、d₁、d_n。

由此，根据公式(5)，获得仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)。

如图3所示，所述仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)包括仿人足球机器人双腿站立时踢球腿足部踝关节三维空间位置至后抬腿最高点时足部踝关节三维空间位置之间运动曲线，仿人足球机器人踢球腿后抬腿最高点时足部踝关节三维空间位置至踢球腿接触足球时足部踝关节三维空间位置之间运动曲线，以及仿人足球机器人踢球腿接触足球时足部踝关节三维空间位置至双腿站立时足部踝关节三维空间位置之间运动曲线。

逆运动学技术主要用来腿部各个关节的角度，针对仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)，通过逆运动学技术，算出下一时刻的关节值，其中研究对象的关节数量、关节旋转/平移数目等力学特性会使得计算复杂。

Robocup3D仿真环境中的Nao机器人有以下特点：臀部偏摆关节轴旋转了45°；仿人机器人的两条腿都由臀部偏摆关节控制。

步骤D.根据踢球腿足部踝关节三维空间运动曲线S(t)，获得臀部关节坐标系下足部位姿矩阵并进而获得臀部滚动关节坐标系下足部位姿矩阵然后进入步骤E，其中，臀部关节坐标系如图4a所示，臀部滚动关节坐标系如图4b所示。

上述步骤D具体包括如下步骤：

步骤D01.根据踢球腿足部踝关节三维空间运动曲线S(t)，获得臀部关节坐标系下足部位姿矩阵然后进入步骤D02。

步骤D02.根据臀部关节坐标系下足部位姿矩阵按如下公式(9)：

获得臀部滚动关节坐标系下足部位姿矩阵然后进入步骤E，其中，l_d为仿人足球机器人两条腿之间的距离，表示沿着x轴旋转表示沿着y轴平移

步骤E.根据臀部滚动关节坐标系下足部位姿矩阵获得膝关节偏移角θ_knee、踝关节偏转角θ_anklePitch、踝关节转动角θ_ankleRoll、髋关节俯仰角θ_hipYaw、髋关节滚动角θ_hipRoll和髋关节偏转角θ_hipPitch，然后进入步骤F。

上述步骤E包括如下步骤：

步骤E01.根据仿人足球机器人大腿长l_thigh、小腿长l_lowerleg、臀部滚动关节坐标系下足部位姿矩阵的平移矢量，以及仿人足球机器人踢球腿后抬腿时大腿根与足部踝关节之间连线长l_trans，按如下公式(10)、(11)和(12)：

θ_ankleRoll＝atan2(y,z)(12)

计算获得膝关节偏移角θ_knee、踝关节偏移角θ_anklePitch、踝关节转动角θ_ankleRoll，然后进入步骤E02，其中，(x,y,z)表示的平移矢量的分量，atan2(y,x)表示点(x,y)与坐标系原点之间连线和x轴之间的夹角。

步骤E02.构建公式(13)

其中，θ_hipYaw表示髋关节俯仰角，θ_hipRoll表示髋关节滚动角，θ_hipPitch表示髋关节偏转角，S_x代表sinθ_hipRoll，S_y代表sinθ_hipPitch，S_z代表sinθ_hipYaw，C_x代表cosθ_hipRoll，C_y代表cosθ_hipPitch，C_z代表cosθ_hipYaw，用式(14)(15)确定：

根据公式(14)和公式(15)，针对(13)求解，获得髋关节俯仰角θ_hipYaw、髋关节滚动角θ_hipRoll和髋关节偏转角θ_hipPitch。

步骤F.控制仿人足球机器人行走至最佳踢球点位置，并根据所获膝关节、踝关节、髋关节的各个角度，控制仿人足球机器人针对足球执行踢球动作。

将上述所设计基于贝塞尔曲线插值的仿人足球机器人踢球方法，应用到实际当中，如图5所示，给出了基于样条插值法得出的向前侧踢的足部轨迹曲线，图中曲线表示踢球时足部轨迹，从左往右依次分别代表三条曲线，其中，依次为t＝0.49s时后抬腿的瞬时位姿，t＝0.52s时摆腿的瞬时位姿，t＝0.56s时复位过程的瞬时位姿，由图5可以看出，三个状态下机器人足部轨迹平滑且易于实现。

并且基于仿人足球机器人踢球腿对应于踢球动作过程的踢球腿足部踝关节三维空间运动曲线S(t)，通过逆运动学技术，获得腿部各个关节的角度，如图6a至图6d所示，其中，每张图分为两部分，左半图为基于本专利基于贝塞尔曲线插值踢球方法，右半图为美国德州大学UT足球机器人的踢球方法。其中，基于图6a和图6b，由图6c和图6d可以看出，UT足球机器人的踢球实验中，机器人先是后抬大腿，小腿相对大腿的位姿不变，然后往回摆动小腿，这样的踢球动作使踝关节无法达到理想角度，大幅降低出脚力度和速度；而本专利基于贝塞尔曲线插值踢球机制下机器人则是在后抬腿过程中一次性将大腿和小腿调整到最合适的位置，再向后调整踝关节，使机器人绷紧脚掌准备踢球。

基于图6a至图6d的分析比较，将本专利基于贝塞尔曲线插值踢球机制与UT足球机器人的踢球方进一步进行实验对比，实验结果如图7所示，其中，每幅图分两部分，上半部分是应用了本专利基于贝塞尔曲线插值踢球机制的实验，下半部分为UT足球机器人的踢球实验。图中的白圈为足球的位置，由图可知，在任意时刻，上半图球的滚动距离都要大于下半图，而且从最终球停止位置而从最终球停止位置来看，本专利基于贝塞尔曲线插值踢球方法可以实现一次性把球从半场踢入对方球门，最大距离可达15m，而UT足球机器人的踢球实验最大距离为11.2m。重复上述实验100次，本专利方法的踢球距离平均为14.62米，UT足球机器人的踢球距离平均为10.58米。执行动作时间上，本专利算法平均在2.56秒，UT足球机器人平均在2.11秒，从以上对比可以看出，本专利方法距离上优于UT足球机器人，而在执行时间上却高于UT足球机器人的踢球方法。

上述技术方案所设计基于贝塞尔曲线插值的仿人足球机器人踢球方法，基于轨迹规划与逆运动学相互协作技术，采用全新设计架构逻辑关系，基于所设计高精准度踢球动作，获得的精准足部运动轨迹的同时，利用逆运动学技术，获得仿人足球机器人踢球腿在进行踢球动作时，腿部关节的角度变化过程，并以此过程作为控制指令，实现针对仿人足球机器人踢球腿的控制，以实现高精准度的踢球动作，并同时获得更优的仿人足球机器人自身稳定性。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。