# 一种基于INS中心多模态的鲁棒且快速的GNSS-惯性-LiDAR里程计，采用IESKF算法 高继新 $^{ID}$、沙建军 $^{ID}$、李宏文 $^{ID}$、王彦恒 $^{ID}$，IEEE学生会员 摘要—全球导航卫星系统（GNSS）、惯性测量单元（IMU）和激光雷达（LiDAR）作为导航系统中常用的传感器，被广泛应用于融合同时定位与建图（SLAM）以提升性能。然而，系统的强鲁棒性和高效率往往未受到足够重视，这给机器人系统的普及应用带来了挑战。本文提出了一种基于惯性导航系统（INS）中心多模态的鲁棒且快速的融合里程计。具体而言，我们的框架包含两个子系统：GNSS-IMU子系统用于估计粗略状态并提供全局约束，LiDAR-IMU子系统则用于细化状态并构建三维地图。即使部分传感器发生故障，状态估计也能通过迭代误差状态卡尔曼滤波器连续进行，不会中断。此外，我们设计了快速且动态的坐标系转换方法以及分布式反向传播机制，用以关联世界坐标系下的状态与全局测量结果，从而应对数据异步问题。我们在公开数据集和真实场景中开展了大量的定性与定量实验，结果表明该系统在精度和效率方面均具有竞争力，且对传感器性能下降或故障具有很强的鲁棒性。 索引词—迭代误差状态卡尔曼滤波器（IESKF）、多模态、多传感器同时定位与建图（SLAM）、鲁棒导航系统、传感器故障。 ## 术语表 $^{i}(\cdot)$、$^{l}(\cdot)$ 和 $^{g}(\cdot)$ 分别表示惯性测量单元（IMU）、激光雷达（LiDAR）和全球导航卫星系统（GNSS）坐标系。 $^{b}(\cdot)$ 和 $^{w}(\cdot)$ 分别表示机体坐标系和世界坐标系。 $^{E}(\cdot)$ 和 $^{N}(\cdot)$ 分别表示地心固定（ECEF）坐标系和东-北-上（ENU）坐标系。 $^{g}T_{l}$、$^{g}T_{i}$ 和 $^{l}T_{i}$ 表示不同传感器之间的外部参数。 $t_{k}$ 表示测量时刻k。 $t_{\rho}$ 表示前向传播时刻$\rho$。 稿件于2023年9月24日收到；2023年11月28日修订；2023年12月20日录用。2024年1月9日发表；2024年1月29日发布当前版本。本研究得到了电磁环境效应与光电工程国家重点实验室资助，项目编号KY3240020001。负责协调审稿流程的副编辑是袁静博士。（通讯作者：沙建军。） 高继新、沙建军、李宏文任职于哈尔滨工程大学青岛创新发展基地，中国青岛266000（电子邮箱：；；）。 王彦恒任职于哈尔滨工程大学智能科学与工程学院，中国哈尔滨150001（电子邮箱：）。 数字对象标识符：10.1109/TIM.2024.3351253 $\mathbf{p}_{l}$ 表示激光雷达采集的点云。 $\mathbf{p}_{g}$ 和 $\mathbf{p}_{i}$ 分别表示GNSS和IMU的位置。 $\mathbf{x},\hat{\mathbf{x}}$以及$\bar{\mathbf{x}}$：真实状态、预测状态和更新状态。 $\delta\mathbf{x}$和$\hat{\mathbf{x}}^{\kappa}$：第$\kappa$次迭代的误差状态与更新状态。 ### 一、引言 近年来，同步定位与建图（SLAM）已成为自主机器人导航的关键技术。它旨在未知环境中实现实时自我定位与地图构建，为自主探索[^1]、[^2]、路径规划[^3]、[^4]、[^5]以及无人驾驶[^6]、[^7]提供重要信息。SLAM可依托多种传感器实现，其中视觉相机和激光雷达是两种典型的基础传感器。视觉SLAM通常以摄像头为基础，具有成本低廉的优势，但受光照条件限制；激光雷达则被认为更可靠，因为它能主动测量环境信息且不受光照影响。然而，以单一传感器为核心的激光雷达SLAM在应对复杂场景（如剧烈运动、开阔场景和无特征隧道）时仍面临诸多挑战。 为了提升导航系统的性能，相关研究聚焦于多传感器融合方案，尤其是激光雷达与惯性测量单元（IMU）的融合[^8]、[^9]、[^10]、[^11]、[^12]。IMU可通过高频测量来补偿点云畸变，而激光雷达则能减少IMU带来的累积误差。因此，二者的集成方式自然、高效，在自主机器人系统的部署中越来越受欢迎。不过，激光雷达-惯性SLAM或激光雷达-惯性里程计（LIO）难以在任何时间、任何地点都实现稳定的定位与建图。由于现实环境复杂且不可预测，任何传感器的性能下降或故障都会导致导航系统失效，这对自主机器人而言是危险且不可接受的。为应对突发情况和复杂环境，一种解决方案是结合额外传感器或多模态数据，以提升系统对传感器性能下降和故障的鲁棒性[^13]、[^14]、[^15]、[^16]。大多数相关的多模态框架采用基于优化器的前端/后端架构，虽能优化多源、多时刻的状态，却需耗费大量计算资源；相比之下，基于滤波器的状态估计框架更轻便、更高效，但它存在不可避免的误差累积问题，会降低系统的长期性能与精度。 --- 这种误差累积会削弱系统的长期性能与精度。 鉴于上述问题，我们重点关注导航系统的以下特性：1）能够容忍传感器性能下降和故障的强鲁棒性；2）便于实际应用的高计算效率；3）长期性能与较高的精度。为满足这些要求，我们重点研究了GNSS和迭代误差状态卡尔曼滤波器（IESKF）。GNSS是导航系统中广泛配备的传感器，用于测量空间中的绝对位置。此外，GNSS/惯性导航系统（INS）作为典型的组合导航方式，能够在户外提供可靠且精确的位置信息。由于LIO具有局部性特点且与INS同属常见的导航系统，二者在特性上互补且具有一致性，因此为了提升系统的鲁棒性和精度，有必要对这两种传感器进行融合。在状态估计方面，IESKF通过误差状态和迭代处理，分别提升了线性化能力[^17]、[^18]以及求解能力[^19]。与优化器相比，IESKF更轻量且高效，能够减少导航系统占用的计算资源。 因此，我们提出了一种基于IESKF将GNSS、IMU和激光雷达融合的SLAM框架。得益于所设计的GNSS-IMU子系统和激光雷达-IMU子系统的架构，该系统展现出极强的鲁棒性，能够在传感器工作时充分利用多种异构信息，并确保在传感器性能下降或发生故障时仍能正常运行。多源状态估计完全由IESKF负责，使得该系统运行速度快且占用资源少，便于实际部署。通过将GNSS集成到LIO中，该系统能够通过提升长期性能来实现较高的精度。此外，本文针对绝对/相对坐标系转换以及多源测量异步问题提出了相应的解决方案。主要贡献如下。 1) 我们提出了一种以INS为中心、采用IESKF的多模态GNSS-惯性-激光雷达融合SLAM框架，该框架不仅能在多种约束条件下实现快速的状态估计并具有较高精度，还具备较强的鲁棒性以容忍传感器的性能下降和故障。 2) 针对我们所采用的基于滤波器的导航框架，开发了一种基于双滑动窗口的快速动态坐标系转换方法，该方法能够在线对齐来自GNSS的全局测量数据与机器人的局部世界状态。 3) 我们设计了用于运动补偿的分布式反向传播机制，以避免点云碎片化及利用率低下的问题，从而充分利用不同频率下的异构传感器数据，确保系统的精度。 本文剩余章节的结构安排如下：第二节介绍了相关研究；第三节阐述了所提方法的系统概述；第四节给出了多传感器状态估计的相关内容；第五节描述了应对传感器性能下降和故障的解决方案；第六节通过在公开数据集及真实场景中的综合实验验证了该方法的有效性；第七节为结论部分。 ### A. 激光雷达-惯性里程计 ### II. 相关研究 LOAM [^20]是一种具有代表性的三维激光雷达SLAM算法，于2014年提出。它分为前端（通过点云配准解决高频里程计问题）和后端（以低频方式完成优化与建图），为后续激光雷达（惯性）系统的发展奠定了基础。LIO映射算法 [^8]继承了这一结构，并在滑动窗口优化器中将IMU与激光雷达紧密耦合。LINS [^10]同样源自LOAM，但通过在前端采用IESKF实现了轻量化的状态估计器。FAST-LIO [^11]完全基于IESKF，无需通过优化器来精细化地图点或状态。在此基础上，FAST-LIO2 [^21]无需进行特征提取即可将点直接注册到地图上，从而充分利用环境信息并适配新型激光雷达。 ### B. LIO中的GNSS融合 通常将具备全局信息的GNSS与LIO融合，以提升导航系统的性能。LIO-SAM [^22]是一种基于因子图优化（FGO）的激光雷达与IMU紧耦合方法，其中还设计了GPS因子和闭环因子来提高全局一致性。LIO-SAM已被广泛应用，被视为最先进的融合系统，推动了众多以FGO为核心框架的研究。在 [^23]中，激光雷达、IMU、轮式编码器和GNSS通过滑动窗口FGO技术进行融合，其中GNSS还用于辅助修正累积的建图误差。基于类似的框架，Zhong等人 [^24]在固定时间滑动窗口优化前，为GNSS和LIO增加了异常值剔除机制。相较于优化框架，基于滤波的融合方法在相关研究中受到的关注较少。在 [^25]中，INS和GNSS通过卡尔曼滤波器与基于网格的SLAM相集成，输出二维位姿；通过刷新地图的策略减少了系统漂移，并通过多源数据的交叉验证提升了可靠性。P $^{3}$-LINS [^26]在前端设计了三阶段基于滤波的估计器以及两种帧变换初始化方法，以实现GNSS、INS和激光雷达的融合，但后端建图仍基于优化技术。 ### C. 多模态SLAM 为了在复杂的现实环境中保持鲁棒性，多模态SLAM框架成为了相关研究的热点。Super Odometry [^16]是一种以IMU为中心的状态估计器，它融合了来自激光雷达和视觉的多模态数据，以提升系统在感知条件较差环境中的性能。Shu和Luo [^27]充分利用了激光雷达和视觉特征提供的多种约束，提出了一种包含不同特征的多模态映射方法，证明了其在图像模糊或激光雷达性能下降情况下的有效性。与利用多模态约束来提升恶劣环境下稳定性的思路不同，GPS-视觉-惯性融合 [^28]考虑了GPS数据中断的情况，并通过类闭环的方式补偿累积漂移。2026年9月11日 05:05:01 UTC，发布于IEEE Xplore。受相关限制条款约束。 --- ![Image](images/2fd91acff0ca0baf42380125dff209d763fb99442ff5e721f26a47444f5e8d69.jpg)
Fig. 1. System overview and the processing of each modality.
一些研究也考虑了核心传感器的故障或性能退化问题。基于RGB、热成像和高光谱相机的VMO [^13]能够实现运动估计，这种多模态方法在部分摄像头出现故障的情况下仍能正常工作。采用激光雷达点云、可见光和热成像融合里程计以及惯性测量单元（IMU）模块来构建前端的MIMOSA [^15]，其多功能后端能够在部分传感器故障时追踪正常的模态数据。LVI-SAM [^29]和R $^{2}$LIVE [^30]基于因子图优化（FGO）作为后端，以紧耦合的方式设计了视觉-惯性子系统和激光雷达-惯性子系统。它们都开发了故障检测机制，以便在激光雷达或摄像头性能退化或发生故障时分离各子系统。LIO-Fusion [^31]利用全球导航卫星系统（GNSS）、先验地图和轮式里程计来增强激光雷达惯性里程计（LIO）的性能，并通过FGO中的健康检测模块来评估传感器的潜在故障。类似地，在激光雷达里程计中设计了健康监测模块和优先级队列，该里程计与多模态里程计及原始IMU数据相集成，虽然计算量巨大，但具备高精度和强鲁棒性 [^14]。GNSS-激光雷达融合系统 [^32]针对开阔场景和无GNSS环境开发了两种基于优化的模式，该系统考虑了GNSS的退化情况但忽略了激光雷达的影响。与基于非线性优化的方法不同，Li等人 [^33]采用了基于扩展卡尔曼滤波（EKF）的多模态网格SLAM技术，在室内外环境中分别切换使用GNSS/INS和激光雷达/INS，但并未涉及GNSS带来的全局约束问题以及传感器故障难题。 ### III. 系统概述 所提出系统的流程如图1所示。来自GNSS、IMU和激光雷达的传感器数据以各自的频率同时输入系统。经过相应的预处理和必要检查后，我们以IMU为核心传感器，基于迭代扩展卡尔曼滤波（IESKF）将整个框架划分为用于粗到细状态估计的两个子系统：GNSS-IMU子系统和激光雷达-IMU子系统。当传感器工作正常时，GNSS-IMU子系统会以GNSS的频率实现粗略状态估计，通过加入全局约束来提高稳定性。粗略估计的结果不会直接作为系统输出，而是被暂时缓存起来。更新后的GNSS状态会作为激光雷达-IMU子系统的输入，进一步细化系统状态。最终，带有GNSS约束的系统状态会以激光雷达的频率输出，同时构建出包含密集点的三维地图。 得益于两个子系统交替更新状态的框架设计，我们的系统具有极强的鲁棒性。无论出现激光雷达还是GNSS方面的传感器故障或性能退化，系统都能依靠其他正常模态继续运行，确保状态估计过程不被中断。 ### IV. 多传感器状态估计 ### A. 定义 本文涉及传感器坐标系、ECEF坐标系和ENU坐标系。传感器坐标系用于描述所安装传感器的空间位置，包括IMU坐标系$i(\cdot)$、激光雷达坐标系$l(\cdot)$以及GNSS坐标系$g(\cdot)$。第一个IMU坐标系被视为世界坐标系$w(\cdot)$，该系统运行后该坐标系便固定下来，里程计和建图输出也均以世界坐标系表示。相比之下，随着系统本体在世界坐标系中移动，IMU坐标系会不断变化，它也可称为机体坐标系$b(\cdot)$。ECEF坐标系$E(\cdot)$是一种与地球固连的笛卡尔坐标系，用于承载GNSS的测量数据。其原点位于地球质心，x轴与本初子午线对齐，z轴垂直于地球赤道平面指向地理北极，y轴遵循右手定则。本文中我们采用WGS-84标准来表示ECEF坐标系下的数据。此外，引入ENU坐标系$N(\cdot)$来将ECEF坐标系下的测量值转换为世界坐标系下的数值，该坐标系的原点基于机体位置，x轴、y轴和z轴分别指向东方、北方和上方。 为方便起见，我们假设IMU与系统机体重合，且与其他传感器通过已知的外部参数刚性连接。本文中定义的状态向量$\mathbf{x}$、输入$\mathbf{u}$以及噪声$\mathbf{n}$如下： $$ \mathcal{M}=\mathrm{SO}(3)\times\mathbb{R}^{15} $$ $$ \mathbf{x}=\left[^{w}\mathbf{R}_{i}^{\mathrm{T}w}\mathbf{p}_{i}^{\mathrm{T}w}\mathbf{v}_{i}^{\mathrm{T}}\mathbf{b}_{\omega}^{\mathrm{T}}\mathbf{b}_{a}^{\mathrm{T}w}\mathbf{g}^{\mathrm{T}}\right]^{\mathrm{T}}\in\mathcal{M} $$ $$ \mathbf{u}=\left[\mathbf{a}_{m}^{\mathrm{T}}\boldsymbol{\omega}_{m}^{\mathrm{T}}\right]^{\mathrm{T}} $$ $$ \mathbf{n}=\left[\mathbf{n}_{\omega}^{\mathrm{T}}\mathbf{n}_{a}^{\mathrm{T}}\mathbf{n}_{b\omega}^{\mathrm{T}}\mathbf{n}_{ba}^{\mathrm{T}}\right]^{\mathrm{T}} $$ 授权许可使用范围仅限于：上海科技大学。于2026年9月11日UTC时间05:05:01从IEEE Xplore下载。受相关限制约束。 --- 其中$^w\mathbf{R}_i$是在$\mathrm{SO}(3)$上的旋转矩阵，$^w\mathbf{p}_i$和$^w\mathbf{v}_i$分别为位置和速度，$\mathbf{b}_\omega$和$\mathbf{b}_a$分别是IMU陀螺仪和加速度计的偏差，$\mathbf{g}$是需要在世界坐标系中估计的重力矢量[^34]，$\mathbf{a}_m$和$\boldsymbol{\omega}_m$分别是IMU的加速度输入和角速度输入，而$\mathbf{n}_\omega$、$\mathbf{n}_a$、$\mathbf{n}_{bw}$、$\mathbf{n}_{ba}$则是分别影响测量值和偏差的高斯白噪声。 此外，我们使用$\boxplus$中定义的封装运算符$\boxminus$和[^35]来对状态误差进行参数化，该误差存在于复合流形$\mathcal{M} = \text{SO}(3) \times \mathbb{R}^n$上，其表达式如下： $$ \begin{aligned}\begin{bmatrix}\mathbf{R}\\ \mathbf{a}\end{bmatrix}\boxplus\begin{bmatrix}\mathbf{r}\\ \mathbf{b}\end{bmatrix}&=\begin{bmatrix}\mathbf{R}\mathbf{E}\mathbf{x}\mathbf{p}(\mathbf{r})\\ \mathbf{a}+\mathbf{b}\end{bmatrix};\begin{bmatrix}\mathbf{R}_{1}\\ \mathbf{a}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_{2}\\ \mathbf{b}\end{bmatrix}\\&=\begin{bmatrix}\mathrm{Log}\left(\mathbf{R}_{2}^{\mathrm{T}}\mathbf{R}_{1}\right)\\ \mathbf{a}-\mathbf{b}\end{bmatrix}\end{aligned} $$ 其中$\mathbf{r} \in \mathbb{R}^3$、$\mathbf{a}$、$\mathbf{b} \in \mathbb{R}^n$、$\text{Exp}(\mathbf{r})$是[^35]中的指数映射，而$\text{Log}(\cdot)$是其逆映射。 ### B. 前向传播 当接收到GNSS或激光雷达的测量数据时，会将累积的IMU数据向前传播，直至与这些测量数据同步。IMU的前向传播可通过其离散时间运动模型来表示，此时噪声设为零。 $$ \hat{\mathbf{x}}_{\rho+1}=\left[\begin{array}{c}^{w}\mathbf{R}_{i,\rho}\mathrm{Exp}\big[\big(\boldsymbol{\omega}_{m,\rho}-\mathbf{b}_{\omega,\rho}\big)\Delta t\big]\\ ^{w}\mathbf{p}_{i,\rho}+{}^{w}\mathbf{v}_{i,\rho}\Delta t+\frac{1}{2}\big[{}^{w}\mathbf{R}_{i,\rho}\mathbf{a}_{m,\rho}^{^{\prime}}+{\mathbf{\omega}_{}}\mathbf{g}\big]\Delta t^{2}\\ {}^{w}\mathbf{v}_{i,\rho}+{}^{w}\mathbf{R}_{i,\rho}\mathbf{a}_{m,\rho}^{^{\prime}}\Delta t+{\mathbf{\omega}_{}}\mathbf{g}\Delta t\\ \mathbf{b}_{\omega,\rho}\\ \mathbf{b}_{a,\rho}\\ {}^{w}\mathbf{g}_{\rho}\end{array}\right] $$ 其中$\hat{\mathbf{x}}_{\rho+1}$是在更新后的状态之间、时刻$t_{\rho+1}$处的预测状态，$^w\mathbf{R}_{i,\rho}$、$^w\mathbf{p}_{i,\rho}$和$^w\mathbf{v}_{i,\rho}$分别是时刻$t_{\rho}$时机体在世界坐标系下的旋转、位置和速度，$\mathbf{a}_{m,\rho}^{\prime}$是不包含$\mathbf{a}_{m,\rho}$和$\mathbf{b}_{a,\rho}$的$\mathbf{n}_a$，而$\Delta t$则是时刻$t_{\rho}$与$t_{\rho+1}$之间的时间间隔。 我们的融合SLAM框架完全基于IESKF实现；因此，误差状态的运动模型$\tilde{\mathbf{x}}_{\rho+1}$和协方差$\hat{\mathbf{P}}_{\rho+1}$被用于驱动状态估计过程，其描述如下： $$ \begin{aligned}\tilde{\mathbf{x}}_{\rho+1}&=\mathbf{x}_{\rho+1}\boxminus\hat{\mathbf{x}}_{\rho+1}\\&=\mathbf{F}_{\tilde{\mathbf{x}}}\tilde{\mathbf{x}}_{\rho}+\mathbf{F}_{w}\mathbf{n}_{\rho}\\\hat{\mathbf{P}}_{\rho+1}&=\mathbf{F}_{\tilde{\mathbf{x}}}\hat{\mathbf{P}}_{\rho}\mathbf{F}_{\tilde{\mathbf{x}}}^{\mathrm{T}}+\mathbf{F}_{w}\mathbf{Q}_{\rho}\mathbf{F}_{w}^{\mathrm{T}}\end{aligned} $$ 其中$\mathbf{F}_{\tilde{x}}$和$\mathbf{F}_{w}$分别是误差状态和噪声的转移矩阵，更多细节读者可参考[^11]。 ### C. GNSS-惯性子系统 1) 在线坐标系转换：GNSS的测量数据是在ECEF坐标系中采样的，而状态则是在世界坐标系中估计的；因此，在更新状态之前需要进行在线坐标系转换。我们会在系统运行过程中以较低的时间开销动态维护这种转换关系，而非在初始化后将其固定下来。ENU坐标系正是用于连接ECEF坐标系与世界坐标系的。 ![Image](images/b18be4916bc50d2424475a056ad4ebbf8ada776e624d83d250184ee3dda40060.jpg)
Fig. 2. Rotation refinement based on double sliding window.
我们引入ENU坐标系来衔接ECEF坐标系与世界坐标系 [^22]，[^36]。ENU坐标系的原点会根据预测的状态来设定，而世界坐标系与ENU坐标系之间的旋转关系（偏航角偏移）则可通过后续的GNSS测量数据与预测状态进行粗略计算。 $$ \begin{aligned}^{w}\mathbf{P}_{N,k+1}&={}^{w}\mathbf{P}_{N,k}={}^{w}\hat{\mathbf{p}}_{i,g_{0}}\\^{w}\mathbf{R}_{N,k+1}^{c}&=\mathbf{I}+\left\|\mathbf{p}_{c,k}\right\|\left[\mathbf{p}_{c,k}\right]_{\wedge}+(1-\mathbf{p}_{d,k})\left[\mathbf{p}_{c,k}\right]_{\wedge}^{2}\\ \mathbf{p}_{c,k}&={}^{w}\hat{\mathbf{p}}_{i,k,\mathrm{proj}}^{n}\times{}^{N}\mathbf{p}_{g,k,\mathrm{proj}}^{n}\\ \mathbf{p}_{d,k}&={}^{w}\hat{\mathbf{p}}_{i,k,\mathrm{proj}}^{n}\cdot{}^{N}\mathbf{p}_{g,k,\mathrm{proj}}^{n}\end{aligned} $$ 其中$^w\mathbf{P}_{N,k+1}$和$^w\mathbf{R}_N^c$分别是从ENU坐标系到世界坐标系的位置和粗略旋转，$^w\hat{\mathbf{p}}_{i,g_0}$是首次GNSS测量时在世界坐标系中的预测位置，$[\mathbf{p}_{c,k}]_\wedge$是$\mathbf{p}_{c,k}$的斜对称矩阵，而$^w\hat{\mathbf{p}}_{i,k,proj}^n$和$^N\mathbf{p}_{g,k,proj}^n$分别是投影归一化后在世界坐标系中的预测位置和GNSS测量值。 随后，我们基于该粗略旋转构建双滑动窗口以进一步细化旋转参数。如图2所示，滑动窗口分为两种类型：稳定窗口和时间窗口。稳定窗口保存从最稳定的测量数据中求解出的$n_s$旋转参数，时间窗口则保存最近的$n_t$旋转参数（本文中记为$n_s \leq 3$和$n_t \leq 5$）。测量数据的稳定性由协方差和角度偏移决定。 $$ \begin{aligned}s_{c}&=\frac{1}{(e_{x}+e_{y}+e_{z})/3}\\s_{d}&=\left|\arccos\frac{^{N}\mathbf{v}_{k}\cdot^{N}\mathbf{v}_{k-1}}{^{N}\mathbf{v}_{k}\times{}^{N}\mathbf{v}_{k-1}}-\arccos\frac{^{w}\mathbf{v}_{k}\cdot^{w}\mathbf{v}_{k-1}}{^{w}\mathbf{v}_{k}\times{}^{w}\mathbf{v}_{k-1}}\right|\\{^{N}\mathbf{v}_{k}}&={^{N}\mathbf{p}_{g,k}}-{^{N}\mathbf{p}_{g,k-1}},\quad{^{w}\mathbf{v}_{k}}={^{w}\hat{\mathbf{p}}_{k}}-{^{w}\hat{\mathbf{p}}_{k-1}}\end{aligned} $$ 其中$s_c$表示粗略旋转的置信度，$e_x$是GNSS协方差在x轴上的特征值，$s_d$是向量差异间的角度偏移，$^N\mathbf{p}_{g,k}$和$^w\hat{\mathbf{p}}_k$分别是GNSS测量得到的ENU坐标系下时间$t_k$的位置，以及预测状态得到的世界坐标系下的对应位置。 基于$s_c$和$s_d$，我们计算双滑动窗口中每个旋转参数的权重因子$w_i$。通过$^w\mathbf{R}_{N,k}^r$和对应的$^w\mathbf{R}_{N,i}^c$计算得到的精细化旋转参数$w_i$，用于转换ENU坐标系中的连续GNSS测量数据。需要注意的是，直到世界坐标系下的GNSS数据与预测状态之间的差异超出某一阈值时，$^w\mathbf{R}_{N,k}^r$才会被固定。 --- 一旦满足阈值条件，我们就能得到位于世界坐标系中的GNSS测量数据 $^{w}p_{k}$。这一过程可总结为如下步骤： $$ \begin{align*}w_{i}&=\frac{s_{c,i}}{2\sum_{j=1}^{n}s_{c,j}}+\frac{s_{d,i}}{2\sum_{j=1}^{n}s_{d,j}}\\{}^{w}\mathbf{R}_{N,k}^{r}&=\sum_{i=1}^{n_{s}}w_{i}{}^{w}\mathbf{R}_{N,i}^{c}+\sum_{j=1}^{n_{t}}w_{j}{}^{w}\mathbf{R}_{N,j}^{c}\\{}^{w}\mathbf{p}_{k+1}&={}^{w}\mathbf{R}_{N,k}^{r}{}^{N}\mathbf{p}_{k+1}+{}^{w}\mathbf{P}_{N,k}.\end{align*} $$ 2) 基于GNSS的迭代更新：将测量数据转换到世界坐标系后，我们通过IESKF执行GNSS的状态更新。GNSS-IMU残差由预测状态的位置和世界坐标系中的GNSS测量值计算得出。测量模型$h_{g}$可表示为： $$ \begin{align*}0&=h_{g}(\mathbf{x}_{k},\mathbf{n}_{g})\\&\simeq h_{g}(\hat{\mathbf{x}}_{k}^{\kappa},0)+\mathbf{H}_{g}^{\kappa}\tilde{\mathbf{x}}_{k}^{\kappa}+\mathbf{v}_{g}\\&={}^{i}\mathbf{T}_{g}{}^{\left(w\mathbf{T}_{N,k-1}{}^{N}\mathbf{T}_{E,k}{}^{E}\mathbf{p}_{g,k}\right)}-{}^{w}\hat{\mathbf{p}}_{i,k}^{\kappa}+\mathbf{H}_{g}\tilde{\mathbf{x}}_{k}^{\kappa}+\mathbf{v}_{g}\end{align*} $$ 其中，$\mathbf{H}_g^\kappa$ 是误差状态的测量模型雅可比矩阵，简称 $\mathbf{H}_g$；$\mathbf{v}_g \in \mathcal{N}(0, \mathbf{R}_g)$ 是来自GNSS的测量噪声，$^i\mathbf{T}_g$ 是从GNSS到IMU的外参矩阵；$^w\mathbf{T}_N$ 和 $^N\mathbf{T}_E$ 分别是从ENU坐标系到世界坐标系以及从ECEF坐标系到ENU坐标系的变换；$^E\mathbf{p}_{i,k}$ 和 $^w\hat{\mathbf{p}}_{i,k}^\kappa$ 则分别是ECEF坐标系下GNSS的测量位置以及世界坐标系中第 $\kappa$ 次迭代的预测位置。 随后，系统状态通过以下公式迭代更新： $$ \begin{align*}\mathbf{K}_{g}&=\hat{\mathbf{P}}_{k}\mathbf{H}_{g}^{\mathrm{T}}\big(\mathbf{H}_{g}\hat{\mathbf{P}}_{k}\mathbf{H}_{g}^{\mathrm{T}}+\mathbf{R}_{g}\big)^{-1}\\\hat{\mathbf{x}}_{k}^{\kappa+1}&=\hat{\mathbf{x}}_{k}^{\kappa}\boxplus\tilde{\mathbf{x}}_{k}\\&=\hat{\mathbf{x}}_{k}^{\kappa}\boxplus\mathbf{K}_{g}\left(-h_{g}\big(\hat{\mathbf{x}}_{k}^{\kappa}\big)\right)-\big(\mathbf{I}-\mathbf{K}_{g}\mathbf{H}_{g}\big)\big(\hat{\mathbf{x}}_{k}^{\kappa}\boxminus\hat{\mathbf{x}}_{k}^{0}\big).\end{align*} $$ 当状态收敛或达到最大迭代次数时，便可计算出时间 $t_{k}$ 下的更新后状态及后验协方差。 $$ \mathbf{\bar{x}}_{k}=\mathbf{\hat{x}}_{k}^{\kappa+1},\quad\mathbf{\bar{P}}_{k}=(\mathbf{I}-\mathbf{K}_{g}\mathbf{H}_{g})\mathbf{\hat{P}}_{k}. $$ ### D. 激光雷达-惯性子系统 1) 分布式反向传播：反向传播与运动补偿能够应对点云畸变问题，从而提高精度。在融合系统中，GNSS测量会触发预测状态从 $t_{k-1}$ 向 $t_k$ 正向传播，如图3所示。当时间 $t_{k+1}$ 的激光雷达测量数据到达时，预测状态会从 $t_k$ 传播至 $t_{k+1}$。然而，扫描点分布在时间 $t_{k-1}$ 到 $t_{k+1}$ 之间。由于缺乏对应的预测状态，部分畸变点无法得到校正，进而降低了系统精度。为了对所有点进行去畸变处理，我们会缓存之前的预测状态并执行分布式反向传播。 分布式反向传播的过程如图3所示。GNSS更新后，会构建一个临时状态缓冲区，用于保存从 $t_{k-1}$ 到 $t_k$ 的所有GNSS预测状态。在进行激光雷达更新时，会从状态缓冲区中取出受GNSS约束的状态并用作 ![Image](images/729ae16be698d28c79d6bd0486442ce07b562e64241f858da7c591ae094fcae5.jpg)
Fig. 3. Process of distributed backward propagation.
激光雷达前向传播的初始状态。当这一过程反向传播时，用于GNSS的预测状态（从 $t_{k-1}$ 到 $t_k$）会被置于初始状态之前。随后，从 $t_{k-1}$ 到 $t_{k+1}$ 的所有预测状态都会被用于运动补偿。对于位于 $\mathbf{p}_{l,\gamma}$ 和 $t_\gamma$ 之间的时间 $t_{k-1}$ 处的点 $t_{k+1}$，补偿后的点 $\mathbf{p}_{l,k+1}$ 可表示为 $$ \mathbf{p}_{l,k+1}={}^{i}\mathbf{T}_{l}^{-1}\big(^{w}\mathbf{T}_{i}^{-1k+1}\mathbf{T}_{\gamma}{}^{w}\mathbf{T}_{i}\big){}^{i}\mathbf{T}_{l}\times\mathbf{p}_{l,\gamma} $$ 其中，$^i\mathbf{T}_l$ 是激光雷达坐标系到IMU坐标系的外参，$^w\mathbf{T}_i$ 是IMU坐标系到世界坐标系的变换，而 $^{k+1}\mathbf{T}_\gamma$ 则是从 $t_\gamma$ 到 $t_{k+1}$ 的相对变换；该变换在 $\gamma \in (t_{k-1}, t_k)$ 时刻由IMU-GNSS预测状态确定，在 $\gamma \in (t_k, t_{k+1})$ 时刻由IMU-激光雷达预测状态确定。 2) 基于激光雷达的迭代更新：经过分布式反向传播后，所有采样的激光雷达点均已完成补偿。我们遵循 [^21] 的方法，无需特征提取即可直接进行扫描与地图配准，并通过激光雷达更新状态。其测量模型为 $$ \begin{align*}0&=h_{j}(\mathbf{x}_{k},\mathbf{n}_{l})\\&\simeq h_{j}\big(\hat{\mathbf{x}}_{k}^{\kappa},0\big)+\mathbf{H}_{j}^{\kappa}\tilde{\mathbf{x}}_{k}^{\kappa}+\mathbf{v}_{l,j}\\&=\mathbf{\Phi}^{w}\mathbf{G}_{j}\big(^{w}\mathbf{T}_{i}^{i}\mathbf{T}_{l}\mathbf{p}_{j,k+1}-^{w}\mathbf{q}_{j}\big)+\mathbf{H}_{j}^{\kappa}\tilde{\mathbf{x}}_{k}^{\kappa}+\mathbf{v}_{l,j}\end{align*} $$ 其中，$\mathbf{p}_{j,k+1}$ 是时间 $t_{k+1}$ 下的第 j 个点，$^w\mathbf{q}_j$ 是由最近邻点拟合得到的小面片的质心，$^w\mathbf{G}_j$ 是对应面片的法向量，$\mathbf{H}_j^\kappa$ 是误差状态的测量模型雅可比矩阵；设 $\mathbf{H}_l = [\mathbf{H}_1^\tau, \ldots, \mathbf{H}_m^\tau]^T$、$^w\mathbf{T}_i$ 和 $^i\mathbf{T}_l$ 分别为从 IMU 坐标系到世界坐标系的变换以及从激光雷达坐标系到 IMU 坐标系的预校准参数，而 $\mathbf{v}_{l,j} \in \mathcal{N}(0, \mathbf{R}_l)$ 则是 $\mathbf{p}_{j,k+1}$ 的测量噪声。 接着，计算卡尔曼增益 $\mathbf{K}_{l}$ 并执行更新操作 [^21] $$ \begin{align*}\mathbf{K}_{l}&=\left(\mathbf{H}_{l}^{\mathrm{T}}\mathbf{R}_{l}^{-1}\mathbf{H}_{l}+\mathbf{P}_{k}^{-1}\right)^{-1}\mathbf{H}_{l}^{\mathrm{T}}\mathbf{R}_{l}^{-1}\\\bar{\mathbf{x}}_{k}^{\kappa+1}&=\hat{\mathbf{x}}_{k}^{\kappa}\boxplus\tilde{\mathbf{x}}_{k}\\&=\hat{\mathbf{x}}_{k}^{\kappa}\boxplus\mathbf{K}_{l}\left(-h_{l}\big(\hat{\mathbf{x}}_{k}^{\kappa}\big)\right)-(\mathbf{I}-\mathbf{K}_{l}\mathbf{H}_{l})\big(\hat{\mathbf{x}}_{k}^{\kappa}\boxminus\hat{\mathbf{x}}_{k}^{0}\big).\end{align*} $$ 经过收敛或达到最大迭代次数后，带有协方差信息的优化后的状态将被作为融合里程计的输出。 $$ \begin{array}{r}{\bar{\mathbf{x}}_{k}=\hat{\mathbf{x}}_{k}^{\kappa+1},\quad\bar{\mathbf{P}}_{k}=(\mathbf{I}-\mathbf{K}_{l}\mathbf{H}_{l})\hat{\mathbf{P}}_{k}.}\end{array} $$ --- ### V. 性能退化与故障的解决方案 ### A. 传感器退化场景 在遮挡区域或室内环境中，GNSS 测量值通常会出现显著波动，从而导致精度下降。为避免使用异常数据，系统会利用测量协方差及其与世界坐标系中预测状态的差异来检测 GNSS 退化情况。我们在预处理前会丢弃置信度较低的数据；对于与预测状态差异较大的不准确的 GNSS 数据，也不会将其用于状态更新。退化检测的条件如下： $$ \begin{aligned}\left\|\boldsymbol{\Sigma}_{g,k}\right\|_{2}&p_{\mathrm{th}}\end{aligned} $$ 其中，$\Sigma_{g,k}$ 和 $\sigma_{g,k}^{2}$ 分别是 GNSS 测量的协方差矩阵和 z 轴方向上的方差，$c_{\mathrm{th}}$ 和 $z_{\mathrm{th}}$ 为阈值（本文中取 $c_{\mathrm{th}} = 45$ 和 $z_{\mathrm{th}} = 50$），$^{w}\mathbf{p}_{g,k}$ 和 $^{w}\hat{\mathbf{p}}_{i,k}$ 分别为 GNSS 测量值和预测状态的位置，而 $p_{\mathrm{th}}$ 为对应的阈值（本文中取 2.5 米）。 在户外环境中，激光雷达退化的一个原因是场景的开阔性，而这种开阔性往往有利于 GNSS 测量的稳定性。因此，我们利用激光雷达退化检测来跳过退化的帧。参考 [^29] 和 [^37]，首次迭代中 $\mathbf{H}_l^T \mathbf{H}_l$ 的最小特征值被设为激光雷达退化的判定条件。由于多模态处理流程以及开阔场景下的稳定 GNSS 数据，即便存在持续的激光雷达退化现象，系统仍能保持定位功能。 ### B. GNSS 故障场景 当 GNSS 测量丢失时，系统将跳过 GNSS 处理环节（包括坐标变换和 GNSS 更新），直接依赖激光雷达-IMU 子系统。在这种模式下（即 LIO 模式），激光雷达扫描之间直接进行前向传播，且不会以分布式形式执行反向传播。系统状态和点云地图会以激光雷达的频率输出，但缺乏 GNSS 提供的全局约束。此外，ENU 坐标系的站点中心以及其到世界坐标系的变换会被暂时保留；一旦 GNSS 信号恢复，这些信息将再次被用于 GNSS-IMU 子系统，无需重新初始化。 ### C. 激光雷达故障场景 为了判断激光雷达是否发生故障，我们的系统中设计了异常数据检测机制以及当前 GNSS 数据与上次激光雷达数据之间的时间间隔 $t_{chk}$。一旦采样到异常点，或者 $t_{chk}$ 超过某一阈值（该阈值根据激光雷达的频率设定，本系统中 10 Hz 激光雷达对应的阈值为 0.2 秒），就说明激光雷达测量已丢失。 $$ t_{\mathrm{chk},k}=t_{g,k}-t_{l,\mathrm{e n d}}<\alpha $$ 其中，$t_{g,k}$ 是当前GNSS的采样时间，而 $t_{l,end}$ 是近期激光雷达测量中最后一个点的时间。
TABLE I
PUBLIC DATASET SPECIFICATIONS
DatasetsEnvironmentsSensors
M2DGRCampus/StreetsVelodyne VLP-32C LiDAR (200 m range, $360^{\circ} \times 40^{\circ}$ FoV, 10 Hz)\nu-blox M8T (GPS/Beidou, 4 Hz)\nHandsfree IMU (150 Hz)\nGT: GNSS/RTK (within 2 cm accuracy)
UrbanNavUrbanVelodyne HDL-32E LiDAR (80 m range, $360^{\circ} \times 40^{\circ}$ FoV, 10 Hz)\nu-blox EVK-M8T (1 Hz)\nXsens Mti10 IMU (100 Hz)\nGT: GNSS/RTK (within 5 cm accuracy)
LIO-SAMParkVelodyne VLP-16 LiDAR (100 m range, $360^{\circ} \times 30^{\circ}$ FoV, 10 Hz)\nMicroStrain 3DM-GX5-25 IMU Reach M GPS
当遇到激光雷达故障时，状态只会向前传播至GNSS测量结果，分布式反向传播也会被暂停，以便单独更新GNSS的状态。此时系统以GNSS-惯性里程计模式运行。在这种模式下，估计出的状态会在GNSS粗略更新后直接输出，而非先缓存起来。此外，由于GNSS测量结果不稳定，ECEF坐标系与世界坐标系之间的变换关系是固定的。需要注意的是，由于激光雷达故障，由点构成的地图将不会被更新。 ### 六、实验 ### A. 数据集与实现 为了验证所提系统的有效性，我们在最新的公开数据集上开展了全面实验，并在现实世界中进行了测试。我们分别从精度、效率和鲁棒性三个方面进行了定性和定量分析。 最新的公开数据集具有挑战性且包含精确的真实值，有助于与其他导航系统进行定量评估。我们选取的公共挑战性数据集来自M2DGR [^38]、UrbanNav [^39] 和 LIO-SAM [^22] 的研究工作，这些数据集包含多传感器数据及不同场景。数据集规格如表I所示。我们还搭建了一辆配备传感器套件的无人地面车辆（UGV）用于现实世界实验，如图4所示。相关传感器包括：Velodyne VLP-16激光雷达（$360^{\circ} \times 30^{\circ}$视场角，100米探测范围，10 Hz）、采样率为200 Hz的JY901惯性测量单元，以及配备低成本天线的u-blox M8T GNSS接收机（SPP解决方案，中国青岛，GPS和北斗型号，4 Hz）。为实现时空同步，多个传感器通过机械模块固定其外部参数，而时间插值则在系统中完成，无需硬件触发对齐。 我们的系统基于C++实现，运行在机器人操作系统（ROS）框架下。无论是在数据集上还是现实世界中的实验，都是在搭载Intel $^{1}$ i7-7700HQ CPU和8 GB RAM的笔记本电脑上进行的，该系统运行Ubuntu 18.04系统。 --- ![Image](images/728e2047a04b3153dc149c2143238f6bf784c6717bf77d5e4562af3757ee513b.jpg)
Fig. 4. Equipment configuration used in our real-world testing. (a) UGV for testing. (b) LiDAR and GNSS antenna. (c) GNSS receiver and IMU.
此外，我们将我们的系统与FAST-LIO2 [^21]、LIO-SAM（无闭环校正）[^22] 以及LINS [^10] 进行了对比，这些都是开源的当前最先进融合方法。为保证公平性，实验参数遵循每种方法推荐的配置。对于我们的系统，我们设置了1000米的局部地图、0.5米的体素滤波器、三点跳过操作、最多三次激光雷达迭代以及最多十次GNSS迭代，这些设置与其他测试系统相同（GNSS迭代次数除外）。 ### B. 精度评估 我们采用上述公开数据集中的共11个序列来评估系统精度。我们使用绝对位姿误差（APE）的均方根误差（RMSE）和标准差（STD）来衡量系统的整体精度和误差波动情况。所有估计轨迹均在世界坐标系下输出，而真实值则位于ECEF坐标系中；因此，在评估前需要进一步进行轨迹对齐。 精度评估的实验结果如表II所示。从结果可以看出，在11个序列中，有5个序列上所提出的方法在APE的RMSE方面取得了最佳精度。在其余6个序列中，FAST-LIO2和支持GPS的LIO-SAM各在3个序列上表现更优，但我们的结果在这些序列上仍具有竞争力。就误差的标准差而言，在11个序列中的6个序列上，我们的系统与其他方法相比实现了相同或更低的误差波动，显示出令人满意的稳定性。部分序列的轨迹对比结果可在图5中直观看到。当所有传感器工作正常时，融合里程计会利用多种约束来实现具有竞争力的精度和稳定性。 此外，我们开展了消融实验以测试分布式反向传播的有效性。我们比较了常规反向传播与分布式反向传播所采用的方法，结果如图6所示，图7则给出了gate01和gate02序列上的更多细节。在大多数序列中，实验结果反映了分布式反向传播对精度的积极影响。由于GNSS与激光雷达之间存在异步问题，未采用分布式反向传播的方法无法充分利用预测状态来进行补偿。
TABLE II
APE (RMSE AND STD, m)
Seq IDLINSFAST-LIO2LIO-SAMLIO-SAM $^{{*}}$Proposed
gate010.15\n(0.06)0.18\n(0.06)0.61\n(0.45)0.15\n(0.06)0.14\n(0.05)
gate020.33\n(0.18)0.33\n(0.18)0.33\n(0.19)0.33\n(0.19)0.31\n(0.18)
street011.71\n(0.93)0.32\n(0.12)7.85\n(5.93)0.49\n(0.20)0.36\n(0.14)
street025.43\n(3.07)2.81\n(1.68)3.96\n(2.44)1.22\n(0.70)2.67\n(1.55)
street030.14\n(0.05)0.19\n(0.06)0.14\n(0.06)0.14\n(0.06)0.13\n(0.05)
street042.65\n(1.18)0.46\n(0.22)0.88\n(0.46)2.36\n(1.20)0.51\n(0.24)
street053.57\n(1.78)0.35\n(0.13)0.43\n(0.16)0.50\n(0.21)0.34\n(0.12)
street061.72\n(0.77)0.39\n(0.11)0.40\n(0.15)0.76\n(0.26)0.42\n(0.11)
street080.21\n(0.09)0.23\n(0.09)0.66\n(0.55)0.19\n(0.09)0.18\n(0.06)
hk03141.88\n(0.68)1.62\n(0.57)1.77\n(0.68)1.37\n(0.48)1.60\n(0.53)
hk04284.46\n(1.30)4.31\n(1.33)7.07\n(2.75)2.45\n(1.04)4.72\n(1.18)
* 启用GPS因子的LIO-SAM。 ![Image](images/c1f5f9ad1e42f4052b3e07a12759df77e4e5576342ae0bf5a1c23b621518a475.jpg) ![Image](images/7233752e70b7fd94b820f126f65607e1c1e8a56c28c499dd858d037c3ff8d65f.jpg) ![Image](images/76af65c2c1827615e939f731989b7d5155065bd6b1401fd914b616f72fbb6238.jpg) ![Image](images/597166c10df62bd9ce93b95ab27384534132a7e87112d1f77c959b367cbf8e87.jpg)
Fig. 5. Trajectories of tested methods on partial sequences: (a) gate01 sequence; (b) street03 sequence; (c) street06 sequence; and (d) ulhk0314 sequence.
运动畸变进一步导致只有部分点能够被补偿，而非全部点。在street04和street06序列中，我们认为分布式反向传播导致精度下降的原因是点的质量问题。环境中的非结构化特征和动态物体增加了错误点的数量，因此在这种情况下，减少点的数量反而能提升精度。 ### C. 效率分析 本节采用相同的配置来分析整体效率以及快速动态帧变换的性能。本次测试共选取了13个序列。对于参与本次测试的SOTA方法…… --- ![Image](images/2c71d3ed3d4361acba9a93b36376c4fda3662a4a730bde9e4a5dac7ea1a47602.jpg)
Fig. 6. Accuracy comparison of distributed backward propagation.
![Image](images/3cf8d45810d51983ccae93cb73ea60e4fca9dce78983fba3d3311db7ac0d55c5.jpg) ![Image](images/1fa93dce50bb7784d8cbff533977dd556174410b19997d4bcb6a5c38b13fe599.jpg) ![Image](images/dbea8727d7c7b6a89eb6ede0c402de27730ce4a16c65398d5ecdede5702bf28b.jpg)
(a)
![Image](images/8d7e4c975601da99e070e9aef24ccad8bb8899889a4824e17e4f597ff1bdc69b.jpg)
(b)
Fig. 7. Number of predicted states involved in backward propagation and the number of compensated points: (a) gate01 sequence and (b) gate02 sequence.
![Image](images/5584e4c31abc6562281b5493584e73d3380a669a2d06485b72fe6ea4f3e96903.jpg)
(a)
![Image](images/13a8e3aa3aa92cd34f927ed5f77f4067ac4fbbfc73e3032b33ec2edba3ccc6cd.jpg)
(b)
Fig. 8. Time consumption of different subsystems on sequences: (a) on the sequences of gate01 and (b) on the sequences of gate02.
![Image](images/b835d0ac093f00117447679d4426c64a976fb2d69f92877592b26250a661d351.jpg)
Fig. 9. Transformation of GNSS measurements between the ENU frame and the world frame.
测试中涉及的LIO-SAM（优化器）、LINS（基于滤波的前端和基于优化的后端）以及FAST-LIO2（基于滤波）采用了不同的架构。这使得能够以直观的方式与所提出的方法进行比较。 ![Image](images/6fc8fc1b280ed76b2b40060831236c873cddf5fef7311c6cfef90e7238b24601.jpg)
Fig. 10. Illustration of the real-world test sites. The areas covered by the yellow indicate the degraded scenes of LiDAR or GNSS. The red, green, and blue indicate the sensor failure situations.
![Image](images/e53e8ba3e91a3cf312f0a932fe826fd340c2eaaf7756466f02f3056205491926.jpg)
(b)
Fig. 11. Environments of the real-world test site. (a) Center of the grass (open and structure-less). (b) Edge of the grass. (c) Test site of GNSS failure. (d) Test site of GNSS degradation.
为进行效率分析，计算了上述方法的平均时间消耗，包括数据预处理、位姿计算和建图环节。结果如表III所示：启用GPS时LIO-SAM的处理时间消耗约为226毫秒，未启用GPS时约为203毫秒，其时间消耗最大；由于采用基于滤波的前端，LINS的效率略高（约112毫秒）。尽管FAST-LIO2的效率最高（约46毫秒），但我们提出的方法仍具有竞争力。我们的平均时间消耗为56毫秒，这显示出相较于基于优化器的方法的显著优势。此外，我们分析了不同子系统的耗时情况，如图8所示。受大量点处理的影响，激光雷达-惯性测量单元子系统消耗了大部分计算资源，而由于数据较为稀疏，GNSS-惯性测量单元子系统的处理速度更快。 为验证帧变换的性能，表IV中计算了序列gate01（时长172秒）上帧变换各步骤的时间使用情况。滑动窗口的维护占据了帧变换的大部分计算资源。不过总的时间消耗仍然很小，总计…… --- ![Image](images/4e5ee8967e5ed5bca59ad98714e5d6885ba345bb3704dc27b3c58da50cf2f8a6.jpg)
Fig. 12. Estimated trajectories and mapping results in LiDAR degraded scenario. (a) Results of the proposed method. (b) Mapping details at the start position. (c) Mapping details at the midway position. (d) Results of FAST-LIO2. (e) Results of LIO-SAM $^{*}$ (GPS factor enabled). (f) Results of LIO-SAM. (g) Results of LINS.
TABLE III
AVERAGE TIME COST PER PROCESSING (ms)
Seq IDLINSFAST-LIO2LIO-SAMLIO-SAM $^{{*}}$Proposed
gate01103.1353.08192.48246.4962.49
gate0295.8948.51231.17240.9257.42
street01111.1447.19243.69265.2659.59
street0292.0444.34144.90196.3950.66
street03113.7450.95272.12313.6066.05
street0488.4945.46210.96198.5653.76
street0593.6241.37124.80167.9248.84
street0694.0046.11111.02180.5251.80
street07116.6949.56312.99304.1462.99
street08116.1351.35326.41336.8566.31
hk0314132.8844.29158.96162.3352.06
hk0428138.1049.18182.34186.3159.13
park01169.3729.85130.61134.6437.26
$^{*}$ 启用GPS因子的LIO-SAM。
TABLE IV
TIME USAGE FOR EACH STEP OF FRAME TRANSFORMATION (ms)
Stage (s)Setting PositionSliding WindowsWeighting ComputationNumber of Adjustment
0~500.00140.20230.058734
50~10000.09700.040317
100~15000.19730.054741
150~17200.04240.015311
Total0.00140.5390.1691103
在172-s序列上，维护成本不到1毫秒。图9展示了GNSS测量在ENU坐标系与世界坐标系之间的转换结果，表明该坐标系转换能够以较低的时间成本实现高精度。 ### D. 现实环境中的鲁棒性测试 由于缺少传感器退化与故障情况，大多数公开数据集无法反映真实环境的复杂性。因此，我们开展了现实世界实验，以测试所提系统的强大鲁棒性。实验地点选在城市区域，这些区域包含传感器退化场景及故障情况，如图10所示。我们在 ![Image](images/1ebcedb1058fe7126b50e4700a2a5758b8465c3d129d88aff6ca71957ea43a37.jpg) ![Image](images/790b3a0f27f485a21be151da37e2e2c6a1aeb6c034fd7d54e58c980859ac17d5.jpg)
Fig. 13. Experiment in GNSS degraded scene (the bottom right panel is the corresponding status of available satellites). (a) Estimated trajectory and mapping results. (b) Utilization of GNSS measurements in the world frame.
开放且无结构特征的场景以及激光雷达和GNSS的遮挡场景中进行了测试。此外，本节还开展了激光雷达与GNSS的故障测试，即系统在运行过程中出现传感器故障的情况。测试的具体环境可参见图11。 1) 退化场景：我们在激光雷达和GNSS的退化场景中测试了我们的系统，这些场景包括开放且无结构的草地，以及建筑物造成的遮挡区域。在激光雷达退化场景中，我们驾驶无人地面车辆穿过典型的退化场景，从草地中心沿人行道返回起始位置。此外，我们还将该方法与当前最先进的FAST-LIO2、LIO-SAM和LINS方法进行了对比。估算的轨迹与建图结果可见图12。结果表明，当激光雷达面临退化时，我们的系统比其他系统展现出更精准的定位与建图性能。估算出的轨迹呈闭环状态， --- ![Image](images/7964660916e81c4b89e3fef0a6a75cacb6c405ff450b1b75db5e56b068bad871.jpg)
Fig. 14. Experiment in the situation of data anomaly and recovery. (a) Estimated trajectory and mapping results in the experiment. (b) Real-world testing site. (c) Details of GNSS data failure and recovery. (d) Details of LiDAR data failure and recovery. (e) 10-s LiDAR failure case. (f) Top view of mapping and trajectory.
且与实际行驶轨迹相符。当无人地面车辆穿过草地后返回起始位置时，右侧墙壁[见图12(b)]会重新被构建，全局地图仍保持一致。相比之下，FAST-LIO2虽也完成了此次退化测试，但可从图12(d)中的白色箭头处直观看到由累积漂移导致的建图变形与非闭环轨迹。此外，基于特征方法的LIO-SAM（开启GPS与未开启GPS）和LINS未能通过退化测试；当无人地面车辆驶入草地中心时，点特征难以被提取。开启GPS因素虽有助于提升LIO的长期性能，但不足以应对激光雷达的多种退化情况。 接着，我们在半遮挡环境中测试系统，以验证其对GNSS退化的鲁棒性。无人地面车辆从开阔广场出发，穿越GNSS测量不稳定的遮挡区域。可用卫星数量、相应的定位精度衰减因子（PDOP）以及平均信噪比（SNR）显示在图13的右下角。进入遮挡区域后，PDOP剧烈波动，最大值达到5.2，SNR则从30 dB以上持续下降。从图13(a)可以看出，当GNSS退化时，所提系统仍能提供精准的定位与建图结果。我们不仅会检查原始GNSS测量的有效性，还会利用系统状态与世界坐标系下GNSS数据的相关性。GNSS的利用情况可见图13(b)：广场处的稳定测量数据会被纳入状态估计，而不稳定的数据则会被剔除。因此，GNSS的退化不会影响系统的整体性能。 2) 故障情况：此处展示了所提系统在面对传感器故障时强大的鲁棒性。为确保实验的全面性，我们将传感器故障分为两种情况：异常后的数据恢复以及数据完全中断。我们手动修改测试数据以模拟数据异常与恢复过程。如图14所示，传感器数据会在固定时间内被设为空白值（GNSS和激光雷达均为5秒）。系统首先会遭遇异常的GNSS数据[图14(c)]，这些异常数据会被剔除，系统则切换为激光雷达-惯性测量单元子系统运行。此时地图构建与状态估计不会停止，一旦GNSS数据恢复，全局约束便会重新生效。随后以类似方式模拟激光雷达数据故障，无效的点云数据也会被系统检测并剔除[图14(d)]。因此，GNSS-惯性测量单元子系统会独立运行，而在激光雷达故障时无法构建地图。数据恢复后，基于融合里程计技术，系统会在同一场景下继续开展状态估计与地图构建工作。我们进一步延长激光雷达故障时间至10秒以增强挑战性，结果如图14(e)所示，其中白色箭头标示了故障持续时间。尽管数据恢复后存在细微误差（红色方框内），但考虑到全局一致的地图与强大的鲁棒性，这一结果仍可接受。 为模拟最恶劣的故障场景，我们还在系统运行时完全切断传感器数据开展实验。初始阶段所有传感器均正常工作，随后GNSS或激光雷达会在硬件层面从无人地面车辆上物理断开。相应数据无法被系统采集，后续也无法恢复。在激光雷达故障测试中，故障时段始于1分46秒，持续45秒。数据序列与测试结果可见图15：系统初期以融合里程计模式（GNSS、激光雷达与惯性测量单元）运行，按激光雷达频率构建地图并估计状态。当激光雷达故障时，系统会无缝切换为GNSS-惯性测量单元里程计模式，从而在当前世界坐标系下维持定位功能，系统输出频率则取决于GNSS测量值。由于激光雷达故障，由点构成的3D地图将不再更新，但既有的全局地图仍可使用。实验结果表明，即便激光雷达发生故障，我们的系统仍能协助机器人完成全程定位功能。在GNSS故障测试中，切断的是GNSS数据而非激光雷达数据，故障时段为 --- ![Image](images/bf39818aaa5b13e52cfc34434704e44610489f29dbfe10b136f45a760914db41.jpg)
Fig. 15. Experimental results in the situation of LiDAR failure.
![Image](images/18eff74aac4b36d1e79658289e4fbceb617d36dbe6386967d60621343b874821.jpg)
Fig. 16. Experimental results in the situation of GNSS failure.
从40秒持续到结束（55秒），如图16所示。缺少GNSS测量数据时，系统会从融合里程计模式切换为激光雷达惯性里程计模式。在此模式下，系统仅丧失来自GNSS的全局约束，定位核心功能并未受影响，地图构建与定位结果按激光雷达频率输出。得益于多模态设计，所提系统具备足够的鲁棒性，能够承受GNSS或激光雷达的故障。 ### 七、结论 反向传播。通过大量实验可以证明，我们的系统达到了较高的精度与效率，且对部分传感器性能退化甚至故障具有强大的鲁棒性。 本文聚焦于导航系统在常见极端环境下的性能表现，以便实际应用。通过融合GNSS、IMU和激光雷达数据，我们提出了一种具备多种模态的鲁棒且快速的SLAM框架。借助基于子系统的多模态特性以及IESKF-based部署方式，该系统能够在应对传感器性能退化或故障时，有效实现从粗到精的状态估计。此外，针对帧不一致和数据异步引发的问题，我们开发了快速且动态的帧转换方法与分布式系统。不过所提出的系统仍存在一定局限性，未来工作中有望加以改进，例如更紧密地耦合GNSS与LIO技术，以及在多模态切换时实现轨迹平滑处理。 ## 参考文献 [^1]: F. Yang, C. Cao, H. Zhu, J. Oh, and J. Zhang, "FAR planner: Fast, attemptable route planner using dynamic visibility update," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2022, pp. 9–16, doi: 10.1109/IROS47612.2022.9981574. [^2]: C. Cao, H. Zhu, H. Choset, and J. Zhang, "TARE: A hierarchical framework for efficiently exploring complex 3D environments," in Proc. Robot., Sci. Syst., Jul. 2021, pp. 1–9, doi: 10.15607/RSS.2021.XVII.018. [^3]: L. Jarin-Lipschitz, X. Liu, Y. Tao, and V. Kumar, "Experiments in adaptive replanning for fast autonomous flight in forests," in Proc. Int. Conf. Robot. Autom. (ICRA), May 2022, pp. 8185–8191, doi:10.1109/ICRA46639.2022.9812235. [^4]: X. Liu et al., “Large-scale autonomous flight with real-time semantic SLAM under dense forest canopy,” IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 5512–5519, Apr. 2022, doi: 10.1109/LRA.2022.3154047. [^5]: K. Mohta et al., “Fast, autonomous flight in GPS-denied and cluttered environments,” J. Field Robot., vol. 35, no. 1, pp. 101–120, Jan. 2018, doi: 10.1002/rob.21774. [^6]: T. Qin, T. Chen, Y. Chen, and Q. Su, “AVP-SLAM: Semantic visual mapping and localization for autonomous vehicles in the parking lot,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2020, pp. 5939–5945, doi: 10.1109/IROS45743.2020.9340939. [^7]: G. Wan et al., “Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2018, pp. 4670–4677, doi: 10.1109/ICRA.2018.8461224. [^8]: H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3D LiDAR inertial odometry and mapping,” in Proc. Int. Conf. Robot. Autom. (ICRA), May 2019, pp. 3144–3150, doi: 10.1109/ICRA.2019.8793511. [^9]: H. Li, B. Tian, H. Shen, and J. Lu, “An intensity-augmented LiDAR-inertial SLAM for solid-state LiDARs in degenerated environments,” IEEE Trans. Instrum. Meas., vol. 71, pp. 1–10, 2022, doi:10.1109/TIM.2022.3190060. [^10]: 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. Autom. (ICRA), May 2020, pp. 8899–8906, doi: 10.1109/ICRA40945.2020.9197567. [^11]: W. Xu and F. Zhang, "FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter," IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021, doi: 10.1109/LRA.2021.3064227. [^12]: D. He, W. Xu, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry,” Adv. Intell. Syst., vol. 5, no. 7, Jul. 2023, Art. no. 2200459, doi: 10.1002/aisy.202200459. [^13]: S. P. Kleinschmidt and B. Wagner, “Visual multimodal odometry: Robust visual odometry in harsh environments,” in Proc. IEEE Int. Symp. Saf., Secur., Rescue Robot. (SSRR), Aug. 2018, pp. 1–8, doi: 10.1109/SSRR.2018.8468653. [^14]: M. Palieri et al., “LOCUS: A multi-sensor LiDAR-centric solution for high-precision odometry and 3D mapping in real-time,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 421–428, Apr. 2021, doi: 10.1109/LRA.2020.3044864. [^15]: N. Khedekar, M. Kulkarni, and K. Alexis, “MIMOSA: A multi-modal SLAM framework for resilient autonomy against sensor degradation,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2022, pp. 7153–7159, doi: 10.1109/IROS47612.2022.9981108. [^16]: S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super odometry: IMU-centric LiDAR-visual-inertial estimator for challenging environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2021, pp. 8729–8736, doi: 10.1109/IROS51168.2021.9635862. ## [^17]: J. Sola, “Quaternion kinematics for the error-state Kalman filter,” 2017, arXiv:1711.02508 --- [^18]: V. Madyastha, V. Ravindra, S. Mallikarjunan, and A. Goyal, “Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation,” in Proc. AIAA Guid., Navigat., Control Conf., Aug. 2011, p. 6615, doi: 10.2514/6.2011-6615. [^19]: J. Havlík and O. Straka, “Performance evaluation of iterated extended Kalman filter with variable step-length,” J. Phys., Conf., vol. 659, Nov. 2015, Art. no. 012022, doi: 10.1088/1742-6596/659/1/012022. [^20]: J. Zhang and S. Singh, “LOAM: LiDAR odometry and mapping in real-time,” in Proc. Robot., Sci. Syst. Conf., 2014, pp. 1–9, doi:10.15607/RSS.2014.X.007. [^21]: 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, doi: 10.1109/TRO.2022.3141876. [^22]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIOSAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2020, pp. 5135–5142, doi: 10.1109/IROS45743.2020.9341176. [^23]: Y. Wang, Y. Lou, W. Song, Z. Tu, Y. Wang, and S. Zhang, “Simultaneous localization of rail vehicles and mapping of surroundings with LiDAR-inertial-GNSS integration,” IEEE Sensors J., vol. 22, no. 14, pp. 14501–14512, Jul. 2022, doi: 10.1109/JSEN.2022.3181264. [^24]: S. Zhong, C. Wei, J. Hu, T. Zhang, J. Yu, and Y. Chen, “An outlier-robust GNSS-inertial-LiDAR localization system,” in Proc. Int. Conf. Sens., Meas. Data Analytics Era Artif. Intell. (ICSMD), Oct. 2021, pp. 1–6, doi: 10.1109/ICSMD53520.2021.9670764. [^25]: K.-W. Chiang, G.-J. Tsai, H.-J. Chu, and N. El-Sheimy, “Performance enhancement of INS/GNSS/refreshed-SLAM integration for acceptable lane-level navigation accuracy,” IEEE Trans. Veh. Technol., vol. 69, no. 3, pp. 2463–2476, Mar. 2020, doi: 10.1109/TVT.2020.2966765. [^26]: T. Li, L. Pei, Y. Xiang, X. Zuo, W. Yu, and T.-K. Truong, “P $^{3}$-LINS: Tightly coupled PPP-GNSS/INS/LiDAR navigation system with effective initialization,” IEEE Trans. Instrum. Meas., vol. 72, pp. 1–13, 2023, doi: 10.1109/TIM.2023.3244811. [^27]: C. Shu and Y. Luo, “Multi-modal feature constraint based tightly coupled monocular visual-LiDAR odometry and mapping,” IEEE Trans. Intell. Veh., vol. 8, no. 5, pp. 3384–3393, May 2023, doi: 10.1109/TIV.2022.3215141. [^28]: S. Boche, X. Zuo, S. Schaefer, and S. Leutenegger, “Visual-inertial SLAM with tightly-coupled dropout-tolerant GPS fusion,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2022, pp. 7020–7027, doi: 10.1109/IROS47612.2022.9981134. [^29]: 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. Autom. (ICRA), May 2021, pp. 5692–5698, doi: 10.1109/ICRA48506.2021.9561996. [^30]: J. Lin, C. Zheng, W. Xu, and F. Zhang, "R2LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping," IEEE Robot. Autom. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021, doi: 10.1109/LRA.2021.3095515. [^31]: W. Wu, X. Zhong, D. Wu, B. Chen, X. Zhong, and Q. Liu, "LIO-fusion: Reinforced LiDAR inertial odometry by effective fusion with GNSS/relocalization and wheel odometry," IEEE Robot. Autom. Lett., vol. 8, no. 3, pp. 1571–1578, Mar. 2023, doi: 10.1109/LRA.2023.3240372. [^32]: G. He, X. Yuan, Y. Zhuang, and H. Hu, “An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments,” IEEE Trans. Instrum. Meas., vol. 70, pp. 1–9, 2021, doi: 10.1109/TIM.2020.3024405. [^33]: N. Li et al., “Indoor and outdoor low-cost seamless integrated navigation system based on the integration of INS/GNSS/LiDAR system,” Remote Sens., vol. 12, no. 19, p. 3271, Oct. 2020, doi: 10.3390/rs12193271. [^34]: T. Lupton and S. Sukkarieh, “Efficient integration of inertial observations into visual SLAM without initialization,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2009, pp. 1547–1552, doi:10.1109/IROS.2009.5354267. [^35]: C. Hertzberg, R. Wagner, U. Frese, and L. Schröder, “Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds,” Inf. Fusion, vol. 14, no. 1, pp. 57–77, Jan. 2013, doi: 10.1016/j.inffus.2011.08.003. [^36]: S. Cao, X. Lu, and S. Shen, “GVINS: Tightly coupled GNSS-visual-inertial fusion for smooth and consistent state estimation,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2004–2021, Aug. 2022, doi:10.1109/TRO.2021.3133730. [^37]: J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2016, pp. 809–816, doi: 10.1109/ICRA.2016.7487211. [^38]: J. Yin, A. Li, T. Li, W. Yu, and D. Zou, “M2DGR: A multi-sensor and multi-scenario SLAM dataset for ground robots,” IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 2266–2273, Apr. 2022, doi: 10.1109/LRA.2021.3138527. [^39]: D. Wang, W. Wen, L.-T. Hsu, and Y. Zhong. (2020). An Open-Sourcing Localization Datasets Collected in Asian Urban Canyons, Including Tokyo and Hong Kong. [Online]. Available: ![Image](images/75cbff609900e72cd25efca1fc18ca6ab56ce20d723a060fc08de5fd7fb3d376.jpg) 高继鑫于1999年出生于中国哈尔滨。他目前正在中国青岛的哈尔滨工程大学青岛创新发展基地攻读控制科学与工程专业的硕士学位。 他的研究方向包括同步定位与建图（SLAM）、多传感器融合以及机器人导航系统。 ![Image](images/bc7389b7ce0abe34f7ffeb0d5aba595efe9b4f3cee3e984cf3530f403d33324e.jpg) 沙建军于2012年在中国北京的中国科学院遥感与数字地球研究所获得测绘与地理信息系统专业博士学位。 他目前是哈尔滨工程大学青岛创新发展基地的自动控制专业副教授。他曾主持过三项以上的重点科研项目。他的研究方向包括遥感图像处理、物体检测、三维视觉与机器人技术，以及光电对抗技术。 沙博士曾多次荣获部级及省级科学技术奖项。 ![Image](images/d629c51720c674be1c0d260cf011bf16859c4250062bdb23a2a587cef5ff970a.jpg) 李宏文于2021年从中国长沙的长沙理工大学电气与信息工程学院获得学士学位。他目前正在中国青岛的哈尔滨工程大学青岛创新发展基地攻读信息与通信工程专业的硕士学位。 他当前的研究方向包括四旋翼无人机吊载系统的运动规划、机器学习以及非线性控制。 ![Image](images/c0c5f1f2e9f0a8cd88e04c5e7c40b737a6898bafb3fc04b08ea7d069167a0c0f.jpg) 王彦恒（IEEE学生会员）于2020年从中国上海的海洋大学信息技术学院获得软件工程专业硕士学位。他目前正在中国哈尔滨的哈尔滨工程大学智能系统科学与工程学院攻读人工智能专业博士学位。 他的研究方向包括遥感图像变化检测、物体检测以及深度学习。