Graph-based Path Planning for Autonomous Robotic Exploration inSubterranean Environments论文笔记

Graph-based Path Planning for Autonomous Robotic Exploration in Subterranean Environments

发表期刊及时间： 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

---

目录

Graph-based Path Planning for Autonomous Robotic Exploration in Subterranean Environments

一、论文概述

二、系统概述

三、算法概述

3.1 局部规划（Local planner）

3.2 全局规划（Global planner）

3.3 如何找到best path

4. 算法优缺点

---

一、论文概述

        本论文以地下矿井、地下管道环境为应用场景， 提出了一种基于快速随机图（RRG）的机器人环境自主探索方法。整个系统搭载在UAV上，并通过激光、相机等多传感器融合，最终建立稠密点云地图。快速随机图是一种类似于RRT的一种网状图，相较于RRT，RRG更加稠密，可选择的路径更多，为机器人在分岔路口处的决策提供了选择。本文提出的方法，

其优点是：适合在大规模、多分支的地下场景中应用。

缺点是：不能解决随机采样算法的通病，难以通过狭小细窄的入口。并且探索后期，全局图的规模巨大，在遇到死胡同时，需要耗费大量资源对全局图进行遍历。

二、系统概述

        

        以上是整个探索过程的流程图，GP（Global planner）和LP（Local Planner）负责生成RRG，局部图为机器人提供了当前要去的目标点以及最佳路径，全局图提供了归巢（返回探索起始点）的路径和机器人遇到死胡同时的其他可探索点。然后用Dijkstra算法算出从根节点到所有末端节点的最短路径。之后通过一个信息增益函数评价每一条最短路径（shortest path），得到信息增益最大的最佳路径（best path）。然后通过dijkstra沿着RRG的方向导航到末端节点，这就是一个迭代过程。当然在产生best path的同时，全局图也会进行一个续航估计和候选点存储的过程。

三、算法概述

本文提出的探索算法其核心是两个planner（即：GP和LP）（PS：当今探索算法的框架貌似都是这样，分为局部和全局）。

3.1 局部规划（Local planner）

LP的作用： 生成一个局部图（LG，local graph），通过LG来计算出机器人当前要去的目标点以及最佳路径。

算法伪代码如下：

         1：获取当前机器人的位置作为局部图的根节点。（PS：本文中，位置坐标均已configuration表示）

        2：通过BuildLocalGraph函数在一个有限的空间内生成局部RRG

        3：生成好的局部图有多个末端节点，通过Dijkstra算法计算出所有由根节点到末端的最短路径

        4：计算所有shortest path的空间增益（未知空间的体积和）

        5-13：对所有的shortest path计算探索信息增益，取最大值的shortest path作为best path

（以上为local graph的一个迭代过程， 也就是说这个过程每到达一次目标点就会迭代一次，直至exploration结束）

下面是BuildLocalGraph函数的伪代码：

         此函数是建立图的关键，也是此探索算法的核心，与RRT十分类似，我们可以用RRT的思想去理解它。先来比较一下它和RRT的异同：下面是RRT算法

         我们可以看出来，他们的相同点是：步骤大致相同，先在有限空间内随机取点，此点作为采样点，找到tree上（或graph）中距离采样点最近的点（Xnearest），然后在Xnearest向着采样点的方向生长一段距离，如果没有碰到障碍物就加入tree或graph。

不同点是：

1. RRT是有一个生长步长δ，Xnearest和采样点之间不是直接连接的，而RRG是如果Xnearest和采样点之间没有碰撞，则直接连接

2.RRG生长出的新节点也会连接一个半径内的其他节点，因此它比RRT的树枝更多，图形更密集

   

 

（左图是RRT，右图是RRG，可以明显看到RRG的每个节点和其他附近节点都有连接，而RRT是单连接的）

接下来我们详细说一说局部图的产生过程：

1. 初始化： 找到根节点Xroot，规定一个生长空间DL以及最大节点数和最大边数和（以免图生长过多），节点的集合V，边的集合E

2. 在DL内随机采样一个点Xrand

3. 找到graph中离Xrand最近的点Xnearest，对Xnearest到Xrand之间的连线做碰撞检查，如果无碰撞（collision-free），则Xrand和边加入图

4. 以Xrand为中心，半径δ的圆，位于这个圆内的所有graph节点都和Xrand连接（只连接无碰撞的边）

5. 反复迭代上述过程，直到节点数或边的数目达到最大值。

 上图为图的生长过程，红色线为新生成的边。

3.2 全局规划（Global planner）

        GP和LP是同时运行的，GP的作用是：提供归巢（返回探索起始点）的路径和机器人遇到死胡同时的其他可探索点。因此，GP也会产生一个全局图

        GP构建全局图的过程与LP几乎相同，构建过程如下：

1. 建图的起点是全局图的根节点，也被称为home

2. LP每次迭代都会产生一条best path，GP会在每个LP迭代后将best path中每一个节点加入全局图

3. 将这些加入的节点作为采样点，执行LP中的操作，就会形成global graph了。

随着探索地进行，全局图会蔓延到整张地图中。

3.3 如何找到best path

我们在3.1中说到局部图的建立，建立局部图后，会找到很多条shortest path，然后再从众多shortest path中筛选出best path。

如何筛选呢？靠的是下面的增益函数

其中，δi是表示第i条shortest path，w1和w2都是权重值，δexp表示一个直线路径，长度与δi相同，方向由δi的起点指向δi的终点，S（…）表示的是δi与δexp的比值，D（…）表示路径长度，Vgain表示第i条路径的空间增益，说白了就是，每个节点所能探索到的未知空间的体积和。

之所以要使用e的负指数函数，是因为要把e的负指数函数在[0,+∞]上的值域为[0,1]，这样就起到一个权重的作用，上述式子的含义就是，长度过大的路径不要，太弯弯绕绕的路径不要，探索空间大的路径保留。

（以上是我对本文的个人理解，至于原文中说的GP的一个作用是re-positioning，这一块不是很明白，我猜测可能是当进入死胡同时，为LP做候选准备的吧）

 

4. 算法优缺点

优点：可以有效解决机器人遇到岔路口时产生次优路径的问题，可以在多分叉环境的环境中找到更优的路径。

缺点：与RRT一样，无法解决狭小入口的问题，原因很简单，采样点在狭小入口处的采样概率很小，所以图不易长到入口处。