四足机器人Trot步态偏航分析及其控制

易 静，李 光，薛晨慷，谭薪兴

(湖南工业大学机械工程学院，湖南 株洲 412007)

0 引言

四足机器人以其高机动性、能适应非结构地形而成为国内外科研工作者的热点研究领域[1-6]。

国内外学者针对四足机器人机体翻转现象进行研究。Inagaki等[7]发现翻转现象产生的原因是由于重力产生绕支撑对角线的力矩，通过在机体附加重力振子来平衡翻转力矩；何冬青等[8]通过建立四足机器人翻转力矩模型，利用三分法调节起步姿态来减少机体翻转；张秀丽等[9]通过施加配重和调整起步姿态来改善机体翻转，但都没有针对翻转导致的姿态变化进行任何反馈调整。而谢惠祥等[10]分析了对角小跑中机体自身绕机体对角线的翻转原因，提出了利用侧摆关节提供力矩使机器人来回移动维持机体平衡保证机体不会翻倒，却没有对机体产生的偏航进行控制。

本文针对四足机器人的对角步态，在对四足机器人整体运动模型进行运动学分析的基础上，通过理论计算分析可知，由于重力产生的绕支撑对角线的翻转力矩以及支撑相髋关节产生的反作用力矩，导致步态时序会提前或延后产生绕机体对角线翻转，两者共同作用是引起偏航的根本原因，从而提出通过姿态反馈控制的办法来进行偏航控制。

1 机体偏航分析

1.1 整机运动学分析

四足机器人模型如图1所示，包含机身和4条腿，每条腿3个关节，从上到下分别为侧摆关节、髋关节和膝关节。本文所研究的四足机器人采用的是“全肘式”结构[11]。

图1 四足机器人运动学模型

根据Modified D-H( Denavit-Hartenbery)方法建立机器人运动学模型，如图1所示。L1为髋部连接长度；L2为大腿连杆长度；L3为小腿连杆长度。在机器人质心处建立坐标系{Ob}，大地上建立世界坐标系{Ow}，在每个侧摆关节处建立{Obi}坐标系，每条腿建立过渡坐标系{O0}，侧摆关节坐标系{O1}，髋关节坐标系{O2}，膝关节坐标系{O3}，足端坐标系{O4}，θ1、θ2和θ3分别为3个关节的转动角度。所有侧摆关节Z轴与Xw轴方向一致，其余关节Z轴与沿Yw轴方向相反。图1中仅列出右前腿的坐标系。

1.1.1 正运动学分析

建立在侧摆关节处的过渡坐标系{O0i}与{Obi}关系，即

(1)

biT0i为{O0i}坐标系相对{Obi}坐标系的变换矩阵；i=1,2,3,4分别为左前腿(FL)、右前腿(FR)、左后腿(BL)、右后腿(BR)。

由于左右两侧腿的D-H参数不一致，其D-H 参数如表1和表2所示。

表1 四足机器人FR腿D-H参数(同BR腿)

表2 四足机器人FL腿D-H参数(同BL腿)

根据表1和表2可求得在{Obi}坐标系下的正向运动学方程为：

(2)

(3)

对式(2)的各关节求偏导得到每条腿的雅可比矩阵为

Ji=

(4)

其中，s23=sin(θ2+θ3),c23=cos(θ2+θ3)，s1=sinθ1,c1=cosθ1。

1.1.2 逆运动学分析

已知每条腿{Obi}坐标系的坐标值(Pxi，Pyi，Pzi)，反求各关节角度。本文根据文献[12]使用的几何法，得出所有腿的各关节角度值为

(5)

θ1i、θ2i和θ3i分别为第i条腿的侧摆关节、髋关节和膝关节的角度。

1.2 重力产生的绕支撑对角线的翻转力矩分析

四足机器人在以对角步态前进过程中，在换相瞬间，可能会由于重力中心(center of gravity，COG)的位置不同，即COG在对角支撑线后方或前方会出现绕支撑线的往后翻转力矩和往前翻转力矩，在此，分析往后翻转力矩对偏航的影响。

图2显示了四足机器人在以对角步态换相瞬间腿部质心及机体质心往地面投影，d0、d1、d2、d3和d4分别为各部分地面投影质心到支撑对角线的垂直距离。COG位于腿2和腿3支撑对角线后方。由翻转力矩模型得静态翻转力矩为

M=mg·d0+mg·(d2+d3+d4-d1)

(6)

图2 重力引起的翻转力矩

m为机体质量，腿1和腿4位于摆动相，腿2和腿3位于支撑相，有d3>d1，d2和d4与d0同侧，因此M>0恒成立。由此可知，四足机器人在行走过程中存在向后侧方的翻转力矩。如果COG位于支撑对角线前侧，则存在向前侧方的翻转力矩。这种现象会导致机体姿态不断发生振荡式变化；对于质心不位于机体中心的情况，会大大增加其机体绕Zw轴旋转的偏航程度。最终会使机器人向着侧方移动，偏离指定位置。

1.3 支撑相髋关节产生的反作用力矩分析

通过合理地调整COG在对角支撑线中的位置可以减少由于重力产生的翻转力矩，进而抑制偏航现象的发生，但是在动步态中，机体绕自身对角线翻转比绕地面支撑线的翻转运动更为明显和剧烈，由翻转引起的偏航运动更加明显[10]。

为了定量分析四足机器人支撑相反作用力矩引起机体翻转出现偏航的原因。首先定义单腿的结构与步态。如表3所示，并规划支撑相的足端轨迹，分析FR和BL腿支撑时所受到的力矩。

表3 结构与步态参数

规划支撑相的足端轨迹(理论上无侧向运动的足端轨迹)，则足端轨迹坐标值为

(7)

为了简化分析，腿部质量忽略不计，则足端受到的地面作用力与驱动关节扭矩的关系[13]为

(8)

为了分析支撑腿对机体的影响，适当对反作用力F做一定的假设，支撑腿以一定加速度a支撑机体向前加速运动，则fx=ma。无侧向运动，所以y方向没有任何作用力，fy=0。Z方向作用力支撑机体，则fz=mg。

将足端轨迹曲线值通过式(5)逆运动学求解出关节角度，得出右前腿x位移与关节角。然后将得到每个坐标点所对应的关节角，通过式(4)求出其雅可比矩阵，利用式(8)，求出关节力矩，最终求得x位移与关节力矩之间的关系，同理可分析与之一起支撑的左后腿。经计算得出图3和图4变化曲线。

图3 右前腿关节角度与力矩

图4 左后腿关节角度与力矩

根据计算出的支撑力矩数值，机体受力分析如图5所示，支撑相侧摆关节力矩τ1在整个支撑相中相反，对机体的反作用力矩相互抵消。而髋关节产生的力矩τ2从变化方向由负变正。在支撑前期，支撑相髋关节反作用力矩作用在机体上，出现后翻，但是腿1和腿4摆动相往前摆动对机体产生的反作用力矩为正， 能抵消一部分机体翻转运动。而在支撑后半段，力矩使机体往前翻，两者交叉变化， 翻转使得摆动相腿提前着地或延后着地，触地时间不一致，使得偏航来回发生变换，变化过大严重影响四足机器人的平衡和稳定。

图5 机体受力分析

2 对角小跑步态规划

通过上述分析可知，四足机器人直线运动出现偏航的根本原因是由于翻转力矩的出现，如果不加以控制，机器人会不断偏离指定路线方向。在进行偏航姿态控制之前，首先确定步态轨迹函数，分为摆动相步态轨迹函数和支撑相步态轨迹函数。

2.1 摆动相步态规划

机器人腿部处于摆动相时，为了保证机器人尽量平稳，本文采用文献[14]提出的摆线方程，摆动相轨迹为

(9)

2.2 支撑相步态规划

支撑相轨迹为

(10)

S为步长；H为抬腿高度；T为步态运动周期；λ为占空比系数。xi(t)、yi(t)和zi(t)为第i条腿轨迹规划生成的值，斜对角线上的腿值一致，两对角线相差半个相位，以产生对角步态。yi(t)本来值为±L1，右腿为-L1，反之，左腿为L1。但这里令其为0，后面在姿态控制处根据姿态计算出值。

3 姿态调整控制器设计

设计合理的足端轨迹使得机器人能持续行走的前提下，然后进行姿态控制，重力产生的绕支撑对角线翻转力矩导致的偏航可以通过配重法和初始姿态调整来减少。但支撑相髋关节产生的反作用力矩导致步态提前或延后的翻转偏航不可通过外部调整抵消，只能在运行中，通过姿态反馈动态调整偏航角度使其偏航减少。首先建立姿态控制模型，如图6所示。

图6 姿态控制模型

图6中，l、b和w为站立姿态初始状态即各关节为0测量出来的值，即l为沿Xw方向两足端长度值，b为沿Yw方向两侧摆关节长度值，w为沿Yw方向两髋关节长度值。O点为世界坐标系{Ow}的原点，O′点位于(质心点)矩形中点。无特殊说明以下向量都是在世界坐标系中。

在Ai侧摆关节处建立与机身坐标系平移坐标系{Obi}，各坐标系与机身{Ob}的关系为

(11)

β值由式(3)给出，α为

(12)

四足机器人足端在惯性坐标系{Ow}中的坐标值为

(13)

站立姿态控制，用欧拉角描述机身坐标系{Ob}在惯性坐标系{Ow}中的姿态，机身位姿可由广义坐标向量q=[xyzψθφ]T表示。机身的位姿矩阵为

(14)

p=[xyz]T为机身坐标系相对惯性坐标系的位置；姿态用ψ表示绕x轴横滚，θ表示绕y轴俯仰，φ表示绕z轴偏转。R姿态矩阵本文采用Zφ-Yθ-Xψ的变换次序[15]。

故

(15)

(16)

转换的{Obi}坐标系下得

(17)

通过姿态计算出的值，然后与运动轨迹规划出来的值，进行相加，以达到调整偏航更正的新坐标点。

所以四足机器人末端腿部坐标值为

(18)

四足机器人姿态调节如图7所示，x_、y_调整四足COG在世界坐标中的位置，默认为(0，0，h)调节身体高度质心到足端高度。xi(t)和zi(t)为轨迹规划生成输出值，xi、yi和zi为姿态计算输出值，θ为逆解求解得到的关节角度，ψ、θ和φ为四足机器人在运行过程通过传感器分别得到的沿x轴横滚、沿y轴俯仰和沿z轴偏航值，反相器调节器实际上是PI控制。

图7 姿态调节

4 实验仿真与分析

为了验证上述分析和姿态控制方法的正确性，搭建MATLAB的Simscape动力学仿真模块，建立四足机器人。其中，机体质量10 kg，每条腿的质量0.5 kg，四足机器人以周期T=1 s,占空比为λ=0.5，即摆动相和支撑相各占总周期的一半，在地面以抬腿高度为H=20 mm，步长为S=40 mm向前对角小跑运动。采用Ode15s求解器，求解时间为32 s,主要在有无姿态控制作用下对比分析偏航和侧向移动距离。为了检测控制器的鲁棒性，分别进行2组实验。

a.实验1，假设质心在机体中心的理想状态，分别进行无姿态控制和姿态控制的运动分析，通过偏航和侧向移动距离的数据检测姿态控制器控制效果。

b.实验2，考虑机体质量分布不均匀实际情况，假设质心在机体中心沿Yw负方向偏移25 mm，进行同样的对比分析。2组实验机体质心位置如图8所示。

图8 四足机器人机体质心位置

仿真在0时刻时，机器人悬空离地面有一段微小距离，为了减少机器人落地瞬间的冲击力对后续运动周期的影响。设定四足机器人在前2 s保持站立姿态，2 s时刻足端轨迹规划器开始工作，机器人实现向前行走。在2 s时刻，先迈左前腿和右后腿，右前腿和左后腿起支撑作用。

4.1 实验1结果分析

从图9中明显看出对机器人施加姿态控制后，一直稳定在0附近，而未施加姿态控制时的偏航角在-1.0°～0.6°范围内波动。

图9 机身偏航角变化对比曲线

在均匀机体下无姿态控制和有姿态控制对机器人侧向距离的影响效果如图10所示。从图10曲线中看出，对机器人施加姿态控制后，其侧向距离在-15～20 mm范围波动，其波动范围小。而未施加姿态控制时，其侧向距离曲线往上成震荡式偏移，逐渐偏离直线曲线行走。

图10 机身侧向位移变化对比曲线

通过对图9和图10的分析，利用姿态反馈控制的方法，可以有效整定偏航运动，提高了四足机器人在对角小跑下的运动稳定性。

4.2 实验2结果分析

如图11所示，在未施加姿态控制作用下，偏航绕Zw正方向偏离2°，然后一直成振荡式往负方向偏。首先绕Zw正方向偏离2° 是由于机身往右后侧翻转时导致摆动的右后腿提前着地，突然产生的地面反作用力导致偏航绕Zw正方向偏，但是总体趋势往负偏是由于机体质心在右侧所导致。在施加姿态控制后，偏航角稳定在0附近，偏航角得到有效控制。

图11 机身偏航角变化对比曲线

在机体质量分布不均匀情况下无姿态控制和有姿态控制对机器人侧向距离的影响效果如图12所示。由于机器人的质心在右侧，右侧的作用力会导致机器人往右侧方偏。使用姿态控制算法后，有效抑制了机器人的偏转幅度。

图12 机身侧向位移变化对比曲线

通过对图11和图12的分析可知，尽管质心不位于机体几何中心处，但是利用姿态反馈控制的方法，还是可以有效地控制偏航角度和抑制侧向偏移的幅度。

5 结束语

本文在研究偏航现象进行理论分析的基础上，得到了重力产生的绕支撑对角线的翻转力矩和支撑相髋关节产生的反作用力矩，导致步态时序提前或延后产生绕机体对角线翻转出现偏航的根本原因，并针对偏航提出机体姿态控制方法，通过MATLAB/Simulink仿真验证了本文方法的正确性和有效性。

a.通过建立四足机器人整体运动学模型，并在一定合理假设下，通过数值计算方法，得到重力产生的绕支撑对角线的翻转力矩和支撑相髋关节产生的反作用力矩导致步态时序提前或延后产生绕机体对角线翻转出现偏航的根本原因。

b.为解决机器人运动偏航问题，建立运动模型。在给定的步态运动中修改足端轨迹坐标值，通过补偿来抑制偏航。

c.仿真结果表明，采用姿态控制模型进行偏航姿态控制，能够抑制偏航的产生，提高了四足机器人对角小跑运动的稳定性和可靠性。同时该方法可同样部署于俯仰、横滚，因此姿态控制模型具有一定的通用性。