参考博文：
https://blog.csdn.net/qq_41839222/article/details/87860819
https://blog.csdn.net/weixin_41843971/article/details/86537228
摘要
精确的状态估计是自主机器人的一个基本问题。为了实现局部精确和全局无漂移状态估计，通常将具有互补特性的多个传感器融合在一起。**局部传感器（摄像机、imu、lidar等）在小范围内提供精确的姿态，而全局传感器（gps、磁强计、气压计等）**在大范围环境中提供噪声大但无漂移的定位。本文提出了一种融合局部状态和全局传感器的传感器融合框架，实现了局部精确、全局无漂移的姿态估计。由现有的VO/VIO方法产生的局部估计在姿态图优化中与全局传感器融合。在图优化中，局部估计被对齐成全局座标。同时，消除了累积漂移。我们在公共数据集和实际实验中评估了系统的性能。将结果与其他最新算法进行了比较。我们强调，我们的系统是一个通用的框架，可以很容易地融合各种全局传感器在一个统一的图优化中。我们的实现是开源的。
I 背景
自主移动机器人(自动驾驶、检查、搜索和救援) 基本技术：定位，精确的6自由度姿态来导航和控制
可用于局部姿态估计的sensor：
- 雷达和激光雷达广泛应用于室内受限环境
- 摄像机和惯性测量单元则适用于室内和室外环境。
局部位姿估计算法缺点：
- 没有全局座标的局部帧（相对于起始点）中产生位姿估计。即使在同一个环境中，我们从不同的角度出发，也可能得到不同的估计。因此，如果没有固定的全局座标，它们不利于重用
- 由于缺乏全局测量，从长远来看，局部估计容易累积漂移。虽然有人提出了一些基于视觉的环路闭合方法来消除漂移，但这些方法无法处理大量数据的大规模环境。
全局传感器优点：
- 提供关于固定地座标系的全局测量，固定地座标系是无漂移的。
全局传感器缺点： - 量通常是不平滑和噪声的，不能直接用于精确控制和导航。以GPS为例，它可以测量米的大致位置，但测量是不连续的低速率。而且，它只测量三维位置而不测量三维方向。因此，只有全局传感器不足以进行实时的六自由度状态估计。
由于局部传感器（相机、imu和lidar）在局部精度方面取得了令人印象深刻的性能，而全局传感器（GPS、磁强计和气压计）是无漂移的，因此将它们融合在一起实现局部精确和全局无漂移的6-dof位姿估计是一种聪明的方法。
为了提高鲁棒性，我们希望尽可能多地融合传感器。因此，需要一个支持多个传感器的通用框架。
传统的基于EKF的方法：
- 虽然可以将局部估计逐步融合到全局帧中，但为了保证收敛性，需要对不同帧之间的变换进行准确的初始猜测。
- EKF方法对时间同步也很敏感。由于状态不能在过滤过程中传播回来，任何迟来的测量都会引起麻烦。
本文提出了一个局部估计与全局传感器测量融合的优化框架。局部估计采用的是最先进的VIO方法，然后将全局传感器作为姿态图中的一般因子，将局部因子和全局因子综合起来构建优化问题。实现局部精确并且全局无漂的状态估计。
本文的贡献如下：
- 在公共数据集和实际实验上对拟议系统进行评估。
- 一个通用框架，用于将各种全局传感器与局部估计融合，从而实现局部精确和全局无漂移定位。
- 社区开源代码
II 之前的相关工作
A 局部定位（摄像机、imu、lidar和RGB-D传感器）
基于视觉的方法： PTAM，SVO，LSD-SLAM，ORB-SLAM，DSO
基于雷达的方法：LOAM
视觉惯性融合：MSCKF，VINS-mono，Ice-ba
视觉激光雷达融合：V-Loam
B 全局感知定位（gps、磁强计、气压计等）
全局测量通常是带有噪声且低频，因此不能单独使用。
通常将全局传感器与局部传感器融合，实现精确的全局感知定位。例如：
- 基于滤波的方法
基于EKF方法的GPS+视觉+惯性测量
基于UKF方法的视觉+激光+GPS测量
基于滤波的方法对时间同步非常敏感，需要特殊的排序机制确保来自多个传感器的所有测量都是有序的。 - 基于优化的方法
基于优化的框架的VIO+GPS
很少有研究工作融合传感器超过三种类型。本文提出的全局定位框架可以支持多个全局传感器。每个传感器都是一个通用的因子，可以很容易地加入到优化问题中。
III 系统概述
A 局部传感器
B 全局传感器
GPS、磁强计、气压计等，这种传感器具有全局参考值。它总是在固定的全局框架下工作，例如地球座标系。参考座标系的原点是预先确定和已知的。它们的测量是全局参考的，但有噪声。误差与行驶距离无关。对于GPS，它测量相对于地球的绝对经度、纬度和高度。经度、纬度和高度可以转换为X、Y和Z座标。对于磁强计，它测量磁场的方向和强度，从而确定磁场的方向。对于气压计，它测量气压，气压可以转换成高度。
我们的框架结构如图2所示。局部传感器（摄像机和imu）用于局部估计。采用现有的VO/VIO方法产生局部姿态。局部结果和全局传感器被输入到全局姿态图中。将它们转化为统一的因子来构造优化问题。全局估计器产生局部精确和全局感知的 6-DoF pose结果。
IV 理论
D位姿图优化
一旦构建了图，优化它就等于找到尽可能匹配所有边的节点配置。ceres解算器[22]用于求解这一非线性问题，它以迭代的方式利用高斯牛顿和levenberg-marquadt方法。我们在低频（1hz）下运行位姿图优化。在每次优化后，我们得到局部帧到全局帧的转换。因此，我们可以通过这种变换来变换后续的高速率局部姿态（vo/vio，200hz），从而实现实时的高速率全局姿态。
由于姿态图是非常稀疏的，计算复杂度随姿态的数目线性增加。为了得到精确的全局无漂移位姿估计，我们可以为位姿图优化保留一个巨大的窗口。当计算复杂度超过实时能力时，我们抛出旧的姿态和度量，并将窗口保持在有限的大小。
V 实验
上图是实验系统
和MSF(EKF方法融合了视觉，惯性，GPS)相比
从上图可以看出改进的方法几乎没有漂移。
VI 总结
本文提出了一种基于优化的局部估计与全局传感器融合的框架。本地估计来自以前的vo/vio工作。在位姿图优化中，全局传感器被视为通用因子。该系统实现了局部精确的和全局无漂移姿态估计。我们在公共数据集和实际实验中展示了我们的系统令人印象深刻的性能。