1. 项目概述
在自动驾驶领域,换道轨迹规划(DLC)是决定行车安全性和舒适性的核心技术之一。今天要分享的是一种基于五次多项式的快速自由换道轨迹规划方法,它能在紧急情况下(如前车突然急刹)在0.1秒内生成多条备选轨迹并选出最优方案。
这种方法的核心优势在于:
- 计算效率高,适合实时系统
- 生成的轨迹满足运动学约束
- 可灵活调整成本函数权重以适应不同驾驶风格
- 易于与现有控制系统集成
2. 五次多项式轨迹生成原理
2.1 五次多项式基础
五次多项式是描述横向位移(y)随时间(t)变化的理想选择,其基本形式为:
python复制def quintic_poly(t, coeffs):
return coeffs[0] + coeffs[1]*t + coeffs[2]*t**2 + coeffs[3]*t**3 + coeffs[4]*t**4 + coeffs[5]*t**5
选择五次多项式的原因在于:
- 能够满足位置、速度、加速度三个维度的边界条件约束
- 计算复杂度适中,适合实时应用
- 生成的轨迹曲率连续,保证乘坐舒适性
2.2 系数求解方法
要确定多项式系数,需要解以下线性方程组:
python复制def calc_coeffs(start, end, T):
A = np.array([
[1, 0, 0, 0, 0, 0], # 初始位置约束
[1, T, T**2, T**3, T**4, T**5], # 终止位置约束
[0, 1, 0, 0, 0, 0], # 初始速度约束
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], # 终止速度约束
[0, 0, 2, 0, 0, 0], # 初始加速度约束
[0, 0, 2, 6*T, 12*T**2, 20*T**3] # 终止加速度约束
])
b = np.array([start[0], end[0], start[1], end[1], start[2], end[2]])
return np.linalg.solve(A, b)
这个方程组确保了轨迹在起点和终点处满足指定的位置、速度和加速度条件。例如,设置start = [0, 5, 0]表示:
- 初始横向位移:0米
- 初始横向速度:5米/秒
- 初始横向加速度:0米/秒²
3. 轨迹簇生成策略
3.1 参数空间采样
为了生成多样化的候选轨迹,我们需要在参数空间进行采样:
python复制traj_pool = []
for T in np.linspace(3, 6, 10): # 换道时间3-6秒,采样10个点
for dy in [3.5, 3.7, 4.0]: # 目标横向位移(标准车道宽3.5米)
coeffs = calc_coeffs(start=[0, 0, 0], end=[dy, 0, 0], T=T)
traj_pool.append((coeffs, T))
这种采样策略的优点是:
- 覆盖了不同的换道时间(从保守到激进)
- 考虑了略微不同的目标位置(应对不同车道情况)
- 计算量可控,适合实时应用
3.2 轨迹有效性检查
生成的轨迹需要满足物理约束:
- 最大横向加速度不超过2.5 m/s²(舒适性限制)
- 最大曲率半径限制(车辆动力学限制)
- 不与静态障碍物相交
python复制def validate_trajectory(traj):
max_acc = np.max(np.abs(2*traj[:,2] + 6*traj[:,3]*t + 12*traj[:,4]*t**2))
if max_acc > 2.5:
return False
# 其他约束检查...
return True
4. 成本函数设计
4.1 多目标优化框架
评价函数需要平衡三个关键指标:
python复制def evaluate(traj, obstacles):
# 舒适性成本(加加速度)
jerk_cost = np.max(np.abs(2*traj[:,3] + 6*traj[:,4]*t + 12*traj[:,5]*t**2))
# 安全性成本
min_distance = min([calc_min_dist(traj, obj) for obj in obstacles])
safety_cost = 1.0 / (min_distance + 1e-5) # 防止除零
# 效率成本
time_cost = T * 0.3
return 0.4*jerk_cost + 0.5*safety_cost + 0.1*time_cost
4.2 各成本项详解
-
舒适性成本:
- 通过加加速度(jerk)衡量
- 人类舒适阈值约为2.0 m/s³
- 计算三次导数得到jerk值
-
安全性成本:
- 与最近障碍物的距离成反比
- 使用指数衰减函数增强安全性权重
- 考虑动态障碍物的预测位置
-
效率成本:
- 鼓励更短的换道时间
- 但受限于舒适性和安全性约束
- 权重通常设置得较低
5. 控制系统集成
5.1 模型预测控制(MPC)框架
将最优轨迹输入MPC控制器实现闭环控制:
python复制mpc_controller = ModelPredictiveControl()
optimal_traj = select_best_traj(traj_pool)
for t in np.arange(0, T, 0.1): # 100ms控制周期
control = mpc_controller.update(optimal_traj[t])
vehicle.apply_control(control)
# 碰撞检测与重规划
if check_collision():
traj_pool = regenerate_trajectory()
optimal_traj = select_best_traj(traj_pool)
5.2 实时性保障措施
为确保系统实时性能:
- 限制轨迹池大小(通常20-50条)
- 使用高效的空间索引结构(如KD-Tree)加速距离计算
- 并行化评估过程
- 采用增量式更新策略
6. 三车道场景仿真实现
6.1 场景构建
python复制def build_three_lane_scenario():
plt.figure(figsize=(12,4))
# 绘制车道线
plt.plot([-5.25, 5.25], [0, 100], 'w--', linewidth=2) # 中央分隔线
plt.plot([-8.75, 8.75], [0, 100], 'w--', linewidth=1) # 路肩线
# 随机生成障碍物
obstacles = np.random.uniform(low=-8, high=8, size=(10,2))
obstacles[:,1] *= 100 # 纵向分布
return obstacles
6.2 可视化效果
仿真结果展示:
- 红色曲线:本车最优轨迹
- 黄色点:周围障碍物
- 虚线:车道线标记
通过调整成本函数权重,可以观察到不同的换道策略:
- 安全性优先:轨迹远离所有障碍物
- 舒适性优先:轨迹更加平缓
- 效率优先:换道时间最短
7. 实际应用中的注意事项
-
参数调优经验:
- 初次实现时,建议先固定换道时间,专注优化轨迹形状
- 安全性权重应随车速增加而提高
- 城市道路和高速公路需要不同的参数集
-
常见问题排查:
- 轨迹震荡:检查加速度约束是否合理
- 计算延迟:优化矩阵运算,考虑使用C++实现核心部分
- 轨迹不自然:增加更多采样点,特别是短时间换道情况
-
扩展方向:
- 结合高精地图信息优化成本函数
- 增加交通规则约束(如虚实线)
- 考虑其他道路使用者行为预测
在实际车辆测试中,我们发现这套系统在80km/h以下速度表现良好。当车速更高时,需要:
- 延长前瞻距离
- 增加安全性权重
- 使用更高阶多项式(七次)以获得更平滑的轨迹
这套方法的优势在于其通用性和可扩展性。通过调整多项式阶数和成本函数,可以适应从低速园区到高速公路的各种场景。对于工程实现,建议先用Python原型验证算法,再用C++实现生产版本以获得最佳性能。