路径规划算法_路径规划金典算法汇总

Path Planning

第1节 基于图搜索的路径规划算法 

Dijkstra算法、A*算法、JPS(Jump Point Search)算法

1、 基本的图遍历算法---BFS and DFS 

1.DFS------remove or expand the deepest node in the container.(因为它维护的是一个堆栈，所以对于图中的节点来说是后进先出)

2.BFS------remove or expand the shallowest node in the container.(因为它维护的是一个队列，所以对于图中的节点来说是先进先出)

https://cs.stanford.edu/people/abisee/tutorial/bfsdfs.html

通过上面我们可以发现广度优先搜索找到的路径一定是最优的，而对于深度优先搜索算法来说它所表现出来的并不符合我们对路径搜索的需求。它确实可以找到一条路径，但是这条路径不是最优的。

3.Heuristic search:A heuristic is a guess of how close you are to the target.

①启发式搜索是一个对目标点goal的一个正确引导。它可以帮助算法朝着目标点而去。

②启发式搜索应该是容易计算的。(欧式距离、曼哈顿距离等)

                  

那么带有启发式的搜索算法就一定能帮助我们更快的更优的路径吗？

https://cs.stanford.edu/people/abisee/tutorial/greedy.html

有障碍物时陷入局部最优！！！由这里我们可以看到，贪心算法可能不是我们想要的，但是它有许多我们借鉴的性质。后面在A*算法里面我们会说。

在前面我们提到的BFS和DFS虽然他们都能找到起始点到目标点的一条路径，但是前提图里面的每条边的weight是均匀的。但是对于广义的机器人路径搜索问题来说，它的每条边的weight是不一样的。所以我必须用别的方法来解决从起始点到目标点的代价最小的路径这个问题。

二、Dijkstra算法

Strategy：

*g(n):The current best estimates of the accumulated cost from the start to the node “n”.

*Update the accumulated costs g(m) for all unexpanded neighbors “m” of node “n”;

*A node that has been expanded/visited is guaranteed to have the smallest cost from the start state. 

Dijkstra算法的伪代码：

✱Maintain a priority queue to store all the nodes to be expended.

✱The priority queue is initialized with the start state Xs.

✱Assign g(Xs)=0,and g(n)=infinite for all other nodes in the graph.   

✱Loop

✱if the queue is empty,return false;break.

✱Remove the node “n”with the lowest g(n) from the priority queue.

✱Mark node “n”as expended.

✱If the node “n”is the goal state,return true;break.

✱For all unexpanded neighbors “m”of node “n”

✱if g(m)=infinite

✱g(m)=g(n)+Cnm

✱p