【开源】基于NMPC和WBC的双足机器人控制框架简介_双足平衡机器人开源

【开源】基于NMPC和WBC的双足机器人控制框架简介

介绍

本人于研一时参与实验室人形机器人的研发工作，在2024年年初时计划设计一套人形机器人的控制框架。计划使用OCS2官方四足机器人的例程实现MPC的部分，然后参考廖洽源大佬的 Legged Control 实现WBC部分。OCS2非常贴心的提供了一个dummy节点来专门调试MPC部分，这极大的方便了代码的单元测试。后来桥介数物（BridgeDP）开源了一套类似的双足机器人控制框架，虽然是五个自由度的腿，但其WBC部分改为六个自由度并不是困难的工作，所以就直接把BridgeDP的WBC部分改一改搬了过来。控制器集成的代码是我参考廖佬框架基础上写的，完成之后先是调试了Cheat Controller（即没有状态观测器，直接从仿真环境中获取状态），然后读了BridgeDP的代码之后，加入了状态观测器并将其调试稳定。仿真器使用了mujoco，虽然类似的开源框架大部分用gazebo进行仿真，但我对gazebo的开发并不熟悉，而且mujoco相对更轻量化、开发起来更加直观，所以选用了mujoco。

完成这一套框架前前后后花费了大约三周的时间，算是本人进入足式机器人领域以来第一个练手的项目，抄前人代码的成分较多，自己开发的成分较少，算法上也没有什么创新性。本人实力有限，花了不少时间进行了一些不切实际的尝试并解决各种各样的bug，现在把框架开源出来，大家一起交流学习。

代码仓库：https://github.com/pocketxjl/humanoid-control

效果视频：NMPC+WBC+mujoco仿真 12自由度双足人形机器人行走控制

仿真环境

本框架使用mujoco进行仿真。在最新版本中，mujoco在python下配置非常简单，只需要使用pip安装相应的包即可，不需要配置其它环境：

pip3 install mujoco
1

搭建仿真环境的过程为，使用solidworks的urdf插件将图纸导出为urdf，然后使用mujoco的compile工具将urdf转换为Mujoco支持的xml文件。

一个注意事项，solidworks的urdf插件直接导出的转动惯量矩阵是错误的，你需要自行修改urdf中的转动惯量矩阵。具体的做法是，使用solidworks的“评估-质量属性”功能，在选项中设置单位制，设置当前link所对应坐标系的坐标值，设置为负张量计数法（要求solidworks版本至少为2021SP5），并使用“由重心决定，并且对齐输出的坐标系。 （使用负张量记数法。）”的惯性矩阵。

在xml文件中添加所需的关节执行器和传感器，添加 freejoint 从而将固定基座修改为浮动基座，调试仿真环境即可。

代码参见mujoco_sim文件夹和humanoid-legged-description文件夹。

MPC部分

这里MPC部分指使用OCS2实现的部分。OCS2全名为 Optimal Control for Switched Systems，是一个专门为机器人MPC控制设计的最优控制的仓库，包括使用pinocchio动力学库自动构建机器人的全身动力学模型，从urdf自动构建优化问题，并快速实现最优控制算法。OCS2提供了OCS2 dummy功能包，该功能包可以单独对OCS2的计算结果进行调试。遗憾的是，它的官方文档非常少，大部分都是介绍性的文档。如果需要了解其详细使用方法，请直接阅读其例程中的源代码。本仓库主要参考了其四足机器人的demo，参见OCS2_legged_robot和OCS2_legged_robot_ros。运行方法参考官方文档。

足端轨迹规划

足端轨迹规划比较简单，即根据步态指令给出接触序列，并对摆动腿进行z轴上的规划（x轴和y轴的步态实际上是OCS2直接优化出来的，并没有额外规划）。摆动曲线为三次样条曲线。

NMPC

在每个周期中，NMPC部分通过OCS2提供的接口来解决以下优化问题：
{ min ⁡ u ( . )    ϕ ( x ( t I ) ) + ∫ t 0 t I l ( x ( t ) , u ( t ) , t )   d t s.t.   x ( t 0 ) = x 0   initial state       x ˙ ( t ) = f ( x ( t ) , u ( t ) , t ) system flow map       g 1 ( x ( t ) , u ( t ) , t ) = 0 state-input equality constraints       g 2 ( x ( t ) , t ) = 0 state-only equality constraints       h ( x ( t ) , u ( t ) , t ) ≥ 0 inequality constraints

$$\begin{split} \begin{cases} \underset{\mathbf u(.)}{\min} \ \ \phi(\mathbf x(t_I)) + \displaystyle \int_{t_0}^{t_I} l(\mathbf x(t), \mathbf u(t), t) \, dt \\ \text{s.t.} \ \ \mathbf x(t_0) = \mathbf x_0 \,\hspace{11.5em} \text{initial state} \\ \ \ \ \ \ \dot{\mathbf x}(t) = \mathbf f(\mathbf x(t), \mathbf u(t), t) \hspace{7.5em} \text{system flow map} \\ \ \ \ \ \ \mathbf g_1(\mathbf x(t), \mathbf u(t), t) = \mathbf{0} \hspace{8.5em} \text{state-input equality constraints} \\ \ \ \ \ \ \mathbf g_2(\mathbf x(t), t) = \mathbf{0} \hspace{10.5em} \text{state-only equality constraints} \\ \ \ \ \ \ \mathbf h(\mathbf x(t), \mathbf u(t), t) \geq \mathbf{0} \hspace{8.5em} \text{inequality constraints} \end{cases}\end{split}$$ ⎩ ⎨ ⎧​u(.)min​  ϕ(x(tI​))+∫t0​tI​​l(x(t),u(t),t)dts.t.  x(t0​)=x0​initial state     x˙(t)=f(x(t),u(t),t)system flow map     g1​(x(t),u(t),t)=0state-input equality constraints     g2​(x(t),t)=0state-only equality constraints     h(x(t),u(t),t)≥0inequality constraints​​
其中 ${\mathbf{h}}_{com}\in {\mathbb{R}}^6$ 是归一化的质心动量，$\mathbf{q}=[{\mathbf{q}}_b^T,{\mathbf{q}}_j^T{]}^T$ 是广义坐标的位置，其中${\mathbf{q}}_b^T$ 是基座的6自由度位置，${\mathbf{q}}_j^T$ 是每个关节的位置。在这个框架中，$\mathbf{x}$​ 的维数是24。

${\mathbf{f}}_c\in {\mathbb{R}}^{12}$ 包含四个接触点的接触力。在这个框架中，我们定义了四个3自由度的接触点，分别对应左右脚的脚尖和脚后跟。${\mathbf{v}}_j$ 是关节速度。

优化问题成本函数是追踪所有状态和输入误差的二次误差，系统动力学模型使用质心动力学模型，并包含以下约束：

• 摩擦锥约束；
• 站立脚无运动；
• 摆动脚不受地面反力；
• 摆动脚的z轴位置满足步态生成的曲线；
• 脚的滚转轴（roll）无运动。

为了解决这个最优控制问题，采用多重打靶法（Multi-Shooting）将最优控制问题转化为非线性规划（NLP）问题，并使用序列二次规划（SQP）来解决NLP问题。QP子问题使用最强非线性优化库HPIPM来解决。

值得注意的是，受益于OCS2强大的From URDF to OCP的功能，你不需要自己实现优化代价、质心动力学方程（即system flow map）的公式，只需要调用相应的接口自动生成即可。不过，其它的约束函数需要你自己来实现。OCS2需要你定义约束函数的值以及对状态和输入的导数值，从而实现约束函数的线性化。对于足端约束，OCS2实现了EndEffectorKinematics类从而计算足端的运动学和运动学导数。相关代码参见humanoid_interface文件夹，针对MPC部分的调试器参见humanoid_dummy文件夹。

WBC

WBC （Whole Body Control）仅考虑当前时刻，其接受来自MPC部分的目标输入，同时也从状态观测器中获取部分信息。WBC描述为一个线性的二次规划（QP）问题。根据 qpOASES，WBC 优化任务以以下形式表示：

min ⁡ x 1 2 x T H x + x T g ( w 0 )  s.t.  l b A ( w 0 ) ≤ A x ≤ u b A ( w 0 ) , l b ( w 0 ) ≤ x ≤ u b ( w 0 ) ,

$$\begin{split} \min_{x} & \frac{1}{2} x^{T} H x+x^{T} g(w_{0}) \\ \text { s.t. } & lbA(w_{0}) \leq A x \leq ubA(w_{0}), \\ & lb(w_{0}) \leq x \leq ub(w_{0}), \end{split}$$ xmin​ s.t. ​21​xTHx+xTg(w0​)lbA(w0​)≤Ax≤ubA(w0​),lb(w0​)≤x≤ub(w0​),​

其中 $x$ 是优化变量，在此框架中其维度为42，定义如下：

x = [ a b T , a j T , f c T , T j T ] T

$$\begin{equation} x= [\mathbf{a}_{b}^T, \mathbf{a}_j^T, \mathbf{f}_c^T, \mathbf{T}_j^T]^T\end{equation}$$ x=[abT​,ajT​,fcT​,TjT​]T​​

${\mathbf{a}}_b^T\in {\mathbb{R}}^6$ 代表基座加速度，${\mathbf{a}}_j^T\in {\mathbb{R}}^{12}$ 代表关节加速度，${\mathbf{f}}_c^T\in {\mathbb{R}}^{12}$ 代表足端接触力，${\mathbf{T}}_j^T\in {\mathbb{R}}^{12}$ 代表关节扭矩。

在加权 WBC 中，部分任务作为加权成本，提供优化问题的 $H$ 和 $g(w_0)$ 。另一部分任务则作为约束，提供 $A$，$lbA$，$ubA$，$lb$ 和 $ub$。任务的定义如下表所示。

关于浮动基动力学方程，可参考【Robot Dynamics Lecture Notes学习笔记之浮动基动力学】

每个任务被定义为一个四元组 $\left(A,b,D,f\right)$，其中
A x − b = w D x − f ≤ v w → 0 , v → 0

$$\begin{split} Ax -b = w\\ Dx - f \leq v\\ w \rightarrow 0, v \rightarrow 0 \end{split}$$ Ax−b=wDx−f≤vw→0,v→0​

为了从任务定义中构造 QP 问题，我们使用以下公式：

对于成本任务，我们有 $H=A^{T}A$ 和 $g=-A^{T}b$。

对于等式约束任务，我们有 $A_{qp}=A,lbA=b,ubA=b$

对于不等式约束任务，我们有 $A_{qp}=D,lbA=-\infty ,ubA=f$

任务的集成定义为矩阵 $A$，$b$，$D$ 和 $f$​ 的矩阵拼接。一旦多个任务被集成，就可以使用上述公式获得 QP 问题的参数。然后使用 qpOASES 求解器来求解。代码参见humanoid_wbc文件夹。

状态观测器

状态估计器仅需要估计躯干的xyz坐标和线速度。它是一个线性卡尔曼滤波器，其中系统的状态方程为

{    x ^ ( k ) = A x ^ ( k − 1 ) + B u ( k ) + ω ( k − 1 )    z ( k ) = H x ^ ( k ) + ν ( k )

$$\begin{split} \begin{cases} \ \ \hat{x}(k) = A\hat{x}(k-1) + Bu(k)+\omega(k-1) \\ \ \ z(k) = H\hat{x}(k) + \nu(k) \\ \end{cases}\end{split}$$ {  x^(k)=Ax^(k−1)+Bu(k)+ω(k−1)  z(k)=Hx^(k)+ν(k)​​

其中 $x$、$u$、$z$ 定义为

x = [ p o s b T , v e l b T , p o s c T ] T u = a c c b z = [ − p o s l o c a l , c T , v e l b T , h e i g h t c T ] T = [ − p o s l o c a l , c T , − v e l l o c a l , c , 0 T ] T , 对于接触脚

$$\begin{split} x &= [\mathbf{pos_b}^T, \mathbf{vel_b}^T, \mathbf{pos_c}^T]^T\\ u &= \mathbf{acc_b}\\ z &= [-\mathbf{pos_{local,c}}^T, \mathbf{vel_{b}}^T,\mathbf{height_c}^T]^T = [-\mathbf{pos_{local,c}}^T, -\mathbf{vel_{local,c}},\mathbf{0}^T]^T,\text{对于接触脚}\\ \end{split}$$ xuz​=[posb​T,velb​T,posc​T]T=accb​=[−poslocal,c​T,velb​T,heightc​T]T=[−poslocal,c​T,−vellocal,c​,0T]T,对于接触脚​

其中下标 $b$ 指躯干，下标 $c$ 指足端接触点，下标 $local$ 指的是躯干坐标系。

在状态估计器中，$z$​​ 从机器人的运动学中获得，只信任支撑脚的运动学。与摆动脚相关的噪声参数将被设定为一个非常大的“不信任值”。此时，由于状态方程只接受来自加速度计的更新，且支撑脚的 Q 参数远小于摆动脚的 Q 参数，可以实现对支撑脚位置保持不变的估计，并在切换到摆动脚时改变位置。此外，由于只信任支撑脚的动力学，脚的高度和速度测量值可以直接设为0。代码参见humanoid_estimation文件夹。

由于MPC需要获取完整状态变量x的观测，只测量xyz坐标和线速度和线速度是不够的。在本框架中，基座角度和角速度直接从IMU获取，关节位置和速度直接从电机获取，将以上观测值整合为measuredRbdState变量后，调用OCS2的接口直接计算质心动力学模型中完整状态变量x的值。

控制器集成

控制根据用户命令，计算MPC状态变量的参考轨迹，从仿真环境（或机器人硬件）中获取传感器的值，调用状态观测器，计算出MPC中的状态变量的观测值，调用MPC控制算法，计算出状态和输入的目标轨迹，将目标轨迹传输给WBC进行优化，得到目标关节力矩和目标关节加速度。再从MPC中获取关节位置和关节速度的当前目标（从目标轨迹获取），传入MIT控制器，将控制指令发送给电机。

MIT控制器为一个简单的PD控制公式：
q d e s = q o p t + 1 2 a j t 2 v d e s = v o p t + a j t T d e s = T j

$$\begin{split} \mathbf{q_des} &= \mathbf{q_opt} + \frac{1}{2}\mathbf{a}_jt^2\\ \mathbf{v_des} &= \mathbf{v_opt} + \mathbf{a}_jt\\ \mathbf{T_des} &= \mathbf{T}_j \end{split}$$ qd​esvd​esTd​es​=qo​pt+21​aj​t2=vo​pt+aj​t=Tj​​
代码参见humanoid_controllers文件夹。

参考资料

legged-control by Qiayuan Liao: Nonlinear MPC and WBC framework for legged robot based on OCS2 and ros-controls.

hunter-bipedal-control by BridgeDP: An open source bipedal robot control framework, based on non-linear MPC and WBC, tailered for EC-hunter80-v01 bipedal robot.

pai-sim by High-Torque: Acknowledgement for the Mujoco simulation framework in python.