上一篇讲了图搜索的基础框架——离散化、图的构建、搜索的基本流程。今天讲最经典的图搜索算法Dijkstra。Dijkstra算法是1956年荷兰计算机科学家Edsger Dijkstra提出的。距今快70年了但依然是很多规划算法的基础也是面试必考内容。面试时被问到说说你了解的图搜索算法Dijkstra是必答项。一、算法思想Dijkstra的目标很明确在带权图中找到从起点到所有其他节点的最短路径。注意是所有其他节点不只是某一个终点。核心思想是贪心扩展——每次从待处理集合中选出离起点最近的那个节点然后用它去更新邻居的距离。这个过程不断重复直到所有节点都被处理过。打个比方你站在一个城市的中心想知道去所有地方怎么走最快。Dijkstra的做法是先看1km内能到哪再看2km内能到哪3km、4km……像水波纹一样一圈一圈往外扩散。每扩散一圈就记录下到每个地方的最短距离。这个水波纹的比喻很关键——Dijkstra的搜索范围是一个以起点为圆心的圆随着距离增大而不断扩大。二、算法流程import heapq def dijkstra(graph, start): # dist[node] 从start到node的最短距离 dist {node: float(inf) for node in graph} dist[start] 0 # 优先队列(距离, 节点) pq [(0, start)] visited set() while pq: d, u heapq.heappop(pq) if u in visited: continue visited.add(u) for v, weight in graph[u]: new_dist dist[u] weight if new_dist dist[v]: dist[v] new_dist heapq.heappush(pq, (new_dist, v)) return dist代码的核心就三步从优先队列取出距离最小的节点遍历它的所有邻居如果经过当前节点到邻居的距离更短就更新来看一个具体例子。假设有一个5节点的图A --1-- B --2-- C | | | 4 3 1 | | | D --2-- E --3-- F从A出发Dijkstra的执行过程初始化dist[A]0其他inf处理A更新B1, D4处理B距离最小1更新C3, E4处理C距离3更新F4处理D或E距离4...最终得到从A到所有节点的最短距离。三、为什么Dijkstra能找到最短路径关键性质当Dijkstra把一个节点标记为已访问时它到起点的距离一定是最短的。为什么因为Dijkstra总是选距离最小的节点来扩展。如果存在一条更短的路径那条路径上的某个中间节点一定还没被访问否则早就更新了而那个中间节点的距离一定比当前节点小——矛盾。这个证明依赖一个前提边的权重非负。如果有负权边Dijkstra就不对了。不过机器人规划中边的权重代表距离或代价不可能为负所以不用担心。四、Dijkstra在机器人规划中的应用在机器人规划中Dijkstra的典型用法# 2D网格地图上的Dijkstra def dijkstra_grid(grid, start, goal): rows, cols len(grid), len(grid[0]) dist [[float(inf)] * cols for _ in range(rows)] dist[start[0]][start[1]] 0 pq [(0, start)] came_from {} while pq: d, (r, c) heapq.heappop(pq) if (r, c) goal: return reconstruct_path(came_from, goal) for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]: nr, nc r dr, c dc if 0 nr rows and 0 nc cols: if grid[nr][nc] 0: # 不是障碍物 cost d 1 if cost dist[nr][nc]: dist[nr][nc] cost came_from[(nr, nc)] (r, c) heapq.heappush(pq, (cost, (nr, nc))) return None工程上Dijkstra常用于计算距离场——每个格子到最近障碍物的距离。距离场在路径规划中很有用——规划器可以优先选择距离场值大的区域离障碍物远提高安全性。全局路径规划地图已知求最短路径作为A算法的基础A就是加了启发式的Dijkstra五、Dijkstra的局限性Dijkstra能保证找到最短路径但有一个明显的问题它不知道终点在哪。Dijkstra像水波纹一样均匀扩散不管终点在什么方向它都要把所有距离更近的节点都探索一遍。如果地图很大终点很远Dijkstra会探索大量无关的节点。举个例子100x100的网格起点在左下角终点在右上角。Dijkstra会探索大约半个网格5000个节点而实际上最短路径只需要走约140步。浪费了90%以上的计算量。另一个问题是内存占用。Dijkstra需要存储所有节点的距离值。对于大规模地图比如自动驾驶的高精地图节点数可能上亿内存消耗是个实际问题。这就是为什么需要A*——A*用启发式函数告诉搜索终点在哪个方向避免盲目扩散。后面会详细讲。六、面试实战QDijkstra和BFS有什么区别ABFS是Dijkstra在等权图上的特例。BFS用普通队列FIFODijkstra用优先队列。等权图中所有边权重相同优先队列退化成普通队列。换句话说BFS就是无权图版的Dijkstra。QDijkstra的时间复杂度是多少A用二叉堆优先队列实现O((VE)logV)。用斐波那契堆可以优化到O(VlogV E)但工程上很少用——斐波那契堆的常数因子太大实际反而更慢。对于稀疏图E≈V二叉堆版本已经够好了。QDijkstra能处理负权边吗A不能。负权边会导致已访问节点的距离被更新破坏Dijkstra的贪心策略。比如A→B权重3A→C权重5C→B权重-4。Dijkstra先处理B距离3但后来发现A→C→B距离只有1。负权边用Bellman-Ford算法时间复杂度O(VE)。QDijkstra和A*的关系是什么AA* Dijkstra 启发式函数。当h(n)0时A退化为Dijkstra。当h(n)等于真实代价时A只走最短路径但现实中不可能知道真实代价。工程上Dijkstra适合一对多的最短路径比如计算距离场A*适合一对一的路径规划。Q实际项目中你用过Dijkstra吗A用过。之前做仓储AGV时全局地图用Dijkstra计算距离场——每个格子到最近货架的距离。然后A*规划路径时把距离场作为额外的代价项让路径尽量远离货架。距离场只需要算一次地图不变时后续每次规划都能用。小结Dijkstra算法每次选距离起点最近的未访问节点用它更新邻居的距离。保证找到最短路径边权非负时。核心数据结构是优先队列最小堆。时间复杂度O((VE)logV)。Dijkstra的问题是盲目搜索——不知道终点方向均匀扩散。A*通过启发式函数解决了这个问题大幅减少搜索范围。下一篇讲A*算法——启发式搜索的原理和最优性证明。这是图搜索系列最重要的一篇。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第207篇 图搜索基础——状态空间离散化的思路下一篇预告第209篇 A*算法详解——启发式搜索的原理和最优性证明有任何问题欢迎评论区留言我会尽量回复。 第208篇 Dijkstra算法——最短路径的经典解法
上一篇讲了图搜索的基础框架——离散化、图的构建、搜索的基本流程。今天讲最经典的图搜索算法Dijkstra。Dijkstra算法是1956年荷兰计算机科学家Edsger Dijkstra提出的。距今快70年了但依然是很多规划算法的基础也是面试必考内容。面试时被问到说说你了解的图搜索算法Dijkstra是必答项。一、算法思想Dijkstra的目标很明确在带权图中找到从起点到所有其他节点的最短路径。注意是所有其他节点不只是某一个终点。核心思想是贪心扩展——每次从待处理集合中选出离起点最近的那个节点然后用它去更新邻居的距离。这个过程不断重复直到所有节点都被处理过。打个比方你站在一个城市的中心想知道去所有地方怎么走最快。Dijkstra的做法是先看1km内能到哪再看2km内能到哪3km、4km……像水波纹一样一圈一圈往外扩散。每扩散一圈就记录下到每个地方的最短距离。这个水波纹的比喻很关键——Dijkstra的搜索范围是一个以起点为圆心的圆随着距离增大而不断扩大。二、算法流程import heapq def dijkstra(graph, start): # dist[node] 从start到node的最短距离 dist {node: float(inf) for node in graph} dist[start] 0 # 优先队列(距离, 节点) pq [(0, start)] visited set() while pq: d, u heapq.heappop(pq) if u in visited: continue visited.add(u) for v, weight in graph[u]: new_dist dist[u] weight if new_dist dist[v]: dist[v] new_dist heapq.heappush(pq, (new_dist, v)) return dist代码的核心就三步从优先队列取出距离最小的节点遍历它的所有邻居如果经过当前节点到邻居的距离更短就更新来看一个具体例子。假设有一个5节点的图A --1-- B --2-- C | | | 4 3 1 | | | D --2-- E --3-- F从A出发Dijkstra的执行过程初始化dist[A]0其他inf处理A更新B1, D4处理B距离最小1更新C3, E4处理C距离3更新F4处理D或E距离4...最终得到从A到所有节点的最短距离。三、为什么Dijkstra能找到最短路径关键性质当Dijkstra把一个节点标记为已访问时它到起点的距离一定是最短的。为什么因为Dijkstra总是选距离最小的节点来扩展。如果存在一条更短的路径那条路径上的某个中间节点一定还没被访问否则早就更新了而那个中间节点的距离一定比当前节点小——矛盾。这个证明依赖一个前提边的权重非负。如果有负权边Dijkstra就不对了。不过机器人规划中边的权重代表距离或代价不可能为负所以不用担心。四、Dijkstra在机器人规划中的应用在机器人规划中Dijkstra的典型用法# 2D网格地图上的Dijkstra def dijkstra_grid(grid, start, goal): rows, cols len(grid), len(grid[0]) dist [[float(inf)] * cols for _ in range(rows)] dist[start[0]][start[1]] 0 pq [(0, start)] came_from {} while pq: d, (r, c) heapq.heappop(pq) if (r, c) goal: return reconstruct_path(came_from, goal) for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]: nr, nc r dr, c dc if 0 nr rows and 0 nc cols: if grid[nr][nc] 0: # 不是障碍物 cost d 1 if cost dist[nr][nc]: dist[nr][nc] cost came_from[(nr, nc)] (r, c) heapq.heappush(pq, (cost, (nr, nc))) return None工程上Dijkstra常用于计算距离场——每个格子到最近障碍物的距离。距离场在路径规划中很有用——规划器可以优先选择距离场值大的区域离障碍物远提高安全性。全局路径规划地图已知求最短路径作为A算法的基础A就是加了启发式的Dijkstra五、Dijkstra的局限性Dijkstra能保证找到最短路径但有一个明显的问题它不知道终点在哪。Dijkstra像水波纹一样均匀扩散不管终点在什么方向它都要把所有距离更近的节点都探索一遍。如果地图很大终点很远Dijkstra会探索大量无关的节点。举个例子100x100的网格起点在左下角终点在右上角。Dijkstra会探索大约半个网格5000个节点而实际上最短路径只需要走约140步。浪费了90%以上的计算量。另一个问题是内存占用。Dijkstra需要存储所有节点的距离值。对于大规模地图比如自动驾驶的高精地图节点数可能上亿内存消耗是个实际问题。这就是为什么需要A*——A*用启发式函数告诉搜索终点在哪个方向避免盲目扩散。后面会详细讲。六、面试实战QDijkstra和BFS有什么区别ABFS是Dijkstra在等权图上的特例。BFS用普通队列FIFODijkstra用优先队列。等权图中所有边权重相同优先队列退化成普通队列。换句话说BFS就是无权图版的Dijkstra。QDijkstra的时间复杂度是多少A用二叉堆优先队列实现O((VE)logV)。用斐波那契堆可以优化到O(VlogV E)但工程上很少用——斐波那契堆的常数因子太大实际反而更慢。对于稀疏图E≈V二叉堆版本已经够好了。QDijkstra能处理负权边吗A不能。负权边会导致已访问节点的距离被更新破坏Dijkstra的贪心策略。比如A→B权重3A→C权重5C→B权重-4。Dijkstra先处理B距离3但后来发现A→C→B距离只有1。负权边用Bellman-Ford算法时间复杂度O(VE)。QDijkstra和A*的关系是什么AA* Dijkstra 启发式函数。当h(n)0时A退化为Dijkstra。当h(n)等于真实代价时A只走最短路径但现实中不可能知道真实代价。工程上Dijkstra适合一对多的最短路径比如计算距离场A*适合一对一的路径规划。Q实际项目中你用过Dijkstra吗A用过。之前做仓储AGV时全局地图用Dijkstra计算距离场——每个格子到最近货架的距离。然后A*规划路径时把距离场作为额外的代价项让路径尽量远离货架。距离场只需要算一次地图不变时后续每次规划都能用。小结Dijkstra算法每次选距离起点最近的未访问节点用它更新邻居的距离。保证找到最短路径边权非负时。核心数据结构是优先队列最小堆。时间复杂度O((VE)logV)。Dijkstra的问题是盲目搜索——不知道终点方向均匀扩散。A*通过启发式函数解决了这个问题大幅减少搜索范围。下一篇讲A*算法——启发式搜索的原理和最优性证明。这是图搜索系列最重要的一篇。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第207篇 图搜索基础——状态空间离散化的思路下一篇预告第209篇 A*算法详解——启发式搜索的原理和最优性证明有任何问题欢迎评论区留言我会尽量回复。