【转载】MIT四足机器人Cheetah 3控制方案笔记

转载：知乎 Wenboxing
https://zhuanlan.zhihu.com/p/190028074

MIT四足机器人Cheetah 3控制方案理解笔记（1）——摆动腿控制、单刚体模型平衡控制器

之前一段时间在阅读MIT四足机器人Cheetah 3以及Mini Cheetah控制方案的相关论文，在此处做一些笔记。Cheetah 3与Mini Cheetah的控制方案大同小异，此处先以Cheetah 3为例。
一、摆动腿控制

腿足机器人低速运动情况下每条腿都会在支持相（维持机器人身体高度以及姿态）、摆动相（将腿摆到期望的位置）之间循环交替。通常的摆动腿的控制方法较为简单，《MIT Cheetah 3：Design and Control of a Robust, Dynamic Quadruped Robot》中提出摆动腿的期望目标位置为：

对摆动腿末端从初始位置到目标位置进行插值（例如三次样条或者三次贝塞尔），获取每个控制周期的腿末端期望位置和速度，在操作空间进行阻抗控制，后利用关节末端相对于身体坐标系的雅可比矩阵计算该腿的各关节力矩即可。如果解算了动力学模型，此处可以加一个动力学力矩前馈：

​​腿足机器人很多时候都会通过选择落脚点来控制机器人的速度，这一点在《legged robot that balance》中进行了详细的阐释，主要公式为如下，此处可将机器人看作一个弹簧倒立摆模型。

二、平衡控制器
支撑腿需要维持机器人身体的高度和姿态，即维持机器人的身体平衡，在Cheetah 3中有两种平衡控制器——基于集中质量模型（lumped model）的平衡控制器和凸模型预测控制（Convex Mpc）。先阐述基于集中质量模型的平衡控制器。

集中质量模型即单刚体模型，也即可以忽略腿的作用，将地面对腿部的作用直接等效作用在质心，这种假设在机器人腿部质量相对于总质量占比很小的情况下是合理的（Cheetah 3与Mini Cheetah的4条腿总质量都不机器人重量的10%）。在将机器人看作单刚体模型的情况下，机器人运动方程为：

所用到的也是基本的牛顿运动定律，合力=ma，合力矩=惯量*角加速度。其中F为作用在四条腿的地面反作用力。然后依旧通过PD控制律计算期望的质心加速度和角加速度：

​这个PD方法详细描述在《High-slope terrain locomotion for torque-controlled quadruped robots》中，其实就是求了一个期望的质心状态

​笔者进行复现过程中，发现此处角加速度的PD求解的Kp项也能用期望、实际角度之差。然后为了求解较理想的地面反作用力，可以将该问题转化为一个二次规划问题：

其中S为正定矩阵，保证了该问题有惟一的全局最小值，此处可以用Quadprog++求解，求得地面反作用力后，则期望的腿末端力为其负值，利用雅可比计算关节力矩即可。

这个平衡控制器的模型是一个高度简化的单刚体模型，但在MIT实际测试中表现出了不错的效果，在Cheetah 3跳跃上桌子的控制方案中，这个模型用来做机器人的落地稳定控制器。

实际在笔者复现过程中，发现这个模型也能用来做跳跃阶段的地面反作用力规划，效果也挺不错。其实这个模型就是《Online Planning for Autonomous Running Jumps Over Obstacles in High-Speed Quadrupeds》中Cheetah 2高速跑跳时用的模型的另一种形式的表述。Cheetah 2正是基于此模型确定腿部的力矩轮廓（force profile），实现Cheetah 2的高速跑跳。

笔者在仿真中使用该模型进行四足机器人跳跃以及落地稳定控制都表现出了不错的效果，例如下图是让机器人身体跳上挡板。

MIT四足机器人Cheetah 3控制方案理解笔记（2）——Convex Mpc身体姿态控制

在“MIT四足机器人Cheetah 3控制方案理解笔记（1）”中，主要简单的总结了下一般情况下腿足机器人的摆动腿控制方法以及Cheetah 3的基于集中质量模型的平衡控制器。此外，MIT还在Cheetah 3以及Mini Cheetah上采用了Convex MPC平衡控制器，在其2018年论文《Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control》中，Cheetah 3通过Convex MPC平衡控制器达到了3m/s的奔跑速度：

为了将论文中的方案部署到自己的腿足机器人上，笔者此处依次从问题分析、模型简化、方程列写、QP表达形式组织等谈谈自己对这篇论文的一些理解。

1）首先是描述机器人状态的主要变量的选取。对于机器人自身运动状态的描述，姿态以及位置层面，可以使用姿态角（roll ,pitch,yaw）和质心位置(x,y,z)来描述，同时为了描述其运动层面，可以加入姿态角角速度以及机器人质心的运动速度。因此，在机器人状态描述层面，共有姿态角、质心位置、姿态角角速度、质心速度共12个量描述。在外界输入层面，现今大多腿足机器人的MPC都是规划质心轨迹，而后采用逆动力学跟随质心轨迹。但这种方法在实际工程操作中，首先是逆动力学求解难度大，其次在机器人存在空中相时，也很难保证能够跟随所规划的质心轨迹，出于力控的角度，更理想的应该是直接规划出关节力矩，亦或能够在其基础上较简单的求出各关节力矩。

2）大致确定了我们希望的状态描述量以及需要规划的量，此处之前提过的单刚体模型恰好满足这些条件，单刚体模型的运动方程如下：

其中第三项其实就是旋转矩阵的导与角速度的关系，在一般的机器人学教材中都有。

不清楚的可以看一下ETH的精简版的机器人运动学动力学讲义：
链接: 讲义

通过IMU获得的角速度仅是机器人相对于身体坐标系的角速度。身体坐标系中的运动变换到世界坐标系可通过左乘一个变换矩阵R，论文中是依据ZYX顺序变换，这也是目前很多论文中通用的变换方式：

当R满秩时，求逆即可得到身体坐标系的角速度与世界坐标系的角速度的关系：
且在运动中，pitch,roll角度非常小，则上式可简化为：

而此时的变换矩阵恰好是绕z轴旋转的变换矩阵，即

同时惯量*角加速度项的微分项为

​此处第二项是因为这个求导操作是在非惯性系下进行的，所以多了个叉乘项。机器人相对于世界坐标系的惯量表达式为：

上文已将R简化为绕z轴的旋转矩阵，此处依旧适用：

此时将上文的单刚体运动方程写成状态空间的表达方式（xdot=Ax+Bu）为：
Cheetah 3中是将最后的重力加速度项也整合到了状态矩阵A中，此处笔者为了表述更加易懂就将其先单拎出来。这是连续时间的状态方程，因为最终需要计算机编程实现，需要将其离散化。要将A、B矩阵离散化，可以将他们分别乘以控制周期dt，而后求矩阵指数即可，Eigen中：

matlab中可用expm()函数。离散化结果如下：
通过这个模型，便可以计算出系统对应的输出，可以将其组织为一个MPC问题，
即求得最优的当前输入使得系统下一时刻输出尽可能接近期望值，同时也要保证输入量尽可能小。因为在实际机器人中，机器人的力矩是有限的，我们并不希望计算出一个很大的力矩。当然这算是一个很简单的cost函数，状态变量、输入量变化率等我们都没有加入约束。

Cheetah 3的MPC问题转化为QP的详细推导过程如下：

假设预测步长为n步，将n步内系统的状态以及输入量写成如下形式：
则上述的优化问题形式可转化为：
其中Q、R为半正定或者正定，以便保证该问题有界。同时系统的离散状态方程解为：
x(t)为系统的初始状态，则上面的X还可以写成：
写为X=Aqpx(t)+BqpU,带入上式，优化问题的表达形式变为：

至此该问题已经转化为一个标准的QP问题，每次取MPC更新后取第一步的U作为控制输入来控制机器人的支撑腿，可以通过对Q、R两个权重矩阵进行调参来改善支撑腿的性能。此处输入的R矩阵，笔者在实际调参中发现R的权重值设的越小，所生成的力就越大，通过查阅IIT的一篇MPC文献，此种现象是正常的。

MIT Cheetah3的力权重：410^-5,

IIT 的HyQReal :110^-9()

MIT四足机器人控制方案理解笔记（3）——Mini Cheetah 19年的MPC+WBC控制方案

2019年，MIT在Mini Cheetah上采用了MPC+WBC的控制方案，论文《Highly Dynamic Quadruped Locomotion via whole-body impulse control and model predictive control》中称加入WBC后Mini的最高运动速度由18年只采用MPC时的2.45m/s增加到19年的3.7m/s。
mini上所用的MPC与Cheetah 3上的相同，笔者在上一节中也谈了下自己对Cheetah 3上的凸模型预测控制的理解。在cheetah 3中，MPC优化出的地面反作用力取负号后左乘雅可比转置得到关节力矩，笔者的理解是此处的关节力矩的计算更多的是反映了静力学层面的关系，其间并没有考虑关节速度、加速度等对关节力矩的影响，即没有考虑动力学。而mini上的WBC恰好是在MPC计算得到地面反作用力的基础上利用动力学模型得到关节的位置、速度、加速度以及力矩这些量，实现更精准的控制。因为机器人是在不断运动的，因此mini所用的是机器人浮动基动力学模型，即机器人的基座是运动的，求解难度是大于基座固定不动的工业机器人的。浮动基动力学方程表达形式如下：
mini中的WBC的核心思想是操作空间有优先级的多任务控制组织方案，关于优先级、多任务这点，举个简单的例子，比如一个机械臂，指定了期望的末端速度，同时我们要求在保证达到期望的末端速度前提下，机械臂的第一、二个关节速度要尽可能接近0，这便是一个简单的有优先级的多任务控制问题。解决此类问题最常用的方法是零空间组织法（null space projection）。

关于null space projection,可以看一下ETH的精简机器人学讲义：https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf，笔者刚所举的例子在这份讲义的第44页有详细的讲解。
mini的关节位置、速度、加速度求解其实也是用了相同的方法：

这里位置、速度的任务雅可比在讲义的43页有阐述，加速度的任务雅可比在讲义的73页也有阐述（主要是基于接触约束导出的null space projection）然后，便是将计算出的位置、速度、加速度带入动力学方程做一个优化，
​关于关节位置的计算论文中是说：

其中qj是此刻测得的关节位置，在摆动腿的控制中，大多是从初始位置到末端位置进行插值，然后计算每个控制周期的期望关节位置、速度。我的理解是此处摆动腿便不能再用插值的方式进行控制，而是每个控制周期都会有基于null space projection的关节位置、速度指令，需要对其做PD控制律，然后再加上一个基于动力学模型的力矩前馈。

论文中优化问题的核心思想是在满足动力学方程约束的情况下，允许机器人基座与期望状态之间存在些许的偏差，这种放松更有利于机器人的高速运动，而后将优化出的地面反作用力、关节加速度以及之前计算】得到的关节位置、速度带到动力学方程里：

便可以求出支撑腿、摆动腿的力矩，（摆动腿没有接触力fr这一项）求出的这个力矩会做为一个前馈，然后加上关节位置、速度的PD控制律，对机器人的支撑腿、摆动腿进行控制。按论文中所述，这种控制方案让mini跑的更快了，如下图：
按照笔者的理解，其实这一套方案要部署到自己的机器人上，难度是很大的。尤其是浮动基的动力学建模，非常复杂，虽说好像目前有部分软件可以直接基于机器人模型文件，例如URDF自动求出机器人的浮动基动力学模型，但笔者还没使用过。如果有使用过的读者，还请赐教一下。