1. 自动驾驶横纵向控制方案概述
在自动驾驶车辆控制系统中,横向和纵向控制是最核心的两个组成部分。纵向控制负责车辆的加速和减速(油门和刹车控制),横向控制则负责方向盘转向。我们采用的方案是纵向使用PID控制,横向使用MPC(模型预测控制)控制,基于车辆的二自由度动力学模型进行设计。
这种组合方案在实际工程中非常实用:
- PID控制简单可靠,特别适合纵向控制这种相对线性的控制问题
- MPC能够处理多变量、有约束的控制问题,非常适合车辆转向控制
- 二自由度模型在保证精度的同时计算量适中,适合实时控制
2. 纵向PID控制实现细节
2.1 双环PID控制结构
参考百度Apollo的方案,我们采用双环PID控制结构:
- 外环:速度控制环
- 输入:期望速度与实际速度的误差
- 输出:期望加速度
- 内环:加速度控制环
- 输入:期望加速度与实际加速度的误差
- 输出:油门/刹车控制量
这种分层结构的好处是:
- 解耦了速度控制和执行器控制
- 可以分别优化两个环路的参数
- 更符合驾驶员的操纵习惯
2.2 油门刹车一体化设计
一个巧妙的设计是使用同一个PID控制器同时控制油门和刹车:
matlab复制function [throttle, brake] = fcn(target_accel, current_speed)
persistent pid;
if isempty(pid)
pid = pidController('Kp',0.8,'Ki',0.05,'Kd',0.02);
end
accel_error = target_accel - getActualAccel();
control = step(pid, accel_error);
if control > 0
throttle = saturate(control, 0, 1);
brake = 0;
else
throttle = 0;
brake = saturate(-control, 0, 1);
end
end
这种设计的优势:
- 避免了油门和刹车之间的切换振荡
- 简化了控制逻辑
- 更接近真实车辆的驱动/制动特性
注意:虽然代码中没有显式的抗饱和处理,但在实际运行中,积分项会自然找到平衡点。实测在匀速30km/h时,油门开度能稳定在18%左右。
2.3 PID参数整定经验
经过多次调试,我们确定了以下参数组合:
- Kp=0.8:提供快速响应
- Ki=0.05:消除稳态误差
- Kd=0.02:抑制超调
调试技巧:
- 先调速度环,再调加速度环
- 在典型工况(加速、减速、匀速)下分别验证
- 关注控制量的平滑性,避免频繁跳动
3. 横向MPC控制实现
3.1 二自由度车辆模型
MPC的核心是基于车辆动力学模型的预测。我们采用经典的二自由度自行车模型:
状态方程:
code复制ẋ = A·x + B·u
y = C·x
其中状态矩阵A为:
cpp复制A << 0, 1, 0, 0,
0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, -(a*Cf - b*Cr)/(m*vx),
0, 0, 0, 1,
0, -(a*Cf - b*Cr)/(Iz*vx), (a*Cf - b*Cr)/Iz, -(a²*Cf + b²*Cr)/(Iz*vx);
模型参数:
- Cf, Cr:前后轮侧偏刚度
- m:车辆质量
- Iz:绕Z轴的转动惯量
- a,b:前后轴到质心的距离
3.2 MPC设计要点
- 预测时域:选择20步(约2秒)
- 控制时域:选择5步
- 采样时间:0.1秒
- 权重矩阵:
- 横向误差权重:500
- 前轮转角权重:10
关键点:低速处理策略。当车速vx低于1m/s时,冻结横向控制器,避免矩阵元素数值不稳定。
3.3 五次多项式轨迹处理
轨迹生成采用五次多项式:
code复制y(x) = a0 + a1x + a2x² + a3x³ + a4x⁴ + a5x⁵
实际应用中发现的问题及解决方案:
- 问题:曲率突变导致方向盘抖动
- 解决方案:增加低通滤波器,限制曲率变化率在0.1m⁻¹/s以内
- 代价:引入约0.5m的跟踪滞后
4. 联合仿真实现
4.1 Carsim-Simulink接口配置
关键配置步骤:
- 在Carsim中设置输出变量(车速、加速度、横摆角等)
- 生成Simulink S-Function接口
- 在Simulink中正确加载carsim2simulink.dll
血泪教训:必须在Initialize子函数中先调用VS_Init,否则会得到NaN的车辆状态。
4.2 仿真调试技巧
- 添加多个示波器监控关键信号:
- 实际vs期望速度
- 实际vs期望路径
- 控制量(油门、刹车、转向)
- 分阶段验证:
- 先验证纵向控制
- 再验证横向控制
- 最后联合调试
- 典型测试工况:
- 双移线测试
- 恒定半径转弯
- 加速-减速循环
5. 性能评估与改进方向
5.1 当前控制性能
测试条件:双移线工况,初始速度30km/h
| 指标 | 性能 |
|---|---|
| 横向误差 | <0.2m |
| 纵向速度误差 | ±3km/h |
| 方向盘抖动 | 无明显抖动 |
| 计算延迟 | <10ms |
5.2 遇到的典型问题及解决
-
问题:MPC在轨迹噪声下失稳
- 现象:加入10cm随机噪声后,车辆画正弦曲线
- 原因:MPC对输入轨迹过于敏感
- 临时方案:增加输入滤波
- 长期方案:开发鲁棒性更强的MPC
-
问题:低速转向控制不稳定
- 现象:车速<1m/s时转向角发散
- 解决方案:低速冻结横向控制
-
问题:急弯跟踪滞后
- 现象:滞后约0.5m
- 原因:曲率滤波引入的相位延迟
- 权衡:牺牲少量跟踪精度换取控制平滑性
5.3 未来改进方向
- 动态预瞄距离:根据车速调整预瞄距离
- 参数自适应:基于工况自动调整MPC权重
- 执行器补偿:考虑转向系统动力学延迟
- 扰动观测器:增强抗干扰能力
6. 工程实践建议
-
开发调试建议:
- 先进行开环测试验证接口
- 分模块逐步验证
- 建立完善的日志系统
-
参数整定经验:
- 纵向PID:先比例后积分
- 横向MPC:先调误差权重,再调控制量权重
- 在多种典型工况下验证
-
计算效率优化:
- 使用高效的QP求解器
- 合理选择预测时域
- 考虑代码生成优化
这套横纵向控制方案虽然还有改进空间,但已经展现出良好的工程实用性。在后续开发中,我们计划进一步优化MPC的实时性能,并增强控制系统的鲁棒性。控制算法的开发永远都是在理想和现实之间寻找最佳平衡点。