MPC和WBC步态控制（以人形机器人控制为例）

MPC和WBC步态控制（以人形机器人控制为例）

前言

人形机器人作为人工智能领域的一个重要分支，其运动控制技术的发展对于实现机器人的自主性、灵活性和智能化至关重要。随着技术的进步，MPC和WBC等先进的控制策略被广泛应用于人形机器人的运动控制中，极大地提升了机器人的性能。

原理介绍

模型预测控制（MPC）原理介绍

模型预测控制是一种基于模型的控制策略，它通过预测未来一段时间内的系统状态和控制输入，来优化当前时刻的控制输出。MPC的核心在于建立一个精确的系统模型，并利用这个模型来预测未来的行为，从而实现对系统的最优控制。

最优控制的基本概念

最优控制（optimal control）是指在给定约束条件下，通过调整系统的控制参数，使系统在某种性能指标下达到最佳状态的问题。最优控制通常需要在整个时间域上进行优化，以达到最佳性能，当存在扰动的情况下，当前时刻的最优比不一定是下一时刻的最优值。最优控制在实际应用中有很广泛的应用，例如在自动驾驶汽车中，通过优化车辆的加速度和转向角度，使其在规定的时间内安全地到达目的地；在制造业中，通过调整生产线的控制参数，实现生产效率的最大化；在经济学中，通过优化投资组合，实现最大的资产回报等。

MPC算法的整体流程

模型预测控制在k时刻共需三步

• 第一步：获取系统的当前状态；

• 第二步：基于u(k),u(k+1),u(k+2),u(k+j)进行最优化处理，代价函数为

• 其中EN 表示误差的终值，也是衡量优劣的一种标准。

• 第三步：只取u ( k )作为控制输入施加在系统上。

在下一时刻重复以上三步，在下一步进行预测时使用的就是下一步的状态值，我们将这样的方案称为滚动优化控制（Receding Horizon Control）。

MPC的关键特点

• 预测性：MPC能够预测未来一段时间内的系统行为。

• 优化性：通过优化控制输入，实现系统性能的最优化。

• 约束处理：能够处理系统状态和控制输入的约束条件。

全身体平衡控制（WBC）原理介绍

全身体平衡控制（whole body control）是一种用于实现人形机器人稳定性的控制方法。其核心思想是将机器人视为一个整体，通过优化各个关节的运动来实现整体的平衡和稳定。这与传统的局部控制方法不同，后者通常只关注单个关节或局部区域的稳定性。

WBC算法的整体流程

1. 系统建模：首先需要建立机器人的动力学模型，包括质心的位置、关节的运动学和动力学特性。

1. 稳定性分析：通过计算ZMP和CMP的位置，分析机器人的稳定性。如果ZMP不在支撑多边形内，机器人就需要调整姿态或步态。

1. 控制目标设定：根据任务需求和稳定性分析的结果，设定控制目标，如保持ZMP在支撑多边形中心、最小化能量消耗等。

1. 优化问题求解：将控制目标转化为优化问题，通过数学方法求解。这通常涉及到线性或非线性规划，目标是找到一组最优的关节运动指令。

1. 实时控制：将优化得到的关节运动指令实时地应用到机器人系统中，实现动态平衡。

1. 反馈调整：根据机器人的实际运动状态和外部环境的变化，对控制策略进行在线调整

部署环境介绍

以OpenLoong项目为例，我们可以分析其在人形机器人运动控制方面的应用。OpenLoong提供了开源的人形机器训练平台和全身动力学控制软件包。根据其官网[https://atomgit.com/openloong/openloong-dyn-control]的内容,可以借鉴部署相关环境。

硬件环境

硬件环境是实现机器人运动控制算法，如模型预测控制（MPC）和全身体平衡控制（WBC）的基础。以下是基于OpenLoong项目介绍的通用硬件环境：

基础硬件组件

1. 控制单元：通常以高性能计算平台（如NVIDIA Jetson系列、Intel Nuc等）为核心，负责运行运动控制算法和实时处理传感器数据。

1. 传感器套件：包括但不限于：

• 力矩传感器：测量关节力矩，用于反馈控制。

• 姿态传感器：如陀螺仪和加速度计，监测机器人的姿态变化。

• 视觉传感器：如摄像头，用于环境感知和目标识别。

• 触觉传感器：感知接触力，用于执行精细操作。

1. 驱动单元：负责将控制指令转换为电机的运动，包括电机驱动器和电机控制器。

1. 电机：执行器，用于驱动机器人关节的运动，常见的有直流电机（DC）、交流电机（AC）和步进电机。

1. 电源管理系统：为整个系统提供稳定电源，包括电池和电源转换器。

1. 通信接口：用于机器人内部模块间的数据交换，以及与外部设备的通信，如以太网、Wi-Fi、蓝牙等。

高级硬件组件

1. AI算力单元：高性能的GPU或专用AI芯片，用于处理复杂的算法和机器学习任务。

1. 实时操作系统（RTOS）：保证控制算法的实时性和稳定性，如FreeRTOS、VxWorks等。

1. 存储设备：用于存储系统软件、算法模型和临时数据，如SSD、eMMC等。

1. 冗余系统：在关键组件（如电源、控制单元）上实现冗余，提高系统的可靠性和容错能力。

1. 安全监控系统：实时监测硬件状态，确保机器人安全运行。

1. 接口扩展板：为机器人提供额外的I/O接口，如GPIO、PWM、SPI、I2C等。

当然如果真的搭载一个人形机器人，成本可能是很高的，所以此处我们还是采用青龙平台提供的仿真环境。可以参考该内容[https://atomgit.com/openloong/openloong-dyn-control]。

仿真部署流程

官方给出了一系列的仿真部署流程，如Linux版本、编译器版本等，大家可以根据本文以下内容操作：

环境建议

• 操作系统：Ubuntu 22.04.4 LTS

• 编译器：g++ 11.4.0

依赖安装

仿真界面需系统支持 openGL

Bash #Update & Install Dependencies sudo apt-get update sudo apt install git cmake gcc-11 g++-11 sudo apt install libglu1-mesa-dev freeglut3-dev

代码获取与编译

Bash #拉取代码仓库 git clone https://atomgit.com/openloong/openloong-dyn-control.git # 编译代码 cd openloong-dyn-control mkdir build cd build cmake .. make # 运行仿真 ./walk_mpc_wbc #or ./walk_wbc or ./jump_mpc

结果演示

运行之后，大家可以看到一个行走的人，按照一定的步态行走。

也有开发者在B站上录制了相关视频，感兴趣的小伙伴可以观看：

https://www.bilibili.com/video/BV1q2aheREic/?spm_id_from=333.337.search-card.all.click&vd_source=11e208184319df67805026f6ffaf9eac

代码解读

整个代码仓库架构如上图所示，看上去还是比较清晰的，最重要的就是algorithm部分，这里是算法的存储文件夹。

以上这两个文件夹就是WBC和MPC实现的代码仓库

MPC的主要实现

先来看到MPC部分，其中关键可以分为七个部分

1. 构造函数 MPC::MPC(double dtIn)

初始化MPC类的成员变量，设置仿真时间步长dt，并初始化各种矩阵和向量为零。

C++ cpp MPC::MPC(double dtIn):QP(nu*ch, nc*ch) {     // ... }

这里QP代表二次规划问题的维度，nu是控制输入的维度，nc是约束条件的数量，ch是时间步数。

2. 设置权重函数 MPC::set_weight

设置MPC问题中的权重矩阵，包括状态权重L和控制输入权重K。

C++ cpp void MPC::set_weight(double u_weight, Eigen::MatrixXd L_diag, Eigen::MatrixXd K_diag) {     // ... }

u_weight是控制输入的权重系数，L_diag和K_diag分别是状态和控制输入的对角矩阵。

3. 数据读取函数 MPC::dataBusRead

从数据总线读取当前系统状态，并更新MPC的当前状态估计。

C++ cpp void MPC::dataBusRead(DataBus &Data) {     // ... }

DataBus是一个结构或类，包含所有需要的传感器数据和系统状态。

4. 计算函数 MPC::cal

执行MPC的主要计算，包括建立优化问题、求解和更新系统状态。

C++ cpp void MPC::cal() {     if (EN) {         // ...         // 建立MPC的动态模型和约束条件         // ...         // 调用qpOASES求解器         // ...         // 更新系统状态预测         // ...     } }

这里使用了qpOASES库来求解二次规划问题。

5. 数据写入函数 MPC::dataBusWrite

将MPC计算结果写回数据总线，供系统执行。

C++ cpp void MPC::dataBusWrite(DataBus &Data) {     // ... }

写入的数据包括期望的控制输入和系统状态预测。

6. 使能/禁用MPC函数

允许用户启用或禁用MPC控制器。

C++ cpp void MPC::enable() { EN = true; } void MPC::disable() { EN = false; }

EN是一个布尔变量，表示MPC是否处于启用状态。

7. 辅助函数 MPC::copy_Eigen_to_real_t

将Eigen矩阵转换为qpOASES所需的real_t数组格式。

C++ cpp void MPC::copy_Eigen_to_real_t(qpOASES::real_t* target, Eigen::MatrixXd source, int nRows, int nCols) {     // ... }

这个函数用于数据格式转换，因为qpOASES库使用的是C风格的数组。

结合到MPC原理的三个流程

第一步：获取系统的当前状态

在MPC的框架中，首先需要获取系统当前的状态信息，这包括系统的位置、速度、加速度等状态变量。在代码中，这一步通常通过读取传感器数据或从数据总线获取。

C++ cpp void MPC::dataBusRead(DataBus &Data) {     // ...     X_cur.block<3,1>(0,0) = Data.base_rpy;  // 获取当前的基座姿态角     X_cur.block<3,1>(3,0) = Data.q.block<3,1>(0,0);  // 获取当前的位置     X_cur.block<3,1>(6,0) = Data.dq.block<3,1>(3,0);  // 获取当前的角速度     X_cur.block<3,1>(9,0) = Data.dq.block<3,1>(0,0);  // 获取当前的线速度     // ... }

第二步：基于u(k),u(k+1),u(k+2),u(k+j)进行最优化处理

在这一步中，MPC会根据当前的状态信息和预定的目标状态，通过求解一个优化问题来计算一系列未来的控制输入。优化问题的目标是最小化代价函数，通常包括跟踪误差和控制输入的平滑性等。

C++ cpp void MPC::cal() {     if (EN) {         // ...         H = 2 * (Bqp.transpose() * L * Bqp + alpha * K) + 1e-10*Eigen::MatrixXd::Identity(nx*mpc_N, nx*mpc_N);         c = 2 * Bqp.transpose() * L * (Aqp * X_cur - Xd) + 2 * alpha * K * delta_U;         // ...         res = QP.init(qp_H, qp_c, qp_As, qp_lu, qp_uu, qp_lbA, qp_ubA, nWSR, &cpu_time, xOpt_iniGuess);         // ...     } }

在这段代码中，H和c是构建优化问题时使用的Hessian矩阵和线性项，QP.init是调用优化求解器初始化问题的函数。

第三步：只取u(k)作为控制输入施加在系统上

优化完成后，MPC只采用优化序列中的第一个控制输入u(k)来控制当前时刻的系统，然后在下一个控制周期重复这个过程。

C++ cpp void MPC::cal() {     if (EN) {         // ...         qpOASES::real_t xOpt[nu * ch];         QP.getPrimalSolution(xOpt);         if (qp_Status == 0) {             for (int i = 0; i < nu * ch; i++)                 Ufe(i) = xOpt[i];         }         // ...     } }

在这段代码中，Ufe存储了优化后的控制输入序列，但在实际应用中通常只取Ufe.block<nu, 1>(0, 0)作为当前时刻的控制输入。

WBC的主要实现

1. 构造函数 WBC_priority::WBC_priority

初始化WBC类的成员变量，设置模型的自由度、摩擦系数、时间步长等，并配置QP问题。

C++ cpp WBC_priority::WBC_priority(int model_nv_In, int QP_nvIn, int QP_ncIn, double miu_In, double dt) : QP_prob(QP_nvIn, QP_ncIn) {     // ... }

这里初始化了QP问题，包括控制变量的数量（QP_nvIn）和约束条件的数量（QP_ncIn）。

2. 数据读取函数 WBC_priority::dataBusRead

从数据总线读取机器人的当前状态，包括关节位置、速度、加速度等。

C++ cpp void WBC_priority::dataBusRead(const DataBus &robotState) {     // ... }

这个函数负责从数据总线获取机器人的当前状态信息，以便进行控制计算。

3. 数据写入函数 WBC_priority::dataBusWrite

将计算结果写回数据总线，包括最终的关节扭矩和力。

C++ cpp void WBC_priority::dataBusWrite(DataBus &robotState) {     // ... }

这个函数负责将计算结果（如关节扭矩和接触力）写回数据总线，供机器人执行。

4. 计算函数 WBC_priority::computeTau

构建和求解QP问题，计算机器人的关节扭矩。

C++ cpp void WBC_priority::computeTau() {     // ... }

这个函数是WBC的核心，它构建了QP问题，包括目标函数和约束条件，并调用求解器求解。求解结果用于计算关节扭矩和接触力。

5. 辅助函数 WBC_priority::copy_Eigen_to_real_t

将Eigen矩阵转换为qpOASES所需的real_t数组格式。

C++ cpp void WBC_priority::copy_Eigen_to_real_t(qpOASES::real_t *target, const Eigen::MatrixXd &source, int nRows, int nCols) {     // ... }

这个函数用于数据格式转换，因为qpOASES库使用的是C风格的数组。

6. 计算函数 WBC_priority::computeDdq

计算机器人的加速度和速度。

C++ cpp void WBC_priority::computeDdq(Pin_KinDyn &pinKinDynIn) {     // ... }

这个函数负责计算机器人的加速度和速度，包括处理各种任务（如保持平衡、跟踪目标位置等）。

结合到WBC原理的六个流程

1. 系统建模

在WBC的构造函数中，通过初始化动力学模型和定义机器人的自由度，为系统建模打下基础。

C++ cpp WBC_priority::WBC_priority(int model_nv_In, int QP_nvIn, int QP_ncIn, double miu_In, double dt) : QP_prob(QP_nvIn, QP_ncIn) {     // ... }

这里model_nv_In表示机器人模型的自由度数，是系统建模的关键参数。

2. 稳定性分析

稳定性分析通常涉及到计算ZMP（Zero Moment Point）和CMP（Center of Mass Projection）。在代码中，虽然没有直接计算ZMP的函数，但在优化问题中隐含了对稳定性的考虑。

C++ cpp void WBC_priority::computeTau() {     // ...     Eigen::MatrixXd eigen_qp_A1 = Eigen::MatrixXd::Zero(6, QP_nv);     // ...     Eigen::VectorXd eqRes = Eigen::VectorXd::Zero(6);     // ... }

在构建QP问题时，通过设置等式和不等式约束，确保了机器人的稳定性，例如通过约束力和力矩来保证ZMP在支撑多边形内。

3. 控制目标设定

控制目标在优化问题的形式化过程中被设定。在WBC中，这些目标可能包括跟踪特定的轨迹、保持平衡或优化能量消耗。

C++ cpp void WBC_priority::computeTau() {     // ...     Eigen::MatrixXd eigen_qp_H = Eigen::MatrixXd::Zero(QP_nv, QP_nv);     // ... }

在这里，eigen_qp_H和qp_g定义了优化问题的Hessian矩阵和线性项，它们体现了控制目标。

4. 优化问题求解

优化问题的求解是通过qpOASES库完成的，它是一个高效的二次规划求解器。

C++ cpp void WBC_priority::computeTau() {     // ...     res = QP_prob.init(qp_H, qp_g, qp_A, NULL, NULL, qp_lbA, qp_ubA, nWSR, &cpu_time, xOpt_iniGuess);     // ... }

这里，优化问题的参数（如qp_H、qp_g、qp_A等）被传递给求解器，求解器返回最优解。

5. 实时控制

实时控制体现在将优化求解的结果应用到机器人系统中。

C++ cpp void WBC_priority::dataBusWrite(DataBus &robotState) {     robotState.wbc_ddq_final = eigen_ddq_Opt;     robotState.wbc_tauJointRes = tauJointRes;     // ... }

在dataBusWrite函数中，计算得到的加速度eigen_ddq_Opt和关节扭矩tauJointRes被写入数据总线，用于控制机器人。

6. 反馈调整

反馈调整涉及到根据机器人的实际状态和外部环境变化对控制策略进行调整。在WBC中，这通常通过在每个控制周期重新计算优化问题来实现。

C++ cpp void WBC_priority::dataBusRead(const DataBus &robotState) {     // ... }

在dataBusRead函数中，从数据总线读取最新的传感器数据和状态信息，这些信息将用于下一个控制周期的优化计算。

参考链接

https://zhuanlan.zhihu.com/p/633715817

https://atomgit.com/openloong/openloong-dyn-control/tree/master/algorithm