四足机器人质心准静态控制器（一）

参考工程代码 GitHub

准静态控制器

在六个维度上采用PD控制器，维持期望的位置和速度获得一个期望的加速度。 六个维度分别为三轴位移和三轴欧拉角。注意其中的Kp和Kd系数都是对每个维度可以调节的。 另外，有些论文还有另一种写法，姿态控制部分，角度误差可以转化为旋转矩阵进行表示，再从李群映射到向量空间。

接下来对矢量力如何组成进行介绍。 我们以四足机器人静态站立为例子，也就是四条腿都是接触地面的，当然还会有两条腿或者三条腿接触地面的情况。这里为了简化表述就以四条腿接触为例子进行讨论。由于，本课程的四足机器人是三自由度腿部结构，点接触地面。所以末端与地面交互的有效空间就只有三个维度，作用点也就是足末端。 那么站立的情况下，可以堆叠足端向量为一个带求解的向量Fc，四腿站立，那么维度就是12，三腿站立那么维度就可以是9。 接下来就是建立堆叠向量力与质心的关系。根据力学原理，质心受力可以为这些向量力的和，这一点体现在A矩阵第一行与堆叠的向量力相乘得到质心受力。 足端相对于质心位置与矢量力叉乘可以得到质心所受转矩，这一点体现在A矩阵第二行。其中的p1x指的是，1号腿再质心坐标系坐标系下的位置，所构成的反对称矩阵。 A和Fc相乘可以得到矢量力Fc对质心的影响。 那么这种影响应该满足，期望加速度与质量惯量相乘后的期望力和转矩。 这里也可在末端添加重力的补偿。

为了让Fc对质心的影响满足期望力和转矩 我们可以构建一个优化目标，最小化该目标，求解堆叠的力向量。按照原理来讲，这一个式子就可已得到足端分配力，且质心受到力控制满足PD控制期望。 但是由于堆叠向量力维度空间很高，所以解有非常多的可能，且有可能还不满足力控的一项原则，输出力尽可能的小，降低能耗。 由于这些原因，所以在优化目标上再额外的添加一项来得到最小的力输出。 当有两个优化目标，优化结果就会有一个偏向性的问题。所以给以权重系数Q和R来决定优化倾向性。 举个例子，当设置Q很大的时候，当最小化目标时，会更多考虑求解力更加准确，这样才能更多的降低第一项，使总体目标更小。同样的，增大R则会更加省力，但是求解力的满足PD控制的期望会降低。 这个Q和R的调整一般基于经验调节，合适即可。 目前的优化目标只有这么两项，但是有些论文中还有额外的添加一项，降低力变化的速度，理论上这其实也是很有必要的，这样不会给关节带来太多的负担。

由于足式机器人要与地面交互，那么很容易想到足端力，还需要满足地面摩擦的约束。 这里有个前提，现在所考虑的摩擦约束都是在平地上的。在给出的工程中也仅仅考虑了平地。 先考虑单腿的摩擦约束，很简单可以得到 Z轴支撑力乘以摩擦系数可以得到xy轴合力的边界。 如此，可以很容易想象到xy轴力应该在一个圆内，那么在考虑z轴可以发现足端力被约束在一个圆锥内。 但由于为了构建QP优化的标准型，约束部分也应该是线性关系，所以这里直接将Z轴力和摩擦系数乘积作为边界。需要说明的是，这个摩擦系数不是真实值，如此表示会比真实值小。有的地方会写出 u/根号2 ，这个其实就是xy约束圆内取个最大四边形，这个此时摩擦系数是一个理想真实值。 不管怎样，这种约束的线性化处理，都会导致摩擦约束空间会是一个四棱锥。 然后实际机器人中，由于地面环境的复杂，这种参数一般依据经验调试。 以一个足端力分配力来说明 约束条件可以要满足gi和hi，我没有给xy轴力的数值边界，而是只用z轴边界。这个式子很重要，他的意义需要大家一行一行对应的来分析过。 gi第一行 约束的是X轴最大力， 第二行是x轴最小力。  gi第三行 约束的是y轴最大力， 第4行是y轴最小力。   第5行约束的是z轴最大力，第6行约束的是z轴最小力。 对于一个堆叠的足端力，那我们可以很容易写成 像 G 和h 这样的表达的形式。 所以到目前为止，质心准静态控制器最核心的地方就出来了。

优化目标的目标和约束构建出来了，现在的问题就是如何求解。 这是一个非常经典的二次规划问题。在工程中，我给出来一份手撕的求解代码和一份利用CVXOPT求解的方式。 用CVXOPT也是考虑的效率问题。 这两个都可以供大家参考。