# JONA-SLAM：结合观测与法向感知的紧耦合回环检测SLAM算法
乔伟·简 $^{iD}$、马庆路、王欣宇、萨利姆·乌拉
摘要—在自主导航领域，精准的定位与建图至关重要，尤其是针对在全球卫星导航系统（GNSS）信号受限或无法接收的区域运行的自动驾驶车辆而言。本文提出了一种结合观测与法向感知（JONA）的同时定位与建图（SLAM）算法，这是一种紧耦合的激光雷达-视觉-惯性SLAM系统，旨在提升复杂场景下自动驾驶平台的状态估计鲁棒性，这类场景包括城市峡谷和隧道等。基于我们此前在点线特征提取方面的研究，所提出的系统通过构建激光雷达-视觉联合观测因子来强化空间约束。该因子能够在特征层面将激光雷达获取的几何信息与视觉纹理特征紧密耦合，有效解决GNSS信号受限区域的观测退化问题。
此外，本文还提出了一种法向感知的混合回环检测方法，该方法引入点云法向一致性验证机制，显著提升了回环识别的可靠性，并能抑制半结构化场景中的误检现象。在22组公开数据集以及自行采集的校园数据集上的评估结果表明，JONA-SLAM的性能优于或可与现有最先进方法相媲美。在23个实验测试案例中，所提方法可将轨迹误差[均方根误差（RMSE）]平均降低35.02%。该系统在保持实时性能的同时，展现出优异的环境适应性与全局一致性，为信号受限环境下的自动驾驶及各类移动平台提供了高精度且鲁棒的定位解决方案。
关键词—联合观测因子、激光雷达-视觉-惯性融合、法向感知回环检测、紧耦合同时定位与建图（SLAM）。
## 一、引言
在过去的二十年间，同步定位与建图（SLAM）已成为自主系统和智能车辆的关键技术[^1]，[^2]。虽然早期的单传感器方案成本较低，但它们存在固有的局限性。
具体而言，摄像头对光照条件和无纹理场景较为敏感，而激光雷达则易受稀疏几何特征和运动畸变的影响。尤其在复杂都市区域的自动驾驶场景中，单传感器方案往往无法满足全球导航卫星系统（GNSS）受限或信号受阻环境下的严苛可靠性要求，比如高楼林立的城市峡谷或隧道中——这些地方的卫星信号常被高层建筑或地下结构阻挡[^3]。
虽然紧密耦合的多传感器融合因其能在原始测量数据的联合优化方面表现优异而备受青睐[^4]、[^5]，但现有框架在从开放道路过渡到这些受限区域时仍面临若干关键瓶颈。具体而言，许多系统缺乏足够的特征级整合能力，无法在低纹理区域将视觉特征与激光雷达几何信息相匹配；同时，线特征所提供的结构信息也尚未得到充分开发[^6]。此外，在大规模城市环境中，由于环境结构的重复性，闭环检测仍容易出现错误匹配，从而影响长期全局一致性[^7]、[^8]。
---
为应对这些挑战，本文提出了JONA-SLAM（联合观测与法向感知闭环检测）算法，这是一种专为恶劣环境下的自主导航设计的鲁棒型紧密耦合激光雷达-视觉-惯性融合框架。基于我们此前在点线特征提取方面的研究成果[^9]，该系统引入了激光雷达-视觉联合观测因子，通过将激光雷达几何信息视为动态观测值来实现无缝的特征级融合。此外，还提出了一种法向感知的混合式闭环检测模块，该模块结合了视觉外观信息与表面法向验证机制，即便在几何结构高度复杂的场景中也能确保全局一致性。我们的主要贡献如下。
1) 我们提出了一种统一的观测模型，通过整合激光雷达的空间约束与视觉特征来实现紧密的特征级融合，从而显著提升了在视觉或几何纹理稀疏的场景中的鲁棒性。
2) 我们提出了一种鲁棒的闭环检测机制，该机制利用激光雷达的法向一致性验证来消除复杂环境中的误检现象。
3) 我们实现了联合观测与法向感知（JONA）-SLAM算法，并通过在多种数据集上的大量实验验证了该算法的有效性。结果表明，相较于LVI-SAM等现有技术，我们的框架在定位精度和鲁棒性方面均表现出更优的性能。
本文的其余部分安排如下：第二节综述相关研究；第三节介绍JONA-SLAM算法；第四节展示实验结果；第五节总结结论与未来工作。
### 二、相关研究
近年来，多传感器融合SLAM因其能够提升定位精度与鲁棒性的潜力而受到研究人员的日益关注[^10]。当前的融合方法主要分为两类：松耦合与紧耦合[^11]。尽管松耦合方法在模块化程度与计算简便性方面具有优势，但紧耦合方法通过联合优化原始传感器数据，通常能实现更高的精度与鲁棒性。
在紧密耦合的框架中，基于卡尔曼滤波（KF）的方法因其高效的计算性能而被广泛应用[^12]。例如，通过施密特正交化和平方根分解对无迹卡尔曼滤波（UKF）进行改进后，能够显著提升SLAM性能[^13]。Fast-LIO采用迭代扩展卡尔曼滤波（EKF）来紧密融合激光雷达特征与惯性测量单元（IMU）数据以实现鲁棒导航，同时引入了依赖状态调整的卡尔曼增益以优化性能[^14]。其后续版本Fast-LIO2则能够直接处理原始点云数据并实时更新地图，无需进行特征提取[^15]。然而，基于递归滤波的方法在复杂的自动驾驶场景中往往面临诸多挑战：它们在高度非线性的环境中容易产生线性化误差，且缺乏内置机制来融入诸如闭环约束之类的全局约束，从而难以保证长期的一致性[^16], [^17]。
为填补这些空白，基于优化的框架——尤其是因子图优化方法，已成为主流范式。紧密耦合的优化方法通过整合多种约束来实现更优的性能。例如，RaI-SLAM[^18]将汽车雷达与IMU结合，可在无全球定位系统（GNSS）辅助的情况下实现精准定位；而Switch-SLAM[^19]则采用激光雷达与视觉里程计之间的切换结构来维持系统的鲁棒性。在多模态融合方面，LVI-SAM[^20]在因子图中整合了激光雷达、视觉及惯性里程计数据，使得各子系统既能独立运行也能协同工作。R3LIVE++[^21]则侧重于密集地图构建，通过分别运行几何映射与光度映射线程来实现这一目标。除了上述传感器外，激光雷达、轮式里程计以及红绿蓝深度（RGB-D）相机的融合也显著提升了系统的鲁棒性[^22]。一个典型的例子是lidAR-aided multi-view（LAMV）-SLAM，它紧密耦合不同传感器的数据来估计并修正误差，从而无需图形处理器（GPU）加速即可实现实时密集映射[^23]。另一种创新方法是让前端各传感器独立运行，随后将所有测量结果整合到后端优化中$\mathbf{p}_f(u_f, v_f)$ [^24]。与递归滤波器不同，这类基于因子图的系统支持迭代重新线性化，并能无缝融入全局闭环约束因子，进而确保轨迹的全局一致性。
尽管取得了诸多进展，现有系统在噪声敏感型匹配及不可靠的闭环检测方面仍存在不足。我们提出的JONA-SLAM是一种专为复杂自动驾驶场景设计的紧密耦合激光雷达-视觉-惯性框架，可实现鲁棒的定位功能。通过整合激光雷达辅助的深度恢复技术与统一的双模态观测模型，JONA-SLAM能够在无纹理或几何特征模糊的场景中维持状态估计的准确性。此外，我们设计的混合闭环机制融合了点云法线与视觉特征，可有效抑制错误匹配，从而保证全局一致性。
### III. 方法论
本文介绍的JONA-SLAM是一种紧密耦合的激光雷达-视觉-惯性SLAM系统。虽然前端采用了我们先前研究中的点线特征提取技术，但本研究通过引入用于深度特征级融合的激光雷达-视觉联合观测因子，以及用于保障全局一致性的法线感知型混合闭环机制，大幅扩展了该框架的功能。所提系统的整体流程如图1所示。
### A. 激光雷达辅助深度恢复与紧密耦合观测模型
为了实现视觉子系统与激光雷达子系统之间的深度融合，该系统构建了基于激光雷达辅助的视觉特征深度恢复以及紧耦合观测框架。该框架首先利用激光雷达点云提供的精确几何信息，为视觉特征提供度量尺度。随后，这些多模态观测数据被统一编码为“激光雷达-视觉联合观测因子”，并输入到滑动窗口优化器中，从而在状态估计层面实现紧耦合。
---
Fig. 1. JONA-SLAM framework diagram. The red and blue parts represent the novel contributions of this article.
对于视觉点特征，我们首先将激光雷达点云投影到图像平面上，生成稀疏深度图$D_{space}(u, v)$。接着，我们在$\mathbf{p}_f$中查找$D_{space}$的最近K个投影邻居，并计算它们深度的标准差$\sigma_z$。为了进一步确保度量尺度的精确度并过滤掉几何不确定性，我们引入了基于特征值分析的局部平面性准则。平面性因子$\eta$是通过邻域协方差矩阵的特征值$\lambda_1, \lambda_2, \lambda_3$来定义的。
$$ \eta>\frac{\lambda_{2}-\lambda_{3}}{\lambda_{1}}. $$
只有当$\eta > \tau_{planar}$和$\sigma_z < \tau_{flat}$满足特定条件时，中位数深度才会被取为$d_f$。这些阈值是基于通用的物理几何约束设定的：$\tau_{planar}$被设为0.8以确保局部邻域具有足够的平面性，而$\tau_{flat}$则被设为0.05米以严格限制几何波动。这些参数并非针对特定序列调整，而是在所有测试场景中保持恒定。由于它们源自基本的局部几何特性，因此能在各种户外驾驶环境中展现出强大的泛化能力，有效缓解物体边界处的深度误关联问题。否则，如果局部几何形状曲率较大，候选点会通过随机样本一致性（RANSAC）算法被反投影以拟合局部平面，随后选取符合该平面模型的点集的中位数深度。对于无法直接关联的视觉点特征，则基于成功关联的周围点，利用Delaunay三角剖分法进行深度插值。对于线特征$L_{img}$，我们首先沿线段均匀采样，并将其投影射线映射到激光雷达坐标系中以形成搜索区域，从而收集局部点云$\mathcal{P}_L$。随后，利用RANSAC算法拟合支撑平面。
$$ \prod:\mathbf{n}^{T}\mathbf{X}+d=0. $$
线段起始像素和结束像素的投影射线会与平面$\prod$相交，从而在激光雷达坐标系中得到三维端点$\mathbf{P}_s^L$和$\mathbf{P}_e^L$。最后，将这些端点转换到世界坐标系，便可得出三维线段$L_{3D}$。系统会检查线段上所有采样点的深度一致性，以过滤掉跨越深度不连续边界的异常线段。通过上述机制，我们便获得了具有精确度量尺度的三维视觉特征集合。
$$ \mathcal{F}_{L V}=\left\{\mathbf{P}_{i}^{3D},\mathbf{P}_{j}^{3D}\right\}. $$
为了将恢复的深度信息深度整合到状态估计中，我们构建了激光雷达-视觉联合观测因子。该因子并不将激光雷达衍生的深度视为固定值，而是将其作为带有不确定性的软约束，同时兼顾视觉重投影与激光雷达几何一致性。对于一个三维点特征，其在第k帧中的联合观测误差定义如下：
$$ \mathbf{e}_{L V,k,i}=\left[\begin{matrix}{\mathbf{e}_{k,i}^{v i s}}\\ {\mathbf{e}_{k,i}^{l i d a r}}\\ \end{matrix}\right]. $$
视觉重投影误差项确保了特征在图像平面上的投影结果与观测数据保持一致。
$$ \mathbf{e}_{k,i}^{v i s}=\mathbf{z}_{k,i}^{v i s}-\pi\left(\mathbf{T}_{W C,k}\cdot\mathbf{P}_{i}^{3D}\right) $$
其中，$\mathbf{z}_{k,i}^{vis}$ 表示像素观测值，$\pi(\cdot)$ 代表相机投影函数，而 $\mathbf{T}_{WC,k}$ 则是第 k 帧的相机位姿。为了确保特征的 3D 位置与局部点云的几何结构保持一致，这里采用点到平面距离来描述点特征。
$$ \mathbf{e}_{k,i}^{l i d a r}=\mathbf{n}^{T}\left(\mathbf{T}_{W L,k}\cdot\mathbf{P}_{i}^{3D}-\mathbf{q}\right) $$
其中，$\mathbf{n}$ 和 $\mathbf{q}$ 是根据特征点 $\mathbf{P}_i^{3D}$ 附近的激光雷达点云拟合得到的平面参数，$\mathbf{T}_{WL,k}$ 为第 k 帧的激光雷达位姿。对于线特征而言，误差项的构建方式类似，通过沿线段采样点来计算点到直线（或点到平面）的距离。该度量指标不依赖于端点的具体位置，因此即便由于视角变化和遮挡导致端点消失或移动，也能保证其鲁棒性。
状态估计更新量 $\delta\mathcal{X}$ 是通过高斯-牛顿法最小化联合能量函数得到的。为了揭示激光雷达几何信息与视觉流形之间的实质性耦合关系，我们显式推导出了联合雅可比矩阵 $J_{IO}$。
$$ \mathbf{J}_{L V}=\left[\begin{matrix}{\mathbf{J}_{v i s}}\\ {\mathbf{J}_{g e o}}\\ \end{matrix}\right]=\left[\begin{matrix}{\frac{\partial\mathbf{r}_{r e p r o j}}{\partial\delta\mathbf{\xi}}}\\ {\mathbf{n}^{T}\left[\begin{matrix}{-[\mathbf{R}_{k}\mathbf{P}_{w}+\mathbf{t}_{k}]_{\times}}&{\mathbf{n}^{T}}\\ \end{matrix}\right]}\\ \end{matrix}\right] $$
其中，$[.]\times$ 表示斜对称矩阵。这种表述形式使得激光雷达几何梯度 $\mathbf{J}_{vis}$ 能够直接用于优化相机位姿流形。这种融合机制确保了即使在纹理匮乏的环境中——此时视觉雅可比矩阵 $\mathbf{J}_{geo}$ 会出现秩亏损——系统仍具备可观测性。该联合因子的协方差矩阵 $\mathbf{\Sigma}_{LV}$ 是一个块对角矩阵，分别包含了视觉观测不确定性 $\mathbf{\Sigma}_{vis}$ 和激光雷达几何拟合不确定性 $\mathbf{\Sigma}_{geo}$。
---
```pseudo
// Algorithm 1: Enhanced Loop Closure Detection with Normal Verification
Input: Current frame features $F_t$, historical keyframes $K_{1:t-1}$, time threshold $T_{th}$, spatial threshold $D_{th}$, normal threshold $\Gamma_{th}$
Output: Loop closure constraints C, updated keyframe database
function DETECT LOOPS $(F_t, K_{1:t-1})$
```
$(P_t, n_t) \leftarrow EXTRACT\_LIDAR\_FEATURES(F_t)$
$V_t \leftarrow EXTRACT\_VISUAL\_FEATURES(F_t)$
$K_v \leftarrow SEARCH\_VISUAL\_KEYWORDS(V_t, K_{1:t-1})$
如果 $K_v \neq \phi$ 成立，则
if LK_TRACKING $(V_t, K_v)$ and $\left|t - t_{K_v}\right| > T_{th}$
and $\left\|\mathbf{p}_t - \mathbf{p}_{K_v}\right\| < D_{th}$ then
$\psi \leftarrow COMPUTE\_NORMAL\_SIMILARIT(n_t, n_{K_v})$
if $\psi > \Gamma_{th}$ then
$\mathbf{e}_{joint} \leftarrow CONSTRUCT\_JOINT\_FACTOR(V_t, P_t, K_v, n_t, n_{K_t})$
$\mathcal{C} \leftarrow \mathcal{C} \cup \{\mathbf{e}_{joint}\}$
end if
end if
end if
return $\mathcal{C}$
结束函数
### B. 关键帧选择与回环检测
当系统状态相对于前一关键帧的变化超过阈值且特征数量满足要求时，该帧便会被选为关键帧。我们会将激光雷达点云、视觉特征以及里程计位姿紧密关联并存储起来。为提高可靠性，本文提出了一种基于分层多模态融合的回环检测方法。关键在于，视觉候选结果并不会进行独立的并行搜索，而是直接触发基于激光雷达的几何交叉验证。通过结合激光雷达法向信息以及在统一时空约束下的结构相似度评分，系统得以强化几何约束。这种方法有效减少了单模态不匹配现象，并在复杂场景中提升了全局一致性。优化后的流程详见算法 1。
如算法 1 所示，视觉回环模块通过LK光流跟踪以及对候选图像匹配的时间一致性检查来进行双重验证。只有满足这些条件的关键帧才会被接受为有效的回环候选。关键在于，这些视觉候选结果并不会进行独立的并行搜索，而是直接触发基于激光雷达的几何交叉验证。具体而言，激光雷达的一致性通过加权相似度评分 $\psi$ 来量化，该评分同时考虑了局部点簇的方向对齐程度与结构分布特征。
$$ \begin{aligned}\boldsymbol{\psi}&=\alpha\cdot\exp\left(-\frac{\left(1-\mathbf{n}_{c u r r}^{\top}\mathbf{n}_{h i s t}\right)^{2}}{2\sigma_{n}^{2}}\right)\\&\quad+(1-\alpha)\cdot\frac{\operatorname{t r}\left(\mathbf{S}_{c u r r}\mathbf{S}_{h i s t}\right)}{\left\|\mathbf{S}_{c u r r}\right\|_{F}\left\|\mathbf{S}_{h i s t}\right\|_{F}}\end{aligned} $$
其中，$\mathbf{n}$ 是平均表面法向量，而 $\mathbf{S}$ 则表示用于表征局部几何拓扑结构的散射矩阵。$\alpha \in [0,1]$ 是一个平衡因子，用于调节法向量方向一致性与几何结构相似性之间的相对重要性。在我们的实验中，$\alpha$ 被设为 0.5，接受阈值则设为 $\Gamma_{th} = 0.85$。这些数值是根据静态场景的几何刚性确定的，旨在确保只有表面方向高度一致的环路候选者才会被接受。为了验证其鲁棒性，这些参数在所有评估中均保持不变，无需针对每个序列进行单独调整。由于这种筛选机制源自基本的几何属性而非数据集特有的特征，因此它具备强大的泛化能力，能有效防止误判的环路干扰因子图。通过引入这些统一的时空约束，双模态检测框架能够在感知模糊的环境中有效抑制误报，显著提升识别精度与全局一致性。
### C. 因子图优化
因子图优化通过利用稀疏性和增量平滑技术，在SLAM中实现高效的状态估计。该框架在保持实时计算性能的同时，确保了全局轨迹的一致性。在本文中，因子图由IMU预积分因子、激光雷达-视觉联合观测因子以及闭环约束因子构成。这些因子被整合到一个紧密耦合的优化框架中，最终得到优化的全局位姿估计结果以及全局一致的点云地图。
如前所述，IMU预积分因子是通过对两个连续关键帧之间的IMU测量值进行积分得到的。激光雷达-视觉联合观测因子则是通过将激光雷达提供的空间约束与视觉中的点线特征紧密结合而得出的，从而有效地在滑动窗口优化器中融合多模态观测数据。闭环因子则是通过将当前关键帧与潜在的闭环关键帧进行匹配而生成的。这些因子被纳入因子图中并进行优化，以得出全局位姿和点云地图。全局优化的目标函数定义如下：
$$ \left\{\begin{aligned}\underset{\mathcal{X}}{\arg\min}&\left\{\sum_{(i,j)\in I_{pre}}\left\|\mathbf{e}_{ij}\right\|_{\mathbf{\Sigma}_{pre}}^{2}+\sum_{(i,j)\in J_{oint}}\left\|\mathbf{e}_{ij}\right\|_{\mathbf{\Sigma}_{Joint}}^{2}\right.\\ &\left.+\sum_{(i,j)\in L_{oop}}\left\|\mathbf{e}_{ij}\right\|_{\mathbf{\Sigma}_{Loop}}^{2}\right\}\\ \mathbf{e}_{ij}&=\ln\left(T_{ij}\left(T_{i}^{w}\right)^{-1}T_{j}^{w}\right)\end{aligned}\right. $$
其中，$\mathcal{X}$ 表示所有待优化的状态变量集合，包括关键帧的位姿、速度以及IMU偏差。与传统上独立处理各传感器的方法不同，我们的联合观测因子 $(e_{ij}, (i, j) \in \text{Joint})$ 将视觉重投影误差与激光雷达几何残差统一为一个代价项。为了平衡尺度差异……
---
Fig. 2. Experimental equipment.
考虑到这些模态之间的噪声特性差异，残差会根据其协方差矩阵的逆进行加权。具体而言，联合协方差$\Sigma_{LV}$被设计为具有自适应性：视觉协方差$\Sigma_{vis}$基于重投影噪声而固定，而激光雷达的几何协方差$\Sigma_{geo}$则会根据第III-A节中得出的深度不确定性$\sigma_z$和平整度$\eta$动态调整。这种机制会对具有精确平面结构的特征赋予更高的置信度，同时能准确建模那些深度变化较大的特征所蕴含的不确定性。通过全局优化，便可得到更新后的位姿和惯性测量单元（IMU）偏差。随后，修正后的IMU偏差会被反馈给预积分模块，用于校准后续的测量数据，从而确保因子图的时间一致性。每处理n个关键帧后，系统会重置因子图优化器，以限制计算复杂度并维持实时性能。
### 四、评估
本节将对所提出的JONA-SLAM的性能进行评估，以验证其紧耦合框架的有效性。实验平台采用Apollo D-KIT lite，其中全球导航卫星系统（GNSS）为定位精度评估提供真实值。我们选用KITTI、M2DGR以及自行采集的校园数据集进行测试。KITTI序列00用于评估该系统在大规模居民区的长期漂移抑制能力，序列08则用于测试法向感知回环闭合模块在反向行驶等复杂条件下的鲁棒性。M2DGR数据集包含严重的光照变化、极低照度以及几何结构退化的走廊等特征，是评估JONA-SLAM在极端场景下性能的绝佳基准。此外，自行采集的校园数据集涵盖了一条长度为500米的环路，代表了行人频繁穿梭、有多处减速带的非结构化复杂环境。这些减速带会引发显著的平台抖动和点云畸变，从而能对激光雷达与视觉的联合优化效果，以及JOHL模块在剧烈运动下保持全局一致性的能力进行严格测试。设备详情如图2所示。
### A. 基于KITTI数据集的实验
为了评估JONA-SLAM的泛化能力，我们首先利用KITTI数据集将其与经典的视觉-激光雷达-惯性框架LVI-SAM进行对比测试。作为广受认可的基准数据集，KITTI能够有效模拟复杂的现实世界驾驶环境，用于评估轨迹均方根误差（RMSE）。为了进一步探究不同回环闭合策略对轨迹精度的影响，我们将JONA-SLAM的结果扩展为三种配置：1）无回环模式（仅里程计模式）；2）无法向约束模式（启用回环闭合但不进行法向一致性验证）；3）完整的JONA-SLAM系统。这种多阶段对比能够凸显出法向感知约束相较于基础传感器融合和标准回环闭合所带来的具体精度提升。对比结果详见表I。
TABLEI
TRAJECTORY ERROR COMPARISON (RMSE) (UNIT: m)
| Seq | LVI-SAM | JONA-SLAM |
| w/o | w/ | w/o | w/o\nNormal | w/ |
| 00 | 7.36 | 3.69 | 3.39 | 1.95 | 1.71 |
| 05 | 4.48 | 3.47 | 3.22 | 1.35 | 1.08 |
| 07 | 1.18 | 0.81 | 0.52 | 0.38 | 0.23 |
| 08 | 7.41 | 4.56 | 3.10 | 1.98 | 1.68 |
TABLE II
QUANTITATIVE EVALUATION: F1-SCORE COMPARISON, ABLATION STUDY, AND AVERAGE COMPUTATIONAL TIME (ms)
| Seq | 00 | 05 | 07 | 08 | Time |
| DBoW2 | 0.842 | 0.761 | 0.611 | 0.568 | 12.5 |
| ScanContext | 0.932 | 0.896 | 0.665 | 0.621 | 22.4 |
| ISC | 0.908 | 0.924 | 0.638 | 0.678 | 18.2 |
| V-only(DBoW2 + LK) | 0.875 | 0.875 | 0.875 | 0.875 | 15.2 |
| L-only(SC+ \Psi ) | 0.943 | 0.915 | 0.692 | 0.656 | 24.8 |
| Simple Fusion (Parallel) | 0.935 | 0.902 | 0.680 | 0.635 | 37.6 |
| JONA-SLAM (w/o Normal) | 0.951 | 0.938 | 0.725 | 0.668 | 16.2 |
| JONA-SLAM | 0.968 | 0.961 | 0.885 | 0.711 | 17.8 |
我们选取复杂的现实驾驶环境来评估轨迹均方根误差（RMSE）。为了进一步分析不同回环闭合策略对轨迹精度的影响，我们将JONA-SLAM的结果扩展为三种配置：1）无回环闭合（仅里程计模式）；2）无法向一致性校验的回环闭合；3）完整的JONA-SLAM算法。这种多阶段对比能够凸显出基于法向约束的精度提升效果，这种提升超越了基础的传感器融合与标准回环闭合机制。具体对比结果详见表I。
如表I所示，JONA-SLAM在几乎所有序列中都表现出卓越的性能，这主要归功于所提出的激光雷达-视觉联合因子，该因子能够在特征退化环境中保持高可观测性。三种配置下精度的逐步提升，凸显了我们回环闭合策略的决定性作用：虽然JONA-SLAM（无法线信息）变体通过消除长期漂移，使得RMSE相比无回环基线有显著下降，但完整的JONA-SLAM进一步优化了全局位姿。这一点在序列07和08中尤为明显——能够感知法线的机制充当了严格的“把关者”，防止误检的回环破坏后端因子图，从而确保更好的全局一致性并抑制轨迹畸变。此外，为了评估全局一致性，我们将这种能感知法线的混合回环闭合策略与DBoW2 [^25]、Scan Context [^26]以及ISC [^27]进行了比较，采用最大的$F_1$分数（精确度和召回率的调和平均值）作为综合指标，以反映检测精度与覆盖范围的平衡程度。定量评估还包括一项多阶段消融研究：具体而言，将JONA-SLAM与仅视觉（V-only）、仅激光雷达（L-only）、松散耦合的简单融合基线，以及JONA-SLAM（无法线信息）变体进行对比。详细的对比结果和消融指标汇总于表II中。
---
如表II所总结，JONA-SLAM取得了0.881的优异平均$F_{1}$分数，有效消除了轨迹漂移，并降低了表I中的全局RMSE。JONA-SLAM与无法线信息变体之间的性能差距（序列07中分别为0.885和0.725）直接凸显了几何拓扑验证的必要性。在序列07中，道路两侧布局呈重复性，类似于隧道或走廊，此时JONA-SLAM（0.885）的表现明显优于DBoW2（0.611）、Scan Context（0.665）以及无法线信息基线（0.725）。即便是在最具挑战性的、包含反向访问情况的序列08中，JONA-SLAM仍能保持0.711的高$F_{1}$分数，而DBoW2的分数则降至0.568。对比结果表明，我们设计的能感知法线的评分机制起到了严格的“把关者”作用，它利用激光雷达的几何表面一致性来消除因重复结构中的基于外观的感知歧义所导致的误检。此外，JONA-SLAM在大规模场景（如序列00）中也能保证实时可行性，平均延迟仅为17.8毫秒，几乎是简单融合算法37.6毫秒的两倍。值得注意的是，加入法线一致性检查后带来的计算开销微乎其微，与无法线信息版本（16.2毫秒）相比，延迟仅增加1.6毫秒。这种高效性源于我们的视觉触发逻辑——只有在识别出潜在的视觉候选对象时，才会进行基于激光雷达的验证，从而避免对密集点云进行冗余的全局搜索。
### B. 在M2DGR数据集上的实验
为了进一步评估JONA-SLAM在复杂环境中的鲁棒性和泛化能力，我们在M2DGR数据集上开展了对比实验。与KITTI数据集平滑的场景不同，M2DGR具有剧烈的运动、高动态变化以及完全黑暗的环境。我们的方法分别与最先进的（SOTA）框架进行对比，包括R3LIVE++（一种基于辐射图的直接法）和DAR-inertial-visual odometry 2（FAST-LIVO2）[^28]（一种基于迭代扩展卡尔曼滤波（IEKF）的高效融合基准算法）。通过用于里程计和SLAM评估的Python工具包EVO生成的轨迹可视化对比结果如图3所示，以便直观地评估性能。
如图1中的轨迹图及放大细节所示，JONA-SLAM在面对M2DGR数据集的极端挑战时表现出卓越的韧性。在door_01/02序列中，室内外突然的光照变化会导致视觉特征严重退化；虽然LVI-SAM和FAST-LIVO2在过渡点处会出现明显的轨迹抖动，但JONA-SLAM通过结合激光雷达获取的几何约束来强化视觉数据，从而保持了轨迹的连续性。在光线昏暗的gate_02环路中，基于外观的方法会因纹理稀缺而失效，而我们基于法向量的匹配策略则能确保精确的环路对齐，消除了LVI-SAM中出现的重影和错位现象。尽管R3LIVE++在稳定的室外环境中表现优异，但JONA-SLAM在高动态运动及剧烈光照变化场景下展现出更优的全局一致性。定量绝对位姿误差（APE）指标汇总于表III中。
(a)
(b)
Fig. 3. Trajectory comparisons. (a) door_01 and door_02 and (b) gate_02.
TABLE III
RMSE OF APE IN M2DGR DATASETS IN METERS
| Sequence | LVI-SAM | FAST-LIVO2 | R3LIVE++ | JONA-SLAM |
| door_01 | 0.265 | 0.215 | 0.201 | 0.178 |
| door_02 | 0.198 | 0.165 | 0.158 | 0.141 |
| gate_01 | 0.152 | 0.132 | 0.125 | 0.106 |
| gate_02 | 0.357 | 0.332 | 0.315 | 0.292 |
| gate_03 | 0.154 | 0.102 | 0.098 | 0.083 |
| hall_01 | 0.252 | 0.218 | 0.208 | 0.198 |
| hall_02 | 0.282 | 0.258 | 0.241 | 0.204 |
| hall_03 | 0.351 | 0.282 | 0.245 | 0.196 |
| hall_04 | 0.855 | 0.812 | 0.792 | 0.725 |
| hall_05 | 1.113 | 0.942 | 0.842 | 0.754 |
| room_01 | 0.141 | 0.118 | 0.102 | 0.079 |
| room_02 | 0.132 | 0.098 | 0.102 | 0.113 |
| room_03 | 0.162 | 0.148 | 0.134 | 0.124 |
| street_02 | 3.46 | 3.12 | 2.65 | 3.112 |
| street_03 | 0.138 | 0.122 | 0.112 | 0.099 |
| street_04 | 0.932 | 0.415 | 0.285 | 0.315 |
| street_05 | 0.345 | 0.305 | 0.295 | 0.298 |
| street_06 | 0.384 | 0.352 | 0.355 | 0.342 |
| Mean | 0.531 | 0.452 | 0.406 | 0.398 |
表III中的定量指标显示，JONA-SLAM的平均均方根误差（RMSE）达到了0.398米，凸显了其出色的适应性。在像gate系列这样环路较多的序列中
---
(a)
(b)
(c)
(d)
Fig. 4. JONA-SLAM. APE error maps of JONA-SLAM. (a) door_01, (b) roomdark_02, (c) gate_04, and (d) hall_05.
以及在hall_03序列中，我们的方法凭借稳定的环境法向量结构优于其他算法。即便由于光照变化导致视觉纹理失效，基于法向量约束的闭环检测仍能确保精确对齐，消除累积漂移现象。在hall_04和hall_05这类高运动场景中，运动模糊会干扰直接法和基于特征的方法，而我们采用激光雷达增强的方案所取得的结果优于R3LIVE++。虽然R3LIVE++在street_02等开阔场景中仍保持极高的精度，但JONA-SLAM对动态干扰和视觉退化的抵御能力更强。为了验证法向量约束的有效性，我们选取了四个极具挑战性的序列：hall_05（几何退化）、roomdark_02（视觉失效）、door_01（光照突变）以及street_04（大规模环路）。这项多维度的消融研究证实了法向量对系统整体鲁棒性的重要影响。APE误差图如图4所示。
如图4所示，尽管存在长距离机动和复杂的环境变换，JONA-SLAM在四个具有代表性的退化序列中仍保持了较高的空间一致性。误差分布较为均匀，绝大多数轨迹都位于代表低误差程度的深蓝色区域内。尤其是在hall_05的直线加速阶段以及roomdark_02的急转弯过程中，轨迹并未出现明显的红色或黄色误差峰值。这从定性角度证实了该系统即使在视觉纹理严重丢失或几何退化的条件下，仍能维持稳健的位姿估计能力。引入法向量约束前后精度的详细定量提升情况汇总于表IV中。
TABLE IV
ABLATION ANALYSIS ON NORMAL CONSTRAINTS
| Sequence | Features | JONA-SLAM(w/o Normal) | JONA-SLAM |
| door_01 | indoor/outdoor | 0.211 | 0.178 |
| roomdark_02 | indoor/dark | 0.208 | 0.158 |
| street_04 | dark/look back | 0.352 | 0.315 |
| hall_05 | Random walk | 0.938 | 0.754 |
TABLE V
APE EVALUATION IN REAL-WORLD SCENARIOS IN METERS
| Error | LVI-SAM | FAST-LIVO2 | R3LIVE++ | JONA-SLAM |
| Max | 5.673 | 4.892 | 4.281 | 4.309 |
| Mean | 2.823 | 2.541 | 2.215 | 2.275 |
| Med | 3.781 | 3.120 | 2.248 | 2.357 |
| Min | 0.296 | 0.185 | 0.082 | 0.104 |
| Rmse | 4.027 | 3.425 | 2.418 | 2.404 |
根据表四中的消融实验数据，集成法向约束后，JONA-SLAM在所有四种具有代表性的退化序列上的ATE RMSE均显著降低。结合APE三维误差图来看，其在hall_05序列中的性能提升尤为显著——该序列为一条长走廊环境，存在明显的几何退化现象。在此场景下，误差从0.938米降至0.754米。这种24.23%的精度提升有力证明：法向约束为优化器提供了明确的方向先验，能够有效抑制仅依赖激光雷达的系统在特征稀疏环境中常见的轴向滑动现象。在roomdark_02和door_01等存在视觉失效或光照骤变现象的序列中，误差的减小反映出法向信息作为高阶几何先验的鲁棒性。当环境退化导致视觉残差无法提供可靠约束时，法向约束能确保因子图优化维持正确的空间拓扑维度，从而避免轨迹漂移。此外，在street_04序列中，这些约束通过提升全局环境结构的描述精度，增强了长距离下的位姿稳定性。
为了进一步评估JONA-SLAM的泛化能力，我们基于自行采集的数据集开展了相关测试。借助EVO工具输出了不同算法对应的APE与相对位姿误差（RPE）数值，具体误差值如表五所示。
表五汇总了在自行采集数据集上的定量评估结果，结果显示JONA-SLAM所达到的高精度性能可与最先进的R3LIVE++框架相媲美。虽然R3LIVE++在最小误差和平均误差等局部指标上略占优势，但JONA-SLAM的APE RMSE仅为2.404米，表现更为优异。这一改进表明：相较于现实校园环境中常见的突发干扰与结构不规则情况，我们的方法具备更强的全局鲁棒性。与LVI-SAM、FAST-LIVO2等基线框架相比，JONA-SLAM实现了……
---
相对于LVI-SAM而言，其精度提升了40.30%，这一数据凸显了所提出的法向感知几何约束在复杂机动场景下抑制位姿漂移方面的实用价值。
### 五、结论与未来工作
本文提出了JONA-SLAM，这是一种紧密耦合的激光雷达-视觉-惯性SLAM系统，旨在提升复杂且GNSS信号受限环境下的定位精度与鲁棒性。该系统将研究重点从基础特征提取转向多传感器深度融合，引入了能够高效融合激光雷达提供的空间约束与视觉点线特征的联合观测因子。此外，还开发了一种基于法向感知的混合回环闭合机制，利用几何一致性显著抑制错误匹配，从而确保全局轨迹的精确度。通过在KITTI和M2DGR数据集的22个公开序列以及自行采集的校园数据集上进行的定量评估表明，JONA-SLAM能够实现高精度的状态估计。在真实的校园场景中，JONA-SLAM的RMSE达到2.40米，优于LVI-SAM的4.03米和R3LIVE++的2.42米。总体而言，在23个实验测试案例中，该方法相较于LVI-SAM框架可使平均RMSE降低35.02%。这些结果均保留至小数点后两位，为系统性能提供了客观且经过统计验证的证据。在真实校园场景下，该系统在突发干扰和结构不规则条件下表现出卓越的稳定性，为自主导航奠定了可靠的定位基础。未来的工作将侧重于优化计算效率，并探索更先进的特征增强技术，以进一步提升系统在感知严重退化条件下的适应性。
## 参考文献
[^1]: T. Ran, L. Yuan, J. Zhang, D. Tang, and L. He, “RS-SLAM: A robust semantic SLAM in dynamic environments based on RGB-D sensor,” IEEE Sensors J., vol. 21, no. 18, pp. 20657–20664, Sep. 2021.
[^2]: Z. Lin, Q. Zhang, Z. Tian, P. Yu, and J. Lan, "DPL-SLAM: Enhancing dynamic point-line SLAM through dense semantic methods," IEEE Sensors J., vol. 24, no. 9, pp. 14596–14607, May 2024.
[^3]: B. Zhou, C. Li, S. Chen, D. Xie, M. Yu, and Q. Li, “ASL-SLAM: A LiDAR SLAM with activity semantics-based loop closure,” IEEE Sensors J., vol. 23, no. 12, pp. 13499–13510, Jun. 2023.
[^4]: J. Zhao, X. Zhang, K. He, Z. Qi, and Y. Yang, “Tightly coupled visual-inertial SLAM with dual-modal confidence perception,” IEEE Sensors J., vol. 25, no. 23, pp. 43138–43149, Dec. 2025.
[^5]: J. Fu, H. Zhu, K. Zhang, T. Ma, J. Liu, and Ye Li, “Online exploratory coverage path planning of incremental SLAM for autonomous vehicles,” IEEE Trans. Ind. Informat., vol. 22, no. 3, pp. 1861–1870, Mar. 2025, doi: 10.1109/TII.2025.3632027.
[^6]: Z. Ying and H. Li, "IMM-SLAMMOT: Tightly-coupled SLAM and IMM-based multi-object tracking," IEEE Trans. Intell. Vehicles, vol. 9, no. 2, pp. 3964–3974, Feb. 2024.
[^7]: J. Zheng, K. Zhou, and J. Li, “Visual-inertial-wheel SLAM with high-accuracy localization measurement for wheeled robots on complex terrain,” Measurement, vol. 243, Feb. 2025, Art. no. 116356.
[^8]: K. Xu, Y. Hao, S. Yuan, C. Wang, and L. Xie, “AirSLAM: An efficient and illumination-robust point-line visual SLAM system,” IEEE Trans. Robot., vol. 41, pp. 1673–1692, 2025.
[^9]: Q. Jian, Q. Ma, X. Wang, and S. Ullah, "A high-precision and fast autonomous vehicle SLAM based on multiscale adaptive feature extraction," IEEE Sensors J., vol. 26, no. 2, pp. 3030–3044, Jan. 2026.
[^10]: H. Liu, S. Pan, X. Wang, W. Gao, B. Yu, and H. Zhao, “Long-term localization method integrated with voxel mapping LiDAR odometry and adaptive updating map in diverse environment,” IEEE Trans. Instrum. Meas., vol. 74, pp. 1–14, 2025.
[^11]: S. Song, Q. Zhu, and H. Hu, “RTS-SLAM: Robust, tightly coupled, and semantic-enhanced LiDAR-inertial SLAM,” IEEE Sensors J., vol. 25, no. 20, pp. 38613–38626, Oct. 2025.
[^12]: Y. Song, Z. Zhang, J. Wu, Y. Wang, L. Zhao, and S. Huang, "A right invariant extended Kalman filter for object based SLAM," IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 1316–1323, Apr. 2022.
[^13]: M. Tang, Z. Chen, and F. Yin, “Robot tracking in SLAM with Masreliez-Martin unscented Kalman filter,” Int. J. Control, Autom. Syst., vol. 18, no. 9, pp. 2315–2325, Sep. 2020.
[^14]: 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.
[^15]: 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.
[^16]: T. Du, D. Shan, and P. Guo, "PFLIO-SAM: Tightly coupled polarization camera/optical flow/LiDAR/IMU odometry via smoothing and mapping," IEEE Sensors J., vol. 25, no. 19, pp. 36243–36253, Oct. 2025.
[^17]: C. Zhang et al., "LIWOM-GD: Enhanced LiDAR-inertial-wheel odometry and mapping by fusion with ground constraint and dynamic points elimination," IEEE Sensors J., vol. 24, no. 19, pp. 30287–30303, Oct. 2024.
[^18]: D. C. Herraez, M. Zeller, D. Wang, J. Behley, M. Heidingsfeld, and C. Stachniss, "RaI-SLAM: Radar-inertial SLAM for autonomous vehicles," IEEE Robot. Autom. Lett., vol. 10, no. 6, pp. 5257–5264, Jun. 2025.
[^19]: J. Lee et al., “Switch-SLAM: Switching-based LiDAR-inertial-visual SLAM for degenerate environments,” IEEE Robot. Autom. Lett., vol. 9, no. 8, pp. 7270–7277, Aug. 2024.
[^20]: 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.
[^21]: J. Lin and F. Zhang, "R3LIVE++: A robust, real-time, radiance reconstruction package with a tightly-coupled LiDAR-inertial-visual state estimator," IEEE Trans. Pattern Anal. Mach. Intell., vol. 46, no. 12, pp. 11168–11185, Dec. 2024.
[^22]: J. Yin, D. Luo, F. Yan, and Y. Zhuang, “A novel LiDAR-assisted monocular visual SLAM framework for mobile robots in outdoor environments,” IEEE Trans. Instrum. Meas., vol. 71, pp. 1–11, 2022.
[^23]: J. Huang, S. Wen, W. Liang, and W. Guan, "VWR-SLAM: Tightly coupled SLAM system based on visible light positioning landmark, wheel odometer, and RGB-D camera," IEEE Trans. Instrum. Meas., vol. 72, pp. 1–12, 2023.
[^24]: C.-C. Chou and C.-F. Chou, “Efficient and accurate tightly-coupled visual-LiDAR SLAM,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 9, pp. 14509–14523, Sep. 2022.
[^25]: J. Chen, P. Hou, Z. Cao, T. Xu, and J. Zhao, “An improved monocular SLAM system by combining geometric features with semantic probability,” IEEE Sensors J., vol. 25, no. 14, pp. 27111–27125, Jul. 2025.
[^26]: G. Kim and A. Kim, "Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Madrid, Spain, Oct. 2018, pp. 4802–4809.
[^27]: K. Fischer, M. Simon, S. Milz, and P. Mader, “MagneticPillars++: Efficient LiDAR odometry via deep frame-to-keyframe point cloud registration,” Appl. Artif. Intell., vol. 39, no. 1, Dec. 2025, doi: 10.1080/08839514.2025.2472105.
## [^28]: C. Zheng et al., “FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Robot., pp. 326–346, 2024
---
乔伟健于2022年在中国重庆的重庆工商大学获得学士学位。他目前正于重庆交通大学攻读硕士学位。
他的研究方向包括智能交通。
王欣雨于2021年在中国重庆的重庆交通大学获得学士学位，目前她正在该校攻读博士学位。
她当前的研究兴趣包括智能交通与安全，以及自动驾驶技术。
马庆禄于2012年在中国重庆的重庆大学获得计算机技术与科学博士学位。
他目前是重庆交通大学的一名教授。他当前的研究兴趣包括智能交通与安全，以及自动驾驶技术。
萨利姆·乌拉于2003年在巴基斯坦巴哈瓦尔布尔的伊斯兰大学获得计算机科学学士学位，并于2005年在巴基斯坦木尔坦的巴哈丁·扎卡里亚大学获得计算机科学硕士学位。
2006年至2009年间，他在多家公司担任网络/IT管理员。目前，他自2016年2月起在巴基斯坦拉希姆·亚尔汗的卡瓦贾·法里德工程与信息技术大学担任副教授。
他在IT领域拥有近14年的行业经验。他是adhoc网络、物联网（IoT）、拥塞控制及安全领域的活跃研究者。