# LIC-Fusion：激光雷达-惯性-相机里程计
左星星*，Patrick Geneva††，李宇硕†，刘勇*，黄国权†
摘要—本文提出了一种名为激光雷达-惯性-相机融合（LIC-Fusion）的紧耦合多传感器融合算法，该算法能够高效融合惯性测量单元（IMU）数据、稀疏视觉特征以及提取出的激光雷达点云。具体而言，所提出的LIC-Fusion可在三种异步传感器之间实时进行空间与时间校准，以补偿可能出现的校准偏差。其核心贡献在于：在高效的基于多状态约束卡尔曼滤波（MSCKF）框架内，对激光雷达扫描中检测并跟踪到的稀疏边缘/曲面特征点，与稀疏视觉特征观测值及IMU读数进行最优（仅存在线性化误差）的多模态传感器融合。我们在室内外环境中开展了大量实验，结果表明，在估计精度以及对剧烈运动的鲁棒性方面，所提出的LIC-Fusion均优于现有的视觉-惯性里程计（VIO）和激光雷达里程计方法。
## 一、引言与相关研究
能够精准追踪自动驾驶车辆与移动感知系统的三维运动至关重要。一种常用的解决方案是搭载单目相机的惯性导航系统（INS），这类系统近年来受到了广泛关注[^1]、[^2]、[^3]、[^4]、[^5]、[^6]，部分原因是它们具备互补的传感模式，且成本低、体积小。然而，相机受光照条件限制，在低光或夜间环境下无法提供高质量的信息。相比之下，三维激光雷达传感器无论光照条件如何，都能提供更稳健、更精确的距离测量数据，因此在机器人定位与建图领域应用广泛[^7]、[^8]、[^9]、[^10]。不过三维激光雷达存在点云稀疏、成本高、采集速率低于相机的问题。截至目前，激光雷达仍较为昂贵，限制了其广泛应用，但得益于新兴技术的发展，预计其成本在未来几年将大幅下降[^11]。惯性测量单元（IMU）可测量局部角速度与线性加速度，能在动态轨迹中提供大量信息，但若不与其他信息融合，就会因噪声而产生较大漂移。在本研究中，我们聚焦于激光雷达-惯性-相机里程计（LIC）。
Fig. 1: The proposed LIC-Fusion fuses both sparse visual features tracked in images and LiDAR features extracted in point clouds. The LiDAR points in red and blue are the edge and plane features, respectively. Estimated trajectory is marked in green.
它能整合各类传感器模式的优势，在所有场景下提供快速且稳健的三维运动追踪解决方案。
尤其是相机与激光雷达数据的多模态测量融合，通常会在同步定位与地图构建（SLAM）框架内予以实现[^12]。例如，Zhang、Kaess和Singh[^13]将激光雷达的深度信息与视觉相机特征关联起来，由此形成了可视为具备增强激光雷达深度的RGBD系统。后来，Zhang和Singh[^14]开发了一个融合视觉里程计（VO）与激光雷达里程计的通用框架：利用高频视觉里程计估算整体自身运动，同时利用低速率的激光雷达里程计将扫描数据与地图匹配，进而优化视觉里程计的估算结果。Shin、Park和Kim[^15]在直接视觉SLAM方法中利用了激光雷达提供的深度信息，通过迭代方式最小化光度误差。同样，在[^12]中，激光雷达也被用于通过拟合局部平面来为视觉特征补充深度信息，该方法在自动驾驶场景中表现优异。
最近，Zhang和Singh [^16] 开发了一种激光视觉-惯性里程计与建图系统，该系统采用了顺序多层处理流程，由三个主要组件构成：IMU预测、视觉-惯性里程计以及扫描匹配优化。具体而言，IMU测量数据用于预测，而视觉-惯性子系统则通过迭代方式最小化IMU预积分与视觉特征重投影误差的联合代价函数。随后，通过迭代最近点（ICP）算法进行激光雷达扫描匹配，以进一步细化先前的位姿估计结果。
---
请注意，迭代优化和ICP都需要复杂的处理流程与并行计算能力，才能实现实时性能。此外，这本质上是一种松耦合融合方法：因为只有来自VIO模块的位姿估计结果才会被输入到激光雷达扫描匹配子系统中，而扫描匹配无法直接处理原始的视觉-惯性测量数据，从而丢失了激光雷达与VIO之间的相关性信息。
在本文中，我们提出了一种快速、紧耦合、单线程的激光雷达-惯性-相机（LIC）里程计算法，该算法在计算效率高的多状态约束卡尔曼滤波（MSCKF）框架内实现了在线空间与时间多传感器校准 [^1]。本研究的主要贡献如下：
- 我们开发了紧耦合的LIC里程计（称为LIC-Fusion），它能够实现高效的6自由度位姿估计，并具备在线空间与时间校准功能。所提出的LIC-Fusion在MSCKF框架内高效融合了IMU测量数据、稀疏视觉特征以及两种不同的稀疏激光雷达特征（见图1）。校准后的外部参数与估计位姿对测量数据的依赖性被明确建模并给出解析解。
- 我们在包括室内和室外环境在内的真实场景中对该方法进行了广泛的实验验证，结果表明所提出的LIC-Fusion比现有最先进的方法更精确且更稳健。
### II. 所提出的LIC-Fusion算法
在本节中，我们将详细介绍所提出的LIC-Fusion里程计，该算法在MSCKF [^1] 框架内紧密融合了激光雷达、惯性传感器和相机的测量数据。
### A. 状态向量
所提方法的状态向量包括时间k时的IMU状态 $\mathbf{x}_I$、IMU与相机之间的外部参数 $\mathbf{x}_{calib\_C}$、IMU与激光雷达之间的外部参数 $\mathbf{x}_{calib\_L}$，以及一个克隆体滑动窗口，其中包含过去m个图像时刻的局部IMU克隆体 $\mathbf{x}_C$ 以及过去n个激光雷达扫描时刻的克隆体 $\mathbf{x}_L$。完整的状态向量表示为：
$$ \mathbf{x}=\left[\mathbf{x}_{I}^{\top}\mathbf{\Sigma}\mathbf{x}_{c a l i b\_{C}}^{\top}\mathbf{\Sigma}\mathbf{x}_{c a l i b\_{L}}^{\top}\mathbf{\Sigma}\mathbf{x}_{C}^{\top}\mathbf{\Sigma}\mathbf{x}_{L}^{\top}\right]^{\top} $$
其中
$$ \mathbf{x}_{I}=\left[\mathbf{\Delta}_{G}^{I_{k}}\mathbf{\bar{q}}^{\top}\mathbf{b}_{g}^{\top}\mathbf{\Lambda}^{G}\mathbf{v}_{I_{k}}^{\top}\mathbf{b}_{a}^{\top}\mathbf{\Lambda}^{G}\mathbf{p}_{I_{k}}^{\top}\right]^{\top} $$
$$ \mathbf{x}_{c a l i b\_{C}}=\left[\mathbf{\Sigma}_{I}^{C}\mathbf{\bar{q}}^{\top}\mathbf{\Sigma}^{C}\mathbf{p}_{I}^{\top}\mathbf{\Sigma}t_{d C}\right]^{\top} $$
$$ \mathbf{x}_{c a l i b\_{L}}=\left[\begin{matrix}{^{L}_{I}\mathbf{\bar{q}}^{\top}\mathbf{\Lambda}^{L}\mathbf{p}_{I}^{\top}\mathbf{\Lambda}t_{d L}}\\ \end{matrix}\right]^{\top} $$
$$ \mathbf{x}_{C}=\left[\mathbf{\Psi}_{G}^{I_{a_{1}}}\mathbf{\Psi}_{G}^{-}\mathbf{\Psi}_{I_{a_{1}}}^{G}\mathbf{p}_{I_{a_{1}}}^{\top}\cdots\mathbf{\Psi}_{G}^{I_{a_{m}}}\mathbf{\Psi}_{G}^{-}\mathbf{\Psi}_{I_{a_{m}}}^{G}\right]^{\top} $$
$$ \mathbf{x}_{L}=\left[\mathbf{\Delta}_{G}^{I_{b_{1}}}\mathbf{\bar{q}}^{\top}\mathbf{\Lambda}^{G}\mathbf{p}_{I_{b_{1}}}^{\top}\cdots\mathbf{\Lambda}_{G}^{I_{b_{n}}}\mathbf{\bar{q}}^{\top}\mathbf{\Lambda}^{G}\mathbf{p}_{I_{b_{n}}}^{\top}\right]^{\top} $$
$I_G^k \bar{q}$ 是对应于3D旋转矩阵 [^17] 的JPL四元组 $I_G^k \mathbf{R}$，它表示从全局参考坐标系 $\{G\}$ 到时间 $\{I_k\}$ 时IMU的局部坐标系 $t_k$ 的旋转。$G_{\mathbf{v}_{I_k}}$ 和 $G_{\mathbf{p}_{I_k}}$ 分别表示时间 $t_k$ 时全局坐标系下的IMU速度与位置。$\mathbf{b}_g$ 和 $\mathbf{b}_a$ 则是陀螺仪与加速度计的偏差。$\bar{I}^{\bar{q}}$ 和 $\bar{C}_{\mathbf{p}_I}$ 表示相机传感器坐标系 $\{C\}$ 与IMU坐标系 $\{I\}$ 之间的刚体变换。类似地，$\bar{I}^{\bar{q}}$ 和 $\bar{L}_{\mathbf{p}_I}$ 表示激光雷达与IMU坐标系之间的3D刚体变换。
我们还共同估计了外部传感器与IMU之间的时间偏移，由于传感器延迟、时钟偏差或数据传输延迟，这种偏移在低成本设备中十分常见。我们将IMU时间视为“真实”的基准时钟，并建模认为相机和激光雷达分别存在偏移量 $t_{dC}$ 和 $t_{dL}$，从而可以按如下方式校正测量时间：
$$ t_{I}=t_{C}+t_{d C} $$
$$ t_{I}=t_{L}+t_{dL} $$
其中 $t_{C}$ 和 $t_{L}$ 分别是相机和激光雷达时钟中记录的时间。更多细节请参阅 [^18]。
在论文中，我们定义状态的真实值为 x，估计值为 $\hat{x}$，对应的误差状态为 $\delta x$，它们通过以下广义更新运算相关联：
$$ \mathbf{x}=\hat{\mathbf{x}}\boxplus\delta\mathbf{x} $$
对于向量空间中的状态 $\boxplus$，运算 $\mathbf{v}$ 就是简单的欧几里得加法，即 $\mathbf{v} = \hat{\mathbf{v}} + \delta\mathbf{v}$；而对于四元数而言，其运算规则如下：
$$ \bar{q}\approx\begin{bmatrix}\frac{1}{2}\delta\boldsymbol{\theta}\\ 1\end{bmatrix}\otimes\hat{\bar{q}} $$
其中 $\otimes$ 表示 JPL 四元数乘法 $^{[17]}$。
### B. IMU 传播
IMU 提供角速度和线性加速度测量值，我们用以下的连续时间运动学模型来描述它们 [^17]：
$$ _{G}^{I_{k}}\dot{\bar{q}}(t)=\frac{1}{2}\mathbf{\Omega}\left(^{I_{k}}\omega(t)\right){}_{G}^{I_{k}}\bar{q}(t) $$
$$ ^{G}\dot{\mathbf{p}}_{I_{k}}(t)=^{G}\mathbf{v}_{I_{k}}(t) $$
$$ {}^{G}\dot{\mathbf{v}}_{I_{k}}(t)={}_{G}^{I_{k}}\mathbf{R}(t)^{\top}{}^{I_{k}}\mathbf{a}(t)+{}^{G}\mathbf{g} $$
$$ \mathbf{b}_{g}(t)=\mathbf{n}_{wg} $$
$$ \mathbf{b}_{a}(t)=\mathbf{n}_{w a} $$
其中 $\boldsymbol{\Omega}(\boldsymbol{\omega}) = \left[ \begin{array}{cc} -\boldsymbol{\omega}^\top & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^\top & 0 \end{array} \right]$、$\lfloor \cdot \rfloor$ 是斜对称矩阵，$I_k \boldsymbol{\omega}$ 和 $I_k \mathbf{a}$ 分别表示局部 IMU 坐标系中的真实角速度和线性加速度，而 $G \mathbf{g}$ 则表示全局坐标系中的重力加速度。陀螺仪和加速度计的偏差 $\mathbf{b}_g$ 和 $\mathbf{b}_a$ 被建模为随机游走过程，分别由高斯白噪声 $\mathbf{n}_{wg}$ 和 $\mathbf{n}_{wa}$ 驱动。随后可对这一连续时间系统进行积分和线性化处理，从而随时间向前传播状态协方差矩阵 [^1]。
---
### C. 状态增广
当系统接收到新图像或激光雷达扫描数据时，IMU 状态会传播到该时间点，传播后的惯性状态会被复制到 $\mathbf{x}_C$ 或 $\mathbf{x}_L$ 状态向量中。为了校准不同传感器之间的时间偏移，我们会将状态传播至 IMU 时间 $\hat{t}_{I_k}$，这也是 IMU 时钟下测量数据收集时间的最佳估计值。例如，如果收到时间戳为 $t_{L_k}$ 的新激光雷达扫描数据，我们会将状态传播至 $\hat{t}_{I_k} = t_{L_k} + \hat{t}_{dL}$，并将新的复制状态估计值纳入状态向量 $\mathbf{x}_L$ 中：
$$ \hat{\mathbf{x}}_{L_{k}}(\hat{t}_{I_{k}})=\left[\mathbf{\Phi}_{G}^{I_{k}}\hat{\mathbf{\xi}}_{Q}(\hat{t}_{I_{k}})^{\top}\mathbf{\Phi}^{G}\hat{\mathbf{p}}_{I_{k}}(\hat{t}_{I_{k}})^{\top}\right]^{\top} $$
我们也会对协方差矩阵进行增广，如下所示：
$$ \mathbf{P}(\widehat{t}_{I_{k}})\leftarrow\left[\begin{matrix}{\mathbf{P}(\widehat{t}_{I_{k}})}&{\mathbf{P}(\widehat{t}_{I_{k}})\mathbf{J}_{I_{k}}(\widehat{t}_{I_{k}})^{\top}}\\ {\mathbf{J}_{I_{k}}(\widehat{t}_{I_{k}})\mathbf{P}(\widehat{t}_{I_{k}})}&{\mathbf{J}_{I_{k}}(\widehat{t}_{I_{k}})\mathbf{P}(\widehat{t}_{I_{k}})\mathbf{J}_{I_{k}}(\widehat{t}_{I_{k}})^{\top}}\\ \end{matrix}\right] $$
其中 $\mathbf{J}_{I_k}(\hat{t}_{I_k})$ 是新复制的 $\hat{\mathbf{x}}_{L_k}(\hat{t}_{I_k})$ 相对于当前状态（1）的雅可比矩阵：
$$ \mathbf{J}_{I_{k}}(\hat{t}_{I_{k}})=\frac{\partial\delta\mathbf{x}_{L_{k}}(\hat{t}_{I_{k}})}{\partial\delta\mathbf{x}}=\left[\mathbf{J}_{I}\mathbf{\Sigma J}_{calib\_{C}}\mathbf{\Sigma J}_{calib\_{L}}\mathbf{\Sigma J}_{C}\mathbf{\Sigma J}_{L}\right] $$
在上述表达式中，$J_I$ 是相对于 IMU 状态 $\mathbf{x}_I$ 的雅可比矩阵，其表达式为：
$$ \mathbf{J}_{I}=\begin{bmatrix}\mathbf{I}_{3\times3}&\mathbf{0}_{3\times9}&\mathbf{0}_{3\times3}\\ \mathbf{0}_{3\times3}&\mathbf{0}_{3\times9}&\mathbf{I}_{3\times3}\end{bmatrix} $$
$J_{calib\_L}$ 是相对于 IMU 与激光雷达之间外部参数（包括时间偏移）的雅可比矩阵：
$$ \mathbf{J}_{c a l i b\_{L}}=\left[\mathbf{0}_{6\times6}\mathbf{\nabla J}_{t_{d L}}\right],\mathbf{\nabla J}_{t_{d L}}=\left[\mathbf{\nabla}^{I_{k}}\hat{\mathbf{\omega}}^{\top}\mathbf{\nabla}^{G}\hat{\mathbf{v}}_{I_{k}}^{\top}\right]^{\top} $$
其中，$I_{k}\hat{\omega}$ 表示时刻 $\hat{t}_{I_{k}}$ 下IMU的局部角速度，而 $\hat{G}_{V_{I_{k}}}$ 则表示时刻 $\hat{t}_{I_{k}}$ 下IMU的全局线速度。类似地，$J_{calib,C}$、$J_{C}$、$J_{L}$ 分别是相对于IMU与相机之间外参的雅可比矩阵，分别对应相机时刻和激光雷达时刻的克隆值，在本场景中它们应当为零。需要注意的是，与激光雷达测量对应的新克隆IMU状态对 $t_{dL}$ 的依赖是通过IMU运动学模型来描述的，因此我们的测量模型（详见第II-D.1节）可以直接以处于IMU时钟帧中“真实”测量时刻的克隆值为输入。这种激光雷达克隆流程与出现新相机测量时所采用的流程类似。
### D. 测量模型
1) 激光雷达特征测量：为了降低所需的计算成本，我们希望从原始激光雷达扫描数据中选择一组稀疏且高质量的特征用于状态估计。借鉴 [^7] 的方法，我们提取激光雷达扫描环中的高曲率区域和低曲率区域，它们分别对应边缘特征和平面表面特征（见图1）。我们通过投影并利用KD树进行快速索引，将当前激光雷达扫描中提取出的边缘特征和表面特征与上一帧扫描中的对应特征匹配起来 [^19]。例如，我们将激光雷达扫描中的某个特征点 $L_{l+1}$、$\mathbf{p}_{fi}$（位于 $\{L_{l+1}\}$）投影到 $\{L_l\}$，投影后的点记为 $L_l\mathbf{p}_{fi}$。
$$ ^{L_{l}}\mathbf{p}_{f i}=\mathbf{\Sigma}_{L_{l+1}}^{L_{l}}\mathbf{R}^{L_{l+1}}\mathbf{p}_{f i}+^{L_{l}}\mathbf{p}_{L_{l+1}} $$
其中，$^L_{L_l+1}$ R 和 $^L_{L_l}$ p $_{L_{l+1}}$ 分别表示两个激光雷达坐标系之间的相对旋转和平移，它们可以通过状态向量中的状态量来计算：
$$ ^{L_{l}}_{L_{l+1}}\mathbf{R}_{}=^{L}_{I}\mathbf{R}^{I_{l}}_{G}\mathbf{R}_{}\left(^{L}_{I}\mathbf{R}^{I_{l+1}}_{G}\mathbf{R}_{}\right)^{+} $$
$$ {}^{L_{l}}\mathbf{p}_{L_{l+1}}={}_{I}^{L}\mathbf{R}_{G}^{I_{l}}\mathbf{R}\left({}^{G}\mathbf{p}_{I_{l+1}}-{}^{G}\mathbf{p}_{I_{l}}+{}_{G}^{I_{l+1}}\mathbf{R}^{\top I}\mathbf{p}_{L}\right)+{}^{L}\mathbf{p}_{I} $$
$$ ^{I}\mathbf{p}_{L}=-_{I}^{L}\mathbf{R}^{\top L}\mathbf{p}_{I} $$
完成上述匹配后，我们会在旧扫描中找到两个与投影后的边缘特征 $L_i \mathbf{p}_{fj}$ 对应的边缘特征 $L_i \mathbf{p}_{fk}$、$L_i \mathbf{p}_{fi}$。我们假设它们与 $L_i \mathbf{p}_{fi}$ 源自同一条物理边缘。如果最近的边缘特征 $L_i \mathbf{p}_{fj}$ 位于第 r 个扫描环上，那么次近的边缘特征 $L_i \mathbf{p}_{fk}$ 应该位于紧邻的扫描环 $r-1$ 或 $r+1$ 上。因此，边缘特征 $L_{i+1} \mathbf{p}_{fi}$ 的测量残差就是其投影特征点 $L_i \mathbf{p}_{fi}$ 与由 $L_i \mathbf{p}_{fj}$ 和 $L_i \mathbf{p}_{fk}$ 构成的直线之间的距离。
$$ r(^{L_{l+1}}\mathbf{p}_{fi})=\frac{\left\|\left(^{L_{l}}\mathbf{p}_{fi}-^{L_{l}}\mathbf{p}_{fj}\right)\times\left(^{L_{l}}\mathbf{p}_{fi}-^{L_{l}}\mathbf{p}_{fk}\right)\right\|_{2}}{\left\|^{L_{l}}\mathbf{p}_{fj}-^{L_{l}}\mathbf{p}_{fk}\right\|_{2}} $$
其中，$\|\cdot\|_2$ 表示欧几里得范数，$\times$ 表示两个向量的叉积。
我们在当前状态估计处对上述边缘特征的距离测量进行线性化处理：
$$ \begin{aligned}\boldsymbol{r}(^{L_{l+1}}\mathbf{p}_{fi})&=h(\mathbf{x})+n_{r}\\&=h(\hat{\mathbf{x}})+\mathbf{H}_{\mathbf{x}}\delta\mathbf{x}+n_{r}\end{aligned} $$
其中，$\mathbf{H}_{\mathbf{x}}$ 是距离相对于状态向量中各状态量的雅可比矩阵，$n_{r}$ 被建模为方差为 $C_{r}$ 的白高斯噪声。$\mathbf{H}_{\mathbf{x}}$ 中的非零元素仅与克隆位姿 $G_{G}^{I_{l}}\bar{q}$、$G_{G}^{p_{I_{l}}}$ 以及 $G_{G}^{I_{l+1}}\bar{q}$、$G_{G}^{p_{I_{l+1}}}$，还有IMU与激光雷达之间的刚性校准参数 $I_{l}^{L}\bar{q}$、$I_{l}^{L}\mathbf{p}_{I}$ 相关。因此我们有：
$$ \mathbf{H}_{\mathbf{x}}=\frac{\partial\delta r\big(^{L_{l+1}}\mathbf{p}_{f i}\big)}{\partial^{L_{l}}\delta\mathbf{p}_{f i}}\frac{\partial^{L_{l}}\delta\mathbf{p}_{f i}}{\partial\delta\mathbf{x}} $$
$\frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial\delta\mathbf{x}}$ 中的非零元素计算如下：
$$ \begin{aligned}\frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial_{G}^{I_{l}}\delta\boldsymbol{\theta}}&=\boldsymbol{\mathcal{}^{L}}\hat{\mathbf{R}}\big\lfloor\boldsymbol{}^{I_{l}}\hat{\mathbf{R}}\boldsymbol{}_{G}^{I_{l+1}}\hat{\mathbf{R}}^{\top}\boldsymbol{^{L}}\hat{\mathbf{R}}^{L_{l+1}}\mathbf{p}_{fi}\big\rfloor\\&\quad+\boldsymbol{^{L}}\hat{\mathbf{R}}\big\lfloor\boldsymbol{}^{I_{l}}\hat{\mathbf{R}}\big(\boldsymbol{^{G}}\hat{\mathbf{p}}_{I_{l+1}}-\boldsymbol{^{G}}\hat{\mathbf{p}}_{I_{l}}+\boldsymbol{^{G}}^{I_{l+1}}\hat{\mathbf{R}}^{\top}\boldsymbol{^{I}}\hat{\mathbf{p}}_{L}\big)\big\rfloor\\\frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial^{G}\delta\hat{\mathbf{p}}_{I_{l}}}&=-\boldsymbol{^{L}}\hat{\mathbf{R}}_{G}^{I_{l}}\hat{\mathbf{R}}\\\frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial_{G}^{I_{l+1}}\delta\boldsymbol{\theta}}&=-\boldsymbol{^{L}}\hat{\mathbf{R}}_{G}^{I_{l}}\hat{\mathbf{R}}\boldsymbol{^{I_{l+1}}}\hat{\mathbf{R}}^{\top}\big\lfloor\boldsymbol{^{L}}\hat{\mathbf{R}}^{\top}\boldsymbol{^{L}_{l+1}}\mathbf{p}_{fi}+\boldsymbol{^{I}}\hat{\mathbf{p}}_{L}\big\rfloor\end{aligned} $$
---
$$ \frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial^{G}\delta\hat{\mathbf{p}}_{I_{l+1}}}=\mathbf{\Sigma}_{I}^{L}\hat{\mathbf{R}}_{G}^{I_{l}}\hat{\mathbf{R}} $$
$$ \begin{aligned}\frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial_{I}^{L}\delta\boldsymbol{\theta}}=&\big\lfloor_{L_{l+1}}^{L_{l}}\hat{\mathbf{R}}\big(^{L_{l+1}}\mathbf{p}_{fi}-^{L}\hat{\mathbf{p}}_{I}\big)\big\rfloor\\&-_{L_{l+1}}^{L_{l}}\hat{\mathbf{R}}\big\lfloor^{L_{l+1}}\mathbf{p}_{fi}-^{L}\hat{\mathbf{p}}_{I}\big\rfloor\end{aligned} $$
$$ \frac{\partial^{L_{l}}\delta\mathbf{p}_{fi}}{\partial^{L}\delta\hat{\mathbf{p}}_{I}}=-\mathbf{\Phi}_{L_{l+1}}^{L_{l}}\hat{\mathbf{R}}+\mathbf{I}_{3\times3} $$
为了执行EKF更新，我们需要知道距离测量的显式协方差$C_r$。由于该测量值并非直接来自激光雷达传感器，因此我们将激光雷达扫描中原始测量值（点）的协方差进行传播，即$C_r$。假设点$L_{l+1}p_{fi}$、$L_l p_{fj}$、$L_l p_{fk}$的协方差分别为$\mathbf{C}_i$、$\mathbf{C}_j$、$\mathbf{C}_k$，则可以计算出$C_r$：
$$ \begin{aligned}&C_{r}=\sum_{x=i,j,k}\mathbf{J}_{x}\mathbf{C}_{x}\mathbf{J}_{x}^{\top},\mathbf{J}_{i}=\frac{\partial\delta r\big(^{L_{l+1}}\mathbf{p}_{fi}\big)}{\partial^{L_{l+1}}\delta\mathbf{p}_{fi}}\\&\mathbf{J}_{j}=\frac{\partial\delta r\big(^{L_{l+1}}\mathbf{p}_{fi}\big)}{\partial^{L_{l}}\delta\mathbf{p}_{fj}},\mathbf{J}_{k}=\frac{\partial\delta r\big(^{L_{l+1}}\mathbf{p}_{fi}\big)}{\partial^{L_{l}}\delta\mathbf{p}_{fk}}\\ \end{aligned} $$
我们基于马氏距离执行简单的概率性异常值剔除：
$$ \boldsymbol{r}_{m}=\boldsymbol{r}\big(^{L_{l+1}}\boldsymbol{\mathbf{p}}_{f i}\big)^{\top}\left(\mathbf{H}_{\mathbf{x}}\mathbf{P}_{\mathbf{x}}\mathbf{H}_{\mathbf{x}}^{\top}+C_{r}\right)^{-1}\boldsymbol{r}\big(^{L_{l+1}}\boldsymbol{\mathbf{p}}_{f i}\big) $$
其中$\mathbf{P}_{\mathbf{x}}$表示相关状态的协方差矩阵。$r_{m}$应遵循$\chi^{2}$“卡方”分布，因此若该值通过此测试，便会在我们的EKF更新中使用$r\left({}^{L_{l}+1}\mathbf{p}_{fi}\right)$。
类似地，对于投影的平面表面特征$^{L_{l}}\mathbf{p}_{fi}$，我们会找到三个对应的表面特征$^{L_{l}}\mathbf{p}_{fi}$、$^{L_{l}}\mathbf{p}_{fk}$、$^{L_{l}}\mathbf{p}_{fl}$，它们被假定为采样于与$^{L_{l}}\mathbf{p}_{fi}$相同的物理平面上。表面特征$^{L_{l+1}}\mathbf{p}_{fi}$的测量残差为其投影特征点$^{L_{l}}\mathbf{p}_{fi}$与由$^{L_{l}}\mathbf{p}_{fj}$、$^{L_{l}}\mathbf{p}_{fk}$、$^{L_{l}}\mathbf{p}_{fl}$构成的平面之间的距离。表面特征距离测量的协方差传播、线性化以及马氏距离测试与边缘特征的情况类似。
2) 视觉特征测量：给定新图像后，我们同样会对状态进行传播和更新。从图像中提取FAST特征，并利用KLT光流法将其跟踪到后续帧中。一旦某个视觉特征丢失，或者它已被在整个滑动窗口内跟踪，我们便利用相机克隆体的当前估计值在三维空间中对该特征进行三角测量[^1]。更新过程中使用标准的视觉特征重投影误差。对于三维视觉特征$\mathbf{z}_i$的一组特征方位测量值$^G \mathbf{p}_{fi}$，其通用线性化残差为：
$$ \mathbf{r}(\mathbf{z}_{i})=\mathbf{h}(\mathbf{x},{}^{G}\mathbf{p}_{f i})+\mathbf{n}_{r} $$
$$ \mathbf{=h}(\hat{\mathbf{x}},{}^{G}\hat{\mathbf{p}}_{f i})+\mathbf{H_{x}}\delta\mathbf{x}+\mathbf{H_{f}}{}^{G}\delta\mathbf{p}_{f i}+\mathbf{n}_{r} $$
其中$\mathbf{H}_f$是视觉特征测量关于三维特征$^G\mathbf{p}_{fi}$的雅可比矩阵，且这两个雅可比矩阵均在当前最优估计值处进行评估。由于我们的测量值是$^G\hat{\mathbf{p}}_{fi}$的函数（见公式(29)），我们利用MSCKF零空间投影来消除这种依赖性[^1]。经过零空间投影后，我们得到：
$$ \mathbf{r}_{o}(\mathbf{z}_{i})=\mathbf{H}_{\mathbf{x o}}\delta\mathbf{x}+\mathbf{n}_{r o} $$
需要注意的是，关于IMU与相机之间刚性变换的雅可比矩阵$\{I^{\bar{q}}, C_{p_I}\}$不为零，这意味着IMU与相机之间的变换可以在线校准。
### E. 测量压缩
在当前状态估计下对激光雷达特征和视觉特征测量进行线性化后，我们本可以直接执行EKF更新，但由于视觉和激光雷达特征测量数量庞大，这会带来巨大的计算成本。考虑所有测量残差和雅可比矩阵的堆叠（这些来自激光雷达或视觉特征）：
$$ \mathbf{r}=\mathbf{H}_{\mathbf{x}}\delta\mathbf{x}+\mathbf{n} $$
其中r和n是包含公式(25)或(30)中残差和噪声的分块元素的向量。通常假设所有测量值在统计上相互独立，因此噪声向量n将不相关。为了降低计算复杂度，我们采用Givens旋转[^20]来执行瘦QR分解以压缩测量值[^1]，即：
$$ \mathbf{H}_{\mathbf{x}}=\left[\mathbf{Q}_{H1}~\mathbf{Q}_{H2}\right]\begin{bmatrix}\mathbf{T}_{H}\\ \mathbf{0}\end{bmatrix} $$
其中$Q_{H1}$和$Q_{H2}$是酉矩阵。测量压缩后，我们得到：
$$ \mathbf{r}_{c}=\mathbf{T}_{H}\delta\mathbf{x}+\mathbf{n}_{c} $$
其中，压缩后的雅可比矩阵$\mathbf{T}_H$应为方阵，其维度与状态向量$\mathbf{x}$的维度一致，而压缩后的噪声则由$\mathbf{n}_c = \mathbf{Q}_{H1}^\top \mathbf{n}$给出。随后，利用这种压缩后的线性测量残差，即可通过标准的扩展卡尔曼滤波算法高效地更新状态估计值和协方差矩阵。
### III. 实验结果
为了验证所提算法的性能，我们在室外和室内环境中开展了多项实验。图2所示的传感器平台由Xsens MTi-300 AHRS惯性测量单元、Velodyne VLP-16激光雷达以及单色全局快门Blackfly BFLY-PGE-23S6M相机组成。各传感器之间的外部参数先离线标定，再在线估计过程中进行微调。在评估时，我们将所提出的LIC-Fusion算法与现有的先进视觉-惯性及激光雷达里程计方法进行比较。具体而言，我们将其与我们实现的标准基于MSCKF的视觉惯性里程计算法[^1]，以及开源的LOAM激光雷达里程计实现[^7]进行对比。需要特别指出的是，我们直接与LOAM的输出结果进行比较，因为LOAM利用ICP匹配算法对其构建的全局地图进行处理，从而利用了隐式的回环闭合信息；而我们提出的LIC-Fusion纯粹是一种基于里程计的算法，它仅在滑动窗口内估计状态，既不维护全局地图，也不利用回环闭合信息。
---
Fig. 2: The self-assembled LiDAR-inertial-camera rig with Velodyne LiDAR, Xsens IMU, and monochrome camera.
TABLE I: Outdoor Experimental Results: Average of average absolute trajectory errors (ATE) and their standard deviation/variability.
| MSCKF [^1] | LIC-Fusion | LOAM [^7] |
| Average ATEs (m) | 10.75 | 4.06 | 23.08 |
| 1 Sigma (m) | 3.56 | 3.42 | 2.63 |
### A. 室外测试
首先，我们将所提出的系统安装在定制的阿克曼机器人平台上，利用该自组装的传感器平台（见图2）采集室外序列数据来测试系统性能。该室外序列长度约为800米，采集时长4分钟。平台上还安装了厘米级精度的RTK GPS，其测量数据被用作评估的基准真值。
考虑到RANSAC算法带来的固有随机性，每种算法均运行六次，以便能够代表性地评估其典型性能。图3展示了所提出的LIC-Fusion、MSCKF和LOAM算法估算出的平均轨迹。图4给出了每种方法的平均均方误差（MSE），其中轨迹是通过“最佳拟合”变换与RTK基准真值对齐的，该变换旨在最小化整体轨迹误差。与标准MSCKF相比，所提出的LIC-Fusion算法使平均误差降低了2.5米；与LOAM相比，平均误差则降低了5米。我们可以发现，与其他两种方法相比，LIC-Fusion的轨迹漂移随时间推移的增长速度要慢得多，且在大部分轨迹段上都能保持最小的误差。表I中还报告了平均绝对轨迹误差（ATE）[^21]及其一sigma偏差/变异性。这些结果表明，通过融合摄像头、惯性传感器和激光雷达等不同传感模态，所提出的系统能够实现高精度定位。
### B. 室内测试
我们进一步在一系列室内数据集上对系统进行了评估，这些数据集是在从正常到低光照条件、从缓慢到剧烈的运动场景下采集的。室内序列是手持传感器平台（见图2）于胸部高度采集的。由于
Fig. 3: Top view of outdoor sequence trajectories, showing the trajectories resulted from proposed LIC-Fusion (blue), MSCKF (red), LOAM (pink), and RTK GPS groundtruth (black)
Fig. 4: Average mean squared errors (MSE) of the proposed LIC-Fusion (blue), MSCKF (red), and LOAM (pink) on the outdoor sequence, over the duration of the trajectory.
由于室内没有真实值可供参考，我们将传感器平台返回到初始位置，并评估其起始位置误差。表二总结了平均起始位置误差结果，相关轨迹则显示在图5中。结果表明，所提出的LIC-Fusion算法能够实现高精度定位；由于融合了三种不同的传感模式，它甚至能够应对高速运动和低光照这类极端情况。如图6所示，Indoor-C序列是在我们尽可能剧烈地晃动传感器装置时采集的，因此该序列同时包含高角速度和强线性加速度等剧烈运动特征。所提出的LIC-Fusion算法能够在该序列中实现定位，而另外两种对比方法则出现了大量误差。
### 四、结论与未来工作
在本文中，我们针对MSCKF框架下的激光雷达-惯性-相机里程计问题，开发了一种紧耦合的高效多模态传感器融合算法，即LIC-Fusion。该算法会对三种传感器之间的空间和时间关系进行在线校准，既能够补偿校准偏差，也能简化传感器的部署流程。所提出的方法能够检测并跟踪激光雷达扫描数据中的稀疏边缘与平面特征点，再将这些测量结果与从单目图像中提取的视觉特征进行融合。
---
Fig. 5: Isometric views of the estimated trajectories on indoor sequences A, B, C and D (from left to right).
Fig. 6: Raw IMU measurements over the high-dynamic Indoor-C sequence.
TABLE II: Indoor Experimental Results: Average trajectory start-end errors
| Sequence | MSCKF [^1] | LIC-Fusion | LOAM [^7] |
| Indoor-A (39m) | 0.99 | 0.98 | 0.66 |
| Indoor-B (86m) | 1.55 | 1.04 | 0.46 |
| Indoor-C (55m) | 49.94 | 1.55 | 2.44 |
| Indoor-D (189m) | 46.03 | 3.68 | 5.99 |
通过充分利用不同传感模式的优势，所提出的LIC-Fusion算法能够在各类环境及剧烈运动场景下提供精准且稳健的三维6自由度运动追踪能力。未来，我们将研究如何高效整合来自激光雷达和相机的回环闭合约束，以限制导航误差的累积。
## 参考文献
[^1]: A. I. Mourikis and S. I. Roumeliotis, "A multi-state constraint Kalman filter for vision-aided inertial navigation," in Proceedings of the IEEE International Conference on Robotics and Automation, (Rome, Italy), pp. 3565–3572, Apr. 10–14, 2007.
[^2]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[^3]: R. Mur-Artal and J. D. Tardós, “Visual-inertial monocular slam with map reuse,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 796–803, 2017.
[^4]: G. Huang, K. Eckenhoff, and J. Leonard, “Optimal-state-constraint EKF for visual-inertial navigation,” in Proc. of the International Symposium on Robotics Research, (Sestri Levante, Italy), Sept. 12–15, 2015. (to appear).
[^5]: S. Leutenegger, P. Furgale, V. Rabaud, M. Chli, K. Konolige, and R. Siegwart, “Keyframe-based visual-inertial slam using nonlinear optimization,” Proceedings of Robotis Science and Systems (RSS) 2013, 2013.
[^6]: Z. Huai and G. Huang, “Robocentric visual-inertial odometry,” in Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems, (Madrid, Spain), Oct. 1-5, 2018.
[^7]: J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.,” in Robotics: Science and Systems, vol. 2, p. 9, 2014.
[^8]: C. Park, S. Kim, P. Moghadam, C. Fookes, and S. Sridharan, “Probabilistic surfel fusion for dense lidar mapping,” in Proceedings of the IEEE International Conference on Computer Vision, pp. 2418–2426, 2017.
[^9]: T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, IEEE, 2018.
[^10]: J. Behley and C. Stachniss, “Efficient surfel-based slam using 3d laser range data in urban environments,” in Robotics: Science and Systems (RSS), 2018.
[^11]: Grand View Research, "Lidar technology: Recent developments and market overview." Available: .
[^12]: J. Graeter, A. Wilczynski, and M. Lauer, “Limo: Lidar-monocular visual odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 7872–7879, IEEE, 2018.
[^13]: J. Zhang, M. Kaess, and S. Singh, “Real-time depth enhanced monocular odometry,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4973–4980, IEEE, 2014.
[^14]: J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift, robust, and fast,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 2174–2181, IEEE, 2015.
[^15]: Y.-S. Shin, Y. S. Park, and A. Kim, “Direct visual slam using sparse depth for camera-lidar system,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–8, IEEE, 2018.
[^16]: J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping with high robustness and low drift,” Journal of Field Robotics, vol. 35, no. 8, pp. 1242–1264, 2018.
[^17]: N. Trawny and S. I. Roumeliotis, “Indirect Kalman filter for 3D attitude estimation,” tech. rep., University of Minnesota, Dept. of Comp. Sci. & Eng., Mar. 2005.
[^18]: M. Li and A. I. Mourikis, “Online temporal calibration for camera-imu systems: Theory and algorithms,” The International Journal of Robotics Research, vol. 33, no. 7, pp. 947–964, 2014.
[^19]: M. De Berg, M. Van Kreveld, M. Overmars, and O. Schwarzkopf, “Computational geometry,” in Computational geometry, pp. 1–17, Springer, 1997.
[^20]: G. H. Golub and C. F. Van Loan, Matrix computations, vol. 3. JHU press, 2012.
[^21]: J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A benchmark for the evaluation of rgb-d slam systems,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 573–580, IEEE, 2012.