基于135模型与MPC的自动驾驶轨迹跟踪控制方案

1. 项目概述

在自动驾驶技术快速发展的今天，车辆轨迹跟踪控制作为核心功能模块，其性能直接影响着自动驾驶系统的安全性和舒适性。本文将详细介绍基于135动力学模型，结合横向MPC（模型预测控制）和纵向MPC，通过Simulink与CarSim联合仿真实现任意轨迹跟踪的完整方案。

这个方案最大的特点在于：

1. 采用135动力学模型作为基础，既保证了计算效率又兼顾了必要的精度
2. 横向和纵向控制分离设计，通过MPC实现最优控制
3. 支持多种轨迹类型，包括五次多项式轨迹和圆形轨迹
4. 利用Simulink和CarSim的联合仿真环境，可以快速验证算法效果

提示：135动力学模型是对车辆运动特性的简化表示，它保留了纵向、横向和横摆三个自由度的运动，是轨迹跟踪控制中常用的折中方案。

2. 135动力学模型详解

2.1 模型结构与假设

135动力学模型是车辆控制领域广泛使用的一种简化模型，其名称来源于它考虑的1个纵向自由度、3个横向自由度和5个轮胎动力学特性。具体来说：

• 纵向运动：考虑车辆前进方向的加速度
• 横向运动：包括侧向位移、侧向速度和横摆角
• 轮胎模型：采用线性轮胎模型，考虑侧偏刚度特性

模型的基本假设包括：

1. 忽略悬架动态特性
2. 假设路面平整且摩擦系数均匀
3. 不考虑空气动力学影响
4. 假设转向系统为刚性连接

2.2 数学模型表达

135动力学模型的状态方程可以表示为：

code复制ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = (v / L) * tan(δ)
v̇ = a
β = atan( (lr * tan(δ)) / (lf + lr) )

其中：

• (x,y)为车辆质心位置
• θ为航向角
• v为车速
• δ为前轮转角
• a为纵向加速度
• lf和lr分别为前后轴到质心的距离
• β为质心侧偏角

3. MPC控制器设计

3.1 横向MPC设计

横向MPC控制器的核心目标是使车辆跟踪期望的轨迹，同时保证行驶的平顺性。其设计要点包括：

1. 预测模型：使用135动力学模型的简化版本

2. 成本函数设计：

   • 轨迹跟踪误差（横向位置偏差）
   • 航向角偏差
   • 控制量变化率（转向角速度）
   • 终端状态惩罚项

3. 约束条件：

   • 最大前轮转角限制
   • 转向角速度限制
   • 车辆动力学稳定性约束

典型的预测时域选择10-20步，控制时域5-10步，采样时间0.05-0.1秒。

3.2 纵向MPC设计

纵向MPC控制器负责速度跟踪，主要考虑：

1. 预测模型：简化的纵向动力学模型

2. 成本函数：

   • 速度跟踪误差
   • 加速度变化率（冲击度）
   • 与前车的安全距离（如有）

3. 约束条件：

   • 最大加速度/减速度限制
   • 速度上下限
   • 执行器响应延迟

注意：在实际实现时，横向和纵向MPC需要协调工作，通常采用主从式结构，横向为主，纵向为辅。

4. Simulink与CarSim联合实现

4.1 仿真环境搭建

联合仿真环境的搭建步骤如下：

1. CarSim配置：

   • 选择适当的车辆模型参数
   • 设置输入输出接口
   • 配置仿真步长和通信周期

2. Simulink模型构建：

   • 创建MPC控制器模块
   • 设计轨迹生成器
   • 搭建数据记录和分析模块

3. 接口配置：

   • 设置S-function接口
   • 配置数据交换协议
   • 测试通信延迟

4.2 轨迹生成方法

4.2.1 五次多项式轨迹

五次多项式轨迹的生成步骤：

1. 确定边界条件：

   • 初始位置、速度、加速度
   • 终止位置、速度、加速度
   • 运动时间

2. 构建方程组求解系数：

   code复制x(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
   v(t) = a1 + 2a2*t + 3a3*t² + 4a4*t³ + 5a5*t⁴
   a(t) = 2a2 + 6a3*t + 12a4*t² + 20a5*t³
   

3. 解线性方程组得到多项式系数

4.2.2 圆形轨迹

圆形轨迹生成方法：

1. 参数设定：

   • 圆心坐标
   • 半径大小
   • 运动方向（顺时针/逆时针）
   • 运动速度

2. 轨迹点计算：

   code复制x = x0 + R*cos(ωt)
   y = y0 + R*sin(ωt)
   

   其中ω为角速度，由期望速度决定

5. 实现细节与调试技巧

5.1 MPC参数调优经验

1. 预测时域选择：

   • 太短会导致控制短视
   • 太长会增加计算负担
   • 建议从10步开始调试

2. 权重矩阵设置：

   • 先调误差项权重
   • 再调控制量权重
   • 最后调变化率权重

3. 采样时间选择：

   • 通常取车辆响应时间的1/5-1/10
   • 建议从0.1秒开始尝试

5.2 常见问题排查

1. 车辆振荡问题：

   • 检查MPC权重设置
   • 确认预测模型准确性
   • 调整约束条件

2. 跟踪延迟问题：

   • 检查控制器响应速度
   • 确认车辆模型参数
   • 调整预测时域

3. 数值不稳定问题：

   • 检查求解器设置
   • 确认矩阵条件数
   • 尝试正则化方法

6. 实际应用中的注意事项

1. 模型失配处理：

   • 增加鲁棒性设计
   • 考虑参数自适应
   • 加入扰动观测器

2. 实时性保障：

   • 代码优化技巧
   • 选择高效求解器
   • 考虑显式MPC

3. 硬件在环测试：

   • 逐步迁移策略
   • 实时性验证
   • 故障注入测试

在多次实际项目中，我发现MPC控制器的初始化阶段特别关键。良好的初始状态估计可以显著提高控制性能。建议在系统启动时加入一段状态观测和参数辨识过程，待状态稳定后再激活MPC控制器。