论文所讲内容主要分为三部分:子图构建、闭环检测以及后端优化。
1子图(submap)的构建
每个子图都由一定数量的扫描点集(scan)组成,每个扫描点集在插入子图中时,都会先寻找在该子图中的最优位姿,进而插入。(道听途说:为了避免子图中插入过多的扫描点集,对于新的扫描点集,计算当前位姿与上次位姿的变化量,当位姿的变化量大于某个距离、角度或时间时,才将扫描点集插入,否则删除,这部分参考源码中的motion_filter)。
在论文中子图是一个分辨率为5cm的栅格地图,栅格值代表被击中的概率。其中,概率更新的方式如下。
当位姿为ξ时,扫描点集合中的某个点p转换到子图中的公式为:
接下来记扫描点集H={hk},k=1,…,K,我们计算扫描点集在子图中的最优位姿。
上式中,Msmooth为双三次插值(bicubic interpolation)函数。对点a进行双三次插值的过程为:取a邻域内的16个点,分别将16个点与a点的距离带入bicubic函数得到各自的对应的权值,16个点自身的值乘以对应权值后的累加和即为a点的插值结果。如果扫描点集的端点根据某个位姿映射到子图中后,端点所对应的栅格越多的是障碍栅格,则上式的值越小。可以根据高斯牛顿法求解一个最优位姿(某个栅格,某个主方向),在该位姿处上式取得最小值。
2闭环检测
每隔一段时间进行闭环检测,对于新来的扫描点集,根据传感器数据得到的初始位姿,设定邻域作为搜索空间。在搜索空间内查找一个最优位姿,扫描点集在该位姿下的得分最高(得分越高说明两者的环境越相似),若最高得分超过阈值,则找到闭环。因此,如果遍历搜索空间,计算扫描点集在每个位姿下的得分,得到最高得分即为最优位姿。这是一种暴力的解法,能得到结果,但是计算量太大,因此文中使用了分支定界法去搜索最优位姿。分支定界法在这里的应用主要分成两部分:搜索流程、结点得分计算。
2.1搜索流程—分支定界法
(1)将搜索空间按步长划分成多个子空间,记每个空间为一个结点,则搜索空间为根节点,每一次划分即为该结点构建孩子结点。每一次划分,树高增加1,层数越高的结点,其对应空间越小。如下图所示为步长的设定方法。
(2)遍历刚划分出的孩子结点,计算每个结点的得分,按从大到小排序;
(3)对第一个结点(得分最大的结点),若其得分大于best_score(初始值为阈值),则将其划分得到四个孩子结点,计算得分并按从大到小排序,否则转到(5);
(4)重复(3),直到结点代表的区域大小为基础栅格大小(5cm),即得到最底层的四个结点,计算它们的得分,若最大得分a大于best_score,则更新best_score=a,否则就转到(5);
(5)返回上一层,比较best_score与兄弟结点的得分,若某个兄弟结点的得分更高,则将该兄弟划分成四个子结点再转到(4),否则转到(5),直到返回到根节点;
(6)返回best_score和所对应的结点(若存在的话)。
也许看伪代码会更清楚一些,论文中给出了伪代码流程如下。
2.2结点得分计算
将扫描点集按位姿ξ映射到栅格地图中,所有激光端点所落栅格的值的和即为该位姿下的得分。但是一个结点是一个区域范围,包含有多个位姿,如何计算一个结点的得分呢?暴力的方法是计算区域内每一个位姿下的得分,记其中最大的为区域得分。不过这样做分支定界就没有意义,还是把搜索空间内的所有位姿都求解了个遍。论文中求解结点c的得分的公式如下。
其具体含义为,对于要计算得分的结点,根据结点所处的树深h,定义一个窗口;遍历栅格地图中的每一个栅格,窗口以当前遍历的栅格为左上角,查找窗口中的最大值,赋值给当前栅格。由此我们可以得到一个新的栅格地图gridMapTemp。取结点内的任意一个位姿,将扫描点集进行映射,所有激光端点所落栅格的值的和即为结点得分。图解如下。
上图中红框为待求得分的结点,篮框表示对左上角的栅格查找窗口下的最大值,带有黑色端点的线表示映射到栅格地图中的扫描点集所形成的光路。上述处理相当于将障碍物变厚,本来只能在某一个位姿下才能得到最高分,但修改过栅格值后的新地图中,只要在结点范围内的位姿,均能得到最高分。
3后端优化
论文中这部分讲的没看懂。