基于图搜索的路径规划方法

    ps:本文的相关图片来自与深蓝学院的课件。

图搜索的基本概念

    Workspace:现实空间。

    配置空间:机器人表示为一个点,障碍物表示为无法达到的点。

图片1.jpg

    下面是不同的图的形式:抽象图、无向图、带权重的图、有向图。

图片2.jpg

    对于一个搜索问题,都对应一个状态空间图,图中节点之间的连接性由有向或无向边表示。如下图左的栅格地图就是以每个栅格为节点而构建一个搜索图,下图右的采样地图需要人为构建一个图:

图片3.jpg

    对图进行搜索可以得到一个搜索树:

图片4.jpg

    对于多数搜索问题,一般无法构建整个树,而仅需要达到目标节点即可。

图搜索算法的框架

    图片5.jpg

    循环的结束条件:当待访问节点容器为空时。为了防止循环访问,需要将访问过的节点从容器中删除,并后面不再放回。拓展节点的规则必须被设计的很好以便能够尽可能快的到达目标。


广度优先搜索(BFS)和深度优先搜索(DFS)

    BFS使用先进先出(FIFO)的容器,比如队列,进行节点的访问:

图片6.jpg

    基于FIFO容器访问节点的实例如下:

图片7.jpg

    DFS使用后进先出(LIFO)的容器,比如栈,进行节点的访问:

图片8.jpg

    基于LIFO容器访问节点的实例:

图片9.jpg

    下面是BFS和DFS两种算法进行路径规划的情况:

图片10.jpg


启发式搜索:贪心算法

    BFS和DFS在访问节点的顺序是先进或后进的,而启发式算法访问节点的顺序是根据某些规则来访问节点的,因此称为启发式算法。比如贪心算法,贪心算法根据规则(欧式距离或曼哈顿距离)来猜测节点距离目标点的远近,从而给出一个路径的方向。

    在没有障碍物的情况下贪心算法和BFS的对比:

图片11.jpg

    而在另一些情况下的对比:

图片12.jpg


Dijkstra算法

    Dijkstra和贪心算法、BFS的不同之处在于访问相邻节点的顺序,Dijkstra根据最小累计代价g(n)来确定要先访问的节点。g(n)即从起点到当前点n的最小累计代价。如果相邻节点n已被访问过,但是从当前节点到该节点计算的g(n)小于它本来计算的g(n),则更新该相邻节点的g(n)值。

    Dijkstra的算法流程如下:

图片13.jpg

    因此Dijkstra需要使用优先级队列来作为容器,比如C++的multimap。示例如下:

图片14.jpg

    Dijkstra的优点:完备的,即如果有解肯定可以找到;找到的解肯定是最优的。

    缺点:仅可以看到累计代价,因此拓展节点的方向比较随便;没有关于全局的位置信息。 因此它搜索的节点数量会较多。


    结合Dijstra和贪心算法的优点,就得到了A*算法。

A*算法

    A*算法与Dijkstra算法整体流程基本一致,只是A*算法在拓展节点时不仅考虑累计代价g(n),同时考虑一个从拓展节点到终点的启发式代价,比如欧式距离。因此拓展节点时对每个邻节点计算f(n)=g(n)+h(n),选择最小f(n)的邻节点优先拓展。A*算法的算法流程如下:

图片15.jpg

    A*算法的实例如下:

图片16.jpg

    需要注意的是,必须使得设计的启发式代价小于等于实际到目标的代价,因此设计的启发式算法必须Admissible,即h(n)<=h*(n),h*(n)是从节点n到目标节点的真实最小代价。比如欧式距离就是Admissible的,还有h(n)=0也是。


Dijkstra和A*的对比

    Dijkstra算法在各个方向进行拓展,因此可以保证找到全局最优;而A*算法主要朝着目标的方向进行拓展,因此无法保证能找到全局最优路径,但是计算量大大减小。


Weighted A*

    对h(n)进行加权,即f(n)=g(n)+epsilon*h(n),可以提高或减小启发式代价的权重。权重一般大于1,可以增加启发式代价的权重,可以使得A*更加贪心,因此计算量更小,但是找到最优解的可能性变小。

    在此基础上还有各种算法,包括Anytime A*,ARA*,D*等算法。


启发式函数的选择

    假设机器人可以沿着八个方向进行运动,在使用欧式距离作为启发式代价的情况下,路径的拓展节点的范围如下:

图片17.jpg

    拓展了非常多的节点,表示这个算法的低效。原因是机器人只能沿着八个方向运动,导致节点n到终点的实际代价h*(n),比启发式代价即欧式距离大很多,即两个代价非常的不“紧贴(tight)”。因此需要找一个跟h*(n)更加接近的启发式函数。实际上我们可以设计一个更加tight的启发式函数,称为对角线启发式距离(Diagonal Heuristic)。

    下面是使用Diagonal Heuristic和欧式距离的结果对比:

图片18.jpg


Tie Breaker

    对于基于栅格地图的图搜索问题来说,存在很多相同f(n)的路径,导致A*会拓展这些节点,导致计算资源的浪费。Tie Breaker的核心思想是在多个相同f(n)的路径中,根据某种倾向性来只选择一条进行拓展。

    第一种方法是轻微修改h(n)去打破不同路径的平衡性,即使得本来相同f(n)略微不同。比如h修改为:

    h=h*(1.0+p)

    p<(minimum cost of one step)/(expected maximum path cost)

    对h进行轻微放大可以减小这个问题,但同时也会影响h(n)的admissibility,虽然影响不大。下面是A*和带Tie Breaker的A*的比较:

图片19.jpg

    第二种方法是对每个坐标预先设置一个随机数来打破这种平衡,这需要提前构建一个坐标的哈希表。

    第三种方法是显示给出一种方向倾向性,比如沿着到终点方向前进。比如可以增加一个额外的代价cross,表示节点n离起点到终点直线路径的偏移量,如下所示:

dx1=abs(node.x-goal.x)

dy1=abs(node.y-goal.y)

dx2=abs(start.x-goal.x)

dy2=abs(start.y-goal.y)

cross=abs(dx1*dy2-dx2*dy1)

h=h+cross*0.001


JPS算法

    JPS直接忽略掉路径中的对称性,只选择其中一条路径。JPS定义的拓展规则如下:

图片20.jpg

    白色节点称为自然邻节点,灰色节点称为较差邻节点。设计原则:如果邻节点可以通过x节点的父节点直接达到,并且花费的代价小于等于经过x节点达到的代价,那么该邻节点就无必要通过x节点到达。比如上图左的1号节点可以从x的父节点直接达到且代价为1,则x无需向1进行拓展。2号节点和3号节点同理。

    下面是存在障碍物的情况,黑色节点是障碍物节点,红色节点称为强制邻节点。

图片21.jpg

    下面是直线跳跃和对角跳跃的实例:

图片22.jpg

    对于直线跳越它会一直前进知道遇到force邻节点;对于对角线跳跃,它会在每个节点先进行直线跳跃,包括水平和竖直,然后再进行对角跳跃。

    满足条件:1.节点 x 是起点/终点;2.节点 x 至少有一个强制邻节点;3.父节点在斜方向(意味着这是斜向搜索),节点x的水平或垂直方向上有满足条件1,2的点,称为跳点。

    下面是一个更具体的例子:

图片23.jpg

    从起点开始,首先沿着水平和垂直方向进行跳跃搜索,当遇到障碍物时结束搜索,若遇到强制邻节点则加入 openlist;然后再斜对角跳跃,直到遇到跳点。下面看到,当向右拓展时,发现了一个强制邻节点,将该邻节点放入open list中。

图片24.jpg

    JPS的算法流程总体与A*差不多,不同之处在于拓展节点的方法:

图片25.jpg

    1.openlist取一个权值最低的节点,然后开始搜索。(这些和A*是一样的)。

    2.搜索时,先进行 直线搜索(4/8个方向,跳跃搜索),然后再 斜向搜索(4个方向,只搜索一步)。如果期间某个方向搜索到跳点或者碰到障碍(或边界),则当前方向完成搜索,若有搜到跳点就添加进openlist。跳跃搜索是指沿直线方向一直搜下去(可能会搜到很多格),直到搜到跳点或者障碍(边界)。一开始从起点搜索,会有4个直线方向(上下左右),要是4个斜方向都前进了一步,此时直线方向会有8个。

    3.若斜方向没完成搜索,则斜方向前进一步,重复上述过程。因为直线方向是跳跃式搜索,所以总是能完成搜索。

    4.若所有方向已完成搜索,则认为当前节点搜索完毕,将当前节点移除于openlist,加入closelist。

    5.重复取openlist权值最低节点搜索,直到openlist为空或者找到终点。

然而JPS并不总是表现的很好,当整个地图中的障碍物区域很小时,JPS的性能可能弱于A*;而在复杂障碍物环境中,JPS的性能则远胜于A*。


代码实现

    A*在ROS中的实现:https://github.com/chenjianqu/Motion-Plan 



上一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于