＜论文阅读＞TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments

TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments

论文地址：https://www.ri.cmu.edu/wp-content/uploads/2021/06/RSS_2021.pdf
github：https://github.com/caochao39/tare_planner

我们的方法的好处是，它可以比目前的最先进的算法更快地探索3D空间。该方法的优点是基于分层框架，将处理分离为两个级别。
第一层保持机器人周围环境的高分辨率表示，即局部规划范围(图中的绿框)。在该范围内，生成一个满足运动学约束的可行路径供机器人跟随。
第二层保持低分辨率表示，并计算连接全局环境中遥远区域的路径，即子空间(图1中的绿色立方体)。

这种框架的意义在于，在机器人附近的精细处理是最高效的，而在远离机器人的地方有限的处理也足够满足。该框架在本地规划范围内执行大量处理，并权衡细节以实现全局范围内的快速处理。

该算法首先规划出一条全局路径，该全局路径表明了机器人自由空间中需要详细探索的区域，接着需要一个详细的路径来引导机器人进行局部探索。这种方法使用全局路径在自由空间内进行大范围距离的移动。两层路径的组合使机器人能够在由局部和全局共同确定的序列中探索区域。

---

定义$\mathcal{Q}\subset {\mathbb{R}}^3$作为自探索的工作区域，${\mathcal{Q}}_{trav}\subset \mathcal{Q}$是可通过的子空间。定义$v\in SE(3)$描述机器人上传感器的姿态，$v=[p_v,q_v]$，其中$p_v\in {\mathcal{Q}}_{trav}$，$q_v\in SO(3)$代表位置和方向。定义$\mathcal{L}\subset SE(3)$代表车辆轨迹中的视角集合。我们使用面来代表自由空间和非自由空间之间的边界，后者代表占据区域和未知区域。$S_v\subset \mathcal{Q}$的意思是传感器在$v$视角下所看到的面。同一个面可以在不同的视角下所观测到。目前为止所观测到的面是
$$S=\underset{v\subset \mathcal{L}}{\cup }{S}_v\text{\hspace{1em}(1)}$$
这里，$S$包括已经覆盖的面，定义为$S_{cov}\subset S$，和还未覆盖到的面，定义为$\stackrel{-}{S}=S\backslash S_{cov}$。当去未覆盖的区域$\stackrel{-}{S}$时，我们应该寻找最短路径，这个路径必须满足运动学约束。设$v_{current}$为当前车辆的传感器位姿下的视角。我们的问题可以如下定义。

---

问题1： 给出$\stackrel{-}{S}$和$v_{current}$，寻找由视角$v_1,v_2,...,$生成的最短路径${\mathcal{T}}^{\ast }$，其中在使得车辆去未覆盖的区域$\stackrel{-}{S}$时，必须使得$v_{current}\in {\mathcal{T}}^{\ast }$，其中${\mathcal{T}}^{\ast }$是满足运动学约束的。

问题1在每个规划周期重复求解。我们使用$\stackrel{-}{S}$去计算探索的路径，当执行路径时，通过传感器当前所读取到的信息及时的更新$S$，处理位移和新观测到的面。然后将未覆盖的面变成覆盖的面，接着使用$\stackrel{-}{S}$去进行下一次的规划。

A.Viewpoint Sampling

我们定义了一个表面点被覆盖的标准，认为一个表面$patch$的中心$p_s\in \mathcal{Q}$，其法线$n_s\in {\mathbb{R}}^3$指向自由空间的一边，表面$patch$上的中心点被视点$v$覆盖，如果
$$|p_s-p_v|\le D\text{\hspace{1em}(2)}$$$$\frac{n_s\cdot (p_v-p_s)}{|n_s||p_v-p_s|}\ge T\text{\hspace{1em}(3)}$$
这里$D$和$T$是两个常量，限制了表面$patch$关于视角的相对的距离和转角。这样的标准可以使得表面被更好的感知，在实践中设置$D$小于传感器范围。

定义$\mathcal{H}\subset \mathcal{Q}$作为局部规划的范围。${\mathcal{H}}_{trav}\subset \mathcal{H}$为考虑障碍物和连通性后可通过的子空间，${\mathcal{C}}_{trav}^{\mathcal{H}}$为考虑旋转和平移所对应配置的空间。定义${\stackrel{-}{S}}_{\mathcal{H}}$为在空间${\mathcal{C}}_{trav}^{\mathcal{H}}$中的视角所能看到的未被覆盖的面。视角抽样的问题是在空间${\mathcal{C}}_{trav}^{\mathcal{H}}$中选择一个最少的视角集合去覆盖${\stackrel{-}{S}}_{\mathcal{H}}$，使用${\stackrel{-}{S}}_v\subset {\stackrel{-}{S}}_{\mathcal{H}}$去定义从视角$v\in {\mathcal{C}}_{trav}^{\mathcal{H}}$中会被看到的未覆盖区域。这个视角$v$的奖励定义为${\stackrel{-}{S}}_v$的面积，定义为$A_v$。

请注意，这个问题展示了子模块性，即选择了更多的视点，选择一个额外视点的回报会减少。这是因为附近的视点有重叠的视场，同一表面可以从多个视点感知。因此，一个视角的回报取决于之前选择的视角。设$v_i,i\in {\mathbb{Z}}^{+}$为第$i$次选择的视角。对于一个在$v_i$视角下未被覆盖的表面${\stackrel{-}{S}}_{v_i}$，需要被调整为 ${\stackrel{-}{S}}_{v_i}-{\cup }_{j=1}^{i-1}(\stackrel{-}{S_{v_i}}\cap \stackrel{-}{S_{v_j}})$，然后对$A_{v_i}$进行相应的调整。

算法1给出了视点采样的过程，该算法首先均匀的从${\mathcal{H}}_{trav}$点阵中产生一系列的视角候选点$\mathcal{V}$。其次，通过使用更新后的环境表示估计其覆盖范围$\stackrel{-}{S_v}$，计算所有视点候选$v\in \mathcal{V}$的奖励$A_v$(第3行)。然后，一个过程迭代$K$次以确定这些视点。在每次迭代中，算法从覆盖${\stackrel{-}{S}}_{\mathcal{H}}$的$\mathcal{V}$中随机抽取一个视点子集。三个视点是预先选择的(第6行)，一个作为当前的视点$v_{current}$，另外两个作为$\mathcal{H}$边界上的视点$v_{boundary}^1,v_{boundary}^2$。连接局部路径和全局路径。优先级队列${\mathcal{Q}}^{\prime }$用于管理候选视点，视点$v$的优先级被设置为它的奖励$A_v$。：视角是从优先队列中选择的，其概率与奖励成比例。由于这种子模块性，考虑到重叠的视场，在选择一个视点后，优先队列中剩余视点的奖励相应减少。当优先级队列为空或添加新视点的微不足道的奖励可以忽略时，视点采样过程结束。该算法调用算法2来生成经过采样视点的路径，通过迭代，找到成本较低的路径，并返回成本最低的路径作为局部路径，记为${\mathcal{T}}_{local}$。

B. Path Generation and Smoothing

给定一组采样视点${\mathcal{V}}^{\prime }$，我们希望生成通过每个视点的路径，理想情况下,我们希望这条路径在运动学上是可行的，可以被高速行驶的车辆跟随。在这里，我们关注曲率约束，当以期望的速度移动时，使路径的最大曲率取决于车辆的最小转弯半径。由于视点${\mathcal{V}}^{\prime }$的分布以及环境的结构，满足曲率约束的连续路径是不可能的，该方法以平滑分段计算路径，如下图所示：

车辆停在每段的结束，然后向不同的方向移动到下一段。这就要求车辆可以在一个地方转弯，考虑到本文的重点不是利用复杂的运动模型进行路径规划，我们采用通用的微分运动模型，对于飞行器，具有独立的高度控制。 我们把这条路径表示为${\mathcal{T}}_{smooth}^{\prime }=[v_1^1,v_2^1,...][v_1^2,v_2^2,...]$，其中$[\cdot ]$表示一个分段，$v_k^j$是第$j$段上的第$k$个视点。注意，一个段的最后一个视点和接下来的段的第一个视点共享相同的视点，设$\stackrel{\backsim }{n}$是线段的数量，定义$l_j$为第$j$段的长度，我们在每一次停止都加一个惩罚$p$，以此来阻止太频繁停车。${\mathcal{T}}_{smooth}^{\prime }$的成本被定义为：
$$c_{smooth}^{\prime }=\sum _{j=1}^{\stackrel{\backsim }{n}}{l}_j+p(\stackrel{\backsim }{n}-1)\text{\hspace{1em}(4)}$$

---

计算路径的问题可以表述如下。
问题2： 给出一些视点${\mathcal{V}}^{\prime }$，找到一条${\mathcal{T}}_{smooth}^{\prime }$的成本$c_{smooth}^{\prime }$最低的路径。该条路径可以逐个的访问在${\mathcal{V}}^{\prime }$中的视点，且每一段都满足曲率约束。

我们用近似算法分两步求解，而不是试图找到精确解，首先，我们用标准TSP解决视点的顺序，其次，我们按照顺序将视点划分为几个部分，这是为了确定一个视点(除了第一个和最后一个)是段中的内点还是两个段之间的断点。

旅行商问题，即TSP问题（Traveling Salesman Problem）又译为旅行推销员问题、货郎担问题，是数学领域中著名问题之一。假设有一个旅行商人要拜访n个城市，他必须选择所要走的路径，路径的限制是每个城市只能拜访一次，而且最后要回到原来出发的城市。路径的选择目标是要求得的路径路程为所有路径之中的最小值。

定义$n^{\prime }$为在${\mathcal{V}}^{\prime }$中的视点数，定义$x=[x_1,x_2,...,x_{n^{\prime }-2}]$作为一系列的布尔变量，用于描述$n^{\prime }-2$个视点（除了第一个和最后一个），还有一个函数$f(x)=c_{smooth}^{\prime }$。

确定视点状态的问题可以表述为：
问题3： 给定一个布尔变量序列$x$和一个函数$f(x)=c_{smooth}^{\prime }$，找到这样一个$x^{\ast }$：
$$x^{\ast }=\underset{x}{\mathrm{argmin}}{c}_{smooth}^{\prime }\text{\hspace{1em}(5)}$$
问题3是一个非线性整数规划问题，并且是NP-hard。特别是，寻找平滑路径涉及耗时的轨迹优化。我们的方法采用贪心策略，每个视点只检查一次，最大限度地减少了运行时间。

算法2给出了从${\mathcal{V}}^{\prime }$点集中计算${\mathcal{T}}_{smooth}^{\prime }$的过程，该算法在${\mathcal{V}}^{\prime }$中使用$A^{\ast }$算法找到每两个视点之间的无碰撞最短路径，并构造包含路径长度的距离矩阵$D^{\prime }$。接下来，算法为视点的遍历顺序求解一个TSP。在确定${\mathcal{T}}_{smooth}^{\prime }$的分段前，该算法初始化所有的$n^{\prime }-2$个视点全为断点。然后该算法试图通过按序的设置每一个视点$v_i$为内点降低成本$c_{smooth}^{\prime }$，然后重新平滑该段。在视点处应用边界条件，曲线的控制点被放置在A*搜索给出的初始路径上，并由非线性优化求解器调整，以考虑碰撞间隙和路径光滑性。为了加快处理速度，非线性优化被边缘化，只对$v_i$的两个相邻视点之间的曲线进行优化，该算法返回${\mathcal{T}}_{smooth}^{\prime }$以及$c_{smooth}^{\prime }$。

C. Global Planning

我们把$\mathcal{H}$外的空间分成偶数立方体子空间。每个子空间存储在探索过程中发现的覆盖表面和未覆盖表面。请注意，数据保存在子空间中只是为了存储，而$\mathcal{H}$中的数据则会随着勘探的进行而及时更新。每个子空间保存“未探索”、“探索”和“已探索”三个状态。 如果子空间不包含任何覆盖或未覆盖的表面，状态是“未探索”。如果一个子空间只包含被覆盖的表面，则状态为“探索”。如果一个子空间包含任何未覆盖的表面，则状态为“探索”。我们只在全局规划中探索子空间。定义${\mathcal{G}}_h\subset Q,h\in {\mathbb{Z}}^{+}$作为一个探索子空间，$\hat{\mathcal{G}}$作为探索子空间的集合。全局规划问题是找到一个经过当前视点$v_{current}$的全局路径${\mathcal{T}}_{global}$和$\hat{\mathcal{G}}$中每一个子空间的形心。在探索过程中，我们从过去的轨迹扩展成为可通行的区域，构建稀疏随机路线图。与局部规划类似，我们在路线图上使用$A^{\ast }$搜索子空间之间的最短路径，然后求解一个TSP。算法3给出了计算探索路径的全部过程。

该算法构造一个距离矩阵，其中包含在路线图上找到的路径的长度(第1行)并解决一个TSP(第2行)。全局路径${\mathcal{T}}_{global}$与$\mathcal{H}$的边界相交的两个点被提出作为$v_{boundary}^1,v_{boundary}^2$。然后使用算法1计算${\mathcal{T}}_{local}$，最后，通过将$\mathcal{H}$内的${\mathcal{T}}_{global}$部分替换为${\mathcal{T}}_{local}$，将$T_{local}$和${\mathcal{T}}_{global}$连接起来。
下图显示了一个探索的例子，其中深蓝色和浅蓝色的路径表示${\mathcal{T}}_{local}$和${\mathcal{T}}_{global}$。

当$\mathcal{H}$区勘探结束时(${\stackrel{-}{S}}_{\mathcal{H}}=\varnothing$)，${\mathcal{T}}_{local}$简化为连接从$v_{current}$到$v_{boundary}^1$和$v_{boundary}^2$的最短路径，然后进一步连接到${\mathcal{T}}_{global}$邻近探索的子空间。车辆沿着路径转移到一个探索子空间继续探索，如果${\stackrel{-}{S}}_{\mathcal{H}}=\varnothing$（局部空间$\mathcal{H}$内没有未覆盖区域），${\mathcal{G}}_h=\varnothing$（$\mathcal{H}$外的子空间也没有未探索的区域），则探索结束。