Efficient Dense Frontier Detection for 2D Graph SLAM Based on Occupancy Grid Submaps

摘要

边界是世界地图上已探索空间和未探索空间之间的边界；
该文研究了一个自主探索的模块化控制系统管道，其中基于占用栅格子地图的2D图SLAM算法来执行地图构建和定位，还提出了一种专门的边界检测方法，该方法有效地约束于活动子地图，但对SLAM闭环检测具有鲁棒性。

介绍

如图：SLAM负责建图与定位，其结果(地图与机器人位姿)输入边界检测模块中，检测到的边界再被后续任务探索和调度以及路径规划等进行使用。其中，边界探测和slam模块紧耦合。
每次SLAM更新地图后，通过插入扫描或优化姿态图来执行闭环检测，对完全组装的全局地图进行边缘检测。

相关工作

常见的边界检测操作是在每次地图更新后对完整的全局地图进行边缘检测。
活动区域——在地图上围绕机器人位置的一个边界框，将最后一次扫描的地图进行了更新。

准备工作

A 重点在图优化的SLAM。
B 基于Carto算法使用的ceres求解器用LM方法优化；当将预定数量的扫描插入到子图中时，它将被标记为已完成，并创建一个新的子图以取代其在活动 子图对 中的位置。而一旦一个子地图完成，它的占用网格从那一刻起是不可改变的。

边界检测

A 定义：
刚体变换：T上b下a ∈SE(3)意为b坐标系相对于a坐标系的位姿，SE(3)位姿合成的群运算写成乘法(T上b下a) ^(-1) = T上a下b；P上b(b的投影)*(T上b下a) = p 上a。
全局地图：全局地图坐标系统用g表示，位姿图优化的解表示为对g图元的位姿。
子图：子图是分辨率的占用栅格即单元格大小r，通常为0.05cm；栅格占用的最初概率为0.5；子图是由插入的激光扫描序列构造的，激光扫描序列和栅格大小为预先固定的，当激光扫描序列插入完毕，子图就被标记完成。
活动子图：意为未完成的子图。
局部坐标系：其原点固定，是Si子图中栅格(k，l)的占用概率值。
全局子图位姿：Si子图局部坐标系的原点的全局位姿是
由于子图是位姿图的成员，子图位姿是优化位姿图解的一部分。
占用栅格分类：等于0.5为未观察，大于0.5为占用，小于0.5为自由。
观察单元：是未观察到的占据网格单元。
局部边界点：是同一子图中与自由单元相邻的一个未观测到的占用栅格单元的中心。
刺入查询：在给定全局点和全局子图位姿中查找给定子图对应的单元格。在全局地图中，对子图Si和全局点P上g的表达为：在Si子图中找一个占据栅格单元，它在Si的局部坐标系中的中心点坐标更接近于；执行一个刺入查询相当于检查是否是一个未被观察的单元。

全局或局部边界的周长：是各自集合中边界点的数目。

边界检测算法

SLAM执行过程中边界检测的情况会出现两种：子图更新事件，即扫描被插入到活动子图中; 姿态图优化事件也周期性地发生，但频率较低。

算法1：子图更新事件的处理

对于SLAM处理的每个激光扫描，都对活动对中的两个子图进行更新，使子图更新成为最频繁发生的事件类型。子地图的更新只会影响活动子地图所覆盖区域的边界，因此边界检测算法可以限制在该区域，以最大限度地提高效率。
1、对新版本的子地图占用网格执行密集局部边界检测，即在局部子地图级别上，选择执行朴素边缘检测方法(第6-9行)；概率阈值、分类(第6行)和边缘检测(第8行)。
2、计算局部边界点过程中会产生一组全局边界点的候选点集，每个局部边界点根据对应子地图的当前全局姿态投影到全局地图框架g中(第10行)。
3、对交叉的子图执行插入查询测试(第11行)。
4、通过了所有子图的刺入查询测试的投影边界点是一个全局边界点，被添加到全局边界点集(第12行)。
5、如果测试失败，测试失败的子图将被记录为提示(第13行)，以便将来更快地执行重新测试——在大多数情况下，对失败的子图提示执行测试会立即产生负结果。
基于占用网格子地图的图SLAM的几个特性：
1：完成的子图占用网格是不变的，没有必要为已经完成的子图重新检测局部边界。
2：处理子地图更新的算法可能假设自上次子地图更新事件以来没有发生任何图优化，因此所有已完成子地图的现有全局边界都是有效的(除了下面描述的情况)。
3：活动子图的单元栅格占用概率具有减小的熵，这意味着只有以前未观察到的单元才能被观察到，反之则不然。这意味着对活动子图的更新可能会使交叉子图的全局边界失效。
6、测试活动子图的新版本是否覆盖交叉的已完成子图的现有有效全局边界。这是通过对活动子图执行全局边界点的插入查询测试来实现的。(第14-17行)
7、如果插入查询测试失败，则新覆盖的全局边界点将从全局边界点集中删除(第17行)
8、已完成子图的全局边界被删除，这些子图的全局边界也被标记为updated(第18行)，用于边界更新的增量发布(第22行)。
9、已完成子图的边界框存储在树数据结构中，这使得能够快速查询与给定边界框相交的子图(例如，查找与活动子图相交的已完成子图，第5行)。
10、当一个子图被标记为完成时，它的边界框被插入到树结构中(第19行)。实现使用R-trees的Boost实现来存储完成的子图的全局轴对齐边界框。

算法2：姿态图优化事件的处理

由于已经为所有子图计算了局部边界，因此需要做的就是将局部边界点重新投影到全局坐标系统，并重新测试它们。
1、重建R-Tree；即重新计算全局边界框(第1-3行)
2、每个局部边界点根据相应的新的全局子地图位姿重新投影(第8行)
3、对交叉的子图执行插入查询测试(第11行)
4、如果插入查询测试通过，则将重新投影和重新测试的边界点插入到新的全局边界集(第12行)
5、在重新测试期间，如果有失败的子图提示，则优先针对它进行测试(第9-10行注释)。

综上所述，算法1假设子地图更新事件之前的所有全局边界点都是有效且最新的。图优化违背了这一假设，它根据新位姿图解的子图位姿替换子图。算法2通过重新计算全局边界来恢复这个不变式。

未来工作:一些被引用的最新技术【10】将连续的边界点分组成段，这对选择探索任务的导航目标很有用。另一种可能会引起兴趣的检测边界的后处理是可达性分析:探测机器人无法到达的、对导航目标没有意义的边界点，如玻璃后、关闭的门后或墙后的边界点(例如，通过错误的激光读数或不同子地图中的墙的轻微调整)。