无人驾驶汽车系统入门——最短路径搜索之A*算法

最短路径搜索是通过算法找到一张图从起点(start)到终点(goal)之间的最短路径(path),为了简化,我们这里使用方格图(该图可以简单地用二维数组来表示),如下动图所示,其中代表起点,代表终点。广度优先算法实际上已经能够找到最短路径,BFS通过一种从起点开始不断扩散的方式来遍历整个图。可以证明,只要从起点开始的扩散过程能够遍历到终点,那么起点和终点之间一定是连通的,因此他们之间至少存在一条路径,而由于BFS从中心开始呈放射状扩散的特点,它所找到的这一条路径就是最短路径,下图演示了BFS的扩散过程:其中由全部蓝色方块组成的队列叫做frontier(参考下面的BFS代码)