五次多项式与MPC在车辆换道轨迹规划中的应用

1. 项目概述

最近在车辆控制领域，轨迹规划与跟踪一直是个热门话题。我花了三个月时间研究五次多项式换道轨迹规划与MPC跟踪控制的组合方案，最终在Simulink+Carsim联合仿真平台上实现了稳定可靠的换道控制。这个方案最大的特点是兼顾了轨迹的平滑性和控制的实时性，特别适合高速公路场景下的自动换道应用。

2. 五次多项式轨迹规划原理

2.1 边界条件设定

五次多项式之所以适合用于换道轨迹规划，是因为它能满足六个边界条件。在换道场景中，我们需要确保：

1. 起始点横向位移为0（当前车道中心线）
2. 终点横向位移为车道宽度（通常3-3.5米）
3. 起始点和终点的横向速度为0
4. 起始点和终点的横向加速度为0

数学表达式为：
y(t) = a0 + a1t + a2t² + a3t³ + a4t⁴ + a5t⁵

2.2 系数矩阵构建

构建系数矩阵时有个关键技巧：使用符号运算避免数值不稳定。我在实际项目中发现，当换道时间T设置较大时（如超过10秒），直接数值求解可能导致病态矩阵问题。解决方法是用Matlab的符号工具箱预先推导解析解：

matlab复制syms t T y_end
A = [0,0,0,0,0,1;
     T^5,T^4,T^3,T^2,T,1;
     0,0,0,0,1,0;
     5*T^4,4*T^3,3*T^2,2*T,1,0;
     0,0,0,2,0,0;
     20*T^3,12*T^2,6*T,2,0,0];
b = [0;y_end;0;0;0;0];
coeff = simplify(A\b);

这样得到的解析解可以直接用于不同换道场景，计算效率更高。

3. 侧向加速度约束处理

3.1 物理限制转化

实际车辆在换道过程中会受到物理限制，最大侧向加速度通常不超过3m/s²（舒适性考虑）。我们需要将这个限制转化为轨迹的曲率约束：

matlab复制vx = 20; % 车速 m/s
max_ay = 2.5; % 最大侧向加速度
max_curvature = max_ay / vx^2; % 最大允许曲率

3.2 曲率实时校验

在轨迹生成后，需要校验整条轨迹的曲率是否超出限制。曲率计算公式为：

κ(t) = |ÿ(t)| / (1 + ẏ(t)²)^(3/2)

我开发了一个自动校验函数，当检测到曲率超限时会自动调整换道时间T：

matlab复制function [T_adjusted] = adjust_T(T_initial, vx, max_ay)
    % 迭代调整换道时间直到满足曲率约束
    T = T_initial;
    for i = 1:10
        [t, y, dy, ddy] = generate_trajectory(T);
        curvature =