近年来，像无人机，无人车等领域，单机器人的技术飞速发展，越来越多的学者开始致力于研究多机器人协同工作。其中，相对位姿估计是多机器人系统中一个亟待解决的难题，稳定、准确、快速的估计出机器人之间的相对姿态可以显著提高机器人协作质量。目前，国内外研究学者相对位姿估计问题上提出了诸多方案。一些研究提出将SLAM 系统同距离等传感器融合的解决方案，但这些方案受限于 SLAM 技术的弊端，依赖于环境特征同时会占用较多的计算资源。也有一些研究针致力于使用距离方位传感器融合解决思路，这些方案要么需要运动激励并且在一些运动模式下会出现退化现象，要么需要比较大的安装平台。此外，还有学者提出给机器人配备人工标签的思路，但目前提出的一些设计方案仅能在很短的范围内有效工作，这对在实际场景中的应用带来了很大的困难。
团队小组提出了利用单次的传感器数据测量解算相对位姿的方案，同时将误差状态卡尔曼滤波器引入到系统中来抵抗传感器噪声对估计精度的影响。当多机器人系统中机器人数目达到三个及以上时，在两机算法的基础上，本文提出了一种简洁有效位姿图优化算法来进一步提高估计精度。本文算法方案占用计算资源较少，经测试在 NUC-i5 计算平台上，ID 识别图像处理时间低于 2ms，两机算法时间低于 1ms，三机及以上位姿图优化算法低于 3ms。
硬件系统主要包括红外 LED，红外鱼眼相机，IMU 和 UWB 四大模块，相比于基于人工标签的其它设计方案，本文的硬件设计方案有效提高了工作范围，实验中测试最远的工作距离可以达到 27.5 米。此系统具有不依赖环境特征，算力资源占用低，单帧的传感器数据解算相对位姿，工作范围更远等特点。
同时做了充足实验来测试系统的相对位姿估计精度以及验证系统的鲁棒性和应用可行性。实验证明本文的相对位置估计误差在 0.113m ∼ 0.161m（中位数），相对旋转估计误差在 0.773° ∼ 1.517°（中位数）。此外，测试证明特殊场景（黑暗，长距离，大姿态 和非视距）下也具有稳定的工作性能。
在定位与建图模块，团队采用雷达惯性里程计的解决方案。根据设计方案，激光雷达惯性里程计可分为松耦合和紧耦合两大类方法。
在系统设计上，LOAM和 LeGOLOAM未利用 IMU 的测量，属于松耦合系统。FAST-LIO 系列和 LIO-SAM等工作属于紧耦合系统，在其系统的设计上将 IMU 三轴角速度和加速度测量值和激光雷达的点云信息进行结合以提供更高的精度和更高的稳定性。
团队选用的方案为 FAST-LIO2，该方案主要有两个创新点，一个是不用像一些基于视觉的 SLAM 系统需要对环境特征进行提取，只需将原始点配准到地图，这使得环境中一些微小的特征能够被利用，从而提高匹配准确性；二是通过增量式 kd 树（ikd-tree）的数据结构维护局部点云地图，支持增量式点云的更新 (即点插入、删除) 和动态重新平衡。算法的整体思路为
（1）按照一定的频率进行扫描，对原始的点云数据进行累积，本文设置的扫描频率为10Hz。在扫描过程中，雷达处于运动状态，因此每个点云数据对应的雷达的位姿会有所差异。为了纠正这种运动，利用 IMU 的高频测量数据积分估计各点云数据相对于扫描结束时间的相对位姿，然后将所有点利用相对位姿投影到扫描结束的时间。
（2）利用紧耦合的迭代卡尔曼滤波器进行状态估计，其中利用维护的局部地图点云和最新一帧扫描的点云数据进行状态估计模块残差的计算，对状态进行修正。
（3）将每一次扫描的点云数据利用估计的里程计信息投影到一个局部的点云地图里，并利用 ikd-tree 的方式维护局部地图的点云信息，这里只维护以雷达当前位置为中心一定范围内的点云信息，超出区域的点云信息将会被从 ikd-tree 中剔除。
栅格地图构建与共享
激光雷达采集的是离散的 3D 空间点，很难直接用其来做导航规划任务，其中有一种算法是将三维空间按照一定分辨率离散为栅格地图，每个格子只有有障碍物（占据）和无障碍物（非占据）两种状态，这种表示方法能够非常快速的查询空间中是非被占据，因此在导航规划算法中很常用。本文采用的为基于贝叶斯概率更新的概率栅格地图，这种算法根据传感器的观测数据（比如雷达数据）来更新格子被占据的概率，将建图问题转化为滤波问题。
轨迹规划
在基于梯度的轨迹优化方法中，求解轨迹和障碍物无碰撞问题时需要用到机器人距离障碍的梯度，对于栅格地图的一般做法是通过构建欧式有符号距离场（ESDF）来提供梯度。ESDF 地图的每个栅格内部存放着该栅格到离它最近障碍物的距离，对于地图中任意位置到障碍物的梯度都可以根据格子中的值利用三线性插值的方式获取。通常，有两种方法来构建 ESDF——增量全局更新方法和批量局部计算方法。
对于轨迹优化任务而言，ESDF 地图的方式有个比较明显的缺点，多了很多冗余栅格中 ESDF 值的计算。具体来说，对于轨迹优化任务而言，优化过程中轨迹通常只会在有限的一个小范围波动，因此仅仅用到了波动范围内的地图的梯度信息，而无论是增量全局更新还是批量局部计算 ESDF 的方法都是很保守的，计算了一些不必要的地图信息。此外也有一些利用手动规则确定只计算某个较窄范围的 ESDF 值，但这种方式缺乏合理性，可能会导致优化任务失败。
Ego-Planner 一种不依赖 ESDF 的轨迹规划算法。该方法首先用B 样条来表示轨迹，将机器人的空间位置通过 B 样条的形式表示为关于时间的多项式函数，B 样条轨迹是由一系列控制点（control point）和时间节点向量组成（knot vector）
组成。
充分利用 B 样条估计的凸包性质使得优化出的轨迹满足无碰撞，目标机器人运动学等约束，同时提出了一种新的地图构建思路，避免了 ESDF 地图很多不必要的距离场的构建，提升了轨迹优化系统的效率。最终，本文采用三阶 B 样条来表示轨迹，考虑小车的运动学约束等，将 Ego-Planner 算法部署到本文的小车实验平台上，实现了小车的自主导航功能。
- 松散耦合导致视觉和激光雷达之间的独立性。数据之间的约束不够强。
- RF-LIO[67]是在LIO-SAM基础上提出的动态SLAM框架。该系统自适应地添加由点云组成的多分辨率距离图像，并使用紧密耦合的激光雷达惯性里程计去除移动物体。然后将激光雷达扫描与子图进行匹配。因此，即使在高度动态的环境中，它也可以获得精确的姿态估计结果。基于固态激光雷达的LIO紧密耦合系统也逐渐受到关注。然而，当长时间在开放场景中移动时，LIO仍会退化。FAST-LIO[68]为无人机系统提出了一种基于紧密耦合迭代卡尔曼滤波器的高效鲁棒LIO框架。然而，该系统放弃了历史数据对当前状态的影响。无法执行全局姿态校正。
- 惯性系统的紧密耦合无疑会增加系统的计算负担，同时提高精度。大多数现有算法通过边缘化历史数据或限制本地地图容量来提高计算速度。后端优化通常只构建激光雷达的姿态图，而不添加IMU测量的偏差和速度。这些方法在大多数情况下都能取得很好的效果。然而，由于对几何特征的依赖，一旦惯性系统在开放的非结构化场景中失去LO约束，SLAM将遭受严重的漂移和退化。表4比较了LIDAR-IMU紧耦合系统的相关工作。
- 随着IMU预集成理论的发展和完善，LO系统可以与IMU建立更强的约束关系。SLAM系统的定位精度也得到了进一步提高。然而，紧密耦合需要大量的计算。在速度和精度之间找到平衡是这一阶段工作的难点。
- 激光雷达视觉IMU紧耦合系统 但由于其体积小、成本低的优点，它很快成为SLAM技术的研究热点。近年来，许多VIO作品都有报道。视觉与激光雷达形成了极好的互补，因为它不受场景结构的限制。因此，LVI系统由于其在传感器退化场景中更强的鲁棒性而受到越来越多的关注。
- 是否要考虑引入GNSS ？？ LIDAR因子、视觉因子、IMU因子和里程计因子被添加为因子图的主要约束。同时，局部约束与GNSS约束紧密耦合，以优化机器人的全局状态。该系统可以实时检测传感器的退化，并灵活配置多种工作模式，适用于各种场景
- LIO-SAM的前端也是基于功能的。然而，由于IMU数据约束的增加，当LO退化时，系统仍可以稳定运行一段时间。这使得它能够有效地在隧道中行驶最长的距离。显然，IMU的紧密耦合不能完全解决长距离隧道问题。长走廊问题是实际应用中经常遇到的难题。上述实验表明，为特征分配权重并添加IMU预集成可以有效缓解里程表退化现象。显然，基于特征的方法在隧道环境中遇到了瓶颈。如果将机器人底盘的精确控制模型和运动学模型与基于特征的里程测量系统相结合或通过视觉特征辅助，则该问题将得到更好的解决[96]。同时，UWB也适用于一定范围内的此类封闭场景。这也是进行多传感器融合的有效方法。
- 多传感器融合是构建鲁棒系统的关键。基于多传感器的复杂系统需要轻量化、精确性、可扩展性以及SLAM的通用性。从实验部分，我们知道动态环境、对象遮挡和长走廊环境是基于特征的SLAM方法的关键挑战。将传感器与机器人或车辆的控制模型相结合，可以有效缓解特殊情况下里程表退化的问题。随着传感器数量、数据量的增加以及应用场景的不断扩展，SLAM系统很难在指定的计算时间内进一步提高定位和映射的准确性。因此，SLAM在各种场景的应用中具有很大的发展空间。分布式多机器人协作、陆空协作和海空协作系统可以有效地解决大型场景中面临的问题。此外，硬件加速、并行处理特征提取和姿态优化可以有效缓解多传感器数据融合带来的系统计算压力。另一方面，深度学习无疑是目前最热门的方向之一。在将深度学习与SLAM系统相结合方面已经做了很多努力。深度学习的应用几乎可以在所有关键步骤中看到，如特征提取、深度估计、环境感知、姿态估计和语义图。在目前的工作中，深度学习仅取代了SLAM系统的有限部分。例如，优化单眼相机的深度估计以获得地标点，直接估计姿态而不进行特征提取，感知环境以区分移动对象，以及构建高精度语义地图。这些都是未来极具潜力的研究方向。深度学习的应用将进一步提高和扩展SLAM的性能和功能。在未来的工作中，将多传感器数据融合和深度学习相结合来优化和改进SLAM算法将受到更多的关注。