matlab 自控控制相关的函数_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函数，状态变量、输入量变化率等我们都没有加入约束。

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()

​