自动驾驶路径规划之图搜索算法
一、Dijkstra 算法
Dijkstra 算法是一种经典的基于图搜索的最短路径搜索算法，由荷兰计算机科学家艾兹格·迪科斯彻于 1959 年提出。Dijkstra 算法的特点是基于贪心策略在每一次迭代中选择距离起点最近的结点作为下一个中转点，故在给定起点和终点的情况下，能够得到从起点到终点的最短可通行路径。其算法过程如下：
1.初始化：将起点设置为当前结点，将起点到自己的距离设置为 0，将其他结点的距离设置为无穷大（表示未知）。
2.遍历所有结点：对于当前结点，计算它到每一个邻接结点的代价之和，如果其中某个代价小于对应邻接结点的代价，则更新该邻接结点的代价。例如，如果当前结点到其某一个邻接结点的代价是 d，而当前结点的代价是 d'，如果 d+d'小于该邻接结点的代价，则更新邻接结点的代价为 d+d'。
3.标记已访问结点：将当前结点标记为已访问，从未访问的结点中选择代价最小的结点作为下一个当前结点。
4.重复步骤 2 和 3，直到终点被标记为已访问或没有可访问的结点。在 MATLAB 中使用 Robotic Tool Box 工具箱构建一个 30*30 的地图，并设置起点于终点，使用 Dijkstra 算法搜寻最短路径，绿色方块为起点，红色方块为终点，黄色区域表示已搜寻的点，蓝色方块表示待搜寻的点，青色方块构成的连线即为搜索的路径。
图 1 Dijkstra 算法路径规划
从图 1 中可以看到，利用 Dijkstra 算法，可以产生一条可行的路径。 Dijkstra 算法的优势是，它所产生的路径是最短的，而且算法实现简单，但该算法由于需要实现知道每个结点到起点代价故只适用于静态图的路径规划，且算法的时间复杂度高，由于缺少起点到终点的启发式信息，大量的时间被浪费在了不必要的搜索上，故实际应用中常与一些优化方法结合使用。
二、A star 算法
Astar 算法是最常用的静态路径搜索算法之一，最初由 Stanford 研究院的 Peter Hart, Nils Nilsson和 Bertram Raphael 于 1968 年发表。讲述 A star 算法前需要提前阐述贪婪算法，贪婪算法利用当前点到目标点之间的距离，每次迭代选取代价最小，即距离终点最近的点作为下一次迭代的点，这样的做可以大大加快路径的搜索速度，但存在陷入局部最优解的问题。Astar 算法综合了 Dijkstra 算法和贪婪算法的特点，其在 Dijkstra 算法的基础上引入了贪婪思想，采用了一种启发式函数，用以缩
小搜索范围，提高搜索的目的性，其代价函数为：
其中：
由上式可知， g(n) 与 h(n) 在中的占比决定了 A star 算法的性能，假如 g(n) 占比更大，那么 A star 算法就更关注起点到当前点的代价，即表现得更像 Dijkstra；反之若h(n) 占比更大，则 Astar 算法就更关注当前点到目标点的估计代价，即表现得更像贪婪算法。可以看出 Astar 算法搜索 的目的性来源于启发式函数h(n) ，故选择一个合适的启发式函数对于 Astar 算法而言尤为重要，常见的启发式函数有：
在 Mtalab 中使用与上文 Dijkstra 算法相同的地图，启发式函数选择曼哈顿距离，使用 Astar 算法进行路径搜索，青色方块的连线即为生成的路径。
从图 2 可以看出，A star 算法的已搜索区域较之 Dijkstra 算法要小很多，并且也成功找到了可通行的最短路径。Astar 算法的优势在于综合考虑了实际代价和估计代价，能够准确、快速的找到最短路径，并且实现简单。然而 A star 的启发式搜索依赖于启发函数，若启发函数估计不准，则可能陷入局部最优解甚至导致错误的路径，并且 A star 算法仍然与 Dijkstra 算法一样逐一遍历地图中的每一个栅格，搜索效率受限，实际应用中常常使用 A star 算法的变种。
