陈冠男1. 自动驾驶轨迹规划技术概述
在自动驾驶系统架构中，轨迹规划模块扮演着"决策执行者"的角色。它需要综合考虑环境感知信息、车辆动力学约束以及行驶目标，生成既安全又舒适的行驶路径。目前主流的解决方案主要分为两类：基于采样的方法（如RRT*）和基于优化的方法（如人工势场法）。我们这次重点探讨后者与模型预测控制（MPC）的协同方案。
人工势场法（Artificial Potential Field, APF）的物理直觉非常直观——想象车辆在一个虚拟力场中运动：目标点像磁铁一样产生吸引力，障碍物则像同极磁铁产生排斥力。这种方法的计算效率高，适合实时性要求严格的自动驾驶场景。但传统APF存在三个典型问题：
- 局部最小值陷阱（车辆被困在势场洼地）
- 动态障碍物处理不足
- 未考虑车辆动力学约束
这正是引入MPC的价值所在。MPC采用滚动时域优化策略，在每个控制周期：
- 基于当前状态预测未来N步的系统行为
- 求解最优控制序列
- 只执行第一步控制量
这种机制既能处理动态约束，又能补偿APF的局部缺陷。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 人工势场法核心实现
2.1 势场建模原理
势场函数的设计直接影响规划效果。我们采用改进的指数型势场函数：
python复制import numpy as np
class PotentialField:
def __init__(self):
self.k_att = 0.5 # 引力增益系数
self.k_rep = 15.0 # 斥力增益系数
self.d_safe = 3.0 # 障碍物影响半径(m)
def attractive_potential(self, q, q_goal):
"""二次型吸引势场"""
return 0.5 * self.k_att * np.linalg.norm(q - q_goal)**2
def repulsive_potential(self, q, q_obs):
"""指数型排斥势场"""
d = np.linalg.norm(q - q_obs)
if d <= self.d_safe:
return self.k_rep * (1/d - 1/self.d_safe) * np.exp(-d)
return 0
关键参数选择经验：
- 引力系数k_att通常取0.3-1.0，过大易导致轨迹震荡
- 斥力系数k_rep建议为k_att的20-30倍
- d_safe建议设为车宽的2.5-3倍
2.2 势场叠加策略
实际场景需要处理多障碍物情况。我们采用势场叠加原则：
code复制U_total = U_att + ΣU_rep_i
F_total = -∇U_total
具体实现时需要注意：
- 对动态障碍物需加入速度项补偿
- 道路边界可建模为线性障碍物
- 势场梯度需做归一化处理
2.3 局部最小值解决方案
针对APF的"陷阱"问题，我们采用三种应对策略：
- 随机扰动法：检测到停滞时施加随机力
- 虚拟目标点法：在障碍物后方设置临时目标
- MPC补偿法：通过优化过程跳出局部最优
3. MPC控制器设计与实现
3.1 车辆模型建立
采用自行车模型作为预测模型：
code复制状态方程：
ẋ = v·cos(θ+β)
ẏ = v·sin(θ+β)
θ̇ = (v/l_r)·sin(β)
v̇ = a
其中：
β = arctan((l_r/(l_f+l_r))·tan(δ))
在Simulink中搭建该模型时，需要特别注意：
- 前轮转向角δ的物理限制（通常±30°）
- 加速度a的饱和特性
- 轮胎侧偏刚度的参数辨识
3.2 优化问题构建
MPC的核心是如下优化问题：
code复制min J = Σ(||x(k)-x_ref(k)||_Q + ||u(k)||_R)
s.t.
x(k+1) = f(x(k),u(k))
u_min ≤ u(k) ≤ u_max
其中权重矩阵的选择原则：
- Q矩阵：位置误差权重 > 航向误差权重
- R矩阵：转向控制权重 > 加速控制权重
3.3 Simulink实现技巧
-
使用MPC工具箱时，建议：
- 采样周期取50-100ms
- 预测时域选2-3秒（N=20-30）
- 开启warm start加速求解
-
调试时重点关注：
matlab复制mpcobj.Optimizer.Algorithm = 'interior-point'; mpcobj.Model.Plant = vehicle_ss; mpcobj.Weights.OutputVariables = [10 1]; % [位置 航向]
4. Carsim-Simulink联合仿真
4.1 接口配置要点
-
Carsim设置：
- 选择S-Function接口模式
- 输出配置至少包含：
- 车辆位置(x,y)
- 航向角
- 车速
- 横摆角速度
-
Simulink配置：
matlab复制% 在Model Properties/Callbacks中添加： csport = get_param(gcs, 'CarsimPort'); vsb_init('vsb.dll', csport);
4.2 联合仿真流程
- 在Carsim中建立测试场景（建议从ISO标准工况开始）
- 配置车辆参数（重点检查轮胎模型）
- 导出S-Function到Simulink
- 搭建控制算法模块
- 设置联合仿真参数：
- 通信步长=控制周期
- 开启实时同步
4.3 典型问题排查
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 仿真崩溃 | 接口版本不匹配 | 检查Carsim与MATLAB兼容性 |
| 控制延迟 | 步长设置过大 | 调整步长≤50ms |
| 轨迹震荡 | 权重矩阵不当 | 重新调节Q/R矩阵 |
5. 实际测试与优化
5.1 换道场景测试
设计双移线工况进行验证：
- 初始速度设为60km/h
- 目标车道偏移3.5m
- 加入静态障碍物
关键指标：
- 横向加速度≤2.5m/s²
- 转向盘转速≤200°/s
- 完成时间≤5s
5.2 跟踪误差分析
采用RMSE作为评价指标：
code复制RMSE = sqrt(mean((y_actual - y_ref).^2))
优化经验：
- 误差>0.3m时，检查势场参数
- 误差波动大时，调整MPC预测时域
- 出现系统性偏差时，校验车辆模型
5.3 动态避障测试
设置20km/h的移动障碍物，验证系统响应：
- 障碍物检测延迟补偿
- 势场力动态调整策略
- 紧急制动触发逻辑
6. 工程实践建议
-
实时性优化：
- 将APF计算移植到C++ S-Function
- 使用QP求解器的热启动功能
- 限制最大迭代次数（建议≤50）
-
安全冗余设计：
c复制// 伪代码示例 if (collision_imminent) { activate_emergency_brake(); override_control = true; } -
参数调试流程：
- 先调APF使静态避障达标
- 再调MPC改善跟踪性能
- 最后联合微调
在实际项目中，我们发现这种组合方案在处理器负载方面表现优异：在Intel i7-1185G7平台上，单次计算周期可控制在8ms以内，完全满足实时性要求。不过要注意，复杂场景下可能需要引入轨迹预测模块来提前识别潜在冲突。