黄泓毅1. 项目概述
作为一名在车辆控制领域摸爬滚打多年的工程师，今天想和大家分享一个非常实用的技术方案——基于模型预测控制（MPC）的车辆换道与轨迹跟踪系统。这个方案最大的特点就是采用了五次多项式来规划换道轨迹，再配合MPC控制器实现精准跟踪，最终通过Matlab和CarSim联合仿真验证效果。
在实际工程应用中，这种组合方案表现非常出色。五次多项式生成的S型换道轨迹平滑舒适，MPC控制器能够有效应对各种扰动和不确定性，而CarSim提供的车辆动力学模型则确保了仿真的真实性。下面我就从原理到实践，详细拆解这个方案的关键技术点。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 五次多项式轨迹规划
2.1 为什么选择五次多项式
在车辆换道场景中，轨迹规划需要满足以下几个基本要求：
- 起点和终点的位置必须精确匹配
- 速度要连续平滑过渡
- 加速度不能有突变，否则会影响乘坐舒适性
- 加加速度（jerk）也应尽量平滑
五次多项式恰好能满足这些要求，因为它有6个自由度，可以同时满足位置、速度和加速度在起终点的边界条件。具体来说，五次多项式的一般形式为：
y(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵
其中y(t)表示横向位移，t表示时间，a₀到a₅是待定系数。
2.2 具体实现方法
在实际编程实现时，我们需要建立一个线性方程组来求解这些系数。以横向位移为例，假设：
- 初始时刻t₀=0，横向位移y(t₀)=0，速度y'(t₀)=0，加速度y''(t₀)=0
- 终止时刻t_f=5s，横向位移y(t_f)=3m，速度y'(t_f)=0，加速度y''(t_f)=0
对应的矩阵方程为：
[A]·[coeff] = [b]
其中：
code复制A = [1 t₀ t₀² t₀³ t₀⁴ t₀⁵
0 1 2t₀ 3t₀² 4t₀³ 5t₀⁴
0 0 2 6t₀ 12t₀² 20t₀³
1 t_f t_f² t_f³ t_f⁴ t_f⁵
0 1 2t_f 3t_f² 4t_f³ 5t_f⁴
0 0 2 6t_f 12t_f² 20t_f³]
b = [y(t₀); y'(t₀); y''(t₀); y(t_f); y'(t_f); y''(t_f)]
在Matlab中，我们可以用简单的矩阵运算来求解这个方程组：
matlab复制t0 = 0; tf = 5; % 换道时间5秒
A = [1 t0 t0^2 t0^3 t0^4 t0^5;
0 1 2*t0 3*t0^2 4*t0^3 5*t0^4;
0 0 2 6*t0 12*t0^2 20*t0^3;
1 tf tf^2 tf^3 tf^4 tf^5;
0 1 2*tf 3*tf^2 4*tf^3 5*tf^4;
0 0 2 6*tf 12*tf^2 20*tf^3];
b = [0; 0; 0; 3; 0; 0]; % 边界条件
coeff = A\b; % 解线性方程组
2.3 轨迹参数化处理
在实际应用中，我们还需要考虑纵向速度的变化。这时候可以采用s-L坐标系（弧长-横向位移坐标系）来处理。具体步骤是：
- 将全局坐标系下的轨迹转换为以路径为参考的s-L坐标系
- 根据车辆当前速度动态调整轨迹的时间参数
- 确保轨迹在s方向的速度与实际车速匹配
这种方法能够很好地适应车速变化的情况，保证轨迹跟踪的稳定性。
3. MPC控制器设计
3.1 车辆动力学模型
MPC控制器的核心是基于模型进行预测。这里我们采用经典的二自由度单车模型作为预测模型：
状态量：x = [y, ψ, v_y, r]ᵀ
- y：横向位移
- ψ：横摆角
- v_y：横向速度
- r：横摆角速度
控制量：u = δ_f（前轮转角）
状态空间方程：
ẋ = A·x + B·u
其中：
code复制A = [0 v_x 1 0
0 0 0 1
0 0 -(C_αf+C_αr)/(m·v_x) (l_r·C_αr-l_f·C_αf)/(m·v_x)-v_x
0 0 (l_r·C_αr-l_f·C_αf)/(I_z·v_x) -(l_f²·C_αf+l_r²·C_αr)/(I_z·v_x)]
B = [0
0
C_αf/m
l_f·C_αf/I_z]
在离散化处理时，我们采用前向欧拉方法：
x_{k+1} = A_k·x_k + B_k·u_k
其中：
A_k = I + A·Δt
B_k = B·Δt
3.2 优化问题构建
MPC的核心是求解一个滚动时域优化问题。在每个控制周期，我们求解如下优化问题：
min J = ∑(x_k - x_ref)ᵀQ(x_k - x_ref) + ∑Δu_kᵀRΔu_k
s.t. x_{k+1} = A_k·x_k + B_k·u_k
u_min ≤ u_k ≤ u_max
Δu_min ≤ Δu_k ≤ Δu_max
在Matlab中，我们可以使用quadprog函数来求解这个二次规划问题：
matlab复制% 构建H矩阵和f向量
H = blkdiag(kron(eye(N),Q), kron(eye(Nc),R));
f = [-repmat(Q*x_ref,N,1); zeros(Nc*nu,1)];
% 构建约束条件
A_cons = [Aeq; Aineq];
b_cons = [beq; bineq];
% 求解优化问题
[delta_opt, ~, exitflag] = quadprog(H, f, A_cons, b_cons, [], [], lb, ub);
3.3 参数调试经验
在实际调试过程中，有几个关键参数需要特别注意：
-
预测时域N：一般选择10-20步，对应1-2秒的时间范围。太短会导致控制短视，太长会增加计算负担。
-
权重矩阵Q和R：
- Q矩阵中横向位移误差的权重通常设为1
- 横摆角误差权重设为0.1
- 控制量变化率权重R通常设为0.01-0.1
-
约束条件：
- 前轮转角限制：一般±30度
- 转向速率限制：通常不超过500度/秒
调试时建议先固定Q矩阵，调整R矩阵观察系统响应。如果出现震荡，适当增大R的值；如果响应太慢，则减小R的值。
4. CarSim与Matlab联合仿真
4.1 接口配置
CarSim和Matlab的联合仿真主要通过Simulink实现。关键接口包括：
CarSim输出：
- 车速vx
- 横向位置误差ey
- 横摆角ψ
- 横摆角速度r
Matlab输入：
- 前轮转角指令δ_f
在Simulink中，建议使用S-Function来实现CarSim接口，这样可以减少通信延迟。配置时需要注意：
- 确保CarSim的采样时间与Simulink保持一致
- 检查单位系统（CarSim默认使用英制单位）
- 验证信号方向（特别是转向角度的正负号）
4.2 常见问题排查
在联合仿真中经常遇到以下问题：
-
单位不一致：CarSim默认使用mph、ft等单位，需要在Dataset中转换为国际单位制。
-
信号方向错误：特别是转向角度，正负号定义可能与Matlab模型不一致，导致车辆反向行驶。
-
通信延迟：如果使用TCP/IP通信，可能会有延迟，建议使用共享内存或S-Function。
-
初始化问题：仿真开始时车辆状态可能与控制器期望的不一致，导致初始时刻的大偏差。
4.3 调试技巧
- 先单独测试CarSim模型，确保基础车辆动力学正确
- 在Matlab中先用简单的PID控制器测试接口
- 逐步增加MPC的复杂度，从简单场景开始
- 使用Scope实时监控关键信号，如横向误差、转向角等
- 保存每次仿真的参数和结果，方便对比分析
5. 实际应用中的注意事项
5.1 非线性特性处理
在实际车辆中，轮胎力与滑移角的关系是非线性的。当侧向加速度较大时，线性轮胎模型不再适用。这时候可以考虑：
- 使用分段线性化方法
- 采用更复杂的轮胎模型，如魔术公式
- 在线估计轮胎特性参数
5.2 路面条件变化
不同路面附着系数会显著影响车辆动力学特性。在实际应用中需要考虑：
- 在线估计路面附着系数
- 根据附着系数调整控制参数
- 增加鲁棒性设计，如H∞控制
5.3 传感器噪声处理
实际传感器信号都带有噪声，需要在控制算法中加入滤波处理：
- 使用卡尔曼滤波器估计车辆状态
- 对关键信号进行低通滤波
- 设置合理的信号有效性检查
6. 性能优化与扩展
6.1 计算效率优化
MPC的在线计算量较大，可以考虑以下优化方法：
- 使用显式MPC，提前计算好控制律
- 采用更高效的QP求解器
- 减少预测时域长度
- 使用简化车辆模型
6.2 功能扩展
基于这个基础框架，还可以实现更多高级功能：
- 障碍物避让：在优化问题中加入避障约束
- 多车协同：考虑周围车辆的预测轨迹
- 自适应巡航：集成纵向控制
- 极限工况控制：考虑轮胎力饱和情况
7. 个人实践经验分享
在实际项目中应用这套方案时，我总结了以下几点经验：
- 轨迹规划阶段要特别注意舒适性指标，如jerk限制
- MPC的权重参数需要根据实际车辆特性仔细调试
- 联合仿真时要反复验证接口信号的正确性
- 实时性是个挑战，需要平衡控制性能和计算复杂度
- 实际车辆测试前，一定要做充分的仿真验证
一个特别有用的调试技巧是：先固定纵向速度，只调试横向控制；等横向控制调好了，再加入速度变化。这样可以简化调试过程。
另外，建议建立一个完整的测试用例库，包括各种典型场景：直线行驶、换道、弯道等。每次算法修改后都跑一遍测试用例，确保不会引入回归问题。