智能驾驶换道轨迹跟踪：MPC与五次多项式实践

1. 项目背景与核心挑战

在智能驾驶领域，车辆换道轨迹跟踪一直是关键控制难题。传统PID控制方法在动态环境下的适应性不足，而模型预测控制（MPC）凭借其滚动优化和反馈校正的特性，成为解决这一问题的理想选择。这个项目通过五次多项式设计换道轨迹，并采用Matlab与Carsim联合仿真验证控制效果。

实际工程中发现，五次多项式轨迹在保证曲率连续的同时，能有效平衡舒适性和换道效率，这是选择它的核心原因。

2. 五次多项式换道轨迹设计

2.1 数学建模原理

五次多项式轨迹方程一般表示为：

matlab复制y(x) = a0 + a1*x + a2*x^2 + a3*x^3 + a4*x^4 + a5*x^5

其中需要满足6个边界条件：

• 起始点位置、速度、加速度
• 终点位置、速度、加速度

通过解这组方程可以得到各系数值。在Matlab中实现时，我习惯使用polyfit函数配合边界条件约束来求解。

2.2 曲率连续性验证

五次多项式的优势在于其三阶导数连续，这意味着：

1. 轨迹曲率变化平滑（直接影响乘坐舒适性）
2. 方向盘转角变化连续（避免突然转向）
3. 横向加速度过渡自然

实测对比发现，三次多项式会导致加速度突变，而七次多项式虽然更平滑但计算量显著增加。

3. MPC控制器设计与实现

3.1 车辆动力学模型搭建

采用经典的二自由度自行车模型：

matlab复制% 状态方程示例
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^2*Cf+b^2*Cr)/(Iz*Vx)];
B = [0; Cf/m; 0; a*Cf/Iz];

其中关键参数包括：

• 前/后轮侧偏刚度(Cf, Cr)
• 质心到前后轴距离(a, b)
• 车辆质量(m)和转动惯量(Iz)

3.2 预测时域与代价函数

经过多次调试，发现以下参数组合效果最佳：

• 预测时域：3s
• 控制时域：0.5s
• 采样时间：0.1s

代价函数采用加权形式：

code复制J = Σ(α·跟踪误差 + β·控