[运动规划算法]基于图搜索的规划算法

---

1. Dijkstra

Dijkstra是一种基于贪心策略的最短路径算法，核心思想：设置两个点的集合S(n) 和 T(n)。集合S(n)中存放以找到的最短路径的点，T(n)集合中存放当前还未找到的最短路径的结点。初始状态时，集合S(n)中中只包含起始点，然后不断地从集合中选择到起始结点路径长度最短的结点加入集合S(n)中，集合S(n)中每加入一个新的结点，都要修改从起始点路径长到集合T(n)中剩余结点的当前最短路径长度值，集合S(n)中各结点新的当前最短路径路径长度值为原来最短路径长度值与起始点过新加入结点到达该结点的路径长度中的较小者，不断重复该过程，直到集合中所有结点全部加入集合中为止。
优点：完备的，保证找到最优解。
缺点：不带启发式，盲目呈同心圆扩散，在地图较小的情况下能得到较好的规划结果，但地图数据量较大时很难满足路径规划的实时性要求。
伪代码

2. A*算法

A*算法是建立在Dijkstra算法基础上的启发式搜索算法。
算法步骤：

1. 建立空的OPEN表和CLOSE表，将起点S放入OPEN表，CLOSE表为空，此时其他结点与起点S的距离为∞；
2. 如果OPEN表为空，则搜索失败，算法解散。否则扩展S结点，选取OPEN表中f’值最小的结点，并将该结点从OPEN表移至CLOSE表中，同时判断该结点是否为目标结点。若是目标结点，则从该结点回溯，即从该结点的后向指针一直到初始结点遍历结点获取最优路径，算法结束；若该结点不是目标结点，则继续扩展下一个结点；
3. 依次扩展S结点后，扩展S结点的所有后续结点组成集合A，遍历结合A中的结点。如果存在某一结点既不在OPEN比和CLOSE表中