避开MPC学习第一个坑：手把手教你用Python复现DR_CAN的SISO/MIMO模型预测例子

从理论到代码：用Python实现DR_CAN的MPC汽车变道案例

在控制工程领域，模型预测控制（MPC）因其处理多变量系统和约束条件的能力而备受青睐。许多学习者通过DR_CAN的系列视频掌握了MPC的理论基础，却往往在将数学公式转化为实际代码时遇到困难。本文将手把手带你用Python复现经典的汽车变道轨迹优化案例，填补理论与实践的鸿沟。

1. 环境准备与问题建模

在开始编码前，我们需要明确汽车变道问题的数学模型。假设车辆在二维平面上运动，状态变量包括位置(x,y)和速度(vx,vy)，控制输入为加速度(ax,ay)。这与DR_CAN视频中提到的MIMO系统完全对应。

首先安装必要的Python库：

pip install numpy scipy matplotlib cvxpy

定义系统参数和初始状态：

import numpy as np # 车辆参数 dt = 0.1 # 时间步长 mass = 1.0 # 车辆质量 N = 20 # 预测步长 T = 10 # 控制步长 # 初始状态 [x, y, vx, vy] x0 = np.array([0, 0, 10, 0]) # 目标状态 [x, y, vx, vy] x_ref = np.array([50, 3, 10, 0])

2. SISO代价函数的Python实现

DR_CAN视频中首先介绍了SISO系统的代价函数，我们可以将其扩展为MIMO形式。代价函数通常包含跟踪误差和控制输入两部分：

def cost_function(x, u, x_ref, Q, R): """ 计算MIMO系统的代价函数 :param x: 状态序列 (N+1, 4) :param u: 控制输入序列 (N, 2) :param x_ref: 参考轨迹 (N+1, 4) :param Q: 状态权重矩阵 (4,4) :param R: 控制权重矩阵 (2,2) :return: 总代价 """ cost = 0 for k in range(len(u)): state_error = x[k] - x_ref[k] cost += state_error.T @ Q @ state_error + u[k].T @ R @ u[k] # 终端代价 terminal_error = x[-1] - x_ref[-1] cost += terminal_error.T @ Q @ terminal_error * 5 # 加大终端权重 return cost

权重矩阵的选择至关重要，它决定了控制器是更关注跟踪精度还是控制效率：

Q = np.diag([1, 10, 0.1, 0.1]) # 位置误差权重 > 速度误差权重 R = np.diag([0.1, 0.1]) # 控制输入权重

3. 状态空间模型与预测方程

根据DR_CAN讲解的离散状态空间模型，我们建立车辆动力学方程：

def vehicle_model(x, u): """ 离散时间车辆动力学模型 :param x: 当前状态 [x, y, vx, vy] :param u: 控制输入 [ax, ay] :return: 下一时刻状态 """ A = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) B = np.array([[0.5*dt**2, 0], [0, 0.5*dt**2], [dt, 0], [0, dt]]) return A @ x + B @ u

预测方程的实现需要考虑整个预测时域：

def predict_trajectory(x0, u_sequence): """ 预测状态轨迹 :param x0: 初始状态 :param u_sequence: 控制输入序列 (N, 2) :return: 状态轨迹 (N+1, 4) """ x_traj = np.zeros((len(u_sequence)+1, 4)) x_traj[0] = x0 for k in range(len(u_sequence)): x_traj[k+1] = vehicle_model(x_traj[k], u_sequence[k]) return x_traj

4. 滚动优化与数值求解

MPC的核心在于每个时间步求解一个优化问题。我们使用cvxpy库来构建和求解这个凸优化问题：

import cvxpy as cp def mpc_controller(current_state, x_ref, Q, R, N): """ MPC控制器求解 :param current_state: 当前状态 :param x_ref: 参考轨迹 :param Q: 状态权重 :param R: 控制权重 :param N: 预测步长 :return: 最优控制序列 """ # 定义优化变量 u = cp.Variable((N, 2)) x = cp.Variable((N+1, 4)) # 初始状态约束 constraints = [x[0] == current_state] # 系统动力学约束 for k in range(N): constraints += [ x[k+1] == vehicle_model(x[k], u[k]), cp.abs(u[k, 0]) <= 2.0, # 加速度限制 cp.abs(u[k, 1]) <= 1.0 ] # 构建代价函数 cost = 0 for k in range(N): state_error = x[k] - x_ref[k] cost += cp.quad_form(state_error, Q) + cp.quad_form(u[k], R) # 终端代价 terminal_error = x[N] - x_ref[N] cost += 5 * cp.quad_form(terminal_error, Q) # 求解优化问题 problem = cp.Problem(cp.Minimize(cost), constraints) problem.solve(solver=cp.OSQP, verbose=False) return u.value[0] if problem.status == cp.OPTIMAL else None

5. 仿真循环与结果可视化

最后，我们实现闭环仿真并可视化结果：

import matplotlib.pyplot as plt def simulate_mpc(x0, x_ref, N, T, Q, R): """ 运行MPC闭环仿真 :param x0: 初始状态 :param x_ref: 参考状态 :param N: 预测步长 :param T: 总仿真时间 :param Q: 状态权重 :param R: 控制权重 :return: 状态轨迹和控制序列 """ # 扩展参考轨迹 full_ref = np.tile(x_ref, (T+1, 1)) full_ref[:, 0] = np.linspace(x0[0], x_ref[0], T+1) full_ref[:, 1] = np.linspace(x0[1], x_ref[1], T+1) # 初始化 x_history = np.zeros((T+1, 4)) u_history = np.zeros((T, 2)) x_history[0] = x0 # 主循环 for t in range(T): # 获取当前参考轨迹段 current_ref = full_ref[t:t+N+1] if len(current_ref) < N+1: # 处理预测时域超出总时间的情况 last_ref = full_ref[-1] current_ref = np.vstack([current_ref, np.tile(last_ref, (N+1-len(current_ref), 1))]) # MPC控制 u_opt = mpc_controller(x_history[t], current_ref, Q, R, N) if u_opt is None: print(f"优化失败于时间步 {t}") break # 应用控制并记录 u_history[t] = u_opt x_history[t+1] = vehicle_model(x_history[t], u_opt) return x_history, u_history # 运行仿真 x_traj, u_traj = simulate_mpc(x0, x_ref, N, T, Q, R) # 可视化结果 plt.figure(figsize=(12, 8)) plt.subplot(2, 2, 1) plt.plot(x_traj[:, 0], x_traj[:, 1], 'b-o', label='实际轨迹') plt.plot([x0[0], x_ref[0]], [x0[1], x_ref[1]], 'r--', label='参考轨迹') plt.xlabel('X位置') plt.ylabel('Y位置') plt.title('车辆变道轨迹') plt.legend() plt.grid(True) plt.subplot(2, 2, 2) plt.plot(u_traj[:, 0], label='X方向加速度') plt.plot(u_traj[:, 1], label='Y方向加速度') plt.xlabel('时间步') plt.ylabel('控制输入') plt.title('控制序列') plt.legend() plt.grid(True) plt.subplot(2, 2, 3) plt.plot(x_traj[:, 2], label='X方向速度') plt.plot(x_traj[:, 3], label='Y方向速度') plt.xlabel('时间步') plt.ylabel('速度') plt.title('速度变化') plt.legend() plt.grid(True) plt.tight_layout() plt.show()

6. 常见数值问题与调试技巧

在实际编码中，你可能会遇到以下典型问题及解决方案：

1. 优化问题不可行

   • 检查约束条件是否过严
   • 确保预测时域内有可达的参考轨迹
   • 尝试放宽控制输入限制

2. 数值不稳定

   • 调整权重矩阵Q和R的相对大小
   • 检查系统矩阵的条件数
   • 考虑添加正则化项

3. 计算时间过长

   • 减少预测步长N
   • 尝试不同的求解器(如OSQP、ECOS)
   • 使用热启动技术

# 调试建议：检查预测轨迹 test_u = np.random.randn(N, 2) * 0.1 pred_traj = predict_trajectory(x0, test_u) print("预测轨迹形状:", pred_traj.shape) print("初始状态:", pred_traj[0]) print("终端状态:", pred_traj[-1])

通过这个完整的实现案例，你应该能够将DR_CAN视频中的MPC理论转化为实际可运行的代码。实践中发现，将终端代价权重设为跟踪误差权重的5倍，能显著改善变道末端的收敛性能。