Skip to content

为你调整并优化后的 MPC 说明文档如下。这里将文档基调统一为了“基于单射法的简易非线性 MPC (NMPC)”，修正了运动学模型的矩阵维度冲突，并补全了代码实现中关于“状态方程隐式迭代”的底层逻辑。

MPC 模型预测控制#

1. 算法思想#

模型预测控制（Model Predictive Control, MPC）是一种基于优化的控制方法。其核心思想是：

  1. 预测：利用系统模型，在每个控制周期预测未来 \(N\) 步的系统状态。
  2. 优化：求解一个优化问题，在满足约束的前提下，找到使目标函数最小的控制序列。
  3. 执行：只执行控制序列的第一步，下一周期重复上述过程（滚动优化）。
时刻 k:
┌─────────────────────────────────────┐
│  预测区间 [k, k+N]                  │
│  ┌───┐───┐───┐───┐                  │
│  │x_k│...│x_N│   │  状态预测        │
│  └───┘───┘───┘───┘                  │
│  ┌─┐─┐─┐─┐           u_k, u_k+1...  │
│  │u│ │ │ │           控制序列       │
│  └─┘─┘─┘─┘                          │
│      ↑                              │
│   只执行 u_k                        │
└─────────────────────────────────────┘

2. 离散系统模型#

2.1 状态空间表示（线性时不变系统）#

\[x_{k+1} = A x_k + B u_k\]

其中：

  • \(x_k \in \mathbb{R}^n\)：状态向量
  • \(u_k \in \mathbb{R}^m\)：控制输入
  • \(A \in \mathbb{R}^{n \times n}\)：状态矩阵
  • \(B \in \mathbb{R}^{n \times m}\)：输入矩阵

2.2 本项目的移动机器人模型（非线性系统）#

对于移动机器人（如差速轮式机器人），其连续时间运动学方程是非线性的。在本项目中，我们使用前向欧拉法对其进行离散化，定义 3 状态（位置 \(x, y\) 与航向角 \(\theta\)）和 2 输入（线速度 \(v\) 与角速度 \(\omega\)）：

\[x_k = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}_k, \quad u_k = \begin{bmatrix} v \\ \omega \end{bmatrix}_k\]

离散化非线性状态方程为：

\[x_{k+1} = x_k + \begin{bmatrix} \cos\theta_k & 0 \\ \sin\theta_k & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} v_k \\ \omega_k \end{bmatrix} dt\]

展开为标量形式：

\[x_{k+1} = x_k + v_k \cos\theta_k \cdot dt\]
\[y_{k+1} = y_k + v_k \sin\theta_k \cdot dt\]
\[\theta_{k+1} = \theta_k + \omega_k \cdot dt\]

架构注记：由于模型中包含 \(\cos\theta_k\) 和 \(\sin\theta_k\)，该系统本质上属于非线性 MPC (NMPC)。

3. MPC 优化问题#

3.1 目标函数与约束#

在每个控制周期，求解器需要寻找到最优的控制序列 \(U = [u_0, u_1, \dots, u_{N-1}]\)，使得系统状态尽可能跟踪参考轨迹，同时消耗的控制能量最小：

\[\min_{u_0, \dots, u_{N-1}} \sum_{i=0}^{N-1} \left( (x_i - x_{ref})^T Q (x_i - x_{ref}) + u_i^T R u_i \right) + (x_N - x_{ref})^T Q_N (x_N - x_{ref})\]
\[\text{s.t. } x_{i+1} = f(x_i, u_i) \quad \text{（运动学方程约束）}\]
\[\underline{u} \leq u_i \leq \bar{u} \quad \text{（执行器物理边界约束）}\]

其中：

  • \(N\)：预测步长（Prediction Horizon）
  • \(Q \in \mathbb{R}^{3 \times 3}, R \in \mathbb{R}^{2 \times 2}\)：状态加权矩阵与控制加权矩阵（对角阵）
  • \(Q_N\)：终端状态误差权重

3.2 优化形式分类#

  • 标准线性 MPC：需要对运动学方程进行泰勒展开一阶线性化，将问题转化为标准的二次规划（QP）形式 \(\min \frac{1}{2} U^T H U + f^T U\)。
  • 本项目处理方案：出于教学与快速验证目的，直接将非线性模型送入通用非线性规划（NLP）求解器。

4. 求解方法#

4.1 求解策略：单射法（Single Shooting）#

本项目采用 单射法（Single Shooting） 构建优化问题。

  • 核心逻辑：只有控制量 \(U\) 是求解器的优化变量，状态量 \(X\) 不是。
  • 状态方程的约束处理：不在求解器中显式传递 \(A_{eq} u = b_{eq}\) 等式约束，而是在自定义的成本函数 cost(u) 内部，通过 for 循环利用运动学方程隐式递推出未来的状态序列，以此计算总成本。这样做极大地简化了代码架构。

4.2 求解器选择#

由于直接处理非线性模型，我们选择 scipy.optimize.minimize 中的 SLSQP（序列二次规划算法） 求解器，无需引入第三方硬核凸优化库（如 CasADi 或 OSQP），适合轻量化仿真。

4.3 求解器代码框架#

import numpy as np
from scipy.optimize import minimize

def mpc_controller(x0, x_ref, dt, Q, R, N, v_max, omega_max):
    """
    x0: 当前状态 [x, y, theta]
    x_ref: 参考状态 [x_r, y_r, theta_r]
    """
    state_dim = 3
    control_dim = 2

    def cost_function(u_flat):
        # 将展平的控制序列还原为 (N, control_dim)
        u_seq = u_flat.reshape((N, control_dim))
        total_cost = 0.0

        # 从当前真实状态开始向前隐式预测 (Single Shooting)
        current_state = x0.copy()

        for i in range(N):
            u_i = u_seq[i]

            # 1. 计算当前步的惩罚项 (状态误差 + 控制能量)
            state_error = current_state - x_ref
            total_cost += state_error.T @ Q @ state_error + u_i.T @ R @ u_i

            # 2. 运动学方程前向状态更新
            theta = current_state[2]
            current_state[0] += u_i[0] * np.cos(theta) * dt
            current_state[1] += u_i[0] * np.sin(theta) * dt
            current_state[2] += u_i[1] * dt

        # 3. 终端误差惩罚
        terminal_error = current_state - x_ref
        total_cost += terminal_error.T @ Q @ terminal_error  # 此处简化令 Q_N = Q

        return total_cost

    # 初始猜测：全 0 控制序列
    u0 = np.zeros(N * control_dim)  

    # 设置控制量边界约束 [v_min, v_max] 和 [omega_min, omega_max]
    bounds = [(-v_max, v_max), (-omega_max, omega_max)] * N

    # 调用 SLSQP 求解器
    result = minimize(cost_function, u0, method='SLSQP', bounds=bounds)

    # 滚动优化：只返回预测序列的第一步控制输入 [v, omega]
    return result.x[:control_dim]

5. 参数调节#

5.1 预测步长 \(N\)#

\(N\) 过小 \(N\) 过大
预测区间太短，对前方弯道感知不足，跟踪性能差 每个周期迭代计算量呈指数级上升，实时性差
控制器反应突兀且相对保守 远期非线性模型的累计预测误差大，失去意义
  • 推荐初始值：\(N = 10 \sim 20\) （配合 \(dt = 0.05\text{s} \sim 0.1\text{s}\)）。

5.2 权重矩阵 \(Q\) 和 \(R\)#

  • \(Q\) 矩阵（对角阵，如 \(\text{diag}([q_x, q_y, q_{\theta}])\)）：

  • 值越大，代表对轨迹跟踪精度要求越高，控制器会不惜代价削减误差。

  • \(R\) 矩阵（对角阵，如 \(\text{diag}([r_v, r_{\omega}])\)）：

  • 值越大，代表对控制能量的惩罚越重，输出动作更平滑。

工程调参指南：

  • 如果轨迹跟踪不上：优先增大 \(Q\) 矩阵的对角元素。
  • 如果小车出现明显的画龙/蛇形振荡：说明控制灵敏度过高，应增大 \(R\) 矩阵元素。
  • 高级防抖扩展：若发现执行器频繁剧烈抖动，说明对控制绝对值的惩罚不够抑制动作变化率。可在代码的 cost_function 中引入控制增量惩罚 \(\Delta u_i^T R_{diff} \Delta u_i\)（即 \(u_i - u_{i-1}\)），能立竿见影地软化动作。

6. 纯跟踪 (Pure Pursuit) vs MPC#

特性 纯跟踪 (Pure Pursuit) 模型预测控制 (MPC)
底层原理 几何几何学关系（寻找前视点切圆弧） 最优化理论（滚动时域优化）
计算复杂度 极小（闭式解析解，适合低端 MCU） 较大（每个周期都需要迭代求解非线性问题）
硬约束处理 无法处理（只能事后做硬剪裁 clip） 极佳（直接将输入/状态边界送入求解器）
预测能力 无（仅依靠单一前视距离反馈） 有（能感知未来 \(N\) 步的轨迹形态与趋势）
调参难度 低（主要调一个前视距离 \(L_d\)） 中等（需要协调 \(N, Q, R\) 多个矩阵参数）

7. 参考实现#

完整仿真及动态可视化见项目源码：02.mpc_sim.ipynb