简介一套基于Python的自动驾驶路径规划系统源码面向自动驾驶、智能车竞赛和机器人导航方向的学习者与开发者。项目将路径规划与控制算法融为一体涵盖RRT、A*、PRM等采样类规划方法以及PID、纯跟踪、动态窗口法、Frenet最优轨迹规划、模型预测控制MPC等经典控制策略并包含三次样条平滑、LQR速度控制等辅助模块覆盖从局部避障到全局路径跟踪的完整技术链路。资源共34个文件其中15个Python脚本呈现核心算法9个C源文件与5个头文件构成工程实现版本另有仿真图与说明文档压缩包仅800KB目录分层清晰便于对照阅读。已有66人学习浏览可快速获取各类算法的可运行代码与实现思路不同算法模块相互独立便于单独提取学习或二次开发应用。1. 基于Python的自动驾驶路径规划系统先把它跑起来再谈怎么改做自动驾驶的路径规划最劝退的不是算法本身而是你打开一个源码包发现几十个.py文件互相 import地图数据是二进制跑起来界面全黑还不知道改哪里。近几年这类基于 Python 的路径规划系统源码在课程设计、课题预研和面试项目里出现频率极高核心套路却很固定一张栅格地图、一个搜索或采样算法、一辆带运动学约束的小车模型再加上一段可视化。把这三层拆开这套系统就再没有什么黑匣子。这个方向适合谁适合正在做自动驾驶算法岗笔试面试的人适合毕设选了路径规划题目的人也适合想把手里的静态 A* 升级成动态避障小车的移动机器人从业者。下面是这套源码最常见的组织方式以及我个人反复用到的落地步骤。2. 路径规划源码里藏的算法骨架栅格搜索、采样与轨迹后处理这类 Python 路径规划系统一般不会从传感器数据开始而是从已知地图 起点终点出发先解决几何路径再逐步加约束。理解它的骨架比直接跑通更有价值。2.1 地图的表示方式栅格图与代价地图的差异打开源码大概率会看到两类地图文件一类是.png.yaml另一类是纯文本矩阵。前者是给 ROS 的map_server用的后者是教学项目常用的numpy二维数组。这两类地图在源码里的处理路径完全不同。教学版源码常用的是一个GridMap类内部是一个numpy.ndarray0 表示可通行1 表示障碍物。读取代码常见长这样import numpy as np from PIL import Image class GridMap: def __init__(self, map_path, resolution0.05): # resolution 单位米/像素栅格地图的物理精度 img Image.open(map_path).convert(L) self.resolution resolution self.data np.array(img) # 常见约定白色(255)为可通行黑色(0)为障碍 self.data np.where(self.data 128, 0, 1) self.height, self.width self.data.shape注意self.height, self.width self.data.shape这一行numpy数组第一维是行对应栅格图的 Y 轴第二维是列对应 X 轴。这是一个极容易翻车的坐标顺序问题后面避坑章会单独展开。代价地图则多一层灰色地带值在 0 到 255 之间越靠近障碍物数值越大。Dijkstra 和 A* 在这类地图上不再只找路径是否可达而是计算累计代价最小。如果你手上的源码用的是代价地图启发函数和邻域遍历的写法会不一样。2.2 搜索式算法与采样式算法的选型判断定位一套源码的算法水平最快的方式是看它的planner目录下有哪些文件。搜索式算法包括 Dijkstra、A*、D* Lite它们的特点是显式维护一个开放列表在小尺寸栅格图上效率高路径质量稳定。采样式算法包括 RRT、RRT*、PRM它们的特点是在高维空间或大范围地图上避免栅格化带来的内存爆炸但路径质量波动大。在这类 Python 系统中A* 和 RRT 通常并存因为它们的适用场景正好互补A* 在 200x200 以内的栅格图上毫秒级出结果RRT 适合越野场景的大门幅地图。下面是一个精简 A* 的核心循环保留了源码里最常见的写法import heapq def astar_search(grid_map, start, goal): open_heap [] heapq.heappush(open_heap, (0, start)) came_from {} g_score {start: 0} f_score {start: heuristic(start, goal)} while open_heap: current heapq.heappop(open_heap)[1] if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(grid_map, current): tentative_g g_score[current] move_cost(current, neighbor) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g heuristic(neighbor, goal) heapq.heappush(open_heap, (f_score[neighbor], neighbor)) return None # 表示无可行路径2.3 路径后处理为什么规划出来的折线根本没法开搜索类算法输出的是一串栅格中心点直接交给下游控制模块车辆会在每个拐点停下来转向原地打转。因此一般会在规划层和控制层之间加一个后处理模块常见做法有两种。第一种是轨迹平滑用三次样条插值或者共轭梯度法把折线变成连续曲线。第二种是运动学约束把转折点替换成最小转弯半径约束的圆弧段。后者在泊车路径规划算法里非常关键例如用 Dubins 曲线处理无倒车场景。后处理代码通常长这样from scipy.interpolate import splprep, splev def smooth_path(waypoints, smoothing3): # 输入是 Nx2 的数组输出是采样更密的平滑轨迹 x waypoints[:, 0] y waypoints[:, 1] tck, u splprep([x, y], ssmoothing) new_points splev(np.linspace(0, 1, 500), tck) return np.vstack(new_points).Tsmoothing参数是这条路线的松紧度值越大越顺滑但越容易切割障碍物的尖角膨胀层一般取 1 到 5 之间。超过 5 容易直接穿过膨胀层产生与障碍物碰撞的假轨迹。3. 用最小配置跑通规划 demo依赖、地图与启动参数如果说算法骨架是这套系统的灵魂那么跑通 demo 就是它的入门考试。多数人倒在环境依赖上而不是代码逻辑上。3.1 环境准备Python 版本与三件套依赖这类 Python 自动驾驶路径规划系统通常依赖numpy、matplotlib、scipy三件套部分源码会额外引入opencv-python或pygame做可视化。我一般建议在一个干净的虚拟环境里安装不要直接装在系统 Python 上。python3 -m venv venv source venv/bin/activate pip install numpy scipy matplotlib opencv-python这里有个细节如果源码是两年前写的numpy新版本可能不兼容旧的 API。遇到module numpy has no attribute bool这类报错不要急着改源码先降到numpy1.23.5试试。这种兼容性坑在开源源码包里出现频率极高属于最常见的翻车现场。3.2 启动流程从 main.py 到可视化窗口绝大多数这类源码都会提供一个main.py或demo.py入口逻辑是加载地图实例化一个规划器给定起终点然后循环刷新可视化窗口。python main.py \ --map maps/map_demo.png \ --planner astar \ --start 120 60 \ --goal 40 180 \ --resolution 0.1启动后能看到一条规划路径画在地图上。如果你的源码没有命令行参数通常会在main.py顶部有一组全局变量直接改START_POINT和GOAL_POINT即可。命令行的四个参数是这套系统最常用的调优入口--map替换成自己的地图注意地图里障碍物要是实心黑色否则二值化会把灰斑当可通行区域--planner切换算法多数源码支持astar、rrt、dijkstra三者之间路径形态差异明显--start/--goal单位是像素坐标不是世界坐标写反 Y 轴会直接导致规划失败--resolution规划结果会乘以它换算成米改错会导致路径长度显示异常我第一次跑这种包的时候直接用了默认参数地图是能显示但规划按钮点了毫无反应。后来加了日志才发现读地图时把0和255的含义搞反了障碍全被当成可通行区域A* 直接穿过墙体因为起点和终点在同一个连通域里。把np.where(self.data 128, 0, 1)改成np.where(self.data 128, 1, 0)就正常了。3.3 三个必调的参数步长、邻居数和膨胀半径跑通只是第一步要让路径质量看起来像自动驾驶系统三个参数必须调。第一个是 RRT 的扩展步长step_size单位是栅格数。步长太小算法会在大地图上龟速探索半天够不到终点步长太大轨迹会频繁撞击障碍物边缘碰撞检测直接不通过。推荐初始值为地图短边的 1% 到 2%。第二个是 A* 的邻居数。四邻域速度慢但路径安全八邻域速度快但可能在墙角斜穿。如果你发现规划路径贴着障碍物对角线切过去多半是邻居定义里没有排除对角穿越的情况。第三个是地图膨胀半径inflation_radius单位通常也是栅格数。车辆是有宽度的按照质点去规划生成路径会让实际车体刮墙。这类源码一般不会默认开启膨胀层需要自己给障碍物矩阵做一次距离变换扩展。from scipy.ndimage import binary_dilation def inflate_obstacles(grid_map, radius3): # 把障碍物向外扩 radius 个栅格生成安全边界 struct np.ones((2 * radius 1, 2 * radius 1)) return binary_dilation(grid_map, structurestruct).astype(np.int8)radius3对应物理尺寸是3 * resolution米如果车辆宽度是 0.4 米、栅格分辨率是 0.05 米膨胀半径至少要 4 个栅格否则地图上的安全距离小于实际车身半径。4. 避坑清单这类源码最常见的五个翻车点这套源码的方向盘后面藏着不少玄学问题很多现象看似是环境问题其实根源在数据约定上。以下是我在复现不同版本源码时反复踩过的坑按出现频率从高到低整理。4.1 规划结果在 Y 轴上镜像翻转现象规划出来的路径在可视化窗口里是正常的但把坐标输出成文本后在别的软件里画图发现路径上下颠倒。原因栅格图的坐标系和常规笛卡尔坐标系不一致。图片的原点在左上角Y 轴向下路径规划算法按数学惯例使用 Y 轴向上。源码里的可视化模块做了翻转但保存路径时没做。解决在最终输出路径前统一坐标转换常见的代码是对 Y 轴取负并加上地图高度def grid_to_world(point, grid_map): row, col point x col * grid_map.resolution y (grid_map.height - row) * grid_map.resolution return x, y4.2 RRT 算法卡死在高维空间现象RRT 跑起来后可视化窗口里的树一直在起点附近反复生长终点方向几乎没有树枝甚至跑了十几秒都没产出路径。原因采样是均匀随机采样当起点和终点之间有一条窄长走廊时随机点落在走廊里的概率极低树扩张速度慢。解决给采样函数加上goal_bias机制以一定概率直接把终点作为采样目标点。常见的做法是每 10 次采样中固定一次取终点。下面的实现片段可以自己加到源码里def sample_random_point(goal, goal_bias0.2): if np.random.random() goal_bias: return tuple(goal) else: return (np.random.randint(0, width), np.random.randint(0, height))如果你改完goal_bias还是看不到效果检查迭代上限max_iterations。很多源码默认只给 500 次迭代在 500x500 的地图上就是在碰运气。调到 5000 次以上会明显改观。4.3 路径频繁与障碍物擦边现象路径目标点是可达的但路径上某些线段距离障碍物只有 1 个栅格视觉上像是硬挤过去的。原因规划器把栅格中心点当作路径的几何位置没有考虑车辆宽度和定位误差。碰撞检测用的是二值化的障碍层没有用膨胀层。解决在规划前对地图做膨胀处理并且碰撞检测也要基于膨胀后的地图。不要在路径生成后再去修正那是事后修补效果不好。4.4 代价函数权重不合适导致路径绕着走现象A* 能出路径但路径明显绕远或者贴障碍物很近明显不符合直觉走向。原因多数源码沿用最朴素的f g h其中h是欧氏距离g是累计移动代价。问题出在move_cost如果它只区分邻域是横竖还是对角而没有引入障碍物距离惩罚项路径就会贴着墙走。如果惩罚项加得过大路径又会在空旷区域绕大圈。def move_cost(current, neighbor): base_cost 1.0 # 惩罚项根据邻居邻域内的障碍物密度加权 obstacle_penalty 1.0 2.0 * local_obstacle_density(neighbor) return base_cost * obstacle_penalty这个函数的取值属于调参玄学区没有标准答案。我的习惯是先以 2.0 的系数起步看路径是否避开了墙面再微调到 1.5 到 3.0 之间。4.5 可视化刷新卡顿与内存泄漏现象规划完成之后拖动地图窗口帧率急剧下降长时间运行时内存占用不断攀升。原因多数源码在while循环里不断调用plt.clf()重建整个画布旧画布没有被真正释放内存泄漏。解决改用matplotlib的FuncAnimation模式或者手动ax.clear()而不是plt.clf()。如果只关心规划结果可以直接把可视化循环去掉输出结果到numpy文件再用外部工具回放。5. 从静态全局规划到动态避障加感知、重规划与行为决策拿到一套可运行的路径规划源码只是开始它的真正实用价值在于能不能从静态全局规划升级成动态避障小车路径规划。这个升级路径在嵌入式小车、ROS 仿真和课程设计里是主线任务。这里讲下我一般怎么做以及这套源码应该改哪几个位置。5.1 给栅格地图加一个动态障碍层静态全局规划的地图是不变的动态避障要求每帧刷新局部地图把实时感知到的障碍物标到新的图层上。常见做法是在原来障碍矩阵之外维护一个同样大小的动态矩阵每次规划前做一次叠加而不是修改原始地图。def update_dynamic_map(static_map, obstacles): dynamic static_map.copy() for point in obstacles: x, y point dynamic[x, y] 1 return dynamic障碍物来源可以是激光雷达的 occupancy grid也可以是视觉的栅格化结果。关键是动态层只在局部窗口生效全局层保留静态地图数据。这套源码如果本身地图对象是全局唯一的建议把叠加操作放在planner.plan()之前这样做不至于污染地图缓存。5.2 重规划循环局部规划器的刷新频率动态场景下的路径规划不可能指望全局规划一次跑完必须做重规划。重规划分为两种定时重规划和事件触发重规划。前者的典型频率是 5 Hz 到 10 Hz在嵌入式平台上会适当降低。后者的触发条件一般是感知模块发现前方路径上新增了障碍物。下面的伪代码结构是我在这类源码基础上加动态避障时用的模板while running: sensor_data perception.get_obstacles() local_map update_dynamic_map(static_map, sensor_data) if is_path_blocked(local_map, current_plan): new_plan planner.plan( map_datalocal_map, startvehicle.get_pose(), goalglobal_goal ) if new_plan is None: emergency_stop() continue current_plan new_plan controller.follow_path(current_plan)注意is_path_blocked这一步它比直接重规划效率高得多。如果车辆只前进了 1 米而全局路径剩余还有 100 米不需要对整个路径重新规划只要检查最近 10 米的路径段是否被新障碍物覆盖。这个局部检查实现很简单遍历近端路径点看它在 local_map 上是不是落在障碍物栅格里。5.3 决策层路径规划之上还有一个行为选择如果源码里只有Planner类没有行为状态机那么在遇到无路可走的情况时代码会直接返回None。这是很多动态避障方案的薄弱点规划器不懂语义只知道没有可行路径。一个低成本的做法是在规划器之上增加一个简单状态机区分直行、绕行、停车等待、倒车重试四个状态。绕行模式下给规划器加一个目标偏移参数。当 A* 返回None时把目标点向垂直于当前前进方向的一侧偏移 N 个栅格再规划一次。这在停车场和窄通道场景中非常管用相当于给规划器提供了自救手段。def plan_with_retry(planner, start, goal, offset_step5): path planner.plan(start, goal) if path is not None: return path # 左右各尝试偏移最多偏移 5 次 for direction in [-1, 1]: for i in range(1, 6): shifted_goal (goal[0] direction * i * offset_step, goal[1]) path planner.plan(start, shifted_goal) if path is not None: return path return None这种规划失败再偏移目标的思路在部分文献里叫目标松弛法虽然在理论上不保证最优但在工程上能大幅提高任务完成率。我在移动底盘上的调试经验是偏移上限限制在 1 米以内偏移步长取 0.2 米效果最稳定。偏移太大会让机器人完全脱离原目标走廊走出一条和全局规划完全无关的路径。5.4 与嵌入式平台的衔接把路径点序列下发给底盘Python 规划系统跑出来的路径最终要下发给嵌入式端去执行。常见的接口是一个 JSON 文件或者共享内存结构体内容就是有序路径点序列。这里有一个高频问题路径点的间隔太密嵌入式端 PID 控制跟不上车辆会出现抖动间隔太疏车辆在各点之间走直线精度下降。我一般会在下发给底盘之前做一次抽稀def simplify_path(path, min_gap0.2): simplified [path[0]] last_pt path[0] for pt in path[1:]: if np.linalg.norm(np.array(pt) - np.array(last_pt)) min_gap: simplified.append(pt) last_pt pt simplified.append(path[-1]) return simplifiedmin_gap单位是米在室内移动机器人上取 0.1 到 0.2在自动驾驶仿真场景里取 0.5 到 1.0。抽稀原则是保证相邻路径点之间的线段不会短于底盘一帧控制周期内可执行的移动距离。6. 用标准 benchmark 验证你的改进参数这套系统改得差不多了有一个问题随之而来怎么判断你调参后的成果真的变好了路径规划领域没有统一的公开测评集但有一组评价指标在学术界和工程界基本达成共识。它们是规划成功率、平均规划耗时、路径长度和路径平滑度。规划成功率表示在 100 次随机起点终点中规划器返回有效路径的次数占比。平均规划耗时反映在线重规划的可承受能力如果你做的是动态避障小车单次规划超过 50 毫秒就要警惕。路径长度直接对比改进前后路径平滑度用相邻轨迹点之间的航向角变化量的方差来量化方差越小越好。我习惯把这四项指标封装成一个测试函数每次改动完立刻跑一遍而不是靠肉眼目测路径图def evaluate_planner(planner, maps, start_goal_pairs, trials100): success 0 total_time 0.0 path_lengths [] for _ in range(trials): map_id np.random.randint(len(maps)) start, goal start_goal_pairs[_] t0 time.time() path planner.plan(map_id, start, goal) total_time time.time() - t0 if path is not None: success 1 path_lengths.append(compute_path_metric(path)) return { success_rate: success / trials, avg_time_ms: total_time * 1000 / trials, path_length_avg: np.mean(path_lengths) }跑这个基准函数时要注意起点终点必须放在可通行区域否则规划失败会污染成功率数据。你可以做一个is_free(map, point)函数去过滤掉非法起终点这样测试结果才是算法能力的真实反映。最后一个技巧是保存规划结果快照。无论是 RRT 还是 A*随机种子不同路径就不同。对比实验时必须固定随机种子否则你无法分辨指标变化是算法改进还是采样随机性导致的。在main.py开头加上以下两行结果才可复现import random random.seed(42) np.random.seed(42)这是我的血泪经验。最初我做 RRT* 算法对比实验连续三天的测试数据都在上下波动一直以为是新代码有问题最后发现是随机种子没固定。从那以后任何规划算法改动我都先固定随机种子再跑基准。希望这个习惯也能帮到你少走一段无谓的弯路。本文还有配套的精品资源点击获取