用Python和NumPy实现自动驾驶换道轨迹规划：从理论到可视化实战

当你在高速公路上看到车辆平稳地完成换道动作时，是否好奇这背后的数学原理？作为自动驾驶领域的核心技术之一，轨迹规划决定了车辆如何安全、舒适地从一个车道移动到另一个车道。本文将带你用Python和NumPy实现基于五次多项式的换道轨迹规划，并通过完整代码示例和可视化分析，深入理解这一过程的每个技术细节。

1. 环境准备与基础概念

在开始编码前，我们需要明确几个关键概念。五次多项式轨迹规划的核心思想是：通过定义车辆在起点和终点的位置、速度、加速度等状态，构建一个时间相关的平滑曲线。这个曲线不仅要满足边界条件，还要保证各阶导数的连续性，从而确保乘坐舒适性。

首先配置开发环境：

pip install numpy matplotlib

五次多项式的一般形式为：

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

其中，t代表时间，a和b是待求系数。我们需要12个边界条件（起点和终点的x,y位置、速度、加速度）来确定这12个系数。

注意：实际应用中，纵向（x）和横向（y）运动通常独立处理，这简化了问题且符合大多数道路场景。

2. 构建矩阵方程求解系数

理解边界条件的数学表达是关键。对于起点t₀和终点t₁，我们有以下约束：

位置约束：

x(t₀) = x₀, x(t₁) = x₁
y(t₀) = y₀, y(t₁) = y₁

速度约束（一阶导数）：

x'(t₀) = vx₀, x'(t₁) = vx₁
y'(t₀) = vy₀, y'(t₁) = vy₁

加速度约束（二阶导数）：

x''(t₀) = ax₀, x''(t₁) = ax₁
y''(t₀) = ay₀, y''(t₁) = ay₁

将这些约束转化为矩阵形式：

T = np.matrix([
    [t0**5, t0**4, t0**3, t0**2, t0, 1],
    [5*t0**4, 4*t0**3, 3*t0**2, 2*t0, 1, 0],
    [20*t0**3, 12*t0**2, 6*t0, 2, 0, 0],
    [t1**5, t1**4, t1**3, t1**2, t1, 1],
    [5*t1**4, 4*t1**3, 3*t1**2, 2*t1, 1, 0],
    [20*t1**3, 12*t1**2, 6*t1, 2, 0, 0]
])

然后构建右侧的边界条件向量X和Y，通过求解线性方程组T×A=X和T×B=Y得到系数矩阵A和B。

3. 完整Python实现与可视化

下面是一个完整的实现示例，包含场景设置、系数求解和轨迹生成：

import numpy as np
import matplotlib.pyplot as plt

def quintic_polynomial_planner(start_state, end_state, T):
    """五次多项式轨迹规划
    
    参数:
        start_state: [x0, y0, vx0, vy0, ax0, ay0]
        end_state: [x1, y1, vx1, vy1, ax1, ay1]
        T: 规划时间长度
        
    返回:
        t: 时间序列
        path: [x, y, vx, vy, ax, ay] 轨迹序列
    """
    # 提取x方向边界条件
    X = np.array([
        start_state[0], start_state[2], start_state[4],
        end_state[0], end_state[2], end_state[4]
    ])
    
    # 提取y方向边界条件
    Y = np.array([
        start_state[1], start_state[3], start_state[5],
        end_state[1], end_state[3], end_state[5]
    ])
    
    # 构建时间矩阵T
    t0, t1 = 0, T
    T_matrix = np.array([
        [t0**5,    t0**4,   t0**3, t0**2, t0, 1],
        [5*t0**4,  4*t0**3, 3*t0**2, 2*t0, 1, 0],
        [20*t0**3, 12*t0**2, 6*t0, 2, 0, 0],
        [t1**5,    t1**4,   t1**3, t1**2, t1, 1],
        [5*t1**4,  4*t1**3, 3*t1**2, 2*t1, 1, 0],
        [20*t1**3, 12*t1**2, 6*t1, 2, 0, 0]
    ])
    
    # 求解系数
    A = np.linalg.solve(T_matrix, X)
    B = np.linalg.solve(T_matrix, Y)
    
    # 生成轨迹
    t = np.linspace(t0, t1, int(T*20))
    path = np.zeros((len(t), 6))  # x,y,vx,vy,ax,ay
    
    for i, ti in enumerate(t):
        # 位置
        path[i, 0] = np.polyval(A[::-1], ti)  # x
        path[i, 1] = np.polyval(B[::-1], ti)  # y
        
        # 速度
        vx_coeff = np.polyder(A[::-1], 1)
        vy_coeff = np.polyder(B[::-1], 1)
        path[i, 2] = np.polyval(vx_coeff, ti)  # vx
        path[i, 3] = np.polyval(vy_coeff, ti)  # vy
        
        # 加速度
        ax_coeff = np.polyder(A[::-1], 2)
        ay_coeff = np.polyder(B[::-1], 2)
        path[i, 4] = np.polyval(ax_coeff, ti)  # ax
        path[i, 5] = np.polyval(ay_coeff, ti)  # ay
    
    return t, path

可视化函数可以帮助我们直观理解轨迹和运动状态：

def plot_trajectory(t, path, start_state, end_state):
    """绘制轨迹和运动状态"""
    plt.figure(figsize=(15, 10))
    
    # 轨迹图
    plt.subplot(2, 2, 1)
    plt.plot(path[:, 0], path[:, 1], 'r-', label='轨迹')
    plt.plot(start_state[0], start_state[1], 'bo', label='起点')
    plt.plot(end_state[0], end_state[1], 'go', label='终点')
    plt.xlabel('x位置 (m)')
    plt.ylabel('y位置 (m)')
    plt.title('车辆换道轨迹')
    plt.legend()
    plt.grid(True)
    plt.axis('equal')
    
    # 速度图
    plt.subplot(2, 2, 2)
    plt.plot(t, np.sqrt(path[:, 2]**2 + path[:, 3]**2), 'b-', label='合速度')
    plt.plot(t, path[:, 2], 'r--', label='x方向速度')
    plt.plot(t, path[:, 3], 'g--', label='y方向速度')
    plt.xlabel('时间 (s)')
    plt.ylabel('速度 (m/s)')
    plt.title('速度变化')
    plt.legend()
    plt.grid(True)
    
    # 加速度图
    plt.subplot(2, 2, 3)
    plt.plot(t, np.sqrt(path[:, 4]**2 + path[:, 5]**2), 'b-', label='合加速度')
    plt.plot(t, path[:, 4], 'r--', label='x方向加速度')
    plt.plot(t, path[:, 5], 'g--', label='y方向加速度')
    plt.xlabel('时间 (s)')
    plt.ylabel('加速度 (m/s²)')
    plt.title('加速度变化')
    plt.legend()
    plt.grid(True)
    
    # 曲率图
    plt.subplot(2, 2, 4)
    v = np.sqrt(path[:, 2]**2 + path[:, 3]**2)
    a_tan = (path[:, 2]*path[:, 4] + path[:, 3]*path[:, 5]) / v
    a_normal = np.sqrt(np.abs(path[:, 4]**2 + path[:, 5]**2 - a_tan**2))
    curvature = a_normal / v**2
    plt.plot(t, curvature, 'b-')
    plt.xlabel('时间 (s)')
    plt.ylabel('曲率 (1/m)')
    plt.title('轨迹曲率变化')
    plt.grid(True)
    
    plt.tight_layout()
    plt.show()

4. 实际应用与参数调优

在实际应用中，参数设置对轨迹质量有重大影响。让我们通过一个具体案例来说明：

# 场景参数
d = 3.5  # 车道宽度(m)
v = 5.0  # 初始速度(m/s)
T = 4.0  # 换道时间(s)

# 初始状态: [x, y, vx, vy, ax, ay]
start_state = [0, -d/2, v, 0, 0, 0]

# 目标状态
end_state = [v*T, d/2, v, 0, 0, 0]

# 生成轨迹
t, path = quintic_polynomial_planner(start_state, end_state, T)

# 可视化
plot_trajectory(t, path, start_state, end_state)

关键参数调优建议：

1. 换道时间T：

   • 太短会导致加速度过大，影响舒适性
   • 太长则影响交通效率
   • 一般3-5秒为宜

2. 初始和目标速度：

   • 保持速度连续（起点和终点速度相同）
   • 速度变化会导致不必要的加减速

3. 加速度约束：

   • 纵向加速度建议不超过2 m/s²
   • 横向加速度建议不超过1 m/s²

常见问题及解决方案：

• 矩阵奇异错误：

  • 检查时间参数是否合理（t1 > t0）
  • 确认边界条件是否自洽（如位置和速度关系合理）

• 轨迹不自然：

  • 调整换道时间T
  • 检查终点状态是否合理
  • 考虑增加jerk（加加速度）约束

• 曲率突变：

  • 检查加速度边界条件
  • 考虑使用更高阶多项式（七次）

5. 进阶扩展与性能优化

基础实现可以进一步优化：

1. 动态障碍物避让：

def check_collision(path, obstacles):
    """检查轨迹是否与障碍物碰撞"""
    for point in path:
        for obs in obstacles:
            if np.linalg.norm(point[:2] - obs[:2]) < obs[2]:  # 简单圆形障碍物
                return True
    return False

2. 多轨迹评分与选择：

def evaluate_trajectory(path):
    """评估轨迹质量"""
    # 舒适性指标（加速度平方和）
    comfort = np.sum(path[:, 4:6]**2)
    
    # 效率指标（时间最短）
    efficiency = len(path)
    
    # 曲率平滑度
    curvature = ...  # 计算曲率变化
    
    return 0.5*comfort + 0.3*efficiency + 0.2*curvature

3. 实时性优化：

   • 预计算和缓存矩阵T的逆
   • 使用更高效的线性代数库
   • 并行计算多条候选轨迹

4. 与车辆动力学结合：

   • 考虑车辆最大转向角限制
   • 加入轮胎摩擦圆约束
   • 与车辆控制器协同设计

在真实自动驾驶系统中，轨迹规划模块还需要考虑：

• 高精度地图数据
• 交通规则约束
• 乘客舒适度指标
• 紧急避障策略
• 系统冗余和故障安全机制

轨迹规划是自动驾驶决策与控制的关键桥梁，好的规划算法能使后续控制任务事半功倍。通过调整多项式阶数和边界条件，这套方法可以扩展到更复杂的场景，如匝道汇入、交叉路口通行等。