基于MPC与WBC的人形机器人控制框架

一、控制算法的概述
整个控制框架图，如下所示，算法分为，步态控制、外部接触控制、内部多关节协同控制、底层关节跟踪与感知模块。
 

主要介绍下外部接触模块与内部多关节协同模块：
外部接触模块是基于单刚体模型，产生支撑腿的末端接触力及腾空腿的末端位置轨迹。其中：

（1）支撑腿采用模型预测控制即MPC，依靠QP，优化产生足底力，输入的系统状态为机身的姿态、位置、角速度与速度，优化目标有两个，一个是当前状态与期望的状态误差尽量小，一个是系统输入尽量小，即足底力与力矩尽量小，令系统能量输出较低，在该优化目标下设置了4组约束，1组是z方向半封闭约束，即z方向力不能小于0的值，2是摩擦力约束，利用摩擦锥约束，3是防止足底反转与扭转的力矩约束，即防止足底力矩过大导致脚面反转或者扭转，采用的是QPOASES进行QP解算，在此优化目标与约束下，产生末端接触力。
（2）对于腾空腿，是应用的基于速度进行x，y方向落脚点计算的方法，具体的计算步骤在四足机器人控制中有详解，就不赘述了，z方向是由贝塞尔曲线组合成的相位可调的轨迹，曲线如下图所示，在相位末端z会延长一段，这是为了保证腾空腿触地。
内部多关节协同控制，此部分主要是基于WBC，不同于前述的MPC，WBC是基于全身动力学模型的方法，将外部接触控制生成的末端力矩速度位置参考映射成为关节位置