基于简化三维线性倒立摆模型的模糊控制射门算法

,, ,,

(1.苏州科技大学 天平学院，江苏 苏州 215009; 2.苏州科技大学 江苏省建筑智慧节能重点实验室，江苏 苏州 215009)

0 引言

机器人足球比赛是一项带有人类行为意识的竞技体育比赛。更为深层次的意义在于通过此类比赛形式，以足球为载体，促进包括人工智能、机器人学、传感器、自动控制和通讯等一系列领域的技术发展。Robocup全球委员会提出未来将建立一支机器人足球队，并将打败人类足球世界杯冠军队作为奋斗目标。国际机器人足球联盟制定的规则表明，在规定比赛时间内进球最多的队伍获胜，因此双足机器人射门成功率是比赛取胜的决定性因素。

目前，研究人员针对射门策略开展了广泛的研究。张彦铎等人在传统射门的基础上提出了一种射门区间模型。在为机器人规划最佳运动路径的同时寻找出最优射门点[1]。但该方法忽略了当射门路径上存在其他机器人时，对射门路径造成的干扰。韩家新等人提出了一种改进直冲射门方法，相比较传统射门算法而言射门成功率有所提高[2]。但是该算法代码的复杂度高、计算时间长，无法保证机器人动作的实时性。宋大雷等人提出的动态椭圆射门方法从减少机器人射门运动距离入手，提高了射门成功率[3]。但是该方法对硬件有着苛刻的要求。梁家海、赵兵等人分别提出了利用机器学习和模糊算法这类人工智能方法来优化射门路径。虽然这类方法可以比较连贯的完成射门动作与避障[4-5]。但是对机器人的硬件设备和计算能力要求较高，从而容易对射门实时性造成影响。

针对上述射门方法中存在的射门精度和实时性的问题，本文主要借助于中国机器人大赛3D双足机器人Robocup 11 vs 11仿真平台[4]，将真实的比赛场景进行高还原度的模拟。并通过分析本球队射门机器人与本球队成员、对方球队成员的位置、行为之间的关系，提出了一种基于简化三维倒立摆模型的模糊控制算法来提高机器人射门精度和实时性。该方法利用模糊控制方法精确地修正机器人的射门姿态和角度，并在射门时提供较大的冲量，从而提高射门成功率。

1 简化三维线性倒立摆模型原理

1.1 简化三维线性倒立摆模型

在双足机器人运动过程中，如果只考虑前后和上下方向的运动而忽略左右的运动，那么容易造成机器人失去平衡而摔倒。因此，需要将线性倒立摆拓展到三维环境，并对机器人进行三维线性倒立摆建模。然而，直接分析研究双足机器人多连杆结构的过程往往较为繁琐，因此本文等价地简化机器人为合理的数学模型，从而便于研究[6]。将机器人的身体视为一个质点，腿视为无质量的支撑杆，可以建立起一个三维线性倒立摆模型。它不需要已知机器人的参数，如质量、质心以及每个关节的惯性，而是通过模型导出，从而便于计算。

图1 简化三维线性倒立摆模型

图1表示简化的三维线性倒立摆模型，假定质心高度固定为zc，重力加速度为g，根据三维线性倒立摆模型，则ZMP轨迹(px(t)、py(t))与质心轨迹(x(t)、y(t))运动的方程为：

(1)

(2)

由公式(1)、(2)可以看出，给定ZMP轨迹(px(t)、py(t))和质心轨迹(x(t)、y(t))就可以求得质心轨迹。再通过指定游动腿轨迹，即能够让双足机器人进行步态行走。

1.2 双腿支撑相射门姿态调整

在实际比赛时，射门成功率不单单取决于射门算法，双足机器人射门前调整姿态所花费的时间也对成功率有一定影响。机器人到达目标射门点时需要调整其姿态后才能进行射门，即将方向和速度调整到合理的数值。

在如图2所示的双腿支撑相中，可以得到机器人的质心位置和加速度关系方程：

图2 双足机器人双腿支撑相

(3)

(4)

(5)

给出质点的初始方位、速度以及最终方位、速度，即已知：

则可得：

(6)

(7)

根据公式(4)～(7)可知，要想求得机器人在双腿支撑相的身体质点轨迹，只需要知道机器人的初始方位和速度、最终方位和速度。因此跟据上述方法可以实现双足机器人一步停止，缩短了射门前的准备时间。

1.3 双足机器人单腿支撑相射门

当双足机器人开始进行踢球动作时，就需要获得最佳的射门动作。在双足机器人抬起游动腿之前，都需要实现将质心位置移动到该单腿支撑项的位置。本文将公式(1)和公式(2)进行变换，从而得到公式(8)和公式(9)：

px=c1*e4+c2*e3+org

(8)

vx=c1*e4/Tc-c2*e3/Tc

(9)

其中：e1=exp(-Ts/Tc)，e2=exp(Ts/Tc)，

e3=exp(-t/Tc)，e4=exp(t/Tc)，

c1=((x0-org)*e1-(c1-org))/(e1-e2)，

c2=((x0-org)*e2-(c-org))/(e2-e1)。

由于双足机器人要实现单腿支撑，就需要对质心的运动轨迹进行规划，同时提前规划其侧平面轨迹。从而使质心能够垂直投影于ZMP范围内。实际比赛时，机器人只有通过迅速调整球与脚之间的距离，在躲避对方拦截和避开干扰球员的同时，实现起脚射门动作。因此，处于单腿支撑相时，本文将质心沿y轴移动的距离设置为一个变量。并通过动态地对机器人射门脚与球之间的距离进行调整，完成快速起脚射门的动作。

2 动腿轨迹规划

本文规定射门的起始位置为ps(xs,ys,zs)，初始速度vs，初始角度为(αs,βs)。球的位置为po(xo,yo,zo)，目标速度vo，目标角度为(αo,βo)，α表示y轴方向上的角度，β表示x轴方向上的角度。关系式如下：

(10)

同理，根据po点得到p3关系式：

(11)

设射门周期为T，t∈[0,T]，得到以ps、po

为起始点的3次贝塞尔曲线方程为:

(12)

其中：0≤t≤T，vs≥0，vo≥0。

这样就可以根据双足机器人的初始位置和球的坐标位置为起点和终点做一条平滑的曲线，作为双足机器人的游动腿轨迹。

3 模糊控制射门算法

3.1 模糊控制射门算法原理

传统曲线射门的思路是让双足机器人沿着一段圆弧的轨迹运动到足球所在的位置，然后继续带球运动到目标射门点之后将球踢出[7]。此类算法虽然优于传统直线射门算法，但是当机器人离足球与目标射门点所成直线的距离较近时，圆弧很大，因此射门效果不够理想。基于上述原因，本文提出了一种模糊控制射门算法。利用模糊规则让机器人用较短的时间走直线取球，接着走一段圆弧轨迹，把方向调整准确后踢球射门。具体实现步骤：

步骤2：从机器人R到圆O的切线也有两条，选择当机器人到达B点的方向与由B指向G相同的那条为切线。如图5中选择点切点P所在的切线，此时切线方向角θ=β-α。其中，β为RP的方向角，α=arcsin(r/d)，d为线段OR的长度。由此可以求出切线RP的方向角θ；

步骤3：让机器人沿直线RP运动到P点，然后沿曲线轨迹射门。

3.2 射门方向修正

以上只解决了双足机器人运动时的路径规划与射门方向问题，没有将机器人初始方向对射门成功率的影响纳入讨论。因为机器人的初始方向是任意的，并不会默认与切线方向θ相同，所以会产生误差。因此本文引入模糊控制方法来动态调整机器人的初始方向。

本文提出的模糊规则表述如下：

当左右腿中任意一个的速度达到设定值之后不再增加，同时另一条腿的速度也增加到设定值不再改变。如果机器人当前的方向角与本文期望的切线方向角的误差到达一定的值，则调整机器人腿的速度，使它们产生速度差以调整角度。其控制函数VM和Vm表达式如下：

VM=a1d+a2δ+Vc

(13)

Vm=a1d-a2δ+Vc

(14)

VM表示游动腿的速度，Vm表示支撑腿速度，Vc为常数，a1，a2为常数，数值根据实际情况修改。

机器人走直线时伴随着周期性的方向调整，当方向角与切线方向角一致时，让机器人走直线运动到P点后，最后让机器人沿曲线轨迹完成射门操作。在仿真与实际测试的时候，可以只通过计算机器人和圆心的距离来判定其是否运动到P点。

4 实验与分析

为了验证本文提出射门算法的性能，先将NAO机器人为原型建立三维线性倒立摆模型，然后再将算法写入RoboCup 11vs11仿真平台。在RoboCup 3D仿真平台中完全模拟真实比赛的场景，对本文提出的模糊控制射门算法进行抬腿、射门、持球、持球射门等项目的测试，如图3所示。同时要求机器人在持球行走和射门的过程中自我调整方向，并实现快速地起脚射门。仿真实验表明，本文的算法射门成功率高于其他射门算法。

图3 双足机器人仿真射门过程

传统射门方法通常采用直线方程生成ZMP轨迹[8]。本文为了在多角度和任意姿态下实现最优射门动作，分别将直线方程、贝塞尔曲线生成的游动腿ZMP轨迹用于本文提出的基于模糊控制算法的双足机器人射门动作轨迹，并分别优化获得如图4所示的适应度函数值。

从图4中不难发现直线方程的ZMP轨迹经过迭代48次后收敛，而塞尔曲线的ZMP轨迹的适应度函数经过迭次78次后才趋于稳定。由此可见ZMP轨迹为直线方程时收敛过早，并且其适应度函数值最终也大于ZMP轨迹为塞尔曲线的情况。最后将两种方法优化后的参数分别应用于仿真射门平台进行射门测试对比。测试结果也验证了本文游动腿采用ZMP轨迹是塞尔曲线时，射门准确性更高、用时也更短。

图4 直线方程、贝塞尔曲线生成ZMP轨迹优化效果对比

图5为机器人射门全过程，采用Robocup实际比赛所用的NAO机器人，将程序输入开发接口进行真实环境测试。测试过程分为初始化的起步阶段，比赛时的正常行走，按照本文算法射门阶段以及最后的进球过程。

图5 NAO机器人全过程射门

如图6所示，在2000次射门实验中，随机算法失球512次，被拦截236次，被扑出435次；基本算法失球235次，被拦截145次；切圆弧轨迹算法失球156次，被拦截81次，被扑出167次；本文算法在45°射门时失球133次，被拦截67次，被扑出136次。从图中可以看出本文算法大幅优于另外几种算法，而且采用本算法比赛的足球队所发生的失球、被拦截、被扑出的数量最少。

图6 4种算法射门实验比较结果

此外，本文还从射门平均距离、平均调整时间、平均射门时间3个方面对比了4种算法，如表1所示。统计结果表明，本文提出的射门算法在平均调整时间和平均射门时间这两项性能上都优于其他3种算法。并且在射门距离上也接近切圆弧轨迹射门法，满足了比赛中射门距离越远越好的需求。

表1 RoboCup3D平台中的4种射门算法性能对比

5 结束语

本文提出的基于三维线性倒立摆的改进圆弧射门算法最大的亮点在于改善了机器人按照既定轨迹运动前初始方向调整的问题。在实现机器人运动路径的平滑过渡的同时提高了运动的冲量，可以大大提高射门成功率，体现了本算法的可行性和优越性。此外，苏州科技大学STU机器人足球队采用本文算法后，在2017年江苏省大学生机器人比赛足球仿真组取得了亚军一等奖的好成绩。