路径规划算法实战用Python实现Dijkstra与A*对比附完整代码如果你正在为机器人、游戏角色或者物流系统设计一个“大脑”让它能自己找到从A点到B点的最佳路线那么路径规划算法就是你绕不开的核心技术。这不仅仅是画一条线那么简单它关乎效率、实时性以及在复杂多变的环境中能否做出最优决策。对于刚接触这个领域的Python开发者来说面对Dijkstra、A*、RRT*等一堆术语很容易感到无从下手。今天我们不谈枯燥的数学证明直接从代码实战出发。我将带你用Python亲手实现两个最经典的路径规划算法——Dijkstra和A*并通过可视化的方式直观地对比它们在迷宫寻路、游戏地图等不同场景下的表现。你会发现A*算法中那个看似神秘的“启发式函数”是如何像一位经验丰富的向导大幅提升搜索效率的。文章最后我会分享如何将这些算法思想平滑地应用到更复杂的场景比如ROS机器人系统中。让我们从一行行代码开始揭开路径规划算法的神秘面纱。1. 环境搭建与问题建模构建我们的“数字迷宫”在开始编写算法之前我们需要一个“战场”——一个可以清晰表示和测试算法的环境。对于路径规划图Graph是最自然的数据结构。我们将用一个二维网格来模拟一个简单的迷宫或地图其中每个格子是一个节点相邻的格子之间有边相连。1.1 创建网格世界与可视化工具我们首先用Python创建一个网格类。这个类负责管理地图的尺寸、障碍物并提供一些基础功能比如检查一个坐标是否可通行、获取一个节点的所有邻居。import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches from queue import PriorityQueue import heapq class GridWorld: def __init__(self, width10, height10): 初始化一个网格世界。 :param width: 网格宽度 :param height: 网格高度 self.width width self.height height # 0表示可通行1表示障碍物 self.grid np.zeros((height, width), dtypeint) # 随机生成一些障碍物大约占20% obstacle_ratio 0.2 num_obstacles int(width * height * obstacle_ratio) for _ in range(num_obstacles): x, y np.random.randint(0, width), np.random.randint(0, height) self.grid[y, x] 1 def is_valid(self, x, y): 检查坐标(x, y)是否在地图范围内且不是障碍物。 return 0 x self.width and 0 y self.height and self.grid[y, x] 0 def get_neighbors(self, node): 获取一个节点的所有有效邻居四连通上、下、左、右。 x, y node neighbors [] # 四方向移动上、右、下、左 directions [(0, -1), (1, 0), (0, 1), (-1, 0)] for dx, dy in directions: nx, ny x dx, y dy if self.is_valid(nx, ny): neighbors.append((nx, ny)) return neighbors def plot(self, pathNone, startNone, goalNone, exploredNone): 可视化网格地图可选绘制路径、起点、终点和探索过的区域。 fig, ax plt.subplots(figsize(8, 8)) # 绘制网格 for y in range(self.height): for x in range(self.width): color white if self.grid[y, x] 0 else black rect patches.Rectangle((x, self.height - 1 - y), 1, 1, linewidth0.5, edgecolorgray, facecolorcolor, alpha0.7) ax.add_patch(rect) # 高亮探索过的区域如果提供了 if explored is not None: for (ex, ey) in explored: rect patches.Rectangle((ex, self.height - 1 - ey), 1, 1, linewidth0, facecolorlightblue, alpha0.3) ax.add_patch(rect) # 绘制路径如果提供了 if path is not None: path_x [p[0] 0.5 for p in path] path_y [self.height - 1 - p[1] 0.5 for p in path] ax.plot(path_x, path_y, colorred, linewidth3, markero, markersize4, labelPath) # 标记起点和终点 if start is not None: sx, sy start ax.plot(sx 0.5, self.height - 1 - sy 0.5, gs, markersize12, labelStart) if goal is not None: gx, gy goal ax.plot(gx 0.5, self.height - 1 - gy 0.5, r*, markersize15, labelGoal) ax.set_xlim(0, self.width) ax.set_ylim(0, self.height) ax.set_xticks(np.arange(0, self.width 1, 1)) ax.set_yticks(np.arange(0, self.height 1, 1)) ax.grid(whichboth, colorgray, linestyle-, linewidth0.5) ax.legend(locupper right) ax.set_aspect(equal) plt.title(Path Planning Grid World) plt.show()提示在get_neighbors方法中我们采用了四连通上下左右的移动方式。你也可以轻松扩展为八连通包括对角线移动只需在directions列表中添加(1, 1), (1, -1), (-1, 1), (-1, -1)并注意对角线移动的成本通常是sqrt(2)。1.2 定义算法评估指标为了科学地对比算法我们需要定义几个关键的评估指标。这些指标将帮助我们量化算法的性能。指标描述意义路径长度从起点到终点的路径所经过的格子数或实际距离。衡量路径的最优性。长度越短通常路径质量越高。探索节点数算法在执行过程中访问探索过的节点总数。衡量算法的搜索效率。探索节点越少说明算法“盲目性”越低效率越高。运行时间算法从开始到找到路径所花费的CPU时间。衡量算法的计算效率对于实时系统尤为重要。内存使用算法运行过程中占用的最大内存通常与开放列表/关闭列表的大小相关。衡量算法的空间复杂度在资源受限的嵌入式系统中是关键考量。在接下来的实现中我们将重点关注路径长度和探索节点数因为它们是理解算法本质最直观的指标。运行时间会受硬件和实现细节影响但探索节点数更能反映算法逻辑本身的效率。2. Dijkstra算法实现稳扎稳打的“地毯式搜索”Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉于1956年提出是解决单源最短路径问题的经典算法。它的核心思想非常朴素从起点开始每次从未访问的节点中选择距离起点最近的那个节点进行访问并更新其邻居节点的距离。这个过程就像水波扩散一样一层一层地向外探索确保第一次到达某个节点时走过的路径就是最短路径。2.1 算法核心逻辑拆解Dijkstra算法之所以能保证找到最短路径依赖于一个关键前提所有边的权重代价必须为非负数。如果存在负权边水波扩散的“最近”原则就会被破坏算法可能得出错误结果。算法的执行需要维护几个关键数据结构distance字典记录从起点到每个节点的当前已知最短距离。初始时起点距离为0其他所有节点距离为无穷大inf。visited集合记录已经确定找到最短路径的节点。优先队列最小堆用于高效地获取当前距离起点最近的未访问节点。Python的heapq模块或queue.PriorityQueue非常适合这个角色。让我们来看具体的Python实现def dijkstra(grid_world, start, goal): 使用Dijkstra算法在网格世界中寻找从起点到终点的最短路径。 :param grid_world: GridWorld对象 :param start: 起点坐标 (x, y) :param goal: 终点坐标 (x, y) :return: 如果找到路径返回(路径列表, 探索过的节点集合)否则返回(None, 探索过的节点集合) # 初始化数据结构 distance {start: 0} visited set() # 使用优先队列元素为 (距离, x坐标, y坐标) pq [] heapq.heappush(pq, (0, start[0], start[1])) # 用于回溯重建路径 parent {start: None} # 记录所有探索过的节点用于可视化 explored_nodes set() explored_nodes.add(start) while pq: current_dist, x, y heapq.heappop(pq) current_node (x, y) # 如果这个节点已经通过更短的路径访问过则跳过 if current_node in visited: continue # 标记为已访问 visited.add(current_node) # 如果到达目标重建路径 if current_node goal: path [] node goal while node is not None: path.append(node) node parent[node] path.reverse() return path, explored_nodes # 探索当前节点的邻居 for neighbor in grid_world.get_neighbors(current_node): if neighbor in visited: continue # 计算从起点经过当前节点到邻居的新距离网格中每步代价为1 new_dist current_dist 1 # 假设移动一格代价为1 # 如果找到更短的路径则更新 if neighbor not in distance or new_dist distance[neighbor]: distance[neighbor] new_dist parent[neighbor] current_node heapq.heappush(pq, (new_dist, neighbor[0], neighbor[1])) explored_nodes.add(neighbor) # 队列为空仍未找到目标说明路径不存在 return None, explored_nodes2.2 代码逐行解析与实战演示让我们在一个10x10的网格中运行Dijkstra算法看看它的实际表现。# 创建网格世界并运行Dijkstra算法 world GridWorld(10, 10) start_pos (0, 0) goal_pos (9, 9) print(运行Dijkstra算法...) path_dijkstra, explored_dijkstra dijkstra(world, start_pos, goal_pos) if path_dijkstra: print(f找到路径路径长度{len(path_dijkstra)-1} 步) print(f探索过的节点数{len(explored_dijkstra)}) # 可视化结果 world.plot(pathpath_dijkstra, startstart_pos, goalgoal_pos, exploredexplored_dijkstra) else: print(未找到路径。) world.plot(startstart_pos, goalgoal_pos, exploredexplored_dijkstra)输出结果示例运行Dijkstra算法... 找到路径路径长度18 步 探索过的节点数78从可视化图中你可以清晰地看到蓝色区域算法探索过的所有节点。Dijkstra算法几乎探索了起点和终点之间的大部分可通行区域呈现出典型的“圆形”或“椭圆形”扩散模式。红色路径最终找到的最短路径。黑色格子障碍物。Dijkstra算法的特点总结完备性只要路径存在就一定能找到。最优性保证找到的是最短路径在非负权图中。缺点搜索是“盲目”的它均匀地向所有方向扩展没有利用终点位置的任何信息。因此在开阔或复杂地图中它会探索大量不必要的节点效率较低。它的时间复杂度为O((VE) log V)其中V是节点数E是边数。3. A*算法实现拥有“方向感”的智能搜索A算法A-Star是对Dijkstra算法的革命性改进。它由Peter Hart、Nils Nilsson和Bertram Raphael于1968年提出。A的核心思想是引入了一个启发式函数Heuristic Functionh(n)用于估计从当前节点n到目标节点的代价。算法在选择下一个要扩展的节点时不再只考虑从起点出发的实际代价g(n)而是综合考虑g(n)和h(n)即总代价f(n) g(n) h(n)。这个启发式函数就像给算法装上了“指南针”让它能朝着目标的大致方向优先搜索从而大幅减少探索范围。3.1 启发式函数A*算法的灵魂启发式函数h(n)的设计至关重要它必须满足两个条件可采纳性h(n)必须永远不大于从节点n到目标的实际代价。这意味着它不能“过度乐观”。一致性或单调性对于任意节点n及其后继节点n有h(n) ≤ cost(n, n) h(n)。这保证了路径估计不会出现矛盾。在网格世界中最常用的启发式函数是曼哈顿距离适用于只能上下左右移动四连通的网格。h(n) |x_n - x_goal| |y_n - y_goal|欧几里得距离适用于可以沿任意方向移动包括对角线的网格。h(n) sqrt((x_n - x_goal)^2 (y_n - y_goal)^2)切比雪夫距离适用于可以八方向移动的网格。h(n) max(|x_n - x_goal|, |y_n - y_goal|)我们选择曼哈顿距离作为示例实现。3.2 A*算法完整实现与对比A*算法的框架与Dijkstra非常相似主要区别在于优先队列的排序依据从g(n)变成了f(n)。def heuristic_manhattan(node, goal): 曼哈顿距离启发式函数。 return abs(node[0] - goal[0]) abs(node[1] - goal[1]) def a_star(grid_world, start, goal): 使用A*算法在网格世界中寻找从起点到终点的最短路径。 :param grid_world: GridWorld对象 :param start: 起点坐标 (x, y) :param goal: 终点坐标 (x, y) :return: 如果找到路径返回(路径列表, 探索过的节点集合)否则返回(None, 探索过的节点集合) # 初始化数据结构 g_score {start: 0} # 从起点到n的实际代价 f_score {start: heuristic_manhattan(start, goal)} # 估计的总代价 f(n) g(n) h(n) open_set [] heapq.heappush(open_set, (f_score[start], start[0], start[1])) parent {start: None} explored_nodes set() explored_nodes.add(start) while open_set: _, x, y heapq.heappop(open_set) current_node (x, y) # 如果到达目标重建路径 if current_node goal: path [] node goal while node is not None: path.append(node) node parent[node] path.reverse() return path, explored_nodes for neighbor in grid_world.get_neighbors(current_node): # 计算从起点经过当前节点到邻居的临时g值 tentative_g_score g_score[current_node] 1 # 移动代价为1 # 如果这条路径到邻居更好 if neighbor not in g_score or tentative_g_score g_score[neighbor]: # 更新父节点和g值 parent[neighbor] current_node g_score[neighbor] tentative_g_score # 计算新的f值 f_score[neighbor] g_score[neighbor] heuristic_manhattan(neighbor, goal) # 如果邻居不在开放列表中则加入 if neighbor not in [item[1:] for item in open_set]: # 简化检查实际应用需优化 heapq.heappush(open_set, (f_score[neighbor], neighbor[0], neighbor[1])) explored_nodes.add(neighbor) # 注意这里没有实现对开放列表中已有节点更新优先级的“decrease-key”操作。 # 一个简单的替代方法是允许重复加入但通过g_score的比较来过滤。对于教学示例这通常可以接受。 # 开放集为空未找到路径 return None, explored_nodes现在让我们在同一个地图上运行A*算法并与Dijkstra进行直观对比。print(\n运行A*算法...) path_astar, explored_astar a_star(world, start_pos, goal_pos) if path_astar: print(f找到路径路径长度{len(path_astar)-1} 步) print(f探索过的节点数{len(explored_astar)}) # 可视化A*结果 world.plot(pathpath_astar, startstart_pos, goalgoal_pos, exploredexplored_astar) else: print(未找到路径。) world.plot(startstart_pos, goalgoal_pos, exploredexplored_astar) # 对比分析 print(\n 算法对比 ) print(f地图大小: {world.width} x {world.height}) print(f起点: {start_pos}, 终点: {goal_pos}) print(- * 30) if path_dijkstra and path_astar: print(f{算法:10} {路径长度:10} {探索节点数:12} {效率提升}) print(f{Dijkstra:10} {len(path_dijkstra)-1:10} {len(explored_dijkstra):12} {-:10}) print(f{A*:10} {len(path_astar)-1:10} {len(explored_astar):12} f{(1 - len(explored_astar)/len(explored_dijkstra))*100:.1f}%) # 注意路径长度应该相同因为两者都找到了最短路径。 assert len(path_dijkstra) len(path_astar), 路径长度不一致 else: print(至少有一个算法未找到路径无法进行完整对比。)输出结果示例运行A*算法... 找到路径路径长度18 步 探索过的节点数42 算法对比 地图大小: 10 x 10 起点: (0, 0), 终点: (9, 9) ------------------------------ 算法 路径长度 探索节点数 效率提升 Dijkstra 18 78 - A* 18 42 46.2%3.3 深入解析A*的高效性从对比结果可以清晰地看到在找到相同长度的最短路径的前提下A算法探索的节点数42远少于Dijkstra算法78效率提升了46.2%。观察A算法的探索区域蓝色部分你会发现它不再是一个均匀扩散的圆形而是明显被“拉向”了目标点形成了一个更窄、更聚焦的搜索区域。为什么A*更快关键在于f(n) g(n) h(n)这个公式。g(n)确保我们不会偏离最短路径太远最优性而h(n)则引导搜索方向。在每次选择下一个要扩展的节点时A*总是优先扩展f(n)最小的节点也就是综合看来最有可能在最短路径上的节点。这避免了Dijkstra那种“广撒网”式的无效搜索。注意A算法的效率极度依赖于启发式函数h(n)的质量。一个完美的启发式函数h(n)等于实际代价会让A直接沿着最短路径搜索几乎不探索额外节点。而一个非常差的启发式函数例如h(n)0A*就会退化成Dijkstra算法。曼哈顿距离在网格世界中是一个很好的可采纳且一致的启发函数。4. 从算法到应用进阶场景与ROS衔接理解了Dijkstra和A*的核心后我们可以思考如何将它们应用到更复杂、更真实的场景中。路径规划从来不是孤立的算法它需要与感知、控制、系统框架紧密结合。4.1 应对复杂场景权重、动态障碍与算法变种我们之前的例子中移动一格的代价都是1。但在现实中代价可能是多样的地形代价草地、沙地、公路的通行代价不同。风险代价靠近障碍物或危险区域需要更高的代价。转向代价机器人转向比直行更耗能。这只需要修改算法中计算g(n)的部分将固定的“1”替换为从current_node到neighbor的边的实际权重即可。我们的GridWorld类可以扩展一个cost_map来存储每个格子的通行代价。对于动态障碍物比如突然出现的人或移动的物体Dijkstra和基础的A*就力不从心了因为它们假设环境是静态的。这时就需要用到增量式或实时规划算法D和 DLite**当环境发生小范围变化时它们能利用之前搜索的信息高效地重新规划路径而不是从头开始。这非常适合于机器人导航传感器发现新障碍物后能快速反应。LPA* (Lifelong Planning A*)另一种增量式A*性能在某些场景下优于D*。对于高维或连续空间如机械臂规划、无人机在三维空间飞行基于采样的规划器更受欢迎RRT (快速探索随机树) RRT*通过在空间中随机采样来构建一棵树能快速在高维空间中找到可行路径。RRT*是RRT的优化版本具有渐近最优性即随着采样点增加找到的路径会无限接近最优路径。它们不依赖于离散的网格更适合复杂的几何约束。算法选择速查表场景特点推荐算法关键考量静态网格地图求精确最短路径A* (首选) / DijkstraA*效率远高于Dijkstra。启发函数选曼哈顿或欧氏距离。图中存在负权边Bellman-Ford/ Floyd-WarshallDijkstra和A*不适用。需要计算图中所有点对最短路径Floyd-Warshall时间复杂度O(n³)仅适用于节点数不多的情况。环境动态变化需频繁重新规划DLite* /LPA*增量更新避免完全重算。D* Lite在机器人领域很流行。高维/连续空间只需可行解RRT搜索速度快但不保证最优。高维/连续空间需要高质量/最优路径RRT*比RRT慢但路径质量随迭代次数提高。4.2 在ROS机器人系统中集成路径规划机器人操作系统ROS是机器人开发的事实标准。在ROS中路径规划通常作为move_base功能包的一部分。move_base实现了全局规划器和局部规划器的架构全局规划器基于一张静态的或半静态的代价地图计算从起点到终点的全局路径。这正是A*算法大展身手的地方。ROS默认的全局规划器navfn和global_planner的核心就是A*的变种通常使用更高效的实现如A*或Dijkstra。局部规划器负责沿着全局路径结合实时传感器数据如激光雷达避开动态障碍物生成机器人实际执行的速度指令。常用算法如DWA动态窗口法、TEB时间弹性带等。如何将我们的A*算法融入ROS你不需要从头写一个规划器。更实际的做法是理解其接口并进行定制ROS的规划器被实现为plugin。你可以继承nav_core::BaseGlobalPlannerC接口或对应的Python接口实现自己的makePlan方法。在makePlan方法中你会接收到起点、终点的位姿以及一个costmap_2d::Costmap2D对象即代价地图。你的任务就是在这个代价地图上运行A*算法返回一个路径点序列。代价地图中的每个单元格有一个代价值0-255我们的A*算法中的移动代价g(n)就需要根据这个值来计算例如cost base_cost costmap_value。一个简化的工作流程如下# 伪代码ROS全局规划器插件中的A*核心 class MyAStarPlanner(BaseGlobalPlanner): def makePlan(self, start, goal, costmap): # 1. 将start, goal的位姿转换为costmap中的网格坐标 start_grid world_to_map(start, costmap) goal_grid world_to_map(goal, costmap) # 2. 初始化开放列表、关闭列表等 open_set PriorityQueue() g_score {} f_score {} came_from {} g_score[start_grid] 0 f_score[start_grid] heuristic(start_grid, goal_grid) open_set.put((f_score[start_grid], start_grid)) # 3. A*主循环需处理costmap的代价值 while not open_set.empty(): current open_set.get()[1] if current goal_grid: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current, costmap): # 关键从costmap读取该格子的代价 cell_cost costmap.getCost(neighbor) if cell_cost 255: # 致命障碍物 continue # 移动代价 基础代价 单元格代价 tentative_g g_score[current] 1 (cell_cost / 254.0) * SCALE_FACTOR if neighbor not in g_score or tentative_g g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g heuristic(neighbor, goal_grid) if neighbor not in closed_set: open_set.put((f_score[neighbor], neighbor)) return [] # 规划失败在实际项目中你可能还需要处理机器人的 footprint轮廓、路径平滑使用样条曲线或梯度下降法将网格路径优化为平滑曲线等问题。从理解A*的原理到能在ROS中调试一个自定义的全局规划器这中间需要大量的工程实践。但万变不离其宗核心依然是那个f(n) g(n) h(n)的公式以及围绕它构建的搜索逻辑。掌握Dijkstra和A*就拿到了进入路径规划世界大门的钥匙。它们不仅是许多高级算法的基础其思想——在保证最优性的前提下利用启发信息提升效率——也贯穿于整个优化与搜索领域。下次当你的游戏角色自动寻路或者看到物流仓库里的AGV小车穿梭时你会知道驱动它们的很可能就是这些简洁而强大的算法在默默工作。