运动规划之搜索算法：前端规划、后端轨迹生成到状态求解( 五 ) _算法

更新所有未扩展邻居“m”的累积成本g(m)

已经被扩展/访问的节点保证具有从起始状态到该节点的最小成本 :::success

维护一个优先队列来存储所有待扩展的节点

用起始状态XS初始化优先队列

设置g(XS)=0, 对图中的其他所有节点设置g(n)=无穷

循环:

如果队列为空,返回FALSE并退出循环

从优先队列中取出g(n)最小的节点“n”

将节点“n”标记为已扩展

如果节点“n”是目标状态,返回TRUE并退出循环

对节点“n”的所有未扩展的邻居节点“m”:

如果g(m) = 无穷

g(m)= g(n) + Cnm

将节点“m”加入队列

如果g(m) > g(n) + Cnm

g(m)= g(n) + Cnm

结束对邻居节点的循环

结束主循环 ::: BFS（Best-First-Search）算法
BFS（Best-First-Search）算法也是可以看作基于启发式的深度优先算法，其按照和Dijkstra类似的流程运行，不同的是它能够评估任意结点到目标点的代价（即启发式函数）。与选择离初始结点最近的结点不同的是，它选择离目标最近的结点。BFS不能保证找到一条最短路径。但是它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic ）能快速地导向目标结点。例如，如果目标位于出发点的南方，BFS将趋向于导向南方的路径。在下面的图中，越黄的结点代表越高的启发值（移动到目标的代价高），而越黑的结点代表越低的启发值（移动到目标的代价低）。这表明了与Dijkstra 算法相比，BFS运行得更快。

文章插图
然而，这两个例子都仅仅是最简单的情况——地图中没有障碍物，最短路径是直线的。现在我们来考虑前边描述的凹型障碍物。Dijkstra算法运行得较慢，但确实能保证找到一条最短路径：

文章插图
另一方面，BFS运行得较快，但是它找到的路径明显不是一条好的路径：

文章插图
由于BFS是基于贪心策略的，它试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。
结合两者的优点不是更好吗？1968年发明的A算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。有点不同的是，类似BFS的启发式方法经常给出一个近似解而不是保证最佳解。然而，尽管A基于无法保证最佳解的启发式方法，A却能保证找到一条最短路径。
A: 带有启发式函数的Dijkstra算法*
把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。在A的标准术语中，g(n)表示从初始结点到任意结点n的代价， h(n)表示从结点n到目标点的启发式评估代价（heuristic estimated cost）。当从初始点向目标点移动时，A* 权衡这两者。每次进行主循环时，它检查f(n)最小的结点n ，其中f(n) = g(n) + h(n) 。