[置顶] 无人驾驶汽车系统入门(十六)——最短路径搜索之A*算法

标签: 无人车 无人驾驶 路径规划 A*算法 最短路径搜索
75人阅读 评论(0) 收藏 举报
分类:

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

路线规划中一个很核心的问题即最短路径的搜索,说到最短路径的搜索我们就不得不提A*算法,虽然原始的A*算法属于离散路径搜索算法(我们的世界是连续的),但是其使用启发式搜索函数的理念却影响着我们后面会介绍的连续路径搜索算法,所以在介绍连续路径搜索算法之前,理解基本的A*算法是很有必要的,本节我们从广度优先算法出发,一步步改良算法直到引出A*算法。

最短路径搜索是通过算法找到一张图从起点(start)到终点(goal)之间的最短路径(path),为了简化,我们这里使用方格图(该图可以简单地用二维数组来表示),如下动图所示,其中这里写图片描述代表起点,这里写图片描述代表终点。

广度优先算法(Breadth-First-Search, BFS)

广度优先算法实际上已经能够找到最短路径,BFS通过一种从起点开始不断扩散的方式来遍历整个图。可以证明,只要从起点开始的扩散过程能够遍历到终点,那么起点和终点之间一定是连通的,因此他们之间至少存在一条路径,而由于BFS从中心开始呈放射状扩散的特点,它所找到的这一条路径就是最短路径,下图演示了BFS的扩散过程:

这里写图片描述
其中由全部蓝色方块组成的队列叫做frontier (参考下面的BFS代码)

然而,BFS搜索最短路径实在太慢了,为了提高BFS的搜索效率,接下来我们从BFS一步步改良到A*算法(其中的代码主要用于表达思路,距离实际运行还缺部分support code)

BFS代码:

frontier = Queue()
frontier.put(start)
visited = {}
visited[start] = True

while not frontier.empty():
   current = frontier.get()
   for next in graph.neighbors(current):
      if next not in visited:
         frontier.put(next)
         visited[next] = True

所涉及到的主要数据结构:

  1. graph,要找到一张图中两点之间的path,我们需要一个最基本的graph数据结构。在本文中,我们只需要得到某一点的邻近点,在这里我们的代码调用graph.neighbors(current),该函数返回点current周围的所有邻近点构成的一个列表,由for循环可以遍历这个列表

  2. queue 为了解释用队列的原因,请看下图,假设此时frontier为空,current当前是A点,它的neighbors将返回B、C、D、E四个点,在将这4个点都添加到frontier当中以后,下一轮while循环,frontier.get()将返回B点(根据FIFO原则,B点最早入队,应当最早出队),此时调用neighbors,返回A、f、g、h四个点,除了A点,其他3个点又被添加到frontier当中去。再到下一轮循环,此时frontier当中有C、D、f、g、h这几个点,由于队列的FIFO原则,frontier.get()将返回C点。这样就保证了整个扩散过程是由近到远,由内而外的,这也是广度优先搜索的原则。可以看到,frontier.get()从队列中取出一个元素(该元素将从队列中被删除)。而frontier.put()将current的邻近点又添加进去,整个过程不断重复,直到图中的所有点都被遍历一遍。

  3. visited列表:接着上面的讨论,graph.neighbors(A)将返回B、C、D、E 4个点,随后这4个点被添加到frontier当中,下一轮graph.neighbors(B)将返回A、h、f、g四个点,而加入此时A再被添加到frontier当中就导致遍历陷入死循环,为了避免这种状况出现,我们需要将已经遍历过了的点添加到visited列表当中,之后在将点放入frontier之前,首先判断该点是否已经在visited列表当中。
    这里写图片描述

下面的动图可以看到整个广度优先算法运行的详细过程:
这里写图片描述

其中绿色方框代表neighbors所返回的current点的邻近点,蓝色方块代表当前frontier队列中的点,由于队列先进先出(FIFO)的特点,越早加入队列的点的序号(图中方块的数字)越小,因此越早被while not frontier.empty()循环中的 current = frontier.get()遍历到

找到路线

现在我们的算法能够对所有的点进行遍历,也就意味着一定能够扩散到目标点,因此从开始到终止点的路径是存在的。为了生成这一路径,我们需要对扩散的过程进行记录,保存每一个点的来源(该点由哪一个点扩散而来),最后通过这些记录进行回溯即可得出完整的路径。将visited数组改为came_from。比如从A扩散到B,则came_from[B]=A。有了这样的线索,我们就能够从终点回溯到起点。

frontier = Queue()
frontier.put()
came_from = {}
came_from[start] = None

while not frontier.empty():
   current = frontier.get()
   for next in graph.neighbors(current):
      if next not in came_from:
         frontier.put(next)
         came_from[next] = current

下面的算法通过came_from数组来生成path的算法,其中goal表示终点。

current = goal
path = []
while current != start
   path.append(current)
   current = came_from[current]
path.append(start)
path.reverse()

效果如下图,其中每个方块上的箭头指向它的来源点,注意观察随着终点的变化,如何通过箭头的回溯得到完整路径。
这里写图片描述

提前结束

目前我们的算法会遍历整个图的每一个点,回想我们最初的目标:找到从起点到终点之间的路径,只需要遍历到终点即可。为了减少无用功,我们设置终止条件:一旦遍历到goal以后,通过break让整个算法停止。

frontier = Queue()
frontier.put(start)
came_from = {}
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal
      break

   for next in graph.neighbors(current):
      if next not in came_from:
         frontier.put(next)
         came_from[next] = current

扩散的方向性

到了这一步,整个BFS的思路已经完整了,但目前它的遍历方法仍然没有明确的目标,扩散朝着所有方向前进,十分蠢笨的遍历了以起点为中心的周围每一个方块,这不就是穷举吗?

在上面的算法运行过程中,frontier队列内部一般都会保持几个点(每次frontier.get()拿出来一个点,frontier.put()又放回去一个点)。而frontier.get()返回这些点中的哪一个决定了我们的扩散向着哪一个方向进行。之前这个函数只是根据queue默认的FIFO原则来进行,因此产生了辐射状的扩散方式,上文在介绍frontier的时候已经解释过这一点。

我们想到,能否让我们的扩散过程有侧重方向地进行呢? 注意,其实我们始终清楚地知道起始点和终止点的坐标,却浪费了这条有价值的信息。在frontier.get()返回了frontier几个点当中的一个,为了“有方向”地进行扩散,我们让frontier返回那个看似距离终点最近的点。由于我们使用的是方格图,每个点都有(x,y)坐标,通过两点的(x,y)坐标就可以计算它们之间的距离,这里采用曼哈顿距离算法:

def heuristic(a, b):
   # 这种距离叫做曼哈顿距离(Manhattan)
   return abs(a.x - b.x) + abs(a.y - b.y)

启发式的搜索

接下来我们改变原来队列的FIFO模式,给不同的点加入优先级,使用PriorityQueue,其中frontier.put(next,priority)的第二个参数越小,该点的优先级越高,可以知道,距离终点的曼哈顿距离越小的点,会越早从frontier.get()当中返回

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      if next not in came_from:
         priority = heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

下面就是启发式搜索的效果,unbelievable!
这里写图片描述

到这里是不是游戏就结束了? 这不就搞定啦,还要A*做什么? 且慢,请看下图中出现的新问题:
这里写图片描述

可以看到,虽然启发式搜索比BFS更快得出结果,但它所生成的路径并不是最优的,其中出现了一些绕弯路的状况。

从起点到终点总会存在多条路径,之前我们通过visited(后来用came_from) 数组来避免重复遍历同一个点,然而这导致了先入为主地将最早遍历路径当成最短路径。为了兼顾效率和最短路径,我们来看Dijkstra算法,这种算法的主要思想是从多条路径中选择最短的那一条:我们记录每个点从起点遍历到它所花费的当前最少长度,当我们通过另外一条路径再次遍历到这个点的时候,由于该点已经被遍历过了(已经加入了came_from数组),我们此时不再直接跳过该点,而是比较一下目前的路径是否比该点最初遍历的路径花费更少,如果是这样,那就将该点纳入到新的路径当中去(修改该点在came_from中的值)。下面的代码可以看到这种变化,我们通过维护cost_so_far记录每个点到起点的当前最短路径花费(长度),并将这里的cost作为该点在PriorityQueue中的优先级。

Dijkstra:

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + 1
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost
         frontier.put(next, priority)
         came_from[next] = current

一方面,我们需要算法有方向地进行扩散(启发式),另一方面我们需要得到尽可能最短的路径,因此A*就诞生了, 它结合了Dijkstra和启发式算法的优点,以从起点到该点的距离加上该点到终点的估计距离之和作为该点在Queue中的优先级,下面是A*算法的代码:

A*算法

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break

   for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + 1
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost + heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

下面的图展现了A*算法如何克服了启发式搜索遇到的问题:
这里写图片描述
这种A*算法用公式表示为: f(n)=g(n)+h(n),

也就指代这句代码:

priority = new_cost + heuristic(goal, next)

其中, f(n) 指当前n点的总代价(也就是priority,总代价越低,priority越小,优先级越高,越早被frontier.get()遍历到),g(n) 指new_cost,从起点到n点已知的代价,h(n) 是从n点到终点所需代价的估算。

查看评论

无人驾驶汽车系统入门(九)——神经网络基础

无人驾驶汽车系统入门(九)——神经网络基础 在上一节中,我们介绍了机器学习的相关基础,尤其是知道了监督学习的基本构成因素:数据,模型,策略和算法。在本节,我们具体学习一种监督学习算法——神经网...
  • AdamShan
  • AdamShan
  • 2018年01月08日 16:52
  • 918

无人驾驶汽车系统入门(四)——反馈控制入门,PID控制

前面几篇博客介绍了卡尔曼滤波的一些基本算法,其实目标追踪,定位,传感器融合还有很多问题要处理,这些我们在以后的系列博客中在进一步细讲,现在我想给大家介绍一下无人驾驶汽车系统开发中需要的控制相关的理论和...
  • AdamShan
  • AdamShan
  • 2017年11月06日 15:41
  • 1583

无人驾驶汽车系统入门(一)——卡尔曼滤波与目标追踪

前言:随着深度学习近几年来的突破性进展,无人驾驶汽车也在这些年开始不断向商用化推进。很显然,无人驾驶汽车已经不是遥不可及的“未来技术”了,未来10年必将成为一个巨大的市场。本系列博客将围绕当前使用的最...
  • AdamShan
  • AdamShan
  • 2017年10月16日 12:30
  • 4691

无人驾驶汽车的体系结构

无人驾驶汽车的体系结构
  • zhang_yin_liang
  • zhang_yin_liang
  • 2016年09月27日 19:57
  • 1943

无人驾驶汽车的基本原理

截止目前为止,谷歌的无人驾驶汽车在公路上已经行驶了100多万公里,没有出现任何事故。这是什么原因呢?难道汽车长了眼睛?老实说,无人驾驶比有人驾驶更加安全,因为,机器不会分心,会专心开车。 ...
  • yuanmeng001
  • yuanmeng001
  • 2015年03月28日 04:18
  • 3695

无人驾驶汽车系统入门(六)——基于传统计算机视觉的车道线检测(1)

无人驾驶汽车系统入门(六)——基于传统计算机视觉的车道线检测(1) 感知,作为无人驾驶汽车系统中的“眼睛”,是目前无人驾驶汽车量产和商用化的最大障碍之一(技术角度), 目前,高等级的无人驾驶...
  • AdamShan
  • AdamShan
  • 2017年12月04日 18:52
  • 2187

有关无人驾驶汽车的思考

近来,有幸了解了谷歌的一款面向世界的新产品——无人驾驶汽车,尽管它还没有面世,但就目前来看,它所带来的影响也是巨大的,现在,我就来谈谈我对这个无人驾驶汽车的未来大一些想法。   无人驾驶汽车,顾名...
  • p641290710
  • p641290710
  • 2014年05月27日 18:45
  • 507

无人驾驶汽车的基本概念

体系结构、环境感知、定位导航、路径规划、运动控制、一体化设计 目前无人驾驶汽车主要关键技术包括:车对车通信、巡航控制、自动刹车、车道维持、雷达、循迹或稳定控制、视讯摄影机、位置估计器、GPS。...
  • zhang_yin_liang
  • zhang_yin_liang
  • 2016年07月24日 19:42
  • 2032

【无人车研究】A*算法实现路径规划

算法过程详解:http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx    由于所构建的2D平面地图像素大小对于A*算法来说过于...
  • LiangYongxin
  • LiangYongxin
  • 2016年12月04日 20:12
  • 1680

【智能驾驶】如何制作一辆真正的无人驾驶汽车

联车科技创始人、CEO张成 就像霍金先生,身体被局限在小小的轮椅中,即便拥有全世界最聪明的大脑,却无法控制自己的行动。这一点反映到汽车当中,我们就会发现对汽车的控制特别重...
  • np4rHI455vg29y2
  • np4rHI455vg29y2
  • 2017年12月04日 00:00
  • 1229
    个人资料
    持之以恒
    等级:
    访问量: 2万+
    积分: 455
    排名: 11万+
    文章分类
    最新评论