参考工程代码 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也是考虑的效率问题。 这两个都可以供大家参考。