自动驾驶路径规划：A*（Astar）算法

1. 最佳优先搜索（Best-First Search）

最佳优先搜索（BFS），又称A算法，是一种启发式搜索算法(Heuristic Algorithm)。[不是广度优先搜索算法（ Breadth First Search , BFS )]BFS算法在广度优先搜索的基础上，用启发估价函数对将要被遍历到的点进行估价，然后选择代价小的进行遍历，直到找到目标节点或者遍历完所有点，算法结束。要实现最佳优先搜索我们必须使用一个优先队列（priority queue）来实现，通常采用一个open优先队列和一个closed集，open优先队列用来储存还没有遍历将要遍历的节点，而closed集用来储存已经被遍历过的节点。1.1 最佳优先搜索的过程最佳优先搜索的过程可以被描述为：1、将根节点放入优先队列open中。2、从优先队列中取出优先级最高的节点X。3、根据节点X生成子节点Y:X的子节点Y不在open队列或者closed中，由估价函数计算出估价值，放入open队列中。X的子节点Y在open队列中，且估价值优于open队列中的子节点Y，将open队列中的子节点Y的估价值替换成新的估价值并按优先值排序。X的子节点Y在closed集中，且估价值优于closed集中的子节点Y，将closed集中的子节点Y移除，并将子节点Y加入open优先队列。4、将节点X放入closed集中。5、重复过程2,3,4直到目标节点找到，或者open为空，程序结束。BFS不能保证找到一条最短路径。然而，它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic function）快速地导向目标结点。这篇博客对BFS进行了详细的介绍用的是罗马尼亚度假问题https://blog.csdn.net/weixin_46582817/article/details/115217489   

2. A-Star算法

1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方如Dijsktra算法结合在一起的算法。A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。•和Dijkstra一样，A*能用于搜索最短路径。•和BFS一样，A*能用启发式函数引导它自己。左图为Astar算法效果图，右图为Dijkstra算法效果图Astar算法与Dijkstra算法的效果差不多，但Astar算法访问的节点数明显比Dijkstra算法少得多，说明其速度更快，运行时间更短。 2.1 Astar算法所属分类 A*算法在最短路径搜索算法中分类为：•直接搜索算法：直接在实际地图上进行搜索，不经过任何预处理；•启发式算法：通过启发函数引导算法的搜索方向；•静态图搜索算法：被搜索的图的权值不随时间变化（后被证明同样可以适用于动态图的搜索 ） 2.2 Astar算法基本概念 A*算法启发函数表示为：f(n)=g(n)+h(n)•f(n) 是从初始状态经由状态n到目标状态的代价估计•g(n) 是在状态空间中从初始状态到状态n的实际代价•h(n) 是从状态n到目标状态的最佳路径的估计代价（对于路径搜索问题，状态就是图中的节点，代价就是距离）Astar算法是启发式搜索算法，启发式搜索是在状态空间中对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。启发函数（Heuristics Function）（估价函数）：H为启发函数，也被认为是一种试探。由于在找到唯一路径前，不确定在前面会出现什么障碍物，计算H的算法（例如，H可采用传统的曼哈顿距离）具体根据实际场景决定父节点（parent）：在路径规划中用于回溯的节点。搜索区域（The Search Area）：前面图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。开放列表(Open List)：将路径规划过程中待检测的节点存放于Open List中。关闭列表(Close List)：将路径规划过程中已经检查过的节点放在Close List。 2.3 启发函数单调性的推导 单调性：单调性是Astar算法非常重要的一个性质，它决定了在用Astar算法进行路径搜索时，一定能找到一条最优路径。设父节点的坐标为(x0,y0)，它的任意一个子节点的坐标为(xi,yi)，所以对两者之间的h(x)，就一定满足 在Astar算法的运行过程中，后继的f(x)值时大于当前f(x)的值，即f(x)在之后对子节点的搜索扩展是单调递增的，不会像人工势场法一样出现局部极小值，因此使用Astar算法规划出的路径一定是最优路径.2.4 设计代价函数时所需注意的点在使用A*算法的过程中，启发代价的值必须尽量接近于真实值（尽量选取能取到的最大值，这样可以提升搜索效率），以保证规划出的路径尽可能地与实际地图环境相符合。根据所需的模型选择不同的启发代价的计算方法时，同样必须保证启发代价所得的估计值小于真实值。 2.5 代价函数的选择 2.5.1 曼哈顿距离 曼哈顿距离函数在标准坐标系中，表示起始和目标两节点的绝对值总和，其估计值就是从当前点做水平和垂直运动，到达目标点的成本的估计，因此，曼哈顿距离函数中，两点的距离如下K——相邻两节点之间的距离，即单位距离；
(x1,y1)——起始节点的坐标;
(x2,y2)——目标节点的坐标; 2.5.2 欧几里得距离 欧几里得距离又称为欧式距离，它是衡量两点之间距离远近的最常用的方法之一。欧几里得距离函数的值可以直接看作起始节点和目标节点，在欧式空间中的直线距离，其表达式如下所示K——相邻两节点之间的距离，即单位距离；
(xi,yi)——当前节点的坐标;
(xk,yk)——目标节点的坐标;欧几里德距离函数作为启发代价的计算方法时，虽然寻径时搜索空间增加从而导致搜索效率的降低，但其所得的估计值最小；而在使用曼哈顿距离函数时，虽然寻径需要遍历的栅格空间少于欧几里得距离函数，搜索效率较高，但这种方法得到的估计值与欧几里得距离函数相比，距离真实值更远。 2.6 确定最终路径 已经确定了目标节点的坐标为，根据此目标节点的位置，和先前设置的方向存储函数之中的方向，从目标节点利用方向反推至起始节点。具体实现的示意图如下 2.7 路径平滑 我们需要对规划出的路径进行平滑处理，将路径的转折处转化为平滑的曲线，使之更加符合无人车的实际运动路径。主要有基于B样条曲线的路径优化方法，基于Dubins圆的路径优化方法，和基于Clothoid曲线的路径优化方法，基于贝塞尔曲线的路径平滑算法。