从零实现线性时变MPC：运动学建模与Python仿真全解析

刚接触模型预测控制（MPC）时，最令人头疼的莫过于那些看似天书般的矩阵推导。我曾花了整整两周时间，在龚建伟教授的《无人驾驶车辆模型预测控制》和无数B站视频之间反复切换，只为搞明白一个简单的运动学模型如何转化为可求解的二次规划问题。直到某天深夜，当我把泰勒展开的线性化过程用Python代码一步步实现时，那些抽象符号突然变得鲜活起来——原来MPC的魔力就藏在这行代码与数学的交织中。

本文将带你完整走通这个"顿悟时刻"。我们从自行车模型出发，用可运行的代码还原每个推导步骤，包括：

• 为什么后轮中心更适合作为参考点？
• 泰勒展开线性化时那些被省略的中间步骤
• 如何巧妙引入松弛变量处理约束冲突
• 把优化问题转化为quadprog能解的标准形式

最终你会得到一个约50行的Python实现，能完成基本的轨迹跟踪任务。更重要的是，你将掌握MPC的"解题套路"，未来遇到更复杂的动力学模型也能举一反三。

1. 运动学建模：从自行车模型到状态方程

1.1 模型选择与坐标系定义

在自动驾驶的轨迹跟踪场景中， 自行车模型 因其简洁性成为首选。假设车辆像自行车一样只有前轮转向，我们通常选择以下两种参考点建立模型：

参考点位置	优点	缺点
车辆质心	惯性参数直观	后轮速度方向与车身不一致
后轮轴心（推荐）	前轮转角直接控制航向	需额外计算质心位置

选择后轮中心作为原点建立坐标系，定义状态变量：

# 状态变量 [x, y, psi, v]
# x: 后轮中心X坐标
# y: 后轮中心Y坐标  
# psi: 车身航向角
# v: 后轮中心速度
state = np.zeros(4)

1.2 非线性运动学方程推导

根据刚体运动学，可以得到连续时间的非线性模型：

dx/dt = v * cos(psi)
dy/dt = v * sin(psi) 
dpsi/dt = v * tan(delta) / L
dv/dt = a

其中 delta 为前轮转角， L 为轴距， a 为加速度。这个模型的关键在于：

• 几何约束 ：前轮转向导致瞬时旋转中心在后轮轴延长线上
• 小角度假设 ：实际中tan(delta)≈delta在±15°内误差<1%

注意：这里忽略了轮胎侧偏角，纯运动学模型假设轮胎永远保持纯滚动

2. 线性化与离散化：泰勒展开的魔法

2.1 参考轨迹上的泰勒展开

非线性模型无法直接用于MPC的凸优化求解，需要在参考轨迹点 (x_ref, y_ref, psi_ref, v_ref) 处进行一阶泰勒展开。以航向角方程为例：

# 非线性项：f = v * tan(delta)/L
def linearize_steering(v_ref, delta_ref, L):
    # 零阶项（参考点函数值）
    f0 = v_ref * np.tan(delta_ref) / L
    
    # 对v的偏导
    df_dv = np.tan(delta_ref) / L  
    
    # 对delta的偏导
    df_ddelta = v_ref / (L * np.cos(delta_ref)**2)
    
    return f0, df_dv, df_ddelta

这个过程中常被忽略的细节是：

1. 先对每个状态方程单独线性化
2. 将控制量 delta 和 a 也视为变量参与求导
3. 最后组合成矩阵形式的误差状态方程

2.2 离散化处理

采用前向欧拉法离散化，采样时间 dt 的选择至关重要：

dt = 0.1  # 典型值100ms
A_discrete = np.eye(n_states) + A_continuous * dt
B_discrete = B_continuous * dt

提示：实际工程中会用更精确的零阶保持法(ZOH)或双线性变换(Tustin)

3. 预测方程：构建未来状态序列

3.1 递推关系的矩阵表达

预测时域 Np 内的状态序列可以表示为：

Psi = np.zeros((Np * n_states, n_states))
Theta = np.zeros((Np * n_states, Nc * n_controls))

for i in range(Np):
    # Psi矩阵块
    Psi[i*n_states:(i+1)*n_states] = np.linalg.matrix_power(A_discrete, i+1)
    
    # Theta矩阵块
    for j in range(min(i+1, Nc)):
        Theta[i*n_states:(i+1)*n_states, j*n_controls:(j+1)*n_controls] = (
            np.linalg.matrix_power(A_discrete, i-j) @ B_discrete
        )

这个双重循环结构正是MPC预测能力的核心——通过 Psi 和 Theta 矩阵将未来状态表示为当前状态和控制序列的线性组合。

3.2 松弛变量的妙用

为避免无解情况，在代价函数中增加松弛变量 epsilon ：

H = 2 * (Theta.T @ Q @ Theta + R)  # 原Hessian矩阵
H = np.block([
    [H, np.zeros((Nc * n_controls, 1))],
    [np.zeros((1, Nc * n_controls)), rho]  # rho为松弛权重
])

这样当约束冲突时，优化器会优先保证安全性而非精确跟踪。

4. 二次规划求解：从理论到代码

4.1 标准形式转换

quadprog求解器要求标准形式：

min 0.5 * x^T H x + f^T x
s.t. A x <= b

我们需要将MPC问题巧妙转换：

# 构建不等式约束矩阵
A_ineq = np.vstack([
    np.hstack([Theta, np.zeros((Np * n_states, 1))]),  # 上界约束
    np.hstack([-Theta, np.zeros((Np * n_states, 1))])  # 下界约束
])

# 对应约束向量
b_ineq = np.hstack([
    X_max - Psi @ x0,  # 状态上界
    -X_min + Psi @ x0  # 状态下界
])

4.2 完整求解流程

from quadprog import solve_qp

def solve_mpc(x0, Psi, Theta, Q, R, X_max, X_min, U_max, U_min):
    # 构造Hessian矩阵和线性项
    H, f = build_cost_matrices(Q, R, Psi, Theta, x0)
    
    # 构造约束矩阵
    A, b = build_constraint_matrices(Psi, Theta, x0, X_max, X_min, U_max, U_min)
    
    # 调用求解器
    u_opt, _, _, _ = solve_qp(H, f, A.T, b, 0)
    
    return u_opt[:n_controls]  # 只取第一个控制量

实际调试时会发现：

• 权重矩阵 Q 的对角元素需要量纲平衡
• 预测时域 Np 过长会导致"近视"问题
• 控制时域 Nc 通常取 Np 的1/3~1/2

5. 闭环仿真：从理论到实践

5.1 仿真框架搭建

def simulate_mpc(track, Np=10, dt=0.1):
    x = x_init  # 初始状态
    trajectory = []
    
    for k in range(len(track)):
        # 获取参考轨迹窗口
        x_ref = track[k:k+Np]
        
        # 求解MPC
        u_opt = solve_mpc(x, x_ref)
        
        # 应用控制量并更新状态
        x = vehicle_model(x, u_opt, dt)
        trajectory.append(x)
        
    return np.array(trajectory)

5.2 典型调试问题

在实车项目中遇到的几个经典问题：

1. 抖动现象 ：增大控制量变化权重 R 可平滑控制
2. 超调严重 ：调整 Q 矩阵中位置误差项的权重
3. 求解失败 ：检查约束是否过紧，或引入松弛变量

# 典型权重设置（需根据实际调整）
Q = np.diag([10, 10, 1, 0.1])  # 位置误差权重较大
R = np.diag([0.1, 0.01])       # 控制量变化权重较小

6. 进阶技巧与工程实践

6.1 参考轨迹预处理

原始GPS轨迹往往存在噪声，需要预处理：

from scipy.signal import savgol_filter

def smooth_trajectory(traj, window=5, polyorder=3):
    # 应用Savitzky-Golay滤波器
    return savgol_filter(traj, window, polyorder, axis=0)

6.2 热启动优化

利用上一时刻的解加速收敛：

u_guess = np.roll(u_prev, -n_controls)  # 移位上次解
u_guess[-n_controls:] = u_prev[-n_controls:]  # 末尾补值

# 将初始猜测传递给求解器
solve_qp(H, f, A.T, b, 0, initvals=u_guess)

6.3 实时性保障

对于嵌入式部署，可以：

• 固定浮点运算为定点数
• 使用显式MPC（预先计算解空间分区）
• 采用OSQP等更快的QP求解器

# OSQP示例配置
solver = osqp.OSQP()
solver.setup(P=H, q=f, A=A, l=lb, u=ub, verbose=False)
results = solver.solve()

在实车测试中，当代码第一次成功让车辆沿着S弯道自主行驶时，那种成就感远超任何理论推导。MPC的魅力正在于此——它将抽象的数学转化为看得见的控制艺术。现在，是时候让你的代码也跑起来了！