从零构建自动驾驶轨迹规划器Python实现Lattice算法核心逻辑1. 环境搭建与基础工具链在开始构建Lattice规划器之前我们需要准备Python科学计算生态的基础工具包。推荐使用Anaconda创建专属虚拟环境conda create -n lattice_planner python3.8 conda activate lattice_planner pip install numpy matplotlib scipy ipykernel关键库的作用说明NumPy处理高维数组和矩阵运算Matplotlib轨迹可视化与调试SciPy提供插值、优化等数学工具IPython交互式开发环境提示建议使用Jupyter Lab进行开发可以实时观察轨迹生成效果2. Frenet坐标系转换实现2.1 坐标系转换原理Frenet坐标系将复杂的二维平面路径规划问题分解为纵向s方向沿参考线的距离横向l方向垂直于参考线的偏移量class FrenetConverter: staticmethod def cartesian_to_frenet(ref_line, x, y): 将笛卡尔坐标转换为Frenet坐标 # 找到参考线上最近点 closest_idx np.argmin(np.sum((ref_line[:, :2] - [x, y])**2, axis1)) ... staticmethod def frenet_to_cartesian(ref_line, s, l): 将Frenet坐标转换回笛卡尔坐标 # 基于参考线进行插值 ...2.2 参考线离散化处理原始参考线通常由稀疏的点组成需要先进行插值处理def discretize_reference_line(ref_points, interval0.1): 将参考线离散化为固定间隔的点 cum_s np.cumsum(np.linalg.norm( np.diff(ref_points, axis0), axis1)) new_s np.arange(0, cum_s[-1], interval) return np.interp(new_s, cum_s, ref_points)3. 轨迹采样策略实现3.1 横向采样方案横向采样决定了车辆可能的变道行为我们采用多项式插值生成候选轨迹def generate_lateral_trajectories(s_conditions, t5.0, num5): 生成横向轨迹簇 trajectories [] for l in np.linspace(-1.0, 1.0, num): # 横向偏移范围 coeffs quintic_poly(s_conditions[0], s_conditions[1], s_conditions[2], l, 0, 0, t) traj [] for t in np.linspace(0, t, 20): traj.append([t, poly_val(coeffs, t)]) trajectories.append(traj) return trajectories参数说明参数说明典型值s_conditions初始状态[s, s_dot, s_ddot][0, 10, 0]t规划时长3-5秒num采样数量5-7条3.2 纵向速度规划纵向速度需要考虑前车跟随、定速巡航等场景def generate_speed_profiles(v_initial, target_speed, obstacles): 生成速度规划曲线 profiles [] # 巡航场景 profiles.append(constant_acc_profile(v_initial, target_speed)) # 跟车场景 if obstacles: profiles.append(follow_profile(v_initial, obstacles[0])) return profiles4. 轨迹评估与碰撞检测4.1 代价函数设计综合评估轨迹的舒适性、效率等因素def calculate_cost(trajectory, obstacles): 计算轨迹综合代价 jerk_cost np.sum(np.diff(trajectory.acc, 2)**2) obs_cost min_distance_to_obstacles(trajectory, obstacles) return 0.3*jerk_cost 0.7*obs_cost4.2 碰撞检测实现基于圆形包围盒的快速碰撞检测class CollisionChecker: def __init__(self, obstacles, vehicle_width2.0): self.obstacles obstacles self.radius vehicle_width / 2 def check_collision(self, trajectory): for point in trajectory: for obs in self.obstacles: if np.linalg.norm(point[:2] - obs[:2]) self.radius obs[2]: return True return False5. 完整规划流程实现将各模块整合为完整规划器class LatticePlanner: def plan(self, ref_line, ego_state, obstacles): # 坐标系转换 frenet_state FrenetConverter.cartesian_to_frenet(ref_line, *ego_state) # 轨迹采样 lat_trajs generate_lateral_trajectories(frenet_state) lon_trajs generate_speed_profiles(ego_state[2], 10, obstacles) # 轨迹组合与评估 best_traj None min_cost float(inf) for lon in lon_trajs: for lat in lat_trajs: traj combine_trajectories(ref_line, lon, lat) if not CollisionChecker(obstacles).check_collision(traj): cost calculate_cost(traj, obstacles) if cost min_cost: min_cost cost best_traj traj return best_traj6. 可视化与调试技巧使用Matplotlib实现动态可视化def visualize_planning(ref_line, trajectories, obstacles): plt.figure(figsize(12, 6)) plt.plot(ref_line[:,0], ref_line[:,1], b--, labelReference) for traj in trajectories: plt.plot(traj[:,0], traj[:,1], alpha0.3) for obs in obstacles: circle plt.Circle(obs[:2], obs[2], colorr) plt.gca().add_patch(circle) plt.axis(equal) plt.legend()调试建议先验证Frenet坐标转换的准确性单独测试横向/纵向轨迹生成逐步增加障碍物复杂度调整代价函数权重观察轨迹变化7. 性能优化实践当采样点增多时需要优化计算效率numba.jit(nopythonTrue) def fast_distance_check(points, obstacles): 使用numba加速距离计算 # 实现向量化距离计算 ... # 并行化轨迹评估 from concurrent.futures import ThreadPoolExecutor def parallel_evaluation(trajectories): with ThreadPoolExecutor() as executor: results list(executor.map(evaluate_trajectory, trajectories)) return min(results, keylambda x: x[1])在实际项目中我发现将碰撞检测实现为Cython扩展可以提升5-8倍性能。对于需要实时性的场景可以考虑以下优化策略空间哈希将环境划分为网格快速排除远距离障碍物轨迹预筛选基于简单启发式规则减少评估数量缓存机制复用上一周期的计算结果