1. 项目概述
在自动驾驶和高级驾驶辅助系统(ADAS)开发中,车辆换道轨迹规划与跟踪控制是核心算法模块。这个Simulink模型实现了从轨迹规划到跟踪控制的完整闭环,采用五次多项式进行换道轨迹规划,配合模型预测控制(MPC)实现高精度轨迹跟踪。我在实际车辆控制系统的开发中发现,这种组合方案在保证舒适性的同时,能够满足不同驾驶风格的需求。
模型基于Matlab R2019a和Carsim2019.1平台开发,主要解决三个关键问题:
- 如何生成符合车辆动力学约束的平滑换道轨迹
- 如何实现复杂工况下的高精度轨迹跟踪
- 如何协调规划层与控制层的参数匹配
2. 五次多项式轨迹规划原理
2.1 边界条件设定
五次多项式轨迹规划的核心在于六个边界条件的确定。对于典型的换道场景,我们需要保证:
- 起始点(t=0):横向位移y=0,速度ẏ=0,加速度ÿ=0
- 终点(t=T):横向位移y=d(车道宽度),速度ẏ=0,加速度ÿ=0
这六个边界条件可以唯一确定一个五次多项式:
y(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀
在实际工程中,我发现边界条件的设置会显著影响乘坐舒适性。比如将终点加速度设为非零值,可以模拟更激进的驾驶风格。
2.2 系数矩阵求解
构建系数矩阵时,数值稳定性是需要特别注意的问题。原始代码中的矩阵求逆方法虽然直接,但当换道时间T较小时可能导致数值问题。我推荐采用符号运算预先推导解析解:
matlab复制syms t T d
y = a5*t^5 + a4*t^4 + a3*t^3 + a2*t^2 + a1*t + a0;
eqns = [
subs(y,t,0) == 0, % y(0)=0
subs(y,t,T) == d, % y(T)=d
subs(diff(y,t),t,0) == 0, % ẏ(0)=0
subs(diff(y,t),t,T) == 0, % ẏ(T)=0
subs(diff(y,t,2),t,0) == 0, % ÿ(0)=0
subs(diff(y,t,2),t,T) == 0 % ÿ(T)=0
];
coeff_sol = solve(eqns,[a0 a1 a2 a3 a4 a5]);
这种方法的优点是避免了数值计算中的条件数问题,特别适合嵌入式系统的代码生成。
2.3 侧向加速度约束处理
侧向加速度约束是保证轨迹可行的关键。根据车辆动力学,最大侧向加速度通常限制在2.5-3 m/s²之间。我们需要将其转化为轨迹曲率约束:
matlab复制vx = 20; % 纵向速度(m/s)
max_ay = 2.5; % 最大侧向加速度(m/s²)
max_curvature = max_ay / vx^2; % 最大允许曲率(1/m)
% 检查轨迹曲率是否满足约束
t_samples = linspace(0,T,100);
ypp = 20*a5*t_samples.^3 + 12*a4*t_samples.^2 + 6*a3*t_samples + 2*a2;
curvature = abs(ypp) ./ (1 + (5*a5*t_samples.^4 + 4*a4*t_samples.^3 + 3*a3*t_samples.^2 + 2*a2*t_samples + a1).^2).^(3/2);
if any(curvature > max_curvature)
error('轨迹曲率超出限制,请调整换道时间或距离');
end
在实际项目中,我通常会加入10%的安全裕度,以应对传感器噪声和控制误差。
3. MPC控制器设计与实现
3.1 车辆动力学模型
MPC控制器的性能很大程度上取决于预测模型的准确性。这里采用经典的自行车模型,考虑了横摆运动和侧向运动的耦合:
matlab复制function [A,B,C,D] = vehicle_model(vx, dt)
% 车辆参数(示例值,需根据实车调整)
m = 1500; % 质量(kg)
Iz = 2500; % 绕Z轴转动惯量(kg·m²)
lf = 1.2; % 前轴到质心距离(m)
lr = 1.6; % 后轴到质心距离(m)
Cf = 80000; % 前轮侧偏刚度(N/rad)
Cr = 120000; % 后轮侧偏刚度(N/rad)
% 连续状态空间模型
A11 = -(Cf+Cr)/(m*vx);
A12 = -vx - (Cf*lf-Cr*lr)/(m*vx);
A21 = -(Cf*lf-Cr*lr)/(Iz*vx);
A22 = -(Cf*lf^2+Cr*lr^2)/(Iz*vx);
A = [0 1 0 0;
0 A11 0 A12;
0 0 0 1;
0 A21 0 A22];
B = [0; Cf/m; 0; Cf*lf/Iz];
C = eye(4);
D = zeros(4,1);
% 离散化
sys = ss(A,B,C,D);
sysd = c2d(sys, dt);
A = sysd.A;
B = sysd.B;
C = sysd.C;
D = sysd.D;
end
注意:车辆参数对模型准确性影响很大,建议通过实车试验进行参数辨识。我在项目中发现,负载变化会导致参数显著改变,因此开发了在线参数估计模块。
3.2 QP问题构建
MPC的核心是将控制问题转化为二次规划(QP)问题。我们需要构建代价函数和约束条件:
matlab复制% 预测时域参数
Np = 20; % 预测步长
Nc = 5; % 控制步长
% 权重矩阵
Q = diag([100, 10000, 50, 5000]); % 状态权重
R = 0.1; % 控制输入权重
% 构建增广矩阵
[Phi, Gamma] = build_prediction_matrices(A, B, Np, Nc);
Q_bar = kron(eye(Np), Q);
R_bar = kron(eye(Nc), R);
% 构建QP问题的H和f矩阵
H = Gamma'*Q_bar*Gamma + R_bar;
f = Gamma'*Q_bar*Phi;
在实际应用中,我发现预测时域和控制时域的选择很有讲究:
- 预测时域Np:通常覆盖3-5秒,对应驾驶员的前视距离
- 控制时域Nc:一般取Np的1/4到1/3,太大会增加计算负担
3.3 约束处理
车辆控制必须考虑物理限制,主要包括:
- 前轮转角限制:通常±30度
- 转角速率限制:取决于转向系统,一般±30度/秒
- 侧向加速度限制:保证舒适性和安全性
matlab复制% 输入约束
delta_max = deg2rad(30); % 最大转向角
delta_rate_max = deg2rad(30); % 最大转向速率
% 构建不等式约束矩阵
Aineq = [
eye(Nc);
-eye(Nc);
tril(ones(Nc));
-tril(ones(Nc))
];
bineq = [
delta_max * ones(Nc,1);
delta_max * ones(Nc,1);
delta_rate_max * dt * ones(Nc,1);
delta_rate_max * dt * ones(Nc,1)
];
4. Simulink模型实现技巧
4.1 模型架构设计
完整的Simulink模型包含三个主要部分:
- 轨迹规划模块:生成五次多项式参考轨迹
- MPC控制器:计算最优控制输入
- 车辆模型:Carsim或Simulink Vehicle Model
关键点:所有模块的采样时间必须一致,否则会导致"玄学"级别的跟踪误差。我建议使用0.01-0.05秒的固定步长。
4.2 代码生成优化
为提高实时性,可以将MPC控制器部署为C代码:
- 使用Embedded Coder生成代码
- 将QP求解替换为更高效的库,如qpOASES
- 启用单精度浮点运算以节省计算资源
matlab复制% 配置代码生成选项
cfg = coder.config('lib');
cfg.TargetLang = 'C';
cfg.GenerateReport = true;
cfg.HardwareImplementation.ProdHWDeviceType = 'Intel->x86-64 (Windows64)';
% 定义输入参数
vx = coder.typeof(0);
dt = coder.typeof(0);
x0 = coder.typeof(zeros(4,1));
ref = coder.typeof(zeros(Np*4,1));
% 生成代码
codegen('mpc_controller','-config','cfg','-args',{vx,dt,x0,ref});
4.3 调试与验证
在模型调试阶段,我推荐以下步骤:
- 先验证开环响应:检查规划轨迹是否合理
- 测试闭环响应:逐步增加MPC的预测时域
- 参数敏感性分析:改变质量、速度等参数观察系统鲁棒性
一个实用的调试技巧是在Simulink中添加信号记录模块,保存关键变量如:
- 实际轨迹与参考轨迹的偏差
- 前轮转角指令
- 计算用时
5. 常见问题与解决方案
5.1 轨迹曲率突变
现象:换道时间设置过短时,五次多项式可能出现曲率突变,导致MPC控制量剧烈变化。
解决方案:
- 增加jerk约束,限制加加速度
- 改用七次多项式,增加更多边界条件
- 对规划轨迹进行低通滤波
matlab复制% 七次多项式示例
A = [
0,0,0,0,0,0,0,1;
T^7,T^6,T^5,T^4,T^3,T^2,T,1;
0,0,0,0,0,0,1,0;
7*T^6,6*T^5,5*T^4,4*T^3,3*T^2,2*T,1,0;
0,0,0,0,0,2,0,0;
42*T^5,30*T^4,20*T^3,12*T^2,6*T,2,0,0;
0,0,0,0,6,0,0,0;
210*T^4,120*T^3,60*T^2,24*T,6,0,0,0
];
b = [0;d;0;0;0;0;0;0];
5.2 稳态误差问题
现象:在长直道跟踪时出现持续偏移。
原因分析:自行车模型未考虑轮胎非线性特性。
改进方案:
- 在MPC中增加积分环节
- 使用自适应轮胎模型
- 加入前馈补偿项
5.3 实时性不足
现象:单步计算时间超过采样周期。
优化方法:
- 减少预测时域Np
- 使用热启动技术,复用上一周期的解
- 采用显式MPC,预先计算控制律
6. 参数调优经验
经过多个项目的积累,我总结出以下调参经验:
-
权重矩阵设置:
- 位移误差权重:100-500
- 速度误差权重:位移权重的100倍左右
- 控制输入权重:0.1-1
-
换道时间选择:
- 舒适型:3-5秒
- 运动型:2-3秒
- 必须满足侧向加速度约束
-
采样时间选择:
- 仿真:0.01-0.05秒
- 实车:0.02-0.1秒(取决于ECU性能)
-
预测时域调整:
- 低速(30km/h):3-4秒
- 中速(60km/h):2-3秒
- 高速(100km/h+):1.5-2.5秒
在实际项目中,我发现MPC对权重比绝对值更敏感。一个实用的方法是保持位移误差权重不变,调整其他权重与它的比值。