自动驾驶中的MPC：从状态空间建模到轨迹跟踪的完整流程解析

自动驾驶中的MPC：从状态空间建模到轨迹跟踪的完整流程解析

当一辆自动驾驶汽车在复杂城市道路中穿行时，它需要实时处理海量传感器数据、预测周围车辆行为，并做出毫秒级的控制决策。这种高动态环境下的精确控制，正是模型预测控制（MPC）技术大显身手的舞台。不同于传统控制方法，MPC通过滚动优化和反馈校正机制，在满足多种约束条件的同时实现最优控制，这使其成为自动驾驶轨迹跟踪任务的理想选择。

本文将深入探讨如何将MPC这一通用控制框架，具体应用于自动驾驶车辆的横向和纵向控制。我们将从车辆动力学建模开始，逐步构建完整的MPC控制器，并分享实际工程中的关键调参经验和性能优化技巧。无论您是正在开发自动驾驶算法的工程师，还是希望了解前沿控制技术的研究者，都能从中获得可直接落地的技术方案。

1. 车辆动力学建模基础

1.1 自行车模型：简化与实用性的平衡

在自动驾驶控制领域，自行车模型因其简洁性和足够精度而被广泛采用。该模型假设车辆只有前后两个轮子，将四个轮胎简化为两个，同时忽略悬架动态和轮胎力非线性等复杂因素。虽然简化，但在常规驾驶工况下（侧向加速度不超过0.5g），其预测精度已能满足控制需求。

自行车模型的核心状态方程可表示为：

# 自行车模型状态方程示例 def bicycle_model(state, u, params): x, y, psi, v, delta = state # 状态：x位置、y位置、航向角、速度、前轮转角 a, delta_dot = u # 控制量：加速度、前轮转角变化率 beta = np.arctan(params.lr * np.tan(delta) / (params.lf + params.lr)) dxdt = v * np.cos(psi + beta) dydt = v * np.sin(psi + beta) dpsidt = v * np.cos(beta) * np.tan(delta) / (params.lf + params.lr) dvdt = a return np.array([dxdt, dydt, dpsidt, dvdt, delta_dot])

模型参数对控制性能影响显著，下表展示了典型乘用车的参数范围：

提示：实际应用中，建议通过实车试验或高精度仿真对模型参数进行辨识校准，特别是在负载变化较大的商用车场景。

1.2 状态空间方程的离散化处理

MPC需要在离散时间域进行优化计算，因此需要将连续状态方程离散化。常用的离散化方法包括欧拉前向差分、后向差分和双线性变换（Tustin方法）。对于采样周期Δt=50ms的典型自动驾驶系统，三种方法的计算复杂度和精度对比如下：

双线性变换虽然计算量稍大，但能更好保持系统稳定性特性，特别适合高速工况下的控制：

# 双线性变换离散化示例 def bilinear_discretize(A, B, C, D, dt): I = np.eye(A.shape[0]) A_d = np.linalg.inv(I - 0.5*dt*A) @ (I + 0.5*dt*A) B_d = np.linalg.inv(I - 0.5*dt*A) @ (B*dt) D_d = np.linalg.inv(I - 0.5*dt*A) @ (D*dt) return A_d, B_d, C, D_d

2. MPC控制器的核心设计

2.1 代价函数：平衡跟踪精度与乘坐舒适性

自动驾驶轨迹跟踪的核心目标可分解为：

• 最小化横向位置误差（通常权重最高）
• 最小化航向角误差（影响后续路径跟踪）
• 控制量平滑性（直接影响乘客舒适度）
• 控制量变化率限制（保护执行机构）

代价函数的数学表达为：

J = \sum_{k=1}^{N_p} ||y_k - y_{ref,k}||^2_Q + \sum_{k=0}^{N_c-1} ||u_k||^2_R + \sum_{k=0}^{N_c-1} ||\Delta u_k||^2_{R_{\Delta}}

实际工程中，各权重矩阵的初始值设置可参考以下经验比例：

注意：权重设置需要在实际车辆上进行闭环验证，不同车速下可能需要采用不同的权重参数集。

2.2 约束处理：确保安全与可行性

自动驾驶系统必须处理多种物理约束，这些约束可分类为：

1. 硬约束（必须严格满足）：

   • 最大转向角（由机械结构决定）
   • 最大转向速率（EPS系统能力限制）
   • 加速度/减速度限制（动力系统与舒适性要求）

2. 软约束（允许暂时违反）：

   • 车道边界约束（保留安全裕度）
   • 跟车距离（考虑感知不确定性）

在MPC框架中，约束通常表示为线性不等式：

# 约束设置示例 u_min = np.array([-3.5, -0.5]) # 最小加速度(m/s²)，最小转向角(rad) u_max = np.array([2.0, 0.5]) # 最大加速度，最大转向角 du_min = np.array([-1.0, -0.3]) # 最小加速度变化率，最小转向速率 du_max = np.array([1.0, 0.3]) # 最大加速度变化率，最大转向速率 # 构建QP问题的约束矩阵 A_ineq = np.vstack([np.eye(Nc*nu), -np.eye(Nc*nu), np.tril(np.ones((Nc*nu, Nc*nu))), -np.tril(np.ones((Nc*nu, Nc*nu)))]) b_ineq = np.concatenate([np.tile(u_max, Nc), -np.tile(u_min, Nc), np.tile(du_max, Nc), -np.tile(du_min, Nc)])

3. 工程实现关键问题

3.1 实时性优化：平衡预测时域与计算效率

预测时域(Np)和控制时域(Nc)的选择需要在控制性能和计算负担之间取得平衡。我们的实测数据显示：

计算效率优化技巧包括：

• 采用热启动(warm start)：利用上一周期的解作为初始猜测
• 代码生成：使用MATLAB Coder或Python Cython将算法编译为高效机器码
• 稀疏矩阵优化：利用QP问题的特殊结构减少计算量

// 使用Eigen库的稀疏矩阵求解示例（C++） SparseMatrix<double> H_sparse = H.sparseView(); VectorXd gradient = G; ConjugateGradient<SparseMatrix<double>> solver; solver.compute(H_sparse); VectorXd U_opt = solver.solve(-gradient);

3.2 模型失配补偿策略

即使精心建模，模型误差仍不可避免。我们推荐采用三重补偿机制：

1. 扰动观测器：估计未建模动态和外部干扰

   # 简化的扰动观测器实现 def disturbance_observer(x_actual, x_pred, prev_d): K_obs = 0.2 # 观测器增益 d_est = prev_d + K_obs * (x_actual - x_pred) return np.clip(d_est, -2.0, 2.0) # 限制扰动范围

2. 自适应权重调整：根据误差动态调整代价函数权重

3. 多模型切换：针对不同工况（如低速泊车、高速巡航）使用不同的模型参数集

4. 实际应用案例分析

4.1 城市道路换道场景

在换道场景中，MPC需要同时处理：

• 参考轨迹生成（五次多项式平滑过渡）
• 周围车辆预测（采用CTRA模型）
• 安全约束动态调整

典型换道性能指标：

4.2 高速弯道跟踪

高速弯道对MPC的挑战主要来自：

• 模型线性化误差增大
• 轮胎非线性特性显现
• 需要更长的预测距离

解决方案包括：

• 基于车速的自适应线性化点选择
• 前馈补偿（计算稳态转向角）
• 曲率前馈+MPC反馈的复合控制架构

# 曲率前馈计算 def curvature_feedforward(v, curvature, understeer_gradient): L = lf + lr # 轴距 K = understeer_gradient # 不足转向梯度 delta_ff = L * curvature + K * v**2 * curvature return delta_ff

在实测中，这套方案将高速弯道的最大横向误差从0.5m降低到0.25m以下，同时减少了60%的转向修正动作。