注：本人并非专业研究足式机器人，只是兴趣爱好开发，如有错误请谅解！

第2章 Moco-8四足机器人控制算法简介

• 2.1 分解式虚拟模型步态算法
• 2.1.1 VMC支撑相控制算法

对于足式机器人来说有许多不同的步态，最典型的如ZMP算法规划重心在着地平面移动轨迹的爬行步态，对角步态是足式机器人最常用的步态由于Moco-8机器人每个腿仅有2Dof、没有胯关节因此其主要以对角步态为主，其余如Bound步态或Gallop步态则较为复杂，采用舵机作为驱动的话难以实现。

在控制机器人时用户往往不关心具体每个腿的运动轨迹，控制命令主要是针对机器人顶层的期望运动状态(如速度、方向、姿态角等期望值)并与传感器反馈得到需要施加的力或者力矩，再通过调整机器人的腿部运动(如改变步幅、出力大小、抬腿高度等)来实现这些力或力矩的施加，从而达到调节机器人运动状态的目的。该方法最著名的是波士顿动力公司Raibert提出的三通道控制框架和虚拟模型控制方法，其将机器人控制分解为三部分：弹跳高度控制、前进速度控制和姿态控制。根据期望跳起的高度，结合机器人的质量、腿的弹性系数和重力的大小，计算得到期望施加的竖直推力大小；根据期望速度和当前速度的差值，计算得到下一步足端相对机体的期望位置并在腾空相摆动至该位置，从而在下一步着地以后通过重力的作用间接影响前进速度大小。该方法中仅需要使用PD控制器来模拟弹簧阻尼效果，通过结合机器人设计参数整定控制器能有效保证机器人的抗扰动性和稳定性。

Moco-8机器人步态算法主要参考了《四足机器人对角小跑步态虚拟模型（VMC ）直觉控制方法研究》并进行了改进和优化。VMC 控制方法是利用假想的虚拟弹簧构件连接机器人内部作用点，或者连接作用点与外部环境，产生相应的虚拟力来“驱使”机器人实现期望的运动。这些虚拟力通过Jacobian 矩阵计算得到期望的关节力矩作为电机的控制输入，因此其首先需要将全局空间下的作用力转换关节力矩，二者的映射矩阵即关节雅克比矩阵。为求取该矩阵首先需要依据机器人腿部构型获取其正运动学（即已知关节角度能计算出足尖位置），以Moco-8采用2Dof腿部构型为例其结构如图所示。

如图所示，机器人小腿长度为L1大腿长度为L2，采用并联的方式以两个电机驱动摇臂改变角度

从而实现足端位置

的运动，采用初中所学的三角函数和余弦定理即可得到运动学正解表达式为：

为简化后续计算可将工作空间坐标系固定在机体轴上，则进一步对虚拟腿长进行分解得到足尖的位置如下：

同理可以得到其运动学逆解公式，即由足端位置得到对应摇臂角度：

进一步有：

则对运动学正解公式求取足端位置对于摇臂角度的偏导得到对应的雅克比矩阵 ：

则有：

其中：

上式中的雅克比矩阵其将关节速度映射成足端移动速度，依据虚功原理可以得到空间作用力与电机力矩的关系如下：

考虑到Moco-8关节伺服为舵机无法实现力矩控制，因此我们直接采用舵机角速度控制命令代替力矩输出并增加一个P控制器调整增益，最终得到关节舵机角速度与空间作用力关节为：

其中Motor为舵机角速度与力矩输出的模型，其可以通过离线辨识的方式得到（在我们的程序中仅使用了一个一阶惯性系统进行模拟），上式实现由某腿作用力转换得到对应舵机转速的目的。进一步，引入IMU反馈通过调整左右两侧腿下蹬力实现对姿态控制，因此基于该思想可以得到如下图所示的虚拟控制模型。

基于上述方法计算关节力矩不需要求解任何逆运算，并能十分直观方便地引入反馈数据（角度传感器或力传感器）。进一步，如上图所示采用三通道控制框架分别在机器人高度、前后和俯仰角上增加虚拟弹簧阻尼器，即构建一个PD反馈控制器：

其中exp为机器人期望的位置或速度，K为弹簧系数D为阻尼系数，通过调整二者能模拟出不同特性的弹簧效果，另外还可以通过修改上式引入不同的控制算法，如Moco-8步态库中就使用了自抗扰ADRC控制器和扩展状态观测器ESO这类非线性控制器来获得更好的性能。进一步可以构建如下的三通道反馈控制系统：

上式中x轴直接使用速度来满足遥控机体速度的目的，由于引入了机器人真实的反馈数据也提高了其在外力作用下的抗扰动能力。另外，其中上标变量均为估计状态值，由于机器人移动中会与地面直接冲击会对IMU造成十，分大的影响，因此需要使用滤波器或观测器来估计机器人的位姿减少控制输出尖峰，如Moco-8步态库中就使用ESO观测机器人角速度在保证机器人姿态控制平稳的前提下增大控制增益。

与四轴飞行器一样需要将控制输出分配到驱动器上，在四轴飞行器中通过调整电机转速实现姿态控制，而四足机器人中基于作用力与反作用力调整各腿下蹬力度实现姿态、位置、速度控制。进一步，采用如下图所示的分解式VMC控制方式，将机体总体虚拟力矩输出分解到各腿中。

则VMC分解原则如下：

(1)x轴：将全局虚拟力分解到前后两组腿中，由于前后腿一起提供前进动力因此可对两腿分别设计独立的x轴反馈控制器，并且将参数调节为  。

(2)z轴：同理将全局虚拟力分解到前后两组腿中，由于前后两腿同时完成支撑因此可独立设计z轴控制器，并调节参数为  ，同时对于机体较长那一轴来说希望机器移动时能如下图所示跟随地面角度变化而移动，因此可以在移动时设定 。

(3)俯仰轴：机器人调节俯仰角可简化为增加前后俩组腿的不同高度，因此同理可将俯仰轴控制输出叠加至原始期望高度上，这里与四轴控制算法中在某一悬停油门上设计高度控制器的思想十分类似，因此可以得到：

• 2.1.2 VMC摆动相控制算法

四足机器人对角步态主要可以划分为支撑相和摆动相，前者可以基于所述分解式VMC算法进行控制，摆动相则有如下两种不同的策略：

(1)轨迹规划+运动学逆解：

摆动相即实现机器人向期望方向的跨腿以保证机器人的持续移动，为保证机器人跨腿时能具有较小的冲击力并且不会出现拖腿的现象，一般采用摆线轨迹足端轨迹减小地面的冲击力。

轨迹规划有许多的方法，如多项式拟合，正余弦组合或最优轨迹，在Moco-8步态库中采用了最小Jerk轨迹规划算法来获取高效平滑可实时调节的足端轨迹。这里仅介绍最常用的摆线规划算法，其与别的算法类似只需要给定起点、中点和端点位置作为输入即可，设足端当前起点位置为

  ，期望落足点为

  ，如跨腿高度为 h ，则中点位置为

  。为了使规划的摆动轨迹能够根据机器人机体速度而自适应调整步幅大小，仍采用Raibert 提出的方法计算落点：

其中

  为步态周期，

  为支撑相占空比。则通过调节增益参数期望落脚点坐标就会随着机器人实时速度的变化而改变，即速度越大步幅越大，速度越小步幅越小，速度为零时则为原地踏步。对于  来说则可直接等于该腿当前的期望高度

  ，则摆线轨迹如下：

其中

  ，则在获取跨腿轨迹后可直接使用各时刻足尖位置采用运动学逆解计算出对应的摇臂角度。如果添加了着地传感器则在跨腿过程中检测到触地，该腿立刻停止并切换到支撑相参与VMC的反馈控制。

(2)轨迹规划+VMC控制：

同理在规划处期望的足尖运动轨迹后，同样可以采用前文所述的VMC虚拟模型控制方法在足尖上设置虚拟弹簧阻尼模块并设计反馈控制器对轨迹进行跟踪，该方法比较适合于无刷电机驱动的机器人，其在跨腿中腿部受外力作用下能柔性的运动（类似拉住机器人的脚），而由于舵机已有独立位置闭环因此跨腿中拉拽会直接翻倒，需要进一步添加力传感器采用导纳控制方法才能实现相同的效果，具体方法可以参考（https://zhuanlan.zhihu.com/p/65034861）。

• 2.2 VMC总体步态算法

综上，可以得到Moco-8所使用的VMC总体步态算法，为实现对角步态行走仍将机器人斜对角线上的腿划分为摆动相和支撑相，采用周期调度的状态机进行逻辑切换，其原理图如下所示。

如图所示的步态算法首先在每个控制周期完成对机器人自身位置、姿态角、速度、足尖位置和雅克比矩阵的计算与状态估计，在完成对IMU数据的采集后进行中值滤波和低通滤波预处理，同时使用传感器标定参数对原始数据进行校正。通过姿态解算获取机器人的姿态角并得到机体坐标系下的加速度，为减少机器人与地面冲击对原始数据的影响进一步结合机器人模型和姿态解算结果观测机体角速度；使用运动学正解得到当前各足尖的位置并计算雅克比矩阵，通过对足端位置微分+低通滤波结合机器人模型得到里程计数据；进一步，基于卡尔曼滤波器融合里程计和外部定位传感器数据估计机器人连续实时的速度和位置。

对于摆动相来说，首先基于机器人实时状态估计结合顶层控制命令对摆动相的落足点进行规划，并计算出足尖运动轨迹，通过运动学逆解得到对应关节角度；对于支撑相则使用所述VMC三通道控制算法得到对应的全局虚拟力，进一步采用分解式VMC理念对全局作用力在各腿进行分解得到各关节力矩，考虑舵机无法进行力矩控制使用伪力矩模型将VMC控制输出转化为舵机期望转速并更新当前舵机摇臂角度。最终，针对不同的伺服系统将期望摇臂角度转化为对应的伺服控制输出，如舵机则将期望角度转化为相应的PWM信号。在跨腿过程中，当检测到触地信号则该腿立即停止并切换至支撑相参加VMC反馈控制，当所有腿都着地后进行支撑相和摆动相的切换，并完成一个步态周期。

（1）横滚轴控制

与俯仰轴类似，基于VMC控制思想同样在横滚轴增加一个虚拟弹簧阻尼器，由于Moco-8机器人没有胯关节，因此无法通过其侧摆来实现横滚轴控制，这里仍然采用俯仰轴改变左右两侧腿长的方式，这也造成了8自由度机器人在横滚轴方向下控制稳定性较差，只能通过增加步态频率来提高机器人的稳定性，无法像SpotMini那样两腿站立仍然可以保证机体平衡，因此构建如下横滚轴PD控制器：

基于VMC分解方法将虚拟力分配至左右两侧的腿期望高度上：

（2）航向控制

由于Moco-8机器人各腿仅有2Dof，因此无法直接在机体侧向产生移动速度，可以通过主动控制横滚角倾斜借助重力分量实现慢速的左右平移。对于航向控制来说这里则采用的是四轮小车底盘差速控制的方式，即调节左右两侧足尖移动速度实现旋转，因此同样增加一个虚拟弹簧模组并将控制输出分配至两侧腿的期望速度上：

基于VMC分解方法将虚拟力分配至左右两侧的腿期望速度上:

（3）动态步频调节

对于足式机器人来说通常情况下其步频越高机器人的稳定性也越好，但是其能量消耗会大大增加，同时当步态过高时会出现严重的打滑情况造成移动效率的降低。当机器人受到外力推动或者上下陡坡时适当增加步频则能有效提高稳定性，由于机器人在运动过程中机体趋于保持与地面平行，机体的俯仰角可以认为是斜坡的坡度值估计。这样就可以根据机体俯仰角的大小来自动调节支撑相周期，也就是希望支撑相周期随着俯仰角(绝对值)的增大而减小，当俯仰角为零时保持一个恒定的支撑相周期，则可使用机器人俯仰角估计构建如下的动态步频调整机制：

（4）摆动腿延伸

由于机器人在崎岖路面行走时在无视觉或激光雷达传感器构建地面信息的前提下是无法知道落足点离地面的高度，如地面高于落足点则当有着地传感器时可以通过触地信号实现摆动腿的停止防止将机器人将自己蹬倒；而对于高度低于落足点时意味着摆动结束足尖仍然无法着地，则此时如直接进行切换会造成机器人的剧烈晃动，因此需要在摆动结束进一步下蹬。

如上图所示为实现全着地的目的可以在跨腿结束后沿着z坐标继续延长足尖位置即：

（5）地形跟随姿态调节

四足机器人移动时往往希望能跟随地面角度变化移动，这样俯仰轴反馈数据无法直接使用IMU解算的角度，为此可以借助机器人结构建立如下的伪俯仰角作为反馈：

其中L为机器人长度，在机器人上下坡时由于重力作用会存在“后坐”的趋势，如果此时不对重心进行调节则十分容易出现机器人前脚悬空打滑的现象，从而造成机器人无法顺利上坡，因此可以使用IMU解算俯仰角对机器人重心进行实时调节。这里则直接将重心调节转化为对跨腿期望落足点的整体平移，原理如下图所示。

如图所示，采用倾斜姿势可以有效解决“后坐”问题。腿部的倾斜使得质心投影大致处于地面支撑线的中间，使得机器人在斜坡上仍然具有较大的稳定裕度，并进一步地使得重力分配更加均衡，则以x轴为例其跨腿修正量为：

其中  为机器人当前的质心高度，其可以采用求取当前所有支撑相腿长均值得到，则修正后的落足点坐标为：

其中

  为固定增加的重心偏移量，其可以补偿在机器人上安装设备（如激光雷达或机器臂）后质量分布的不均匀。同时，考虑到机体倾斜时重力作用下机器人足尖打滑引起的机体下滑，可以调整VMC控制算法增加积分项或者引入前馈控制，这里采用后者在虚拟力输出中补偿重力分量：

其中 m 为机器人总体质量。

在下一节中将介绍Moco-8的导航算法，包括姿态解算、状态观测器设计、运动学里程计推算和卡尔曼滤波组合导航！