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

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

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

（1）支撑腿采用模型预测控制即MPC，依靠QP，优化产生足底力，输入的系统状态为机身的姿态、位置、角速度与速度，优化目标有两个，一个是当前状态与期望的状态误差尽量小，一个是系统输入尽量小，即足底力与力矩尽量小，令系统能量输出较低，在该优化目标下设置了4组约束，1组是z方向半封闭约束，即z方向力不能小于0的值，2是摩擦力约束，利用摩擦锥约束，3是防止足底反转与扭转的力矩约束，即防止足底力矩过大导致脚面反转或者扭转，采用的是QPOASES进行QP解算，在此优化目标与约束下，产生末端接触力。
（2）对于腾空腿，是应用的基于速度进行x，y方向落脚点计算的方法，具体的计算步骤在四足机器人控制中有详解，就不赘述了，z方向是由贝塞尔曲线组合成的相位可调的轨迹，曲线如下图所示，在相位末端z会延长一段，这是为了保证腾空腿触地。
内部多关节协同控制，此部分主要是基于WBC，不同于前述的MPC，WBC是基于全身动力学模型的方法，将外部接触控制生成的末端力矩速度位置参考映射成为关节位置、速度、力矩参考。
他分为两个步骤：
第一，是运动学求解器（逆运动学），根据机身当前和期望状态，生成关节的位置、速度、加速度，这部分是应用基于零空间投影的多任务优先级控制方法，低优先级任务会在高优先级解的范围内寻找解，不会干扰到高优先级任务，实现冗余自由度机器人的控制，任务一般会分成以下几个，足底接触、机身位置、机身速度、摆动腿轨迹、手部关节控制、冗余关节，这些任务可以根据实际进行顺序的调节，经过几个任务的累加，生成关节的位置、速度、加速度；
第二，是动力学求解器，根据运动学求解器的输出还有期望的足底接触力，生成最终的关节参考，这里实际上是一个QP优化问题，对输入的期望足底接触力与关节的加速度的调节增量进行优化，设置动力学约束，来保证求解结果符合动力学模型，加入摩擦约束及接触力大小约束来保证求解结果在正常范围内，最终得到关节的位置、速度、力矩参考。
二、开发流程
开发流程如下所示：
 

依照以上过程，整个调试过程就完成了，如果有还没上手过实物调试的朋友，可以参考。

三、pinocchio动力学求解库
pinocchio仓库基于C++的开源动力学求解库，同时也提供了python的接口，可以根据输入的URDF模型进行机器人运动学及动力学的参数计算，相关安装步骤可参考官网，也可使用openloong-dyn-control中的third party
具体功能如下：

• 动力学方程参数的求解；
• 逆运动学解算；
• 正运动学求解；
• 雅可比矩阵求解。

Pinocchio在以上模块中的底层关节跟踪与感知模块中，其计算结果在控制算法中主要用在以下几个地方，一个是正逆运动学的求解，其获取的末端位置在摆动腿轨迹计算、WBC的任务优先级计算等模块都有应用，第二是任务优先级中的雅可比矩阵的获取，第三是WBC的QP求解中，需要动力学约束，这里应用了pinocchio库计算的动力学方程参数，第四是状态估计中的足底力估计，也是应用到了动力学方程参数。
关于pinocchio应用的注意点如下所示：
1、由于人形机器人是个浮动基，q包含了质心位置、质心姿态、关节位置信息，其中前7个为浮动基的数据，浮动基的姿态采用四元数，因此q比dq多一维；
2、pinocchio中默认的关节顺序，是根据urdf中joint的名字及串联关系确定的，先根据串联关系，然后根据其首字母进行排列；
3、输入pinocchio的dq浮动基速度为本体坐标系下的速度；
4、urdf关节类型要选revolute而不是continuous，不然会q的个数会多一个；
5、获取雅可比的三种坐标形式，pinocchio中的雅可比有三种坐标形式，分别是WORLD、LOCAL、LOCAL_WORLD_ALIGNED，在获取雅可比时选取何种坐标决定了雅可比根据dq计算出的末端速度是在哪个坐标系下，world坐标系是坐标轴朝向与世界坐标系平行，原点位于机身最初的base处，local的坐标与雅可比求解的末端坐标重合，例如我们获取的是脚踝的雅可比，则local坐标系即与脚踝的坐标重合，而LOCAL_WORLD_ALIGNED的坐标系是指坐标轴朝向与世界一致，坐标轴原点与脚踝坐标系原点重合，openloong程序中雅可比应用的是LOCAL_WORLD_ALIGNED，因为程序中用的都是末端在世界坐标下的速度，而这个坐标系的坐标轴朝向与世界相同，因此速度符合算法中的要求。
6、逆运动学的求解，pinocchio官网中提供了一种闭环逆运动学的求解方法，迭代获得解，要注意为了避免奇异点问题，采用阻尼伪逆，这种方式能够减少求解时的震荡，官网中有逆运动学的范例程序，可以先用其中的默认值，如果出现求解问题，可以调节DT与参数，前面说DT类似于Kp，那就有类似于阻尼的作用，即Kd的作用，大家可以根据pd的作用机理去调节这两个参数。