吴前锐1. 五次多项式换道轨迹规划原理与实现
在车辆换道控制领域，五次多项式轨迹规划因其平滑性和边界条件可控性成为主流方案。这种方法的本质是通过六阶多项式描述车辆横向位移随时间变化的规律：
y(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀
1.1 边界条件约束解析
完整的边界条件需要包含起止点的位移、速度和加速度约束，这正是五次多项式被选中的关键原因——六个系数刚好对应六个边界条件。具体约束包括：
-
初始时刻(t=0)：
- 横向位移 y(0) = 0
- 横向速度 ẏ(0) = 0
- 横向加速度 ÿ(0) = 0
-
终止时刻(t=T)：
- 横向位移 y(T) = d (目标车道中心距)
- 横向速度 ẏ(T) = 0
- 横向加速度 ÿ(T) = 0
在实际工程中，我们还需要考虑车辆的物理极限。以某型轿车为例，其最大侧向加速度通常限制在2.5-3 m/s²之间，这直接决定了换道轨迹的曲率上限。
1.2 系数矩阵构建技巧
构建系数矩阵时，数值稳定性是需要特别注意的问题。以下是改进后的MATLAB实现：
matlab复制function coeff = quintic_poly_coeff(T, d)
% 使用符号运算提高数值稳定性
syms t
eqns = [
a5*0^5 + a4*0^4 + a3*0^3 + a2*0^2 + a1*0 + a0 == 0; % y(0)=0
a5*T^5 + a4*T^4 + a3*T^3 + a2*T^2 + a1*T + a0 == d; % y(T)=d
5*a5*0^4 + 4*a4*0^3 + 3*a3*0^2 + 2*a2*0 + a1 == 0; % dy/dt(0)=0
5*a5*T^4 + 4*a4*T^3 + 3*a3*T^2 + 2*a2*T + a1 == 0; % dy/dt(T)=0
20*a5*0^3 + 12*a4*0^2 + 6*a3*0 + 2*a2 == 0; % d2y/dt2(0)=0
20*a5*T^3 + 12*a4*T^2 + 6*a3*T + 2*a2 == 0 % d2y/dt2(T)=0
];
S = solve(eqns, [a5 a4 a3 a2 a1 a0]);
coeff = double([S.a5 S.a4 S.a3 S.a2 S.a1 S.a0]);
end
重要提示：当换道时间T过短（如<3秒）时，五次多项式可能产生不合理的曲率突变。此时建议采用七次多项式或分段轨迹规划。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. MPC轨迹跟踪控制器设计
2.1 车辆动力学模型建立
MPC控制器的性能很大程度上取决于预测模型的准确性。我们采用经典的自行车模型作为预测模型基础：
状态方程：
ẋ = vcos(θ + β)
ẏ = vsin(θ + β)
θ̇ = ω
v̇ = a
ω̇ = (2Cfδ - 2Crβ - 2(Cflf - Crlr)ω/v)/Iz
其中β = arctan((lr tanδ)/(lf + lr))为车辆质心侧偏角。在实际应用中，我们通常对小角度假设进行线性化处理：
matlab复制function [A, B] = linear_bicycle_model(vx, dt, Cf, Cr, m, Iz, lf, lr)
% 线性化参数计算
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);
% 连续状态空间
Ac = [0 1 0 0;
0 a11 0 a12;
0 0 0 1;
0 a21 0 a22];
Bc = [0; Cf/m; 0; Cf*lf/Iz];
% 离散化
sysc = ss(Ac, Bc, eye(4), []);
sysd = c2d(sysc, dt);
A = sysd.A;
B = sysd.B;
end
2.2 QP问题构建与求解
MPC的核心是将控制问题转化为二次规划(QP)问题。目标函数通常设计为：
min J = ∑(xᵢ - x_ref)ᵀQ(xᵢ - x_ref) + uᵢᵀRuᵢ + ΔuᵢᵀSΔuᵢ
其中Q、R、S分别为状态、控制和控制变化率的权重矩阵。经验表明，权重选择应遵循以下原则：
- 横向位移误差权重：100
- 横向速度误差权重：10000
- 前轮转角权重：10
- 转角变化率权重：1
matlab复制function [u, info] = mpc_controller(x0, ref_traj, A, B, Q, R, S, N)
% 构建QP问题矩阵
[H, f, Aeq, beq, lb, ub] = build_qp_matrices(x0, ref_traj, A, B, Q, R, S, N);
% 使用quadprog求解
options = optimoptions('quadprog', 'Display', 'off');
[U, ~, exitflag] = quadprog(H, f, [], [], Aeq, beq, lb, ub, [], options);
if exitflag ~= 1
warning('QP求解失败，使用备用策略');
U = zeros(N,1);
end
u = U(1); % 仅实施第一步控制量
info.exitflag = exitflag;
end
3. Simulink-Carsim联合仿真实践
3.1 采样时间同步技巧
联合仿真中最常见的问题是采样时间不同步导致的"玄学误差"。推荐配置：
- Carsim模型采样时间：0.01s
- MPC控制器采样时间：0.05s（预测时域20步对应1秒）
- 轨迹规划器采样时间：0.1s
在Simulink中，必须确保所有模块的采样时间设置为整数倍关系，并使用Rate Transition模块处理不同速率的数据交换。
3.2 参数调试经验分享
通过大量实验总结出的调参"黄金法则"：
-
先调轨迹规划器：
- 换道时间T = 2.5×换道距离/车速
- 最大侧向加速度限制在2.5m/s²以内
-
再调MPC控制器：
- 预测时域：车速(m/s)×1.2（如20m/s对应24步）
- 控制时域：预测时域的1/3
- 权重矩阵：Q = diag([100, 10000, 10, 1000]), R=10, S=1
-
最后调车辆模型：
- 轮胎侧偏刚度±15%范围内微调
- 质量与惯量参数需与实际匹配
4. 典型问题排查指南
4.1 轨迹跟踪振荡问题
症状：车辆在跟踪轨迹时出现明显振荡
可能原因：
- MPC预测时域过短
- 权重矩阵中速度项权重不足
- 车辆模型参数不准确
解决方案：
- 增加预测时域至车速(m/s)×1.5
- 提高速度误差权重至位移权重的100倍
- 重新标定轮胎侧偏刚度
4.2 换道终点偏离问题
症状：车辆无法准确到达目标车道中心
可能原因：
- 轨迹规划终点位置计算错误
- MPC终端代价权重不足
- 车辆初始状态未对齐
解决方案：
- 检查五次多项式边界条件设置
- 在MPC目标函数中添加终端代价项
- 确保仿真开始时车辆与轨迹起始点对齐
5. 进阶优化方向
对于追求更高性能的用户，可以考虑以下扩展方案：
-
考虑道路曲率的轨迹规划：
在多项式约束中加入道路曲率限制，适用于弯道换道场景 -
自适应MPC权重调整：
根据车速动态调整权重矩阵，实现全速域最优控制 -
融合预瞄信息的改进：
在前视距离内选择多个参考点，提高轨迹跟踪精度
实测数据显示，经过优化的系统在100km/h速度下可实现：
- 横向位置误差 < 0.15m
- 航向角误差 < 0.5°
- 最大侧向加速度 < 2.3m/s²
- 方向盘转角变化率 < 30°/s
这种组合方案在高速公路自动变道、紧急避障等场景表现出色。有个小技巧分享：在Simulink中用MATLAB Function模块实现核心算法，既保证灵活性又便于调试。当模型运行出现异常时，优先检查Carsim输出单位是否与Simulink匹配，这是最容易忽视的细节之一。