2022-DISCO-分布式激光雷达协同SLAM框架

DiSCo-SLAM: Distributed Scan Context-Enabled Multi-Robot LiDAR SLAM With Two-Stage Global-Local Graph Optimization

结论：该分布式激光雷达协同SLAM 仅需要较小的通信带宽
Scan Context descriptors解决-该描述符有效地描述了环境-及高效地数据交换—《Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map》（将扫描投影到低分辨率2D平面上）
二阶全局-局部图优化–解决大场景机器人回合次数少，远距机器人间相对位姿变换。-因子图《Factor graphs and GTSAM: A hands-on introduction》问题：机器人之间的回环检测通常无法从里程计信息中获得准确的初始猜测
DGS方法对比
激光惯性里程计 和GPS真值 时间戳时间戳在里面的作用是-保证同时和频率

引言

二阶全局-局部图优化：在全局步骤中，优化包含机器人局部帧到全局帧变换的因子图。在随后的局部步骤中，优化了包含（1）局部里程测量、（2）机器人内约束和（3）与局部机器人相关的机器人之间约束的局部姿态图。

相关研究

单SLAM可以通过高频的里程计值获得一个精确的初始先验信息（这是一个问题），但是多机器人之间的约束都是通过感知数据获得。
**Pairwise consistent measurement set maximization for robust multi-robot map merging–（PCM）**一个多地图融合的方法-DOOR-SLAM也是用他
多机器人之间的约束DGS—Distributed trajectory estimation with privacy and communication constraints: A two-stage distributed gauss-seidel approach

问题建模

多机器人SLAM系统是单体SLAM与机间SLAM的集合体。
所以整个机器人群SLAM的目标函数包含了单体状态误差和机间状态误差误差，如下：

但是由于本文采用的是分布式架构，每个机器人都会优化自身对目标函数的贡献，比如机器人a，它不仅优化自己，还优化机器人群中与自己相关的状态。这就对目标函数有了自己的贡献。如下：

所有的SLAM方案都是为了优化上面的函数，但是，这个函数包含了旋转矩阵R SO(3)，R-一个非凸函数，很容易陷入局部最优。
分布式高斯赛德尔通过将上述机器人a的目标函数分解为Ra和ta 再通过高斯牛顿的方法进行全状态图优化–但是没有解决这个非凸问题。
本文的解决方案：第一步全局优化-求解机器人间的变换；第二步局部优化-机器人内部的变换
第一步：为每个机器人设置一个局部坐标系，进行系间的状态投影。

如果以1号机器人作为基准-全局坐标系（分布式），那么每个机器人到1号的变换就是集合

所以目标就变成了找到T使得机器人间相对位姿的误差最小，选择列文伯格-马奎尔特法来求解非线性最小二乘问题。

注意：R的非凸性
**我的问题：**列文伯格-马奎尔特法可以用于求解非凸函数，但也不能保证一定能找到全局最优解。
如果时间足够可以使用多次运行该算法，并从多个不同的初始点开始，以尝试找到更好的解决方案。或者，也可以尝试使用其他更适合求解非凸函数的优化算法，如遗传算法、粒子群算法等

上述优化完后，现在我们已经可以求得其他机器人到全局的变换了，从而可以间接实现机器人间的状态投影

第二步：局部优化
局部优化的目标函数还记得么，上面提到过

但是，这个函数包含了机器人间的状态变换误差项
–所以需要进行一下变化，下面这个变化很巧妙！a在i时刻的状态与b在j时刻状态是一对，a在k时刻和y在l时刻的状态是一对，借助全局可以求得b和y的变换，根据这个桥梁就可以将ai和ak的变换求出，然后得到变换误差。
现在，所有的误差项都是a自己状态相关的了-再用列文伯格-马奎尔特法求X*a吧。

到这里二步优化的数学模型就欧克了

算法

整个算法的框架是什么样子呢？
下图以机器人a为例。原文翻译–哈哈哈 拿来主义确实不太好
“一旦系统接收到LiDAR扫描，则同时激活本地SLAM线程和特征提取线程。我们采用LIO-SAM作为我们的本地SLAM框架，或者在紧密耦合的LiDAR惯性数据不可用时使用LeGO LOAM。扫描上下文是3D激光雷达的一种轻量级空间特征描述符，用于描述和匹配特征。然后，移动机器人交换扫描上下文特征并执行扫描匹配。一旦检测到潜在的机器人间回路闭合候选，就执行增量成对一致测量集最大化（PCM）以去除异常值，每个机器人执行两阶段优化，首先建立全局到局部坐标变换，该变换通知后续的局部姿态图优化。最后，在机器人之间交换坐标变换以进行进一步优化。”
机器人a的雷达点云直接的用途有两个方面：1、局部SLAM；2、特征提取。
局部SLAM在运行过程，除了需要点云数据，还需要从消息池（包括局部消息池）获得其他数据-其他机器人在本系的状态，本机在其他系的状态变换。
特征提取的值会传入到局部消息池中-用于回环，局部消息池会共享给全局消息池。
局部消息池像是一个中枢，接收两两机器的状态变换，接收本机到全局的变换。这些变换都是这些深绿色的框得到

然后就是对一些方法的介绍
A、Scan Context Feature
从上到下的二维点云投影，z轴的值作为强度值（有点不太适应，因为点云本身是有反射强度的，老是想错），
再划分为小扇形，扇形数量有雷达线数和环视分部数进行分割，如下

每个网格的强度值（怎么感觉没提到）是该区域的最大z值，通过统计每个ring的非零值，作为一个ring的关键特征。
构建一个ring关键KDTREE进行回环检测，比较所有的sc特征，选出最好的。sc沿着扇形轴移动确定旋转不变性。

B、增量式PCM

多机器人间仅能同通过特征匹配进行回环
PCM剔除错误的回环，避免了不同场景相似外观的无匹配
只有两个机器人存在两对满足下面约束的状态变换才认为是一致的

阈值设为5
IPCM不会立刻执行，只有回环候选的数量满足一定值–设为5

C、二步优化
当其他机器人的所有单独位姿变换到局部系后，执行半径搜索寻找最近的机间约束。

之后这个变换就在局部图中进行优化

D、机间通信
两个机器人会分享之前的SC特征、他们之间的坐标变换，
机器人会在其KDtree中搜索共享的SC特征邻域，找到后，会查询相应的边和平面特征，以及其在邻域机器人系中对应的位姿。然后进行扫描匹配，将匹配变换的结果发送给相关机器人。

实验

数据集：KITTI 08 dataset；KITTI 00 dataset；Stevens dataset；Park dataset
卫星图像也能定性评估结果？
通信数据量的评估–评估单位kb