宇树机器人丝滑的二人转动作背后，MPC到底是什么？

本文链接：https://blog.csdn.net/Barok/article/details/146313844

模型预测控制（Model Predictive Control）

引言

模型预测控制（MPC）是一种先进的控制策略，通过预测系统未来行为并优化控制动作来实现目标。它广泛应用于工业过程、机器人和航空航天等领域。本报告将详细介绍MPC的概念、数学原理、代码示例和最新发展，并探讨其与视觉-语言-动作（VLA）模型在人形机器人中的协同作用。

MPC的概念介绍

MPC的核心是使用系统模型预测未来行为，然后计算一序列控制动作以最小化成本函数，仅应用第一个动作，并重复该过程。这种“后退地平线”方法使其能够适应实时变化和不确定性。MPC的历史可以追溯到20世纪70年代和80年代，最初用于化学过程工业，如今扩展到机器人、汽车和电力系统等领域。

数学原理

MPC依赖于系统模型，通常为线性或非线性模型。例如，考虑离散时间线性系统：

$x_{k+1} = A x_k + B u_k + w_k \\ y_k = C x_k + v_k$

其中， $x_k$ 是状态向量， $u_k$ 是控制输入向量， $y_k$ 是输出向量， $w_k$ 和 $v_k$ 是噪声， $A$ 、 $B$ 、 $C$ 是系统矩阵。

成本函数通常包括输出误差和控制努力的惩罚项：

$\sum_{i=0}^{N-1} \left( (y_{k+i} - r_{k+i})^T Q (y_{k+i} - r_{k+i}) + u_{k+i}^T R u_{k+i} \right) + (x_{k+N}^T P x_{k+N})$

其中， $N$ 是预测地平线， $r_{k+i}$ 是参考值， $Q$ 、 $R$ 、 $P$ 是权重矩阵。

每次时间步，MPC解决优化问题以最小化 $J$ ，受限于系统动态和状态/输入约束，仅应用第一个控制动作，然后重复。

代码示例

下面的代码示例展示了一个简单的模型预测控制（MPC）实现，用于控制房间温度。我们将从头开始，逐步分析每一行代码的功能和意义。

import numpy as np
from scipy.optimize import minimize

a = 0.9  # 假设值
b = 0.1  # 假设值

N = 10  # 预测地平线
Q = 1.0  # 温度误差权重
R = 0.1  # 控制努力权重

def mpc_step(T_current, r):
    T_pred = np.zeros(N)
    u_pred = np.zeros(N)
    T = T_current
    
    def cost(u):
        for i in range(N):
            if i < N-1:
                T_next = a * T + b * u[i]
                T_pred[i+1] = T_next
                T = T_next
            else:
                T_pred[i] = T
        error = T_pred - r
        control_cost = np.sum(u**2 * R)
        tracking_cost = np.sum(error**2 * Q)
        return tracking_cost + control_cost
    
    bounds = [(0, 1)] * N
    u0 = np.ones(N) * 0.5
    
    res = minimize(cost, u0, method='SLSQP', bounds=bounds)
    u_k = res.x[0]
    
    return u_k, T_pred

T_current = 20  # 当前温度
r = 25  # 参考温度
u_k, T_pred = mpc_step(T_current, r)
print("控制动作:", u_k)
print("预测温度:", T_pred)

逐行解释

导入库

import numpy as np

作用：导入 numpy 库，这是一个用于科学计算的强大工具，提供数组操作和数学函数支持。
在代码中的用途：用于创建数组（如预测温度和控制输入序列）以及进行矩阵运算。

from scipy.optimize import minimize

作用：从 scipy.optimize 模块中导入 minimize 函数，用于求解优化问题。
在代码中的用途：MPC的核心是通过优化找到最佳控制输入，minimize 是实现这一目标的关键工具。

定义系统参数

a = 0.9  # 假设值

作用：定义系统模型中的参数 a，表示温度的自回归系数。
意义：在这个例子中，假设房间温度遵循一个简单的离散时间线性模型： $T_{k+1} = a T_k + b u_k$ 。a = 0.9 表示当前温度对下一时刻温度有90%的保留效应。

b = 0.1  # 假设值

作用：定义系统模型中的参数 b，表示控制输入对温度的影响系数。
意义：b = 0.1 表示控制输入（例如加热器功率）对下一时刻温度的贡献较小，每次增加1单位的控制输入会使温度增加0.1度。

定义MPC参数

N = 10  # 预测地平线

作用：设置预测地平线，表示MPC控制器会预测未来10个时间步的状态。
意义：预测地平线决定了控制器“看”多远，N 越大，控制器考虑的未来影响越多，但计算复杂度也会增加。

Q = 1.0  # 温度误差权重

作用：定义温度误差在成本函数中的权重。
意义：Q = 1.0 表示对温度偏离参考值的惩罚力度。如果 Q 更大，控制器会更努力地使温度接近目标值。

R = 0.1  # 控制努力权重

作用：定义控制输入在成本函数中的权重。
意义：R = 0.1 表示对控制输入大小的惩罚较轻。如果 R 更大，控制器会倾向于使用较小的控制输入以节省能量。

定义MPC控制步骤函数

def mpc_step(T_current, r):

作用：定义一个函数 mpc_step，实现MPC的一个控制步骤。
参数：
- T_current：当前房间温度。
- r：参考温度（目标温度）。
返回值：最佳控制输入和预测的温度序列。

    T_pred = np.zeros(N)

作用：创建一个长度为 N 的零数组，用于存储预测的温度序列。
意义：初始化预测温度数组，稍后会在成本函数中填充具体值。

    u_pred = np.zeros(N)

作用：创建一个长度为 N 的零数组，用于存储预测的控制输入序列。
意义：虽然定义了 u_pred，但在这个实现中未直接使用它，仅作为占位符。

    T = T_current

作用：将当前温度 T_current 赋值给变量 T，作为预测的起点。
意义：T 会在成本函数中被更新，用于模拟温度随时间的变化。

定义成本函数

    def cost(u):

作用：定义一个嵌套的成本函数 cost，计算给定控制输入序列 u 的总成本。
参数：u，一个长度为 N 的控制输入序列。
意义：MPC通过最小化这个成本函数来找到最佳控制策略。

        for i in range(N):

作用：开始一个循环，遍历预测地平线中的每个时间步（从0到 N-1）。
意义：模拟未来 N 个时间步的温度变化。

            if i < N-1:

作用：检查是否为最后一个预测步之前的时间步。
意义：对于前 N-1 步，可以使用控制输入更新温度；最后一步没有后续控制输入。

                T_next = a * T + b * u[i]

作用：根据系统模型计算下一时刻的温度。
意义：使用 $T_{k+1} = a T_k + b u_k$ 公式，基于当前温度 T 和控制输入 u[i] 计算下一温度。

                T_pred[i+1] = T_next

作用：将计算得到的下一温度存储到预测数组 T_pred 中。
意义：T_pred[i+1] 表示第 i+1 步的预测温度（数组索引从0开始）。

                T = T_next

作用：更新 T 为下一时刻的温度。
意义：为下一次循环准备新的当前温度，继续预测。

            else:
                T_pred[i] = T

作用：对于最后一个时间步（i = N-1），将当前温度 T 赋给 T_pred[i]。
意义：因为没有更多的控制输入可用，最后一步的预测温度保持不变。

        error = T_pred - r

作用：计算预测温度与参考温度之间的误差。
意义：error 是一个长度为 N 的数组，表示每个时间步的温度偏差。

        control_cost = np.sum(u**2 * R)

作用：计算控制成本，即控制输入的加权平方和。
意义：u**2 计算每个控制输入的平方，乘以权重 R 并求和，反映控制努力的代价。

        tracking_cost = np.sum(error**2 * Q)

作用：计算跟踪成本，即温度误差的加权平方和。
意义：error**2 计算每个时间步误差的平方，乘以权重 Q 并求和，反映温度偏离目标的代价。

        return tracking_cost + control_cost

作用：返回总成本，即跟踪成本和控制成本之和。
意义：MPC的目标是最小化这个总成本，平衡温度跟踪和控制努力。

设置优化参数

    bounds = [(0, 1)] * N

作用：定义控制输入的边界条件。
意义：[(0, 1)] * N 创建一个长度为 N 的列表，每个元素是元组 (0, 1)，表示每个控制输入 u[i] 的取值范围在0到1之间。

    u0 = np.ones(N) * 0.5

作用：设置优化问题的初始猜测。
意义：np.ones(N) 创建一个长度为 N 的全1数组，乘以0.5得到初始控制输入序列，每个值为0.5。

求解优化问题

    res = minimize(cost, u0, method='SLSQP', bounds=bounds)

作用：调用 minimize 函数求解优化问题。
参数：
- cost：要最小化的目标函数。
- u0：初始猜测。
- method='SLSQP'：使用顺序最小二乘规划法（适合带约束的优化）。
- bounds：控制输入的约束范围。
意义：找到使成本函数最小的控制输入序列。

    u_k = res.x[0]

作用：从优化结果中提取第一个控制输入。
意义：res.x 是一个长度为 N 的数组，包含优化后的控制输入序列。MPC只应用第一个控制输入 u_k，然后在下一时间步重新优化。

返回结果

    return u_k, T_pred

作用：返回当前时间步的控制输入 u_k 和预测温度序列 T_pred。
意义：u_k 用于实际控制系统，T_pred 可用于分析或可视化。

主程序

T_current = 20  # 当前温度

作用：设置当前房间温度为20度。
意义：模拟系统的初始状态。

r = 25  # 参考温度

作用：设置目标温度为25度。
意义：MPC将尝试使房间温度接近这个值。

u_k, T_pred = mpc_step(T_current, r)

作用：调用 mpc_step 函数，计算控制输入和预测温度。
意义：执行一次MPC控制步骤。

print("控制动作:", u_k)

作用：打印计算得到的控制输入。
意义：显示将要应用的控制动作（例如加热器功率）。

print("预测温度:", T_pred)

作用：打印预测的温度序列。
意义：展示未来 N 个时间步的温度变化趋势。

输出示例

运行代码后，可能会得到类似以下的输出：

控制动作: 1.0
预测温度: [20.   21.   21.9  22.71 23.439 24.0951 24.68559 25.217031 25.6953279 26.12579511]

解释：
- u_k = 1.0：当前时间步应用的最大控制输入（受限于 bounds）。
- T_pred：从20度开始，温度逐步接近25度，但因预测地平线有限，未完全达到目标。

总结一下，这段脚本实现了一个基本的MPC控制器，通过以下步骤控制房间温度：

建模：使用简单的线性模型 $T_{k+1} = a T_k + b u_k$ 预测温度。
成本函数：定义温度误差和控制努力的加权和。
优化：利用 minimize 找到最佳控制输入序列。
控制：应用第一个控制输入，并在下一时间步重复整个过程。

这种方法展示了MPC的核心思想：预测未来状态并优化控制策略，适用于需要精确控制的场景，如温度调节、机器人运动等。

MPC与VLA模型的协同作用

在人形机器人中，MPC与视觉-语言-动作模型的结合构成了其“大脑”。VLA模型处理视觉感知和语言理解，例如根据口头指令识别并定位物体，而MPC则负责精确控制机器人的运动，确保动作平稳并遵守物理约束。

例如，考虑一个机器人执行“拿起杯子”的任务。VLA模型通过视觉和语言输入确定目标物体和位置，输出目标状态或动作序列，MPC则规划手臂轨迹，考虑关节限制和碰撞避免，确保平稳执行。

对比分析

以下表格总结MPC与VLA模型的角色和协同作用：

模型/功能	角色	示例应用
VLA模型	视觉感知和语言理解，输出目标状态或动作序列	根据“拿起杯子”指令定位物体
MPC	精确控制运动，遵守动态和约束	规划手臂轨迹，考虑关节限制
协同作用	VLA提供高层次目标，MPC执行低层次控制	确保机器人平稳执行复杂任务