自动驾驶路径跟踪控制：MPC算法与动力学模型实践

1. 自动驾驶路径跟踪控制的核心挑战

在自动驾驶系统中，路径跟踪控制是确保车辆安全、稳定行驶的关键技术。当车辆以不同速度行驶时，特别是在弯道或低附着系数路面上，传统控制方法往往难以同时满足跟踪精度和行驶稳定性的双重要求。我在实际项目中发现，单纯依靠PID或LQR控制器在高速过弯时容易出现两种典型问题：一是转向不足导致路径偏离，二是过度转向引发车辆失稳。

动力学跟踪误差模型的出现为解决这一难题提供了新思路。与传统的几何跟踪模型不同，这种模型将车辆简化为一个质点，动力学模型则考虑了轮胎侧偏特性、质量分布等物理参数，能够更准确地描述车辆在复杂工况下的实际运动状态。特别是在处理20m/s（约72km/h）以上的中高速场景时，动力学模型的优势更为明显。

2. MPC算法在路径跟踪中的独特优势

模型预测控制（MPC）之所以成为自动驾驶路径跟踪的首选算法，主要得益于其三大核心特性：

2.1 滚动优化机制

MPC不像传统控制方法那样只计算当前时刻的最优解，而是基于当前状态预测未来N个时间步（预测时域）的系统行为，通过求解优化问题得到控制序列。在实际操作中，我们通常将预测时域设置为3-5秒，控制时域为预测时域的1/3-1/2。这种"看多远想多远"的策略特别适合处理车辆运动中的惯性延迟问题。

2.2 显式约束处理能力

在实车调试中，我发现转向执行机构存在明显的物理限制：

• 转向角速率限制（通常≤500°/s）
• 最大转向角度限制（通常±35°）
• 轮胎侧向力饱和特性

MPC能够将这些约束直接纳入优化问题，避免控制指令超出执行机构能力范围。相比之下，LQR等算法需要通过后处理进行限幅，容易导致性能下降。

2.3 多目标协调优化

一个实用的MPC控制器需要平衡多个有时相互冲突的目标：

python复制cost = w1*路径跟踪误差 + w2*控制量 + w3*控制增量 + w4*稳定性指标

其中权重系数w1-w4的选取直接影响控制效果。经过多次实车测试，我总结出一套经验公式：

code复制w1 = 1.0/(0.1*v + 0.5)  # 车速越高，跟踪误差权重越低
w2 = 0.01 + 0.001*v     # 控制量权重随车速增加
w3 = 0.1                # 控制增量保持恒定

这种动态权重策略能在不同车速下取得较好的平衡。

3. 动力学跟踪误差模型的构建细节

3.1 车辆动力学建模

采用经典的二自由度自行车模型作为基础，考虑以下关键参数：

matlab复制% 车辆参数示例（对应某型测试车）
m = 1412;       % 质量(kg)
lf = 1.015;     % 前轴到质心距离(m)
lr = 1.895;     % 后轴到质心距离(m)
Iz = 1537;      % 绕Z轴转动惯量(kg·m²)
Cf = 149000;    % 前轮侧偏刚度(N/rad)
Cr = 82200;     % 后轮侧偏刚度(N/rad)

状态方程可表示为：

code复制ẋ = v*cos(ψ+β)
ẏ = v*sin(ψ+β)
ψ̇ = γ
β̇ = (Fyf*cos(δ)+Fyr)/m/v - γ
γ̇ = (lf*Fyf*cos(δ)-lr*Fyr)/Iz

其中β为质心侧偏角，γ为横摆角速度，δ为前轮转角。

3.2 跟踪误差定义

不同于全局坐标系下的绝对误差，我们采用Frenet坐标系定义误差：

• 横向误差ey：车辆质心到参考路径的垂直距离
• 航向误差eψ：车辆航向角与参考路径方向的差值

误差动力学模型经过线性化处理后得到：

code复制ėy = v*(eψ + β)
ėψ = γ - κ*v/(1-κ*ey)

其中κ为路径曲率。这个模型清晰地揭示了车速、侧偏角和路径曲率对跟踪误差的影响机制。

4. MPC控制器的实现与优化

4.1 离散化与预测模型

采用Tustin双线性变换将连续模型离散化，采样周期通常设为50-100ms。预测模型的状态空间表示为：

code复制x(k+1) = A*x(k) + B*u(k)
y(k) = C*x(k)

其中状态量x=[ey eψ β γ]ᵀ，控制量u=δ。

4.2 目标函数设计

经过多次仿真对比，我推荐使用以下形式的二次型目标函数：

code复制J = Σ(eyᵢQeyᵢ + eψᵢReψᵢ + δᵢSδᵢ + ΔδᵢTΔδᵢ)

其中Δδ表示转向角增量，用于保证控制平滑性。Q、R、S、T为对应项的权重矩阵。

4.3 约束条件设置

实际应用中需要考虑三类约束：

1. 执行机构约束：

   • |δ| ≤ 35°（机械限位）
   • |Δδ| ≤ 5°/s（电机能力限制）

2. 稳定性约束：

   • |β| ≤ 5°（避免侧滑）
   • |γ| ≤ 0.3*v/l（基于阿克曼转向几何）

3. 舒适性约束：

   • |Δδ| ≤ 3°/s²（降低乘客不适感）

5. 仿真平台搭建与结果分析

5.1 Carsim-Simulink联合仿真

建议采用Carsim提供高精度车辆动力学模型，Simulink实现控制算法，两者通过S-Function接口通信。关键配置步骤：

1. Carsim中设置车辆参数（如前述质量、轴距等）
2. 定义输入输出接口：
   • 输入：转向角指令δ
   • 输出：车辆状态x,y,ψ,v,β,γ等
3. Simulink中配置固定步长求解器（推荐0.01s）

5.2 典型测试场景

设计包含以下特征的参考路径：

• 直线段（验证稳态性能）
• 正弦曲线（频率0.1Hz，幅值4m，测试动态响应）
• 急转弯（半径30m，检验极限性能）

5.3 结果对比分析

在20m/s车速下测试，得到关键指标对比：

改进措施包括：

• 采用PSO优化预测时域和控制时域
• 引入侧偏角软约束
• 使用GPR进行参数自适应

6. 实车部署的工程经验

6.1 模型简化技巧

为满足实时性要求（计算周期≤50ms），需要进行合理简化：

• 将非线性轮胎模型分段线性化
• 减少预测时域点数（通常取20-30）
• 使用QP求解器的热启动功能

6.2 参数标定流程

建议按以下步骤进行参数标定：

1. 静态参数测量（质量、轴距等）
2. 动态参数辨识（通过正弦扫频试验获取Cf,Cr）
3. 控制参数离线优化（PSO+交叉验证）
4. 在线微调（基于实车测试数据）

6.3 典型问题排查

在实际项目中遇到的几个典型问题及解决方案：

1. 高频振荡：

   • 现象：转向电机频繁小幅摆动
   • 原因：Δδ权重过低
   • 解决：增加T矩阵对角元素值

2. 响应迟滞：

   • 现象：转向动作明显滞后
   • 原因：预测时域过短
   • 解决：增加Np至30-40步

3. 弯道超调：

   • 现象：出弯时偏离路径另一侧
   • 原因：曲率变化率未考虑
   • 解决：在目标函数中加入曲率变化率项

7. 前沿改进方向

7.1 数据驱动参数自适应

传统固定参数MPC在复杂工况下表现受限，我们正在测试两种改进方案：

1. 基于GPR的参数映射：

   • 输入：车速v、曲率κ、路面μ
   • 输出：最优MPC参数
   • 优势：无需精确机理模型

2. 在线学习策略：

   • 采用递归最小二乘更新模型参数
   • 设置性能指标阈值触发重优化
   • 计算负载增加约15%

7.2 考虑执行器动力学的补偿设计

实际转向系统存在约80-120ms的延迟，我们采用两种补偿方法：

1. 前馈补偿：

   • 建立转向电机的一阶惯性模型
   • 在MPC输出端串联逆模型

2. 状态预测：

   • 扩展状态向量包含电机状态
   • 增加4个状态变量（转子位置、速度等）

7.3 多速率控制架构

为解决不同子系统响应速度差异问题，建议采用：

• 路径规划层：100ms周期
• MPC控制层：50ms周期
• 执行器控制层：10ms周期
  各层间通过双缓冲机制交换数据，确保时序一致性。