1. 车辆换道轨迹规划与控制概述
在智能驾驶和高级辅助驾驶系统(ADAS)开发中,车辆换道轨迹规划与跟踪控制是核心技术难点之一。我最近在做一个基于Simulink和Carsim联合仿真的项目,采用五次多项式进行换道轨迹规划,配合模型预测控制(MPC)实现轨迹跟踪,效果相当不错。这套方案最大的优势在于既能保证轨迹平滑性,又能处理各种约束条件,非常适合实际工程应用。
五次多项式轨迹规划之所以被广泛采用,是因为它能完美满足换道过程中的六个边界条件约束:起始点和终点的横向位移、速度和加速度。这确保了换道动作的平顺性,避免了急打方向盘带来的不适感。而MPC控制器的加入,则让系统具备了处理动态环境和模型不确定性的能力。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 五次多项式换道轨迹规划
2.1 边界条件与系数求解
五次多项式的一般形式为:
y(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀
我们需要确定的六个系数,可以通过以下边界条件来求解:
- 初始时刻(t=0)的横向位移y(0)=0
- 终止时刻(t=T)的横向位移y(T)=d(车道宽度)
- 初始时刻(t=0)的横向速度y'(0)=0
- 终止时刻(t=T)的横向速度y'(T)=0
- 初始时刻(t=0)的横向加速度y''(0)=0
- 终止时刻(t=T)的横向加速度y''(T)=0
在实际编程实现时,我建议采用符号运算来避免数值计算的不稳定性。下面是我在项目中使用的MATLAB代码示例:
matlab复制syms t T d a5 a4 a3 a2 a1 a0
y = a5*t^5 + a4*t^4 + a3*t^3 + a2*t^2 + a1*t + a0;
ydot = diff(y,t);
yddot = diff(ydot,t);
eq1 = subs(y,t,0) == 0;
eq2 = subs(y,t,T) == d;
eq3 = subs(ydot,t,0) == 0;
eq4 = subs(ydot,t,T) == 0;
eq5 = subs(yddot,t,0) == 0;
eq6 = subs(yddot,t,T) == 0;
sol = solve([eq1,eq2,eq3,eq4,eq5,eq6],[a0,a1,a2,a3,a4,a5]);
2.2 侧向加速度约束处理
在实际应用中,我们必须考虑车辆动力学限制,特别是最大侧向加速度约束。这个约束可以转化为轨迹曲率的上限:
matlab复制max_ay = 2.5; % 最大侧向加速度[m/s^2]
vx = 20/3.6; % 纵向速度[m/s]
max_curvature = max_ay / vx^2; % 最大允许曲率[1/m]
为了确保规划出的轨迹满足曲率约束,我们需要在轨迹规划阶段进行检查。如果发现曲率超过限制,可以通过以下方式调整:
- 延长换道时间T
- 降低换道横向位移d(部分换道)
- 采用更高阶多项式(如七次多项式)增加设计自由度
重要提示:在实际项目中,我发现当换道时间设置过短(如小于3秒)时,五次多项式可能会产生曲率突变。这种情况下,MPC控制器会产生剧烈的方向盘动作,影响乘坐舒适性。解决方案是加入jerk(加速度变化率)约束或改用七次多项式。
3. MPC轨迹跟踪控制器设计
3.1 车辆动力学模型
MPC控制器的性能很大程度上取决于预测模型的准确性。我采用简化的自行车模型作为预测模型,其状态空间方程为:
matlab复制function [A,B] = updateModel(vx,dt)
% 车辆参数(来自实车辨识)
Cf = 80000; Cr = 120000; % 前后轮侧偏刚度[N/rad]
m = 1500; % 质量[kg]
Iz = 2500; % 绕Z轴转动惯量[kg·m²]
lf = 1.2; % 前轴到质心距离[m]
lr = 1.6; % 后轴到质心距离[m]
% 连续时间状态矩阵
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_cont = [0 1 0 0;
0 a11 0 a12;
0 0 0 1;
0 a21 0 a22];
B_cont = [0; Cf/m; 0; Cf*lf/Iz];
% 离散化
sysc = ss(A_cont,B_cont,eye(4),[]);
sysd = c2d(sysc, dt);
A = sysd.A;
B = sysd.B;
end
3.2 MPC问题构建
MPC的核心是在每个控制周期求解一个优化问题。我采用二次规划(QP)的形式:
matlab复制% 预测时域
Np = 20;
% 权重矩阵
Q = diag([100, 10000, 100, 10000]); % 状态权重
R = 0.1; % 控制输入权重
% 构建QP矩阵
[H,f,Aeq,beq,lb,ub] = buildQP(A,B,Q,R,Np,x_ref,u_ref);
% 求解QP
options = optimoptions('quadprog','Display','off');
u_opt = quadprog(H,f,[],[],Aeq,beq,lb,ub,[],options);
调参经验:MPC的权重矩阵设置很有讲究。我发现速度误差权重应该是位移误差权重的平方级。例如当位移误差权重设为100时,速度误差权重设为10000左右能获得更好的动态响应。
4. Simulink与Carsim联合仿真
4.1 模型集成要点
在Simulink中集成Carsim模型时,有几个关键点需要注意:
-
采样时间同步:必须确保Carsim模型的采样时间与MPC的预测步长一致,否则会出现难以调试的跟踪误差。我通常设置为0.05秒(20Hz)。
-
接口设计:Simulink与Carsim之间的信号接口要仔细设计。主要包含:
- 输入:方向盘转角、油门/刹车指令
- 输出:车辆位置、速度、姿态角等状态
-
初始化顺序:必须先初始化Carsim模型,再运行Simulink模型,否则会导致参数传递失败。
4.2 仿真结果分析
通过大量仿真测试,我总结了以下经验:
-
在车速20m/s(约72km/h)时,横向跟踪误差可以控制在0.1米以内,满足高速公路换道要求。
-
前轮转角应限制在±30度以内,超过这个范围会导致轮胎进入非线性区,影响控制精度。
-
当换道时间过短(如小于3秒)时,五次多项式轨迹会出现曲率突变,此时MPC控制器的性能会显著下降。
5. 常见问题与解决方案
5.1 数值不稳定问题
问题现象:在求解五次多项式系数时,矩阵求逆出现数值不稳定。
解决方案:
- 使用符号运算代替数值计算
- 采用条件数更小的参数化方法
- 增加正则化项
5.2 MPC求解失败问题
问题现象:QP求解器返回无解或求解时间过长。
解决方案:
- 检查预测模型是否合理
- 放宽部分约束条件
- 减小预测时域Np
- 使用更高效的QP求解器(如OSQP)
5.3 联合仿真不同步问题
问题现象:Simulink与Carsim仿真结果不一致。
解决方案:
- 检查采样时间设置
- 验证信号单位是否一致
- 确保初始化顺序正确
- 检查仿真步长是否匹配
6. 实际项目中的调优技巧
经过多个项目的实践,我总结出以下调优技巧:
-
轨迹平滑性优化:在五次多项式基础上加入jerk约束,可以进一步改善乘坐舒适性。具体做法是在目标函数中加入jerk的惩罚项。
-
MPC权重调整:不要使用固定的权重矩阵。我开发了一套根据车速自适应调整权重的算法:
matlab复制function [Q,R] = adaptiveWeights(vx) % 基础权重 Q_base = diag([100, 10000, 100, 10000]); R_base = 0.1; % 速度相关调整 vx_scale = vx / 20; % 归一化到20m/s Q = Q_base * (1 + 0.5*vx_scale); R = R_base / (1 + 0.3*vx_scale); end -
实时性能优化:对于嵌入式部署,我采用以下方法提升MPC的实时性:
- 使用显式MPC(离线计算控制律)
- 采用热启动策略(复用上一周期的解)
- 减少预测时域(通常10-20步足够)
-
异常处理机制:在实际应用中,必须设计完善的异常处理机制:
- 轨迹规划失败时的备用策略
- MPC求解失败时的降级控制
- 传感器失效检测与处理
这套五次多项式换道轨迹规划+MPC跟踪控制的方案,经过我在多个项目中的验证,证明其可靠性和实用性都很出色。特别是在高速公路场景下,能够实现平滑、安全的自动换道操作。对于想深入研究的同行,我建议可以从以下几个方面进一步优化:
- 考虑道路曲率的影响
- 加入交通参与者预测
- 开发更高效的实时求解算法
- 研究数据驱动的模型参数辨识方法
