【泡泡图灵智库】R2LIVE: 一个鲁棒实时的雷达-惯导-视觉紧耦合的位姿估计和建图系统
标题：R2LIVE: A Robust, Rea-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping.
作者：Jiarong Lin, Chunran Zheng, Wei Xu, and Fu Zhang
来源：arXiv:2102.12400v1
编译：方川
审核：段逸凡、Yuri
这是泡泡图灵智库推送的第663篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权。
摘要
本文提出一个鲁棒实时的多传感器紧耦合框架，可以融合激光雷达、惯导器件和相机的数据完成精确的状态估计。本文提出的框架由基于滤波方法的里程计模块和基于因子图的优化模块组成。为了保证实时性，我们只估计Kalman滤波器中的当前帧状态，然后在因子图优化环节进一步提高滑动窗口中的所有传感器位姿。得益于多模态传感器数据，我们的算法可以应对视觉退化、激光退化的场景，并且可以在端上计算平台实时运行。在我们采集的大量室内外数据上验证后，证明我们的框架得到的结果相对于其他sota的激光-惯导里程计、视觉-惯导里程计方法提高了精度。
主要贡献
我们以紧耦合的方式融合了激光雷达、惯导、视觉的数据，实验证明我们的方法可以鲁邦处理包括剧烈运动、传感器退化、甚至是有很多移动物体的隧道场景，包括激光雷达只有很小FoV的场景。 我们提出的框架包括一个高频运行的基于滤波方法的里程计模块，和一个低频运行的基于因子图的优化模块。里程计模块以一个kalman滤波器迭代融合雷达、惯导、视觉传感器，取得了实时的状态估计。因子图优化模块进一步优化滑窗内的各传感器位姿和视觉地图点位置。 基于这种多模态传感器紧耦合的方式，我们的方法得到的很高精度的位姿。大量实验证明我们的方法是可以构建大规模室内外建筑的稠密点云地图的。
算法流程
整体框架
算法步骤
本文选取IMU坐标系为body系，并假设LiDAR,IMU, Camera之间的外参和时间偏移都已知，但为了支持camera-imu外参在线标定. 令第帧IMU数据的状态向量为:
通过流行在幺元处的向量空间的'加减'运算我们可以得到离散化的状态向量更新方程：
记真值为， 估计值为，我们可以得到每次状态估计的误差:
综上，基于IMU数据的状态更新由状态向量的更新和状态误差向量的更新组成：
到这一步，目前还是一个kalman滤波器问题，那么如何把LiDAR、camera数据融合到这个滤波器中呢？作者在kalman滤波器中提出了一个最大后验估计量以及它的状态误差，于是公式(6)可被重新写成：
这样，状态估计问题就可以转变为优化IMU提供的位子先验和根据LiDAR/Camera数据估计的 的最大后验估计问题。
LiDAR数据融合
LiDAR数据的融合，使用的是plane-plane的误差来构造LiDAR的参差模块：
然后把这个参差模块写成与相关的后验估计问题的形式：
Camera数据融合
对于Camera数据，理论上与LiDAR数据相同：
因子图优化
基于因子图的优化模块会去优化滑动窗口中的LiDAR 位姿、Camera位姿，以及视觉特征三角化出来的稀疏地图点：
实验结果
我们使用自己搭建的手持式设备采集了各种剧烈运动、传感器退化、存在大量移动物体的长隧道场景，以及较大规模的室内外建筑物场景的数据。在鲁棒性、轨迹精度、建图精度几个方面与VINS-Mono, Fast-lio对比，都取得了较好的结果，证明了本文提出方法可以取得高精度、低漂移的里程计和建图结果。
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。