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的精简版的机器人运动学动力学讲义：

https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf

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

当R满秩时，求逆即可得到身体坐标系的角速度与世界坐标系的角速度的关系：

且在运动中，pitch,roll角度非常小，则上式可简化为：

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

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

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

 

此时将上文的单刚体运动方程写成状态空间的表达方式（xdot=Ax+Bu）为：

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

matlab中可用expm()函数。离散化结果如下：

通过这个模型，便可以计算出系统对应的输出，可以将其组织为一个MPC问题，

即求得最优的当前输入使得系统下一时刻输出尽可能接近期望值，同时也要保证输入量尽可能小。因为在实际机器人中，机器人的力矩是有限的，我们并不希望计算出一个很大的力矩。当然这算是一个很简单的cost函数，状态变量、输入量变化率等我们都没有加入约束。有了cost函数，实际已经可以开始求解了，但我们的控制对象是对实时性要求较高的腿足机器人，若直接对这种形式求解然后用于机器人控制，是不能将MPC周期控制在几十ms内的，会严重影响控制性能。

为了提高MPC的计算速度，除了减少约束条件，缩短预测/控制时域等外，可以将MPC转为显示预测控制（Explicit Mpc）,当今用于汽车ECU等对实时性要求较高的MPC大多也是Explicit Mpc形式，简单来说，Explicit Mpc是求出预测步长内的解的组合域，如下图所示，然后直接根据各个状态量查找对应的解。通过这种方案，MIT Cheetah 3上的MPC的频率大约为30hz。

关于Explicit Mpc的详细介绍，可以参考一下意大利的Bemporad大神的MPC教程，Benporad参与了MPC Toolbox和Hybrid Toolbox的开发，很多source code都是他自己亲自写的。Bemporad也有自己的公司ODSYS提供一些工业界可用的MPC算法。Bemporad教授是有一定的实际工程经验的，所以其教材在实际运用MPC方面也更有心得：http://cse.lab.imtlucca.it/~bemporad/mpc_course.html

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的力权重：4*10^-5,

IIT 的HyQReal :1*10^-9()