四足机器人Trot步态规划与仿真分析

谢楚政，李 光，于权伟，吴陈成

1 研究背景

移动机器人可代替人类在一些复杂工况和恶劣环境中的工作，例如战场的援救、地下管道的维修、高层楼房的建设等。不仅解放了人力、提高人们的生活质量、提升幸福感，更是对生产力技术的一次革命。移动机器人技术研究，目前已经成为国内外科研工作者的热点研究领域[1]。

根据移动方式不同，可将机器人分为轮式机器人、履带式机器人、步行机器人等。其中，步行机器人由于其结构模仿自然界中经过优胜劣汰、自然选择后生存下来的各种生物结构[2]，所以相比轮式机器人、履带式机器人具有更为优异的环境适应能力。设计性能优异的步行机器人，几乎可以在各种路况的地面行走[3]。

四足机器人作为步行机器人中的一种，与双足、六足机器人相比，具有更多优势。双足机器人运动稳定性较差，六足机器人运动速度较慢，而四足机器人兼具运动稳定性[4]、快速性[5]，以及较强的负载能力[6]等优势。因此，本文选择四足机器人作为研究目标。

目前，一些知名的研究机构已经研究出很多性能优异的四足机器人[4-6]。例如：波士顿动力学工程公司研究开发了Big dog[7]、Spot 和Spot Mini[8]，麻省理工学院研发了MIT Cheetah Mini[9]，瑞士苏黎世联邦理工大学机器人系统实验室向外界展示了其开发的狗型机器人ANYmal[10]等。已有对四足机器人的研究工作，主要集中在结构设计、轨迹规划、控制策略等方面[11]。而轨迹规划是机器人运动研究中最关键的环节，因此机器人的使用离不开轨迹规划[12]。不管是没有环境感知能力的机器人按照既定的轨迹规划进行作业，还是能够获取环境信息、进行自律控制的机器人作业，都需要提前做好轨迹规划[13]。李贻斌等[14]提出了一种Trot 步态轨迹，并将其在液压驱动四足机器人上实现。张培强等[15]采用调整腿长的方式，对四足机器人Trot 步态进行了优化。马慧姝等[16]采用Kinect 摄像机采集犬类动物落足点，并用3 次样条曲线拟合，对步态进行轨迹规划。马宗利等[17]采用调整质心位置的方法，提高了四足机器人坡面运动的稳定性。然而以上研究均未考虑四足机器人足端初始位置对Trot 步态稳定性的影响。

基于此，本研究针对四足机器人的轨迹规划问题展开研究，在运动学分析的基础上，从足端轨迹优化、初始足端位置选择这两个方面对四足机器人的Trot步态进行规划，并用Matlab/Simulink 仿真平台验证此步态规划的合理性。从而实现四足机器人以Trot步态相对稳定地行走。

2 四足机器人结构与运动学方程

2.1 四足机器人实体结构

四足机器人腿部结构一般分为4 种方式：“全肘式”、“全膝式”、“前膝后肘式”和“前肘后膝式”。为了满足运动需求，一般四足机器人每条腿至少需要3 个自由度。

本文所研究的四足机器人采用的是“全肘式”结构，每条腿具有3 个旋转自由度关节，结构示意图如图1 所示。

图1 四足机器人结构示意图Fig.1 Schematic diagram of quadruped robot structure

2.2 四足机器人正向运动学方程

采用改进D-H 法建立四足机器人的运动学模型，其示意图如图2 所示。

图2 四足机器人的运动学模型示意图Fig.2 Kinematic model of quadruped robots

在图2 中坐标系皆为笛卡尔直角坐标系，3 个坐标轴下标为坐标系名称。坐标系{W}为世界坐标系，其坐标原点为OW，3 个坐标轴分别为xW，yW，zW。坐标系{B}为机体坐标系，建立在机体质心处，其坐标原点为OB，3 个坐标轴分别为xB，yB，zB。坐标系{0}、{1}、{2}分别表示侧摆、大腿、小腿坐标系。坐标系{3}为足端坐标系。θ1、θ2、θ3分别为3 个关节的转动角度。Lx为机体前腿和后腿的关节1 沿xB轴距离，Ly为左右腿关节1 沿yB轴距离。四足机器人前左、前右、后左、后右4 条腿分别用FL、FR、BL、BR 表示。L1、L2、L3代表侧摆、大腿、小腿连杆的长度，其中L1=76 mm，L2=210 mm，L3=210 mm。由于4 条腿坐标系的建立方法大致相同，图中仅标出前右腿（FR）的坐标系，其D-H 参数见表1。

表1 四足机器人前右腿（FR）的D-H 参数Table 1 FR leg D-H parameters of quadruped robots

根据表1 所示的D-H 参数可求得足端正向运动学方程为

2.3 四足机器人逆向运动学方程

四足机器人逆向运动学的简化参数示意图如图3所示。

图3 FR 腿的逆向运动学参数示意图Fig.3 FR leg inverse kinematics parameters of quadruped robots

四足机器人的逆向运动学求解是轨迹规划、运动控制以及其他相关研究的基础。常用的方法有解析法、几何法、迭代法等。本研究根据四足机器人的运动需求，采用几何法求解逆向运动学方程。同正向运动学方程类似，四足机器人的4 条腿逆向运动学方程求解选取前右腿（FR）为例，以此说明逆向运动学方程的求解过程，所用到的参数已在图3a、b 两个平面标出。

由图3a 可知，在yBOBzB平面内：

如图3b 所示，在切于关节2、关节3 的A-A 视图平面内：

由方程（2）～（15）可得到FR 腿逆向运动学方程为

3 四足机器人轨迹规划

四足动物小跑过程中，同时抬起、放下前左、后右腿（或前右、后左腿）每条腿都会在支撑相和摆动相之间来回切换，以此形成对角Trot 步态，如图4所示。

图4 对角步态示意图Fig.4 Diagram of diagonal Trot gaits

本研究中，四足机器人采取Trot 步态，同时抬起、放下相对的两条腿，瞬时会有少于3 条腿支撑，没有稳定的支撑域，属于动步态的一种。

3.1 足端轨迹规划原则及约束

为满足实际使用需求，需减小四足机器人腿部在支撑相和摆动相之间切换时的足端惯性力以及与地面的冲击力。因此本研究以足端冲击力为0 的原则，即将足端状态定为理想状态，与地面接触时，应该满足足端速度与加速度都为0。以FR 腿为例，建立坐标系{D}描述足端位置参数，如图5 所示。该图以关节1为坐标原点，机器人前进方向为x轴，竖直向上为z轴。

图5 足端位置参数示意图Fig.5 Schematic diagram of parameters of position of the extremities

机器人对角步态的时序图如图6 所示，设机器人初始机体质心高度为h。设步态周期为T，步长为S，抬腿高度为H，腾空率（理论摆动相占整个运动周期比率）为λ。

图6 对角步态时序图Fig.6 Trot gait timing diagram

设机器人足端初始位置为（x0，y0，z0）。由于机器人在y轴方向无运动，所以y0=-L1。显然z轴足端初始位置的绝对值等于初始机体质心高度，即z0=-h。x0值的设定将对机器人运动的稳定性能有较大影响，后续仿真分析时将对x0值的设定进行详细分析。

由于对角步态向前运动时关节1 不动，即θ1≡ 0，y(t)≡-L1。只需给出x轴和z轴方向上的约束。根据上述原则，得到每个周期T内，前进方向足端约束：

竖直方向足端约束：

3.2 足端轨迹函数

基于约束方程（17）～（23），摆动相足端轨迹拟采用复合摆线的形式。参考文献[14]给出的摆动相足端轨迹为

由足端轨迹求导计算出速度、加速度曲线可知：在z轴方向，t=0，t=λT时，加速度不符合约束方程（19）、（21），所以需要对z轴方向轨迹进行优化。优化后的摆动相足端轨迹函数如下：

得出摆动相足端轨迹后，根据约束方程，容易得到支撑相足端轨迹方程为

可根据实际情况调整步态周期T、步长S、抬腿高度H。实际使用时，足端轨迹函数还需加上足端初始位置（x0，y0，z0）的设定值。

4 Simulink 仿真分析

4.1 三维实体模型的建立

以优宝特“YOBOGO”型四足机器人为物理模型，根据其实际尺寸和质量，结合正向运动学公式（1），在SolidWorks 软件中建立三维实体模型，见图1。用基于SolidWorks 的SW URDF Exporter 插件导出URDF（unified robot description format）文件，该文件是一种机器人描述文件，在机器人系统里大量使用。也可用于各种仿真平台内搭建机器人仿真模型。

4.2 Simulink 仿真模型的建立

将上一步生成的URDF 文件导入Matlab/Simulink仿真软件中，并将足端轨迹函数，式（25）～（27），输入相应的Simulink 模块中，生成足端轨迹信号；并结合上文中给出的逆向运动学方程式（16）与Simulink 模块解出的关节角度信号，从而对机器人的实体模型进行仿真控制。

Simulink 仿真运动过程如图7 所示。图7a 为一个周期T=0.500 s 的运动。t=0 s 时为四足机器人初始状态，t=0.125 s 为一组摆动相运动状态；t=0.250 s 摆动相与支撑相切换；t=0.375 s 为另一组摆动相运动状态。图7b 为4 个周期的运动。t=0.5, 1.0, 1.5, 2.0 s 分别为4 个周期的初始状态。

图7 Simulink 仿真运动过程示意图Fig.7 Diagram of simulink simulation movement

4.3 仿真条件设置

为了分别验证四足机器人足端轨迹、足端初始位置对运动稳定性的影响，本研究进行了如下两组对比仿真实验。

实验一：设定变量为足端轨迹，选取文献[14]中给出的足端轨迹函数为优化前轨迹和优化后的足端轨迹，以此形成对比。

实验二：采用优化后的足端轨迹函数，设定变量为足端初始位置。

四足机器人由于自身机体、腿部的重力惯性等因素，当进行对角步态行走，抬起对角两条腿时，会向另一对角倾斜，造成对角步态两条腿不能同时着地，从而增大足端与地面冲击力造成运动的不稳定性。而初始足端位置的选择将极大影响对角倾斜方向和大小。所以这里对四足机器人对角步态的足端初始位置进行仿真分析。四足机器人向前运动时，x轴方向足端初始位置对机器人稳定性影响最大，y轴方向可不动，z轴方向决定的是机体质心高度，需根据实际使用需求选取。所以本文主要对x轴方向足端初始位置变化进行仿真对比。

实验中需设定的参数有：周期T，步长S，抬腿高度H，腾空率λ，足端初始位置x0、y0、z0。结合四足机器人腿部连杆长度参数：L1=76 mm，L2=210 mm，L3=210 mm。两组对比实验中均设定：周期T=0.5 s，步长S=20 mm，抬腿高度为10 mm，腾空率λ=0.5（即FL、BR 腿运动周期与FR、BL 腿运动周期相位差为0.5T）。实验一足端初始位置为x0=-40 mm，y0=-76 mm，z0=-200 mm。实验二足端初始位置变化范围为-70～0 mm，每间隔10 mm 取值进行仿真，一共仿真8 次，y0=-76 mm，z0=-200 mm。

实验中四足机器人先迈FL、BR 腿。每个实验均仿真5 s，10 个周期。

4.4 仿真结果分析

两组对比实验分别从足端冲击力F，机体质心位移x、y、z，机体俯仰角（pitch）φ、滚动角（roll）ω、偏航角（yaw）κ等具体数值来评价四足机器人的运动稳定性能。仿真在0 s 时刻时，机器人此时是悬空的，距离地面有微小高度，以及机器人本身惯性所致，前几个运动周期会有不规律的现象。因此，以下数据分析皆不考虑前3 个不规律周期。

4.4.1 实验一结果分析

在实验一中，对比的是优化前和优化后足端轨迹对四足机器人运动稳定性的影响。影响结果如图8 所示，由图8 可以看出，优化后轨迹的足端冲击力小于优化前轨迹的足端冲击力。

图8 优化前后实验一足端冲击力变化曲线Fig.8 Impact force curves at the extremity of the first experiment of before and after optimization

图9 为机体的质心位移曲线，从图9 可以看出，5 s 仿真时间内：在x轴方向，优化后前进更远；在y轴方向，优化前有较大偏移位移，优化后y轴偏移较小，前进方向更准确；在z轴方向，优化前后差距不大。

图9 优化前后实验一机体质心的位移变化曲线Fig.9 Mass center displacement curves of body in the first experiment of before and after optimization

机体质心位移的变化角度如图10 所示，根据图10 欧拉角的变化曲线可以知道，优化前俯仰角φ、滚动角ω都比优化后的波动大。优化后的偏航角κ比优化前更小。

图10 优化前后实验一机体质心欧拉角的变化曲线Fig.10 Mass center Euler angle variation curves of body in the first experiment of before and after optimization

综上对实验一优化前、后轨迹的各种仿真曲线的分析，可以看出优化后足端与地面冲击力较小，机体俯仰角、滚动角较小，运动较为稳定，优化后偏航角也变小，前进方向更准确。

4.4.2 实验二结果分析

在实验二中，对比的是足端初始位置x轴方向的变化对四足机器人运动稳定性能的影响。四足机器人稳定向前运动时，足端冲击力F越小越好，x轴方向位移越大越好，y轴方向位移也是更小为宜。故上述3 个评价标准给出其仿真数值变化的最大值，见表2。分析表2 的数据可知，当x0=-40 mm 时，足端冲击力F最小，为19.9 N；x轴方向的位移最大，为0.402 m；y方向的偏移最小，为-1.32×10-2m。

表2 实验二F、x、y 随x0变化的最大值Table 2 Maximum change of F, x and y in the second experiment as a function of x0

而z轴方向有地面作为约束，理论位移变化越小越好。同理，俯仰角φ、滚动角ω、偏航角κ亦是变化越小，四足机器人运动越稳定。因此，本研究给出上述评价标准的仿真数值变化的方差，如表3 所示。结合表2 和表3 可知，当x0=-40 mm 时，φ、ω的方差最小；当x0=-30 mm 时，z、κ的方差最小。

表3 实验二中z、φ、ω、κ 的方差Table 3 Variance of z, φ, ω and κ in the second experiment

在四足机器人的对角步态仿真过程中，根据仿真视图可以看出：当x0≥-40 mm 时，四足机器人有向后倾倒的趋势，后腿先于前腿着地。x0≤-50 mm时有向前倾倒的趋势，前腿先于后腿着地。机器人前腿先着地时明显不如后腿先着地运动稳定。结合上述7 项数值的对比，可以看出当x0=-40 mm 时，各项数据都较为理想。由于篇幅限制，这里仅列出x0=-40 mm 以及x0=0 mm 两次仿真数值的变化曲线，详细对比不同初始足端位置对机器人稳定性的影响。以下将足端初始位置x0=-40 mm 称为位置一；x0=0 mm 称为位置二，仿真结果如图11～13 所示。

图11 实验二不同初始位置下足端冲击力变化曲线Fig.11 Impact force variation curves at the extremity of the second experiment at different initial positions

图12 实验二不同初始位置下机体质心的位移变化曲线Fig.12 Mass center displacement variation curves of body in the second experiment at different initial positions

图13 实验二不同初始位置下机体欧拉角变化曲线Fig.13 Euler angle variation curves of body in the second experiment at different initial positions

由图11 可知，位置一的足端冲击力较小。由图12 可知：x轴方向，位置一的位移较远；y轴方向，位置一的波动较小；位置二有向y轴负方向位移的趋势；z轴方向，两位置波动差距相当。由图13 可知，位置一的角变化较小，较为稳定，而位置二俯仰角φ、滚动角ω都波动较大，航偏角κ也有变大趋势。

综合实验二对四足机器人足端初始选择的仿真分析可得：当x0=-40 mm 时，足端与地面冲击力较小，机器人前进较远，机体俯仰角、滚动角波动较小，航偏角也较小。因此，此时四足机器人具有较优异的性能，运动较为稳定，前进方向较为准确。

5 结语

通过建立四足机器人正向、逆向运动学模型、基于足端零冲击原则进行足端轨迹规划，实现了四足机器人对角步态的运动；并且从足端轨迹函数、足端初始位置两方面去优化四足机器人的对角步态；采用Matlab/Simulink 仿真实验分析足端轨迹、足端初始位置对机器人运动稳定性的影响。仿真实验结果表明，从足端轨迹、足端初始位置两方面对机器人的对角步态进行优化规划，使得机器人在运动过程中的稳定性能有较大的提高，实现了四足机器人有较为稳定的对角步态行走。