1. 自动驾驶横纵向控制方案设计
在自动驾驶车辆控制系统中,横向和纵向控制是两个最核心的子系统。纵向控制负责车辆的速度和加速度调节,而横向控制则负责车辆的转向和轨迹跟踪。我采用的方案是纵向PID+横向MPC的组合,这种架构在工业界有广泛应用,比如百度Apollo就采用了类似的设计。
1.1 系统架构设计
整个控制系统采用模块化设计,主要包含以下几个关键模块:
- 轨迹规划模块:生成五次多项式形式的参考轨迹
- 纵向控制模块:双环PID控制器
- 横向控制模块:基于模型预测控制(MPC)
- 车辆模型:二自由度车辆动力学模型
- 执行器接口:油门/刹车和方向盘控制
这种架构的优势在于:
- PID控制器简单可靠,适合处理纵向控制这类相对线性的问题
- MPC能够处理多变量、有约束的控制问题,非常适合横向控制
- 模块化设计便于调试和性能优化
1.2 车辆动力学模型
控制系统的基础是二自由度车辆动力学模型,也称为自行车模型。这个模型虽然简化,但能很好地反映车辆的基本动力学特性。
模型的状态方程可以表示为:
code复制ẋ = v*cos(ψ + β)
ẏ = v*sin(ψ + β)
ψ̇ = v/l_r * sin(β)
β = arctan(l_r/(l_f + l_r) * tan(δ_f))
其中:
- (x,y)是车辆质心位置
- ψ是车辆航向角
- β是质心侧偏角
- v是车速
- l_f和l_r分别是前后轴到质心的距离
- δ_f是前轮转角
这个模型在Simulink中实现时,我特别注意了以下几点:
- 车速v接近零时的数值稳定性问题
- 模型参数的准确性,特别是轮胎侧偏刚度
- 采样时间的选择,要与控制器频率匹配
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 纵向PID控制器实现
纵向控制采用双环PID结构,外环控制速度,内环控制加速度。这种结构能够很好地处理车辆纵向动力学的非线性特性。
2.1 双环PID设计
外环PID控制器:
- 输入:速度误差(目标速度-实际速度)
- 输出:目标加速度
- 参数整定:Kp=0.6, Ki=0.1, Kd=0.02
内环PID控制器:
- 输入:加速度误差(目标加速度-实际加速度)
- 输出:油门/刹车指令
- 参数整定:Kp=0.8, Ki=0.05, Kd=0.02
在Simulink中实现时,我使用了PID Controller模块,并启用了抗饱和功能。特别需要注意的是积分项的处理,我采用了条件积分的方法,当误差超过一定阈值时暂停积分,防止积分饱和。
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
这种设计的优势是:
- 避免了油门和刹车之间的切换振荡
- 控制逻辑简单可靠
- 便于参数整定和调试
注意:在实际实现中,需要在油门和刹车之间设置一个死区,防止两者同时作用。
2.3 参数整定经验
通过大量仿真实验,我总结了以下参数整定经验:
- 先整定内环,再整定外环
- 比例系数Kp主要影响响应速度
- 积分系数Ki影响稳态误差消除能力
- 微分系数Kd影响系统阻尼
- 不同速度区间可能需要不同的参数集
在Carsim联合仿真中,我发现当车速低于10km/h时,需要适当增大积分项权重,以克服静摩擦的影响。
3. 横向MPC控制器实现
横向控制采用模型预测控制(MPC),这是因为它能够:
- 显式处理系统约束
- 考虑多步预测信息
- 处理多变量耦合问题
3.1 MPC预测模型
MPC的核心是预测模型,我采用线性化的二自由度模型:
状态方程:
code复制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)];
B = [0; Cf/m; 0; a*Cf/Iz];
其中:
- Cf, Cr是前后轮侧偏刚度
- m是车辆质量
- Iz是绕z轴的转动惯量
- a,b是前后轴到质心的距离
在实现时,我特别注意了以下几点:
- 车速vx接近零时的数值稳定性
- 模型参数的准确性
- 离散化方法的选择(我采用零阶保持)
3.2 权重矩阵设计
MPC的性能很大程度上取决于权重矩阵的选择。经过大量实验,我确定了以下权重设置:
状态权重矩阵:
code复制Q = diag([500, 0, 10, 0]);
控制权重矩阵:
code复制R = 1;
这个设置的含义是:
- 横向位置误差权重最大(500)
- 前轮转角权重适中(10)
- 其他状态不直接控制
提示:权重选择需要平衡跟踪精度和控制量变化率。过大的权重可能导致控制量剧烈变化。
3.3 约束处理
MPC的一个重要优势是能够处理约束。在我的实现中,考虑了以下约束:
- 前轮转角物理限制:±30度
- 前轮转角变化率限制:±15度/秒
- 横向加速度限制:±0.3g
这些约束在QP求解器中通过不等式约束实现:
code复制-δ_max ≤ δ ≤ δ_max
-Δδ_max ≤ δ(k)-δ(k-1) ≤ Δδ_max
4. 联合仿真与调试
Carsim和Simulink的联合仿真是整个项目中最具挑战性的部分。下面分享一些关键经验。
4.1 接口配置
正确的接口配置是联合仿真的基础。关键步骤包括:
- 在Carsim中正确设置车辆参数
- 配置Simulink S-Function接口
- 确保采样时间同步
- 验证信号单位一致性
最常见的错误是忘记在S-Function的Initialize子函数中调用VS_Init,这会导致所有状态变量为NaN。
4.2 调试技巧
有效的调试方法可以大大节省时间:
- 使用Simulink的Signal Logging功能记录关键信号
- 添加足够的Scope模块实时监控信号
- 使用MATLAB的Debug工具逐步执行S-Function
- 保存每次仿真的参数和结果,便于比较
我发现最有用的调试手段是在MPC控制器中添加状态观测器,实时监控预测状态和实际状态的差异。
4.3 性能优化
通过以下方法可以优化控制性能:
- 轨迹预处理:对参考轨迹进行平滑滤波
- 参数自适应:根据车速调整控制器参数
- 多速率控制:不同控制回路采用不同频率
- 代码优化:使用C-MEX S-Function提高执行效率
特别是在处理五次多项式轨迹时,我发现添加一个低通滤波器限制曲率变化率可以有效减少方向盘抖动。
5. 实测结果与分析
在双移线工况下,控制器的表现如下:
5.1 横向控制性能
- 最大横向误差:0.18m
- 平均横向误差:0.05m
- 方向盘转角变化平滑,无剧烈抖动
横向误差主要出现在轨迹曲率变化较大的位置,这与预测时域的选择有关。
5.2 纵向控制性能
- 速度跟踪误差:±3km/h
- 油门/刹车切换平滑,无冲击感
- 稳态速度控制精度高
纵向控制的主要误差来源是车辆动力学的非线性,特别是在低速区域。
5.3 计算负荷分析
- MPC求解时间:平均8ms
- 控制器总周期:20ms
- CPU利用率:约15%
计算负荷完全满足实时性要求,有余量处理更复杂的模型。
6. 常见问题与解决方案
在实际开发中,我遇到了许多问题,以下是典型问题及解决方法:
6.1 车辆状态发散
现象:仿真开始后车辆状态迅速发散
原因:
- Carsim接口初始化不正确
- 单位不匹配
- 采样时间设置错误
解决方案: - 确保VS_Init被正确调用
- 检查所有信号单位
- 统一采样时间设置
6.2 MPC求解失败
现象:QP求解器返回无解
原因:
- 预测模型不稳定
- 约束条件冲突
- 权重矩阵不正定
解决方案: - 检查模型线性化是否正确
- 放宽部分约束
- 调整权重矩阵
6.3 方向盘抖动
现象:方向盘频繁小幅摆动
原因:
- 轨迹曲率变化剧烈
- MPC权重设置不当
- 执行机构延迟
解决方案: - 对参考轨迹进行滤波
- 增加控制量权重
- 添加执行机构模型
7. 扩展与改进方向
虽然当前控制器表现良好,但仍有改进空间:
7.1 动态参数调整
目前的控制器参数是固定的,可以考虑:
- 根据车速调整MPC预测时域
- 根据道路曲率调整权重矩阵
- 自适应PID参数
7.2 抗干扰增强
提高对以下干扰的鲁棒性:
- 定位噪声
- 道路不平
- 侧风影响
可能的解决方案包括:
- 增加扰动观测器
- 扩展状态估计
- 强化MPC鲁棒性
7.3 执行机构建模
更精确的执行机构模型可以改善控制效果:
- 油门/刹车动力学
- 转向系统延迟
- 轮胎非线性特性
在后续工作中,我计划重点研究参数自适应和抗干扰增强这两个方向。控制系统的开发永远是一个不断迭代优化的过程,每个问题的解决都会带来新的认知和提升。
