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

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

图搜索的基本概念

Workspace：现实空间。

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

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

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

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

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

图搜索算法的框架

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

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

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

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

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

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

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

启发式搜索：贪心算法

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

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

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

Dijkstra算法

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

Dijkstra的算法流程如下：

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

Dijkstra的优点：完备的，即如果有解肯定可以找到；找到的解肯定是最优的。

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

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

A*算法

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

A*算法的实例如下：

需要注意的是，必须使得设计的启发式代价小于等于实际到目标的代价，因此设计的启发式算法必须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*等算法。

启发式函数的选择

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

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

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

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*的比较：

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

第三种方法是显示给出一种方向倾向性，比如沿着到终点方向前进。比如可以增加一个额外的代价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定义的拓展规则如下：

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

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

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

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

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

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

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

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

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

基于图搜索的路径规划方法相关推荐

【自动驾驶】基于采样的路径规划算法——PRM（含python实现）
文章目录参考资料 1. 基本概念 1.1 基于随机采样的路径规划算法 1.2 概率路图算法(Probabilistic Road Map, PRM) 1.3 PRM算法的优缺点 1.4 PRM算法伪 ...
对Probabilistic Road Map(PRM)概率路图路径规划方法的理解
PRM的总体思路基于采样的路径规划方法在原理上与基于搜索的路径方法有较大区别.基于搜索的路径方法,如A*和Dijkstra,常常用于grid地图.它们需要搜索目标点到终点间的所有栅格.基于采样的路径 ...
机器人运动路径规划方法
机器人运动路径规划方法来源:-- 编辑:创泽时间:2020/3/10 机器人运动路径规划的性能指标包括:实时性.安全性和可达性等.在动态环境中,由于环境信息是时刻变化的,如果移 ...
matlab8邻域搜索算法,一种基于可搜索连续邻域A*算法的路径规划方法与流程
本发明涉及的是一种UUV全局路径规划方法. 背景技术: 无人水下航行器(Unmanned underwater vehicle,UUV)作为一种高技术手段,在海洋这块未来极具价值的发展空间中起着至关重 ...
栅格法路径算法C语言,基于地图栅格与QPSO算法结合的机器人路径规划方法与流程...
本发明属于机器人路径规划领域,提出一种基于地图栅格与QPSO结合的机器人路径规划方法. 背景技术: 移动机器人路径规划是寻找一条无碰撞的可行路径问题的方法.近些年,群智能优化算法逐渐成为移动机器人路径 ...
基于粒子群算法的智能车辆避障路径规划方法研究
基于粒子群算法的智能车辆避障路径规划方法研究 1.环境生成 1.1 环境生成方法的选择 1.2 坐标法生成环境 1.3 车辆简化 1.4 障碍物数据 2.粒子初始化 2.1 速度迭代设置 2.2 避障 ...
python路径规划算法可视化_基于粒子群算法的牙齿正畸路径规划方法python实现
这篇是基于粒子群算法的牙齿正畸路径规划研究的python实现,参考的是徐晓强等人的<基于改进粒子群算法的牙齿正畸路径规划方法>,与这篇文章的区别在于: 1.徐等的文章设计了一种改进的粒子群 ...
Apollo学习笔记（24）基于采样的路径规划算法
之前的文章都是基于搜索的路径算法,这两天在又学习了一下基于采样的路径规划算法,这里做一下记录,最后会奉上大神的链接基于采样的路径规划算法大致可以分为综合查询方法和单一查询方法两种. 前者首先构建路线 ...
【路径规划】基于PSO的路径规划问题【Matlab代码#7】
文章目录 [`获取资源`请见文章第5节:资源获取] 1. 粒子群算法(PSO) 2. 路径规划问题介绍 3. 部分代码展示 4. 结果图展示 5. 资源获取 [获取资源请见文章第5节:资源获取] 1. ...

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

基于图搜索的路径规划方法相关推荐

最新文章

热门文章