车辆换道轨迹规划与MPC控制技术解析

1. 五次多项式换道轨迹规划原理与实现

在车辆换道控制领域，五次多项式轨迹规划因其平滑性和边界条件可控性成为主流方案。这种方法的本质是通过六阶多项式描述车辆横向位移随时间变化的规律：

y(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀

1.1 边界条件约束解析

完整的边界条件需要包含起止点的位移、速度和加速度约束，这正是五次多项式被选中的关键原因——六个系数刚好对应六个边界条件。具体约束包括：

1. 初始时刻(t=0)：

   • 横向位移 y(0) = 0
   • 横向速度 ẏ(0) = 0
   • 横向加速度 ÿ(0) = 0

2. 终止时刻(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 + 1