路径规划算法详解：从BFS到D*
路径规划算法详解：从BFS到D*
路径规划算法是人工智能、机器人等领域的重要研究方向。本文详细介绍了几种常见的路径规划算法，包括广度优先搜索（BFS）、深度优先搜索（DFS）、Dijkstra算法、A算法和D算法。通过具体的示例和图解，帮助读者理解每种算法的原理和应用场景。
前言
路径规划，就是在地图上，从起点到终点规划出一条最优路径。
【最优】，就像我们看高德地图一样，上面显示的：最短路径，最短时间，红灯最少，不走高速，避开监控等等。
这是【最优】的几种情况。
如果放在室内机器人，或者别的情况，【最优】还可能包含路径平滑性等等。
那如何得到这些路径呢。
那最最简单的，从一个节点开始，把所有所有的路径都写出来，然后计算每个路径的长度，红绿灯数，是否有高速，曲率等等，然后再选出来。
但是很明显，这数据量会大到可怕，消耗的时间和资源都很难承受起。
于是就开始了路径规划算法的研究，也就是说所有的路径规划算法都是为了能够解决暴力搜索的效率低下和占用资源高的问题，同时适应动态坏境。
路径规划的算法有很多，这片文章主要讲其中六种，主要为了让大家更好的进行理解，不涉及到具体的程序实现。
这六种算法分别是：
1.广度优先搜索
2.深度优先搜索
3.Dijkstra
4.A*
5.D*
6.RRT
一、输入输出
在讲解具体算法前，先明确一下输入和输出。
1.输入
输入：环境、约束条件和目标。
环境
地图通常用三种方式表示：
1.图结构：
节点（顶点）：表示关键位置（如城市、路口、房间等）。
边：表示节点之间的连接路径，可能带有权重（如距离、时间或费用）。
示例：邻接矩阵、邻接表、边列表等。
比如：
邻接矩阵:
[
[0, 2, ∞],
[2, 0, 3],
[∞, 3, 0]
]
2.栅格地图
将环境划分为网格，每个网格包含通行性信息（如可通行/障碍物）。
0 0 1
0 1 0
0 0 0 （0表示可通行，1表示障碍）
3.连续空间表示
使用多边形、线段或函数描述环境边界和障碍物。
约束条件
最大路径长度，最长时间。
目标
起点和终点。
目标函数的优化目标，如最短路径、最少时间、最大安全性等。
其他
动态更新的环境变化，比如哪修路，哪突然不能走了等。
2.输出
输出主要有两部分：路径，路径代价
路径就是一堆离散的点。
路径代价就是每段路径的总距离，总时间，总红绿灯数等等。
二、广度优先搜索——BFS
广度优先搜索和深度优先搜索都是最基础的路径规划算法。
网上有很多动画，可以帮助大家快速理解。
在此简单说明一下，广度优先搜索就是一种层式遍历，从起点开始，一次访问下一层的节点，以此遍历下去，直至找到目标节点。
三、深度优先搜索——DFS
深度优先搜索就是先一条路走到底，然后再返回上一个节点，看还有没有别的路，再走到底，要是没路了，就再返回上一个节点，走别的路走到底。
先讲这两种最基础最简单的搜索，是为了给下面的几种算法做铺垫，可以说，
四、Dijkstra
Dijkstra也很好理解，它是一种层式贪心算法。
所适用的是权重图。
在实际的道路中，每两个点之间的道路都有距离，这个距离就可以作为权重，当然像是堵车程度，红绿灯数量，都可以作为权重。这就是权重图。
贪心算法大家经常听到，指的是把一个大问题拆成若干小问题，每次只考虑每个小问题最优。
放到路径规划中，就是从节点出发，遍历下一层的节点时，选择权重最小的节点。
借用抖音上的一张图：
从节点0开始，一层一层的遍历，并更新最近点路径，并记录已经到达的节点，比如说，到节点7，有直接到7和先到1再到7两种，一个权重8，一个权重15，那就将8的那条作为到点7的最优路径进行记录，以此类推，优先搜索权重小的那一条路径。
只要图的联通的，Dijkstra会将所有的节点都遍历一遍，找出到所有节点的最短路径。
五、A*
以上的三种算法，都有个共性：都是盲人在走迷宫。
也就是说，以上三种算法，都是不知道终点与起点的位置关系在走，所以只能一层一层，或者一根一根的依次把所有能走的路走一遍，然后撞到“终点”才停止。
很明显这种方式都很傻，因为我们明明是知道终点距离起点的位置和方向的。
所以，就有了A算法。
A算法，使用了启发式函数f(n)，来进行优化。
启发式函数包含两部分：
g(n)：从起点到当前节点的成本，也就是Dijkstra中的权重。
h(n)：从当前节点到终点的启发式估计成本。
细讲一下h(n)是怎么回事。
以距离最短为目标，起点到终点的距离表示，除了欧拉距离之外，还可以用曼哈顿距离表示，也就是两条直角边的和，入图：
h(n)就是当前节点距离终点的曼哈顿距离。
f(n)=g(n)+h(n)
每一步层式遍历后，都选取f(n)最小的节点，进行下一步遍历，直到h(n)=0.也就相当于走到了终点。
可以发现，A*算法可以说是在Dijkstra算法上进一步变种。
六、D*
上面的所有算法都有一个问题：就是无法适应动态环境，如果地图有变化，那就需要重新算一遍。
于是，就有了D算法。
D（Dynamic A*）算法又是A算法的改进版本。
D算法的核心目标是在动态环境中找到从起点到目标的最优路径，同时在环境发生变化时，通过局部更新而非全图重新计算路径，而不再需要重新从头到尾算一遍。
继续带着问题找答案。
为了实现路径的动态调整，D* 算法采用了一种“反向搜索+增量更新”的方式。
首先是【反向搜索】，顾名思义，与上面的算法都不同，D* 是从终点到起点的搜索。
【增量更新】，就是当当环境中某条边的权重发生变化（如障碍物阻挡路径），仅更新受影响节点的 rhs(n) 和 g(n)，并将这些节点重新加入开放集合。
哎？这里的rhs(n) 和 g(n)指的是什么呢？
上面说过D是A的改进版本，所以A的思想依然延续。
依然是计算总代价函数f(x)。
f(x)=min(g(n)，rhs(n))+h(n)
其中：
g(n)：从起点到节点n 的当前已知最小代价。
rhs(n)：从当前节点通过某条边到达邻居节点的估计代价。
h(n)：启发式估计代价，用于引导搜索。
双代价系统（g(n) 和 rhs(n)）：g(n)：实际路径代价，表示从起点到当前节点的最佳已知代价。
rhs(n)：备选代价，表示从当前节点通过某条边到目标节点的代价。
在每次节点扩展时，D会调整 g(n) 和 rhs(n) 的值，以维护路径最优性。
理解不了没关系，毕竟是干巴巴的概念，下面让我们举个栗子。
假设路径图如下：
A
/ \
1 5
/ \
B---2---C
| \ |
4 6 3
| \ |
D---3----E
\ /
2 1
\ /
F
数字表示每个节点之间的路径之间的权重。
目标：从 A 到 F 找最优路径。
解题过程如下：
1.初始路径规划（环境未变化）
1.所有节点初始化为g(x)=∞。
2.从目标节点 F 开始，设置g(F)=0。
3.反向计算邻居节点的g 值：
g(D)=g(F)+2=2
g(E)=g(F)+1=1
4.继续更新其余节点：
于是得到了从F-A的最优路径。
2.环境变化
假设环境发生变化，路径 B -> D 被障碍阻塞，权重从 4 变为 ∞，需要动态调整路径。
3.动态调整
1.受影响节点标记
B -> D 的权重变化影响节点 B 和 D。
B和D 被标记为受影响节点，加入开放集合（Open List）。
2.局部更新.
1.重新计算 g(D)：
新的 g(D)=min(g(F)+2,g(E)+3)=min(2,4)=2。
由于g(D) 未变化，停止对 D 的进一步更新。
2.重新计算 g(B)：
新的路径：g(B)=min(g(D)+4,g(E)+6)=min(∞,7)=7。
更新后，g(B) 的值变为 7。
3.重新计算 g(A)：
更新后，g(A)=min(g(B)+1,g©+5)=min(8,9)=8。
D*算法在局部更新路径的原理，就是当某段路径权重发生变化时，将影响的节点从起点到终点，重新进行路径规划，然后一个开放集合，一个封闭集合，分别放这些东西。
路径规划这部分的逻辑不难，感觉实现起来怎么存储路径，这几个集合翻腾起来反而是难度比较大的地方。
总结
以上算法是一些基础路径规划算法，核心逻辑都是BFS和DFS，除了这类算法之外，还有像RRT等规划算法，在之后的规划算法介绍中会逐一和大家解释。