【MIT Cheetah】整体设计学习

控制框图：

主要流程上述框图已经交代的清楚了，但是具体中间是怎么样的，来一起捋一下。

控制指令

首先，控制指令中包括步态，最主要的是给出了狗的速度，通过质心轨迹规划器测算出质心的位置、速度包括角度、角速度之后，将这些数据传给MPC控制器。

MPC控制器

目的： 通过调节四足接地力，进而使机器狗的质心跟上指令给定的期望质心。
系统输入： 足端反作用力
状态变量： 质心位置、速度、姿态角、姿态角速度
状态方程依据： 单刚体动力学：即将机器狗看作单个刚体，忽略腿的影响（因为其四肢的轻量化设计及和全身相比具有较低的惯量，“By design, the robot has light limbs with low inertia as compared to the overall body. For this reason, we can reasonably simplify the control model to ignore the effects of the legs for planning ground reaction forces from the stance feet.”）。

进一步可以写成下面的形式：

其中，上述F与f代表含义相同，都是足端反作用力；pi 分别是4个足端的位置。

最终会化成一个线性模型：

A F = b

根据单刚体动力学方程，转化成状态空间方程

离散化

确定约束：

设计代价函数J：

进行优化，得到优化的足端力。

WBC控制器

得到MPC优化的足端力后，有两个方向：
一个是直接传给腿部关节控制器，根据力雅可比的计算转化成关节扭矩，直接进行PD控制，下发执行端。
另一个就是送到WBC控制器。

输入：
接触状态、支撑力、足端轨迹、质心轨迹 （绝大部分是从MPC计算得出的）
输出：
腿部各关节的力矩

为什么需要WBC控制器呢？
注意到MPC优化时的约束是基于现实世界摩擦等约束下的地面反作用力，在摆动相的时候认为足端接触力为0了。这在机械狗慢速行走的时候可以，如果行动速度变快，每条腿在空中的时间变多，足端接触力只在很短的时间内有，那再用这一项来进行整机的控制是说不通的。此时机体处于欠约束状态，视为与虚拟浮动基座相连。

自由度变为 12+6 出现了冗余自由度，状态不确定，所以需要更换动力学模型对其进行更高精度的控制。

动力学模型采用浮动基动力学模型：

选择矩阵定义如下：

WBC控制器(Whole Body Control)，在这里的应用是采用了零空间理论优先级控制的思想，适用于这种自由度冗余的情况。

1. 划分任务优先级：

上面4个优先级的设置其实各对应一个输入，可以根据这个任务的划分得到每个任务对应的雅可比矩阵。相关介绍有一篇博客写的非常清楚。

2. 计算各关节位置、速度、加速度
根据零空间理论，得到每个人物的雅可比矩阵J1 J2 J3 J4就可以导出关节位置递推关系：

3. 松弛优化 计算关节力矩
WBC作为腿部快速摆动时计算关节力矩的控制器，是作为补充。算法中是希望MPC和WBC的结果都满足动力学方程，所以添加松弛变量，使用QP方法再进行优化：


QP的优化约束中可以看出什么？
等式约束是让关节扭矩更小，剩下俩是我们设定的机身浮动加速度和足端力的浮动值，代价函数中权重矩阵Q1和Q2则调节的是你更能够接受哪个浮动变量。

将上述计算得到的位置、速度、扭矩下发给电机控制器：

状态估计

估计顺序：ContactEstimator->OrientationEstimator->PositionVelocityEstimator
触地估计——>质心（姿态、角速度、加速度）估计——>足端（位置、线速度）估计
姿态估计： 使用启动时的朝向作为rz = 0 其他姿态及角速度直接使用IMU数据。
**位置估计：**位置和速度估计采用kalman滤波融合imu数据和足底里程计，其中足底里程计假设支撑足不打滑，加速度直接采用加速计数据。

问题记录：

1. 步态如何与MPC相结合？
2. WBIC 是MPC+WBC，可以说是WBC包含在MPC中，运行一次MPC可以包括好多次WBC，频率不一样的，具体机理及原因是？
3. 浮动基动力学目的？
4. 程序中还用到旋量，具体目的是？（目前猜测是用来计算足端位置等）