# LVIO-Fusion：在退化环境中的紧耦合激光雷达-视觉-惯性里程计与建图
张宏凯 $^{ID}$、杜亮 $^{ID}$、鲍胜 $^{ID}$（IEEE会员）、袁建军 $^{ID}$（IEEE高级会员）、马书根 $^{ID}$（IEEE会士）
摘要—在本文中，我们提出了一种创新的紧耦合激光雷达-视觉-惯性里程计与建图框架，命名为LVIO-Fusion。该框架能够在激光雷达特征退化且纹理缺失的环境中实现鲁棒且精准的状态估计与地图构建。LVIO-Fusion由两个子系统组成：激光雷达-惯性里程计（LIO）以及利用哈希表和八叉树来直接高效构建和更新点云地图的动态体素建图方法。地图点被直接投影到图像上以构建光流匹配，从而在更深层次上耦合激光雷达与相机的测量数据。随后，视觉惯性里程计（VIO）子系统采用由粗到精的状态估计策略，通过最小化帧间重投影误差和光度误差来实现图像对齐。此外，VIO子系统利用在线光度校准来获取环境中的真实辐射信息，进而提升系统的精度。我们在具有挑战性的环境中对本方法进行了广泛评估。结果证实，与激光雷达特征退化且纹理缺失场景下的其他最先进方法相比，本方法具有更高的精度和鲁棒性。
索引词—激光雷达特征退化、建图、SLAM、状态估计、纹理缺失。
## 一、引言
同时定位与建图（SLAM）是移动机器人的一项基础能力。由于SLAM能够实时估计六自由度（DoF）的自身运动，它在机器人导航、自动驾驶汽车、无人机等诸多应用中不可或缺。
与视觉传感器相比，激光雷达传感器能够提供精确且不受光照影响的环境感知能力，这使得激光雷达SLAM在状态估计和地图构建方面具备更高的精度和鲁棒性。然而，在隧道、管道、走廊等几何特征较少的场景中，激光雷达SLAM的性能很容易退化。
现有SLAM方法在纹理或几何特征丰富的环境中，采用相机 [^1]、[^2]、[^3] 或激光雷达（LiDAR）传感器 [^4]、[^5]、[^6] 等单一传感器时，已展现出令人满意的性能。视觉SLAM是通过最小化跟踪特征点的重投影误差或像素的光度误差来估计相机运动的技术。目前，在纹理变化丰富且光照稳定、充足的环境下，视觉SLAM能够实现令人满意的定位精度。通过多视角图像的视差三角测量，视觉SLAM可以恢复像素点的深度信息，并重建环境地图。然而，由于相机参数和多视角图像的基线存在校准误差，容易导致地图点的深度估计出现偏差。此外，由于相机的观测噪声、视野遮挡和曝光效应，视觉SLAM在构建精确的室外地图方面效果较差。而且，在低纹理、高速运动以及光照波动的条件下，视觉SLAM的性能也容易受到影响。
最重要的是，基于单一传感器（激光雷达或摄像头）的SLAM方法在退化场景下很难实现高鲁棒性的定位与建图。因此，融合激光雷达和摄像头的观测数据可以充分利用它们各自的优势并弥补其不足，从而解决退化问题。受此启发，我们提出了LVIO-Fusion，其贡献总结如下：
- 我们提出了一种激光雷达-视觉-惯性里程计与建图框架，该框架由直接激光雷达-惯性里程计（LIO）和直接视觉-惯性里程计（VIO）组成。所提出的系统能够捕捉细微的几何与辐射变化，从而克服无几何特征和无纹理环境中的传感器退化问题。
- 为了降低大量激光雷达点带来的计算负担，LIO子系统采用了动态体素映射技术来高效地进行点云配准与建图。
- 为了在几何稀疏且无纹理的环境中实现精确且鲁棒的状态估计，我们直接将
---
地图点投影到图像上以构建光流匹配，从而在更深层次上整合激光雷达与摄像头的测量数据。此外，VIO子系统利用在线光度校准来获取真实的辐射信息，进一步提升了在光照变化场景下的鲁棒性。
### II. 相关工作
在本节中，我们回顾了与我们的系统密切相关的先前工作，包括激光雷达SLAM、视觉SLAM以及激光雷达-视觉SLAM。
### A. 激光雷达SLAM
激光雷达SLAM通过迭代最近点（ICP）[^7]等扫描匹配方法来确定连续两帧之间的平移关系。为了提升扫描匹配的性能，通常会将激光雷达传感器与其他传感器（惯性测量单元、编码器）结合使用以实现定位与建图。在LOAM [^4]中，惯性测量单元用于校正扫描数据的偏移，并为扫描匹配提供初始平移估计。不过，由于惯性测量单元的测量数据与激光雷达里程计结果并未在联合优化中融合，因此LOAM系统被归类为松耦合框架。与松耦合系统相比，紧耦合系统通常具有更高的精度，因而受到越来越多的关注。基于图优化的LIOM [^8]在滑动窗口内对惯性测量单元和激光雷达的测量数据进行联合优化，从而得到状态估计。LIO-SAM [^5]提出了一种紧耦合的平滑与建图框架，该框架在图因子中融合了惯性测量单元预积分、激光雷达里程计、回环闭合因子以及GPS里程计数据，以实现更高精度的轨迹估计。与基于因子图优化方法的紧耦合SLAM框架相比，基于滤波器的框架通常效率更高。LINS [^9]提出了一种基于迭代卡尔曼滤波方法的紧耦合框架，用以获取六自由度自我运动信息。为了降低测量维度带来的计算复杂度，FAST-LIO [^10]提出了一种利用状态维度来计算卡尔曼增益的新公式。然而，上述激光雷达SLAM方法均基于特征点，会忽略细微的几何变化。FAST-LIO2 [^6]提出了一种增量式k维树（ikd-Tree），可高效地进行最近邻搜索以降低计算负担，并直接使用原始激光雷达点进行扫描匹配。
### B. 视觉SLAM
以往的视觉SLAM研究可分为基于特征的方法和直接法。基于特征的视觉SLAM会检测特征点，建立帧间的特征对应关系，并通过最小化重投影误差来估计相机的相对运动。MonoSLAM [^11]是首个采用扩展卡尔曼滤波（EKF）来解决单目SLAM问题的系统。该系统利用Shi Tomasi特征点来建立数据关联，进而估计相机状态并构建稀疏地图。不过，该系统仅对当前状态和路标点进行优化，而将对先前相机状态的信息进行边缘化处理，这会导致线性化误差的累积。相比之下，基于关键帧的系统通过选定的帧来估计相机状态，这类系统虽计算成本更高，但能执行更精确的捆绑调整（BA）优化。基于这一思路，PTAM [^12]是最具代表性的系统，它将状态跟踪与建图任务分配到并行线程中，能够在小型环境中实现实时运行。在此基础之上，于[^2]提出的ORB-SLAM更为完善，它通过提取ORB特征来实现短期、中期和长期的数据关联。基于ORB-SLAM，Mur-Artal提出了ORB-SLAM2 [^13]，该系统利用了立体相机或RGB-D相机提供的尺度信息。在RGB-D或立体模式下，该系统能够直接利用深度信息，从而提升状态跟踪的鲁棒性。其后续作品ORB-SLAM3 [^3]是一个紧耦合的视觉-惯性SLAM系统，它在滑动窗口内联合优化相机和IMU测量数据，从而实现了更高的定位精度。与ORB系列方法不同，VINS-Mono [^1]采用光流跟踪方法而非视觉描述符来为角点建立数据关联。随后，VINS-Mono采用BA优化方法来最小化重投影误差。
直接法视觉SLAM跳过了特征提取步骤，通过最小化连续帧间的光度误差来估计相机状态。早期的直接法视觉里程计如DTAM [^14]，通过将整幅图像与稠密深度图对齐来估计相机状态。不过，从单目图像中生成稠密深度图需要大量计算，通常需借助GPU才能实现实时运行。在[^15]提出的LSD-SLAM中，采用了一种大规模直接法单目SLAM方案，该系统通过直接最小化光度误差来跟踪相机位姿，并通过图优化来修正累积误差。需要注意的是，LSD-SLAM的运行依赖于对光度不变性的强假设。在[^16]提出的单目视觉里程计的直接稀疏建模方案中，进一步充分利用了相机的光度校准信息，包括已知的曝光时间、伽马校正和镜头衰减参数。与以往的直接法视觉SLAM系统相比，该系统基于相机光度校准提升了精度和鲁棒性。
此前的激光雷达与视觉SLAM研究分别展现了基于特征的方法和直接法的优势与特点。像FAST-LIO2这样的直接法激光雷达SLAM能够捕捉无几何特征环境中的细微几何变化，而在低纹理环境中，直接法视觉SLAM通常比基于特征的视觉SLAM具备更高的精度和鲁棒性 [^17]、[^18]、[^19]。因此，为了克服低纹理场景中的状态估计难题，LVIO-Fusion直接利用激光雷达和相机测量数据进行状态估计。
### C. 激光雷达-视觉SLAM
与LIO系统和VIO系统相比，LVIO系统[^20]、[^21]、[^22]通常具有更高的精度和鲁棒性，因为它能在传感器性能退化任务中融合激光雷达、相机和IMU的测量数据。在[^23]中提出的激光雷达-视觉-惯性里程计与建图系统采用了一种松耦合框架，用于从粗略到精细地估计状态。该系统结合IMU预测结果来执行视觉里程计计算，从而得到粗略的自身运动估计。随后，激光雷达里程计系统会进一步细化状态估计，并将当前扫描数据与全局地图进行配准。与松耦合系统相比，紧耦合的LVIO框架通常表现更优，这一点已在近期的研究中得到证实。LIC-Fusion[^24]便是一个基于MSCKF框架[^25]构建的紧耦合系统，它能够融合激光雷达、惯性传感器和相机的测量数据。该系统通过追踪稀疏的激光雷达特征点与视觉特征，来提供精确的运动信息。
---
Fig. 1. (a) Process of projecting map points onto the image. The LIO subsystem uses point-to-plane matching method to register the current scan to the global map, and projects map points to the current image. The VIO subsystem utilizes the projected points and Lucas Kanade (LK) optical flow algorithm to establish associations between two consecutive images, aligning them to refine the state estimation. (b) Green points are the projected points. (c) Based on this key innovation, Our LVIO-Fusion is able to reconstruct a dense, clear 3D map of the challenging scenarios that contain illumination change, altitude change, and scale change. The blue path is the travelling trajectory, and the details of the 3D map is shown in (d).
该系统能够在不同环境中实现精准跟踪。其后续版本LIC-Fusion 2.0 [^26]引入了一种新颖的平面特征跟踪方法以及新的异常值剔除方法，以实现高质量的数据关联。在[^27]中提出的LVI-SAM展示了一种结合了视觉-惯性系统（VIS）与激光雷达-惯性系统（LIS）的LVIO框架：LIS为VIS提供深度信息以辅助初始化，同时LIS利用VIS的状态作为扫描匹配的初始猜测。与LVI-SAM类似，mVIL-Fusion [^21]提出了一种基于双滑动窗口的新颖优化方法，包含扫描间平移与扫描到地图的旋转操作，以提升状态估计的精度。在[^28]中提出的R2LIVE则采用误差状态迭代卡尔曼滤波方法融合多种测量数据；当其中某个子系统失效时，R2LIVE的LIO与VIO子系统仍能独立运行。然而，上述LVIO框架均依赖特征来估计状态，会丢失许多细微特征，因此不适用于无几何结构或纹理缺失的退化环境。
### 三、系统概述
与上述基于特征的LVIO系统不同，本文提出的LVIO-Fusion由基于直接法的LIO子系统与VIO子系统构成。系统的整体架构如图2所示：LIO子系统利用IMU预积分对点云进行去畸变处理，并采用动态体素映射方法重建稠密地图。LVIO-Fusion将LIO子系统输出的状态作为VIO子系统的初始值，并利用在线光度校准获取真实的辐射度信息；随后，如图1(a)所示，将局部地图投影到当前图像上以直接获取被跟踪点。VIO子系统利用LK方法跟踪这些投影点以实现数据关联。
Fig. 2. System overview of LVIO-Fusion.
通过最小化帧间重投影误差，即可获得粗略的状态估计；进一步通过最小化帧间光度误差来对齐连续的两帧图像，从而得到更精细的状态估计，并更新全局点云地图。随后，将LVIO约束添加到滑动窗口因子图优化中，以修正IMU偏差。为了降低计算负载，投影地图点的分辨率被设定为0.05米；此外，为提高跟踪点的质量，如图1(b)所示，不会在图像边缘区域投影地图点。
### 四、预备知识
### A. 符号说明
在本节中，我们定义了本文涉及的坐标系和符号。我们用$^W(\cdot)$表示世界坐标系，用$^L(\cdot)$表示激光雷达坐标系，用$^C(\cdot)$表示相机坐标系；用$\mathbf{x}_k$表示第k次激光雷达扫描结束时刻的状态$\mathbf{x}$，用$\mathbf{b}_g$和$\mathbf{b}_a$分别表示陀螺仪和加速度计的偏差。
### B. 状态向量
在我们的系统中，将状态x定义为：
$$ \mathbf{x}=\begin{bmatrix}w\mathbf{R}^{T}&w_{\mathbf{p}^{T}}&w_{\mathbf{v}^{T}}&\mathbf{b}_{a}^{T}&\mathbf{b}_{g}^{T}\end{bmatrix} $$
其中$^W_R, ^W_P, ^W_V$表示世界坐标系下状态的旋转、位置和速度，$b_a$和$b_g$分别是加速度计偏差和陀螺仪偏差。我们将惯性测量单元（IMU）坐标系称为机体坐标系，并将第一个IMU坐标系作为世界坐标系。在此，我们离线校准了激光雷达、相机和IMU的外参与内参，并进行了时间同步[^29]。
### C. 相机光度校准
相机通过CMOS传感器记录反射光的强度，并以图像像素的形式呈现世界的辐射度。
---
图像像素所接收到的光能量。在CMOS传感器位置$\mathbf{q}$处接收到的总功率被称为辐照度$\mathbf{C}_i(\mathbf{q})$。直接法通过利用图像像素强度的平移来估计相机姿态。直接法依赖于光度不变性假设，即两幅图像中代表同一空间点的像素强度保持不变。通常，同一场景点在不同空间位置拍摄的图像中，像素值所反映的强度是一致的。然而，对于大多数相机而言，当像素强度靠近图像边缘时会出现渐晕效应。这种所谓的渐晕效应可能是由于镜头筒对光线的部分遮挡或镜头几何形状引起的。此外，接收到的辐照度是在曝光时间内累积而成的。为了克服渐晕效应，第k帧中位置$\mathbf{C}_i(\mathbf{q})$处的辐照度值$\mathbf{q}$可以通过将该场景点的辐射度$c_i$乘以一个渐晕因子$\tilde{V}(\mathbf{q}):\tilde{V}(\mathbf{q}) \in [0,1]$来获得。该渐晕因子取决于镜头区域的位置。
$$ \mathbf{C}_{i}(\mathbf{q})=V(\mathbf{q})c_{i}. $$
随后，像素强度$\mathbf{I}_i(\mathbf{q})$通过相机响应函数$\mathbf{O}_i$计算得出：
$$ \mathbf{I}_{i}(\mathbf{q})=\mathbf{O}_{i}(\sigma_{i}V(\mathbf{q})c_{i}),\mathbf{I}_{i}\in[0,255], $$
其中$\sigma$为曝光时间。如果曝光时间未知，则根据[^18]进行估算；否则从相机的API中获取。借助光度校准模型，可计算出观测到的像素辐射度：
$$ c_{i}=\frac{\mathbf{I}_{i}^{-1}(\mathbf{q})}{\sigma_{i}V(\mathbf{q})} $$
完成光度校准后，像素辐射度值$c_{i}$会被用于帧间光流跟踪以及最小化光度残差，相关细节将在V.C和V.D节中详细阐述。
### A. IMU积分
### V. 状态估计
为了解决剧烈运动期间的轨迹漂移问题，系统利用相邻两帧（激光雷达帧或视觉帧）之间的IMU积分（参考[^20]）来获取高频状态输出。此处我们假设在两帧的时间间隔内IMU偏差保持恒定。设j为IMU测量的索引，状态更新可计算如下：
$$ \mathbf{x}_{j}=\mathbf{x}_{j-1}\boxplus\triangle t\mathbf{f}_{j} $$
其中$\boxplus$是在流形上的运算符[^20]。IMU积分函数$f_j$定义如下：
$$ \mathbf{f}_{j}=\left[\begin{matrix}{\mathbf{\omega}_{r}-\mathbf{b_{g}}-\mathbf{n_{g}}}\\ {^{W}\mathbf{v}+\frac{1}{2}(^{W}\mathbf{R}_{j}(\mathbf{a}_{r}-\mathbf{b_{a}}-\mathbf{n_{a}})+^{W}\mathbf{g})\triangle t}\\ {^{W}\mathbf{R}_{j}(\mathbf{a}_{r}-\mathbf{b_{a}}-\mathbf{n_{a}})+^{W}\mathbf{g}}\\ {\mathbf{b}_{g}^{*}}\\ {\mathbf{b}_{a}^{*}}\\ \end{matrix}\right] $$
其中，$\omega_r$ 和 $\mathbf{a}_r$ 表示原始的IMU角速度和加速度，$\mathbf{b}_g$ 和 $\mathbf{b}_a$ 为IMU偏差，$\mathbf{n}_g$ 和 $\mathbf{n}_a$ 分别是 $\omega_r$ 和 $\mathbf{a}_r$ 的测量噪声，而 $\Delta t$ 则表示
Fig. 3. (a) Illustration of the incident angle, hidden point, denoted as $\alpha$, an $P_{4}$. (b) Illumination of the point-to-plane association.
IMU采样周期。上述数据是索引为j的IMU的测量值。$\mathbf{b}_{\mathbf{g}}$ 和 $\mathbf{b}_{\mathbf{a}}$ 会与激光雷达里程计约束以及视觉里程计约束一起在因子图中联合优化，从而得到更新基准 $\mathbf{b}_{\mathbf{g}}^{*}$ 和 $\mathbf{b}_{\mathbf{a}}^{*}$。
### B. 激光雷达点到平面残差
考虑到激光雷达的测量机制（反射角、反射能量），我们采用一种筛选策略来提取可靠的激光雷达点。入射角是指激光雷达光线与局部平面之间的夹角，其计算公式为
$$ \alpha(\mathbf{P}_{1})=\cos^{-1}\left(\frac{(\mathbf{P}_{0}-\mathbf{P}_{2})\cdot\mathbf{P}_{1}}{\left|\mathbf{P}_{0}-\mathbf{P}_{2}\right|\left|\mathbf{P}_{1}\right|}\right) $$
为了提高扫描匹配的准确性并为VIO子系统提供可靠的深度值，会移除以下激光雷达点：1) 激光雷达点的强度值过大或过小。由于强度过高通常会导致信号失真，而强度不足会降低信噪比，这两种情况都会降低激光雷达点的测距精度。2) 入射角值接近 $\pi$ 或 0。因为这类激光雷达点的光斑较大，其测量到的距离实际上是光斑内所有距离的平均值，而非某个特定点的距离。这种现象会降低激光雷达点的测距精度。3) 激光雷达点被物体遮挡。因为被遮挡的激光雷达点会导致错误的深度投影，进而影响视觉里程计的准确性。如果满足 $P_4$ 和 $|D(P_4) - D(P_3)| \geq 0.1D(P_4)$，则图3(a)中的激光雷达点 $D(P_4) \geq D(P_3)$ 就是被遮挡的点，其中D表示激光雷达点的距离。
为了消除运动引起的点云畸变，我们采用了基于IMU积分的线性插值方法。用 $(\mathbf{R}_k, \mathbf{T}_k)$ 表示当前激光雷达帧k结束时的状态，$(\mathbf{R}_{k-1}, \mathbf{T}_{k-1})$ 表示上一帧结束时的状态，$(\mathbf{R}_k^{k-1}, \mathbf{T}_k^{k-1})$ 表示通过IMU积分计算得到的上一帧与当前帧之间的相对旋转和平移量，即 $\Delta \mathbf{x} = \sum_{s=j}^{j+n} \Delta t \mathbf{f}_s$。$\Delta t$ 是IMU采样周期，$s = j$ 表示上一激光雷达帧结束时的IMU索引，$s = j + n$ 表示当前激光雷达帧结束时的IMU索引。
$$ \mathbf{R}_{k}=\mathbf{R}_{k-1}\mathbf{R}_{k}^{k-1},\mathbf{T}_{k}=\mathbf{R}_{k-1}\mathbf{T}_{k}^{k-1}+\mathbf{T}_{k-1} $$
随后可计算出第i个点的线性插值位姿：
$$ \mathbf{R}_{i}^{k-1}=\mathbf{I}+\widehat{\boldsymbol{\omega}}_{i}\sin(s\theta)+\widehat{\boldsymbol{\omega}}_{i}^{2}(1-\cos(s\theta)),\mathbf{T}_{i}^{k-1}=s\mathbf{T}_{k}^{k-1}\quad $$
其中 $\widehat{\omega}_i$ 是 $\omega_i$ 的反对称矩阵，它是 $\mathbf{R}_i^{k-1}$ 旋转轴的单位向量，$\theta$ 是上一帧与当前帧之间的角度变化，s 计算如下
---
$s = (t - t_{k-1})/(t_k - t_{k-1})$，t为 $\mathbf{P}_{k_i}$、$t \in [t_{k-1}, t_k]$ 的时间戳。因此，我们可以得到时间t时 $\mathbf{P}_{k_i}$ 的旋转和平移量：
$$ \mathbf{R}_{k_{i}}=\mathbf{R}_{k-1}\mathbf{R}_{i}^{k-1},\mathbf{T}_{k_{i}}=\mathbf{R}_{k-1}\mathbf{T}_{i}^{k-1}+\mathbf{T}_{k-1} $$
通过IMU积分估算出 $R_{k_i}$ 和 $T_{k_i}$ 后，即可消除激光雷达点的偏斜，并在最小化点到平面残差前将其投影到全局地图中。
$$ ^{W}\mathbf{P}_{k_{i}}=\mathbf{R}_{k_{i}}\mathbf{P}_{k_{i}}+\mathbf{T}_{k_{i}} $$
为了在运动过程中捕捉细微的几何特征，我们省略了特征提取模块，而是将所有激光雷达点都用于扫描匹配。采用点面关联的激光雷达子系统会将新帧中的点注册到局部地图（最近50帧激光雷达数据）中，以进行状态估计。对于当前帧中的一个点$^W\mathbf{P}_{k_c}$，局部地图中会找出构成平面的10个最近点，如图3(b)所示。根据[^6]的描述，如果由这10个最近点构成的协方差矩阵的最小特征值小于0.01，那么这组点就构成一个平面。为了提升搜索速度，我们采用了动态体素图来进行点云与地图的数据关联，具体细节将在第VI-A节中详述。随后即可计算出点面残差。
$$ \mathbf{e}_{k_{i}}=\mathbf{n}_{k_{i}}^{T}(^{W}\mathbf{P}_{k_{i}}-\mathbf{q}_{k_{i}}) $$
其中$\mathbf{n}_{k_i}^T$是对应平面的法向量，而$\mathbf{q}_{k_i}$则是位于该平面上的一个点。因此，点面误差的代价函数可表示为
$$ \mathbf{f}_{l}=argmin\frac{1}{2}\sum_{i=0}^{n}\left\|\mathbf{e}_{k_{i}}^{2}\right\| $$
接着，采用Levenberg-Marquardt（L-M）方法来最小化点面残差，计算得到的状态变换$\triangle \mathbf{x}_L$可按如下方式叠加到$\mathbf{x}_{k-1}$上：
$$ \mathbf{x}_{k}=\mathbf{x}_{k-1}\boxplus\triangle\mathbf{x}_{L} $$
### C. 帧间视觉重投影残差
假设当前局部地图中的激光雷达点$\mathbf{M}_k = \{\mathbf{P}_{k_0}, \ldots, \mathbf{P}_{k_n}\}$被投影到前一图像帧中，记为$\gamma_{k-1} = \{\gamma_{k-1_0}, \ldots, \gamma_{k-1_n}\}$。我们利用Lucas-Kanade光流算法来提取当前图像帧中$\gamma_{k-1}$对应的像素，记为$\gamma_k = \{\gamma_{k_0}, \ldots, \gamma_{k_n}\}$。对于每个地图点，都需要将其投影到当前相机帧中。我们以第i个点$\mathbf{P}_{k_i}$为例说明：
$$ \begin{aligned}{{^{C}}\mathbf{P}_{k_{i}}}&{{}={(^{W}\mathbf{R}_{I}\cdot{^{I}\mathbf{R}_{C}})^{T}}\cdot{^{W}\mathbf{P}_{k_{i}}}-{({^{W}\mathbf{R}_{I}\cdot{^{I}\mathbf{R}_{C}})^{T}}\cdot{^{W}\mathbf{P}_{I}}}}\\ {}&{{}\quad-{^{I}\mathbf{R}_{C}^{T}}\cdot{^{I}\mathbf{P}_{C}},}\\ \end{aligned}\quad(1 $$
其中$^C\mathbf{P}_{k_i}$是投影到当前相机帧上的地图点，$^I\mathbf{R}_C$和$^I\mathbf{P}_C$则是IMU与相机之间的外部参数。
在迭代过程中，投影后的地图点$^C\mathbf{P}_{k_i}$的像素位置需要不断趋近于像素点$\gamma_{k_i}$。此时便可计算出重投影误差：
$$ \mathbf{e}\big(\gamma_{k_{i}},\mathbf{x}_{k},{}^{C}\mathbf{P}_{k_{i}}\big)=\gamma_{k_{i}}-\zeta\big({}^{C}\mathbf{P}_{k_{i}},\mathbf{x}_{k}\big), $$
其中$\mathbf{x}_k$是由IMU积分或IO系统（当相机时间戳与激光雷达时间戳对齐时）在当前时刻估算出的状态，而$\zeta$则是投影函数。
$$ \begin{array}{r}{\zeta_{m}(^{C}\mathbf{P}_{k_{i}},\mathbf{x}_{k})=\bigg[f_{m,u}\frac{\mathbf{p}_{x_{k,i}}}{\mathbf{p}_{z_{k,i}}}+c_{m,u},f_{m,v}\frac{\mathbf{p}_{y_{k,i}}}{\mathbf{p}_{z_{k,i}}}+c_{m,v}\bigg],}\end{array} $$
其中$(f_{m,u}, f_{m,v})$和$(c_{m,u}, c_{m,v})$分别是与相机相关的局部长度和主点。重投影误差的代价函数可表示为
$$ \mathbf{f}_{r}=argmin\frac{1}{2}\sum_{i=0}^{n}\left\|\mathbf{e}(\gamma_{k_{i}},\mathbf{x}_{k},^{C}\mathbf{P}_{k_{i}})^{2}\right\| $$
在最小化帧间重投影误差后，状态变换$\triangle \mathbf{x}_r$可按如下方式叠加到$\mathbf{x}_k$上：
$$ \widehat{\mathbf{x}}_{k}=\mathbf{x}_{k}\boxplus\triangle\mathbf{x}_{r} $$
### D. 帧间视觉辐射度残差
通过最小化帧间重投影误差，我们已经得到了粗略的状态估计。借助这个粗略的状态$\widehat{x}_k$，就可以将局部地图中当前的激光雷达点$M'_k$投影到图像视场内的当前图像中，从而确定其对应的像素位置。此处，每个图像都经过了光度校正，通过计算观测到的像素辐射度来实现，相关细节见第IV-C节。以当前图像中投影点$M'_k$为例，其帧间辐射度误差计算如下：
$$ \mathcal{F}(\rho_{k_{i}},\widehat{\mathbf{x}}_{k},\mathbf{M}_{k}^{\prime})=\rho_{k_{i}}-\varphi_{k_{i}} $$
其中，$\rho_{k_i}$ 是当前图像像素块的平均辐射度，$\varphi_{k_i}$ 是上一幅图像像素块的平均辐射度。为了充分利用图像信息，我们采用图像块的光度信息而非单个像素。此处我们将9个像素点定义为一个像素块，如图1(a)所示。该像素块的中心位置由$M'_k$的投影点确定。
通过最小化辐射度误差，状态变换$\Delta x_p$可按照如下方式叠加到$\hat{x}_k$上：
$$ \mathbf{f}_{p}=\underset{}{a r g m i n}{\frac{1}{2}}\sum_{i=0}^{n}\left\|\mathcal{F}(\rho_{k_{i}},\widehat{\mathbf{x}}_{k},\mathbf{M}_{k}^{\prime})^{2}\right\| $$
$$ \mathbf{x}_{k}=\widehat{\mathbf{x}}_{k}\boxplus\triangle\mathbf{x}_{p} $$
$f_{p}$ 是用于最小化光度误差的成本函数。这一优化问题可通过L-M优化方法求解。计算得到的状态用于将当前扫描数据配准到全局地图中。
### 六、地图更新与优化
### A. 地图更新
受FASTER-LIO [^30]的启发——该方法采用增量体素作为点云的空间数据结构，我们为点云与地图的查询、插入和更新操作引入了动态体素结构。该动态体素地图由哈希表和八叉树共同组织。具体而言，首先在世界坐标系中将局部地图空间划分为粗粒度体素；接着，包含地图点的体素会被索引到哈希表中。若某个体素内的地图点构成平面，则这些地图点的平面参数
---
Fig. 4. Datasets are collected on two platforms. (a) A handheld device. (b) A mobile vehicle.
会被存储起来，用于点对面扫描匹配。否则，该体素会基于八叉树被进一步划分为八个子体素，并在每个子体素中计算平面参数，直至达到最大层级。此外，不含地图点的体素被标记为非活动体素，反之则为活动体素。非活动体素不参与点对面匹配过程。
当新的激光雷达扫描数据被配准到全局地图时，体素的平面参数需要动态更新。然而，随着新点的不断输入，更新平面参数的处理时间也会随之增加。为解决这一问题，我们记录了添加新点时当前体素平面法向量的变化。幸运的是，当新点数量达到20个时，平面法向量往往趋于稳定，此时便会保存当前的平面法向量。出于效率考虑，接收到新点时，仅用最新的10个点来计算新的平面法向量。若最新计算的平面向量与之前存储的法向量差异较大，则需按照上述步骤重新构建体素地图。
### B. 因子图优化
如第V-A节所述，惯性测量单元积分的精度会受到惯性测量单元偏差的影响，而这些偏差可通过其他约束条件进行校正。为了进一步提升惯性测量单元积分的鲁棒性与精度，我们利用因子图优化方法，在图像关键帧的滑动窗口内对惯性测量单元偏差进行优化。我们的因子图优化框架与LVI-SAM类似，但融入了激光雷达-视觉里程计约束，并在测量层面实现融合。我们采用iSAM2 [^31]算法来构建滑动窗口因子图，以优化惯性测量单元偏差。
### 七、实验与结果
在本节中，我们在广泛的挑战场景下验证了所提系统相较于其他最先进系统的优势。我们的手持式数据采集设备如图4(a)所示，包括一台HIKROBOT MV-CA016-10UC相机、一台LIVOX AVIA激光雷达、一台Xsens MIT-600惯性测量单元（IMU）以及一台计算平台（配备Intel I5-10210U CPU和64G内存）。移动车辆还额外配备了实时动态定位（RTK）设备。为了评估LVIOL-Fusion的精度，我们利用实时动态定位（RTK）来获取真实轨迹。在GPS信号受限的环境中，我们采用重定位技术来评估LVIOL-Fusion的精度。
Fig. 5. Experiments in LiDAR-degenerated and low-texture environments. (a) Without loop closure detection, LVIO-Fusion can loop by itself. (b) The point cloud is clear and free from any overlapping. (c) The white wall that is both LiDAR-degenerated and low texture.
Fig. 6. Comparison of the estimated trajectories in Experiment-1.
### A. 实验1：退化环境下的精度与鲁棒性评估
在本实验中，我们测试了LVIO-Fusion在无几何特征和无纹理特征场景下的鲁棒性与精度。我们选择一面白墙作为算法的验证场景，该墙面为单一平面，几何与纹理特征极为有限，如图5(c)所示。我们将手持平台移动一段距离后使其朝向白墙，随后再将其移回初始位置。我们对比了本系统与当前最先进的激光雷达-视觉-惯性（LVI）SLAM方法的性能。本系统、FAST-LVIO [^20]、R2LIVE [^28]、LVISAM [^27]的估计轨迹如图6所示。由于白墙仅包含单一平面且无明显纹理特征，基于几何与视觉特征的R2LIVE和LVI-SAM系统均出现了明显的轨迹漂移现象。FAST-LVIO系统直接利用激光雷达点云与辐射信息来进行状态估计，但FAST-LVIO仅利用了与图像对齐的稀疏点云地图（密度较低）中的光度信息，丢弃了大量图像信息。此外，FAST-LVIO缺乏光度校准过程，导致图像光度对齐出现偏差并引发轨迹漂移。
我们提出的系统首先通过最小化帧间重投影误差来获得粗略的状态估计，随后通过最小化帧与帧之间图像块的光度误差来优化这一估计结果。在此类退化环境中，采用由粗到精的状态估计方式能够抑制光度对齐的偏差。此外，利用图像块而非单个点来最小化光度误差，能够充分利用图像信息。
---
Fig. 7. Estimated results (position and attitude) of all tests in Experiment-1. We test our system ten times using the collected data from Experiment-1. We compute the mean, the maximum, and minimum values of the estimated position and attitude from ten tests. Solid line: mean value. Shaded part: the range between the maximum and minimum values.
Fig. 8. Reconstructed 3D maps in Experiment-2. (a) shows LVIO can return back to the origin without loop closure detection. Some details of the map are shown in (b) and (c).
最终系统成功返回原点，如图5(a)所示。实验1中所有测试的估计结果如图7所示。阴影部分几乎与平均值对齐，这表明我们的系统在此类退化条件下运行稳定。我们提出的系统在退化环境中表现优异，返回原点时的平移漂移量为0.0514米，旋转漂移量为2.75度。
### B. 实验2：室内外环境下的精度评估
在本实验中，我们让所提系统应对极其复杂的环境：包括从室外到室内、从室内到室外的光照变化，从地面到屋顶的高度变化，以及从开阔区域到狭窄区域的尺度变化。我们在上海大学周边采集了四次数据，采集路径略有不同，但起始点和终点均相同。重建后的3D地图鸟瞰图如图8所示，其中的点云3D地图清晰可辨且无重复现象。
Fig. 9. Four traveling end-to-end trajectories in Experiment-2. The length of four traveling trajectories are 1465 m, 1531 m, 1892 m, and 1963 m respectively.
TABLEI
COMPARISON OF END-TO-END TRANSLATION ERRORS IN EXPERIMENT-2
| Algorithm | Type | End-to-End Translation Errors [m] |
| Seq.0 | Seq.1 | Seq.2 | Seq.3 |
| 1465m | 1531m | 1892m | 1963m |
| LIO-SAM[^5] | LIO | 3.473 | 4.398 | 5.560 | 4.357 |
| FAST-LIO2[^6] | LIO | 0.155 | 0.129 | 0.996 | 1.229 |
| VINS-Mono[^1] | VIO | 5.821 | 18.798 | 21.003 | 14.421 |
| LVI-SAM[^27] | LVIO | 1.080 | 2.317 | 3.457 | 3.526 |
| R2LIVE[^28] | LVIO | 0.159 | 0.131 | 1.005 | 1.159 |
| FAST-LIVO[^20] | LVIO | 0.123 | 0.141 | 0.865 | 0.904 |
| Ours | LVIO | 0.082 | 0.098 | 0.420 | 0.217 |
TABLE II
RELATIVE TRANSLATION ERRORS IN EXPERIMENT-3
| Relative translation errors (m) w.r.t RTK in Experiment-3 |
| Algorithm | 200 s | 400 s | 600 s | 800 s | 1000 s |
| Ours | 0.295 | 0.274 | 0.264 | 0.367 | 0.385 |
| LIO-SAM[^5] | 1.049 | 2.913 | 2.253 | 3.1188 | 5.197 |
| FAST-LIO2[^6] | 0.221 | 0.352 | 1.039 | 1.565 | 1.393 |
| VINS-Mono[^1] | 0.453 | 3.766 | 19.385 | 24.871 | 15.069 |
| LVI-SAM[^27] | 1.334 | 1.515 | 2.037 | 2.655 | 3.129 |
| R2LIVE[^28] | 0.351 | 0.873 | 0.984 | 1.211 | 1.189 |
| FAST-LIVO[^20] | 0.201 | 0.488 | 0.859 | 1.147 | 0.826 |
或者出现轨迹漂移现象。图9展示了这四条运动轨迹。在缺乏闭环检测技术的情况下，这四条轨迹仍能形成闭合环路且漂移极小，这表明所提出的算法在如此复杂的环境中具有极高的鲁棒性和精度。此外，表I列出了这四条轨迹的端到端平移误差。结果显示，LVIO-Fusion的性能优于现有的LIO、VIO和LVIO方法。
### C. 实验三：基于RTK的精度评估
在本实验中，我们采集了一段包含RTK测量值作为真值的户外序列数据。如图10所示，我们将LVIO-Fusion、LIO-SAM、FAST-LIO2、VINS-Mono、LVI-SAM、R2LIVE、FAST-LVIO所估计的运动轨迹与真值轨迹进行对比。结果表明，LVIO-Fusion生成的轨迹与真值轨迹最为接近。此外，表II给出了采样时刻相对于RTK测量值的平移误差。结果显示：
---
Fig. 10. Comparison of estimated trajectories with RTK ground-truth in Experiment-3.
在整个运动过程中，LVIO-Fusion均保持了较高的精度，其相对平移误差最小。
### 八、结论
本文提出了一种紧耦合的激光雷达-惯性-视觉框架，该框架在测量层面将激光雷达与摄像头数据相融合。所提出的系统直接将地图点投影到图像上以建立数据关联，并通过最小化帧间重投影误差和光度误差来实现由粗到精的状态估计。在所有测试中，即便在传感器退化的情况下，我们的系统也能在极具挑战性的场景中实现高定位精度和鲁棒性。我们还与其他先进的LIO、VIO、LVIO SLAM方法进行了多项对比，结果表明我们的系统具备更好的精度和鲁棒性。在未来的工作中，我们将进一步探索点云与图像之间的映射关系，以充分利用高分辨率的图像信息。
## 参考文献
[^1]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018.
[^2]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015.
[^3]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021.
[^4]: J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping in real-time,” Robot. Sci. Syst., vol. 2, no. 9, pp. 1–9, 2014.
[^5]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5135–5142.
[^6]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 2022.
[^7]: J. Zhang, Y. Yao, and B. Deng, “Fast and robust iterative closest point,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 44, no. 7, pp. 3450–3466, Jul. 2022.
[^8]: S. Zhao, Z. Fang, H. Li, and S. Scherer, “A robust laser-inertial odometry and mapping method for large-scale highway environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 1285–1292.
[^9]: C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, "LINS: A lidar-inertial state estimator for robust and efficient navigation," in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 8899–8906.
[^10]: W. Xu and F. Zhang, "FAST-LIO: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter," IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.
[^11]: A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM: Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052–1067, Jun. 2007.
[^12]: G. Klein and D. Murray, “Parallel tracking and mapping for small or workspaces,” in Proc. IEEE/ACM 6th Int. Symp. Mixed Augmented Reality, 2007, pp. 225–234.
[^13]: R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017.
[^14]: R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “DTAM: Dense tracking and mapping in real-time,” in Proc. Int. Conf. Comput. Vis., 2011, pp. 2320–2327.
[^15]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular slam," in Proc. Eur. Conf. Comput. Vis., Springer, 2014, pp. 834–849.
[^16]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018.
[^17]: S. Zhong and P. Chirarattananon, “Direct visual-inertial ego-motion estimation via iterated extended Kalman filter,” IEEE Robot. Automat. Lett., vol. 5, no. 2, pp. 1476–1483, Apr. 2020.
[^18]: D. Luo, Y. Zhuang, and S. Wang, "Hybrid sparse monocular visual odometry with online photometric calibration," Int. J. Robot. Res., vol. 41, no. 11-12, pp. 993–1021, 2022.
[^19]: J. Mo, M. J. Islam, and J. Sattar, "Fast direct stereo visual SLAM," IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 778–785, Apr. 2021.
[^20]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 4003–4009.
[^21]: Y. Wang and H. Ma, “mVIL-fusion: Monocular visual-inertial-LiDAR simultaneous localization and mapping in challenging environments,” IEEE Robot. Automat. Lett., vol. 8, no. 2, pp. 504–511, Feb. 2023.
[^22]: X. Zhong, Y. Li, S. Zhu, W. Chen, X. Li, and J. Gu, "LVIO-SAM: A multi-sensor fusion odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Biomimetics, 2021, pp. 440–445.
[^23]: J. Zhang and S. Singh, “Laser-visual-inertial odometry and mapping with high robustness and low drift,” J. Field Robot., vol. 35, no. 8, pp. 1242–1264, 2018.
[^24]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “LIC-fusion: LiDAR-inertial-camera odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 5848–5854.
[^25]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. Robot. Automat., 2007, pp. 3565–3572.
[^26]: X. Zuo et al., “LIC-fusion 2.0: LiDAR-inertial-camera odometry with sliding-window plane-feature tracking,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5112–5119.
[^27]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 5692–5698.
[^28]: J. Lin, C. Zheng, W. Xu, and F. Zhang, “R $^{2}$ LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021.
[^29]: C. Yuan, X. Liu, X. Hong, and F. Zhang, “Pixel-level extrinsic self calibration of high resolution LiDAR and camera in targetless environments,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7517–7524, Oct. 2021.
[^30]: C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Faster-LIO: Lightweight tightly coupled LiDAR-inertial odometry using parallel sparse incremental voxels,” IEEE Robot. Automat. Lett., vol. 7, no. 2, pp. 4861–4868, Apr. 2022.
[^31]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, "iSAM2: Incremental smoothing and mapping using the Bayes tree," Int. J. Robot. Res., vol. 31, no. 2, pp. 216–235, 2012.