LPVIMO-SAM框架：紧密耦合 LiDAR 偏振视觉等传感器，于复杂环境实现高精度定位与地图构建 ！

点击下方卡片，关注「AI视界引擎」公众号

( 添加时备注：方向+学校/公司+昵称/姓名 )

作者提出了一种通过平滑与映射紧密耦合的LiDAR/偏振视觉/惯性/磁力计/光流里程计（LPVIMO-SAM）框架，该框架将LiDAR、偏振视觉、惯性测量单元、磁力计和光流集成在紧密耦合的融合中。

该框架能够在具有挑战性的环境中实现高精度和高鲁棒性的实时状态估计和地图构建，例如LiDAR退化、低纹理区域和特征Sparse区域。

LPVIMO-SAM包含两个子系统：偏振视觉-惯性系统和LiDAR/惯性/磁力计/光流系统。

偏振视觉通过提取场景的偏振信息增强了低特征和低纹理场景中视觉/惯性里程计的鲁棒性。磁力计获取航向角，光流获取速度和高度以减少累积误差。设计了一个磁力计航向先验因子、一个光流速度观测因子和一个高度观测因子，通过因子图优化消除LiDAR/惯性里程计的累积误差。

同时，即使其中一个子系统失效，LPVIMO-SAM也能保持稳定的定位，进一步扩展了其在LiDAR退化、低纹理和低特征环境中的应用。 代码可在https://github.com/junxiaofanchen/LPVIMO-SAM获取。

同步定位与地图构建（SLAM）是一种在未知环境中同时进行定位和地图重建的方法。在过去的二十年中，人们已投入大量努力，使用单一传感器如激光雷达[1]-[2]或相机[3]在未知环境中实现SLAM。基于激光雷达的方法能够从远距离捕获详细的环境信息。然而，当环境结构信息Sparse或缺失时，这些方法可能无法正常工作。基于视觉的方法适用于位置识别，但在低纹理环境中，其精度往往会显著下降。为此，多传感器融合技术日益受到关注[4]-[5]。利用优化方法使多传感器在传感器退化环境中整合以实现精确的姿态估计。

在现有的SLAM框架中，最广泛使用的是相机、激光雷达和惯性测量单元。这项工作得到中国国家重点研发计划（项目编号2023YFB4704404）和国家自然科学基金（项目编号62388101）的部分支持。

。 Shan、P. Guo和D. Tao是北京北方工业大学信息学院的研究行人。2w. Li是北京航空航天大学杭州创新学院的研究行人。

惯性测量单元（IMU）。近年来，提出了多种激光雷达-视觉-惯性里程计（LIvO）系统，以实现高度鲁棒和精确的状态估计，例如R2LIVE [6] 和 LVI-SAM [7]。这些LIvO系统通常由一个激光雷达/惯性里程计（LIO）子系统和一个视觉/惯性里程计（VIO）子系统组成，这些子系统首先分别处理每种类型的数据，然后联合融合状态估计。然而，这些子系统无法解决诸如LIO系统在非结构化场景中的Z轴漂移问题 [8]、VIO系统在低纹理场景中姿态估计精度低的问题 [9]等。为了解决这些问题，作者通过平滑和映射引入了一种紧密耦合的激光雷达/偏振视觉/惯性/磁力计/光流里程计（LPVIMO-SAM）。

通过多种传感器的互补，所yinqing-LPVIMO-SAM_2504可以实现更鲁棒和精确的姿态估计。本文的贡献如下：

1. 为克服低纹理和LiDAR退化环境中的导航与定位挑战，提出了一种基于因子图的紧耦合LPVIMO-SAM框架。该系统集成了LiDAR、偏振视觉、IMU、磁力计和光流测距模块。
2. 针对在LiDAR受损或非结构化环境中LIO子系统产生的Z轴漂移和航向误差累积问题，引入了一种光流测距模块，用于构建高度因子和速度因子，从而减少LIO系统的Z轴漂移。此外，使用磁力计传感器提供初始航向，并作为先验因子以减轻航向误差的累积。
3. 针对低纹理场景下VIO子系统特征提取和匹配失败等问题，将偏振相机引入紧密耦合系统。利用偏振相机获取的偏振度（DOP）和偏振角（AOP）图像辅助灰度图像的特征提取，增强VIO子系统在低纹理场景下的特征提取能力。随后，将增强后的VIO子系统与LIO子系统进行集成。

作者将与本文提出的系统及所涉及技术相关的工作分为以下两部分：

A. 激光雷达-视觉-惯性SLAM

将多个传感器融合到LVIS SLAM中，使系统能够在具有挑战性的环境中实现鲁棒定位，尤其是在某个传感器失效或其性能下降时。近年来，研究界涌现出多种LiDAR-视觉-惯性SLAM系统。根据LIO系统和VIO系统在状态估计层和原始测量层是否融合，LIVO系统可分为松耦合和紧耦合两类。

VIL-SLAM 引入了一种在状态估计层面松散耦合的激光雷达-视觉-惯性系统（LVIS）。LIO 子系统仅使用 VIO 子系统提供的初始位姿，激光雷达、相机和 IMU 的测量值没有进行联合优化 [10]。与此同时，一些系统 [11]-[12] 使用 3D 激光雷达点云为视觉模块提供深度测量 [3]-[4]。由于这些系统在状态估计中不直接从激光雷达测量中获取约束，它们仍然保持松散耦合。基于此，Cheng 等人引入了关键帧和滑动窗口算法以及基于迭代最近点（ICP）的后端回环检测算法，以减少计算负载和大规模漂移。与 ORB-SLAM3 相比，均方根误差（RMSE）降低了约 70% [13]。为提高精度和鲁棒性，传感器数据的紧密耦合联合优化方法不断更新和完善。基于 MSCKF 框架 [14]，LIC-Fusion 紧密集成 IMU 数据、Sparse视觉特征以及激光雷达的平面和边缘特征。在室外场景中，其绝对轨迹误差比 MSCKF 方法降低了约 62% [15]。

近年来，越来越多的系统在测量与状态估计方面实现了完全紧密耦合。LVI-SAM基于因子图框架紧密耦合激光雷达、视觉和惯性传感器，其VIO子系统利用激光雷达扫描来跟踪视觉特征并提取特征深度，与LIO-SAM系统相比，RMSE降低了约81%[7]。FAST-LIVO提出了一种快速、Sparse直接、图像到地图的LVI融合框架，通过误差状态迭代卡尔曼滤波实现多传感器融合，与R2LIVE相比，RMSE降低了约58%[16]。随后，FAST-LIVO2被提出用于图像块 Level 的视觉跟踪，并降低了残差构建和渲染的成本。

与FAST-LIVO系统相比，其RMSE降低了约50%[17]。LVIO-Fusion的VIO子系统通过在线光度标定获取真实辐射信息，捕捉微小的几何和辐射变化。这有效地克服了无几何和无纹理环境中的传感器退化问题，增强了系统的鲁棒性。与FAST-LIVO相比，定位精度提高了约47.8%[18]。然而，现有方法在低纹理和激光雷达退化场景下的研究仍然有限。LIVO系统仍然面临LIO子系统扫描匹配失败和VIO子系统特征提取与匹配失败等问题。这些问题仍需进一步研究和解决方案。

B. 极化集成导航系统

许多动物，如沙漠蚂蚁和螳螂虾，能够感知偏振光，这有助于它们通过偏振视觉实现精确的导航和定向[19]-[20]。近年来，仿生偏振光传感器被广泛应用于导航领域，因为它们能够从偏振天空光或场景中获取精确的航向角[21]。

从极化信息来源来看，可分为天空极化与场景极化。大多数现有集成导航方法依赖于充分利用天空极化信息。提出了一种仿生极化姿态和航向参考系统，以提高惯性传感器的姿态精度，俯仰和滚转的标准差降低至

，航向为

[22]。在[23]中开发了一种仿生极化光传感器/惯性导航系统/轮速里程计集成导航系统，并采用边缘化无迹卡尔曼滤波器将六边形轨迹中的RMSE降低至约

。引入因子图优化方法以获得惯性导航系统/LiDAR/极化相机集成导航系统的最优解，以提高位姿估计精度[24]。为消除极化/惯性/GNSS集成导航系统因建模不准确和未知参数产生的多源干扰，设计了一种抗异常值扩展卡尔曼滤波器，与GNSS/惯性导航系统集成导航系统相比，精度提高了约50%[25]。开发了一种紧密耦合的极化视觉/惯性定位系统，用于航向确定和定位[26]。随后，极化天空光传感器在水下和月光环境中成功应用[27]-[29]。

场景极化也被用于目标检测[30]、密集重建[31]以及提升ORB-SLAM3的性能[32]。然而，场景极化很少用于激光雷达/视觉/惯性融合。因此，作者使用偏振度（DoP）图像和偏振角（AoP）图像进行特征提取。场景极化可用于低纹理和低特征场景，并增强VIO系统的鲁棒性。因此，获得了一个高度鲁棒的激光雷达/IMU/极化视觉/磁力计/光流系统。

A. 系统概述

如图1所示，LiDAR/Polarization Vision/Inertial/Magnetometer/Optical Flow系统（LPVIMOS）由一个LiDAR、一个偏振相机、一个IMU、一个磁力计传感器和一个光学流测距模块组成，它由两个子系统构成：一个Polarization Vision/Inertial

系统（PVIS）和激光雷达/惯性/磁力计/光流系统（LIMOS）。PVIS处理偏振RGB图像和IMU测量数据，并利用激光雷达数据获取视觉特征的深度。关于视觉/惯性系统（VIS）的具体建立过程和原理，请参考[4]。LIMOS提取激光雷达特征，并通过将提取的特征与特征地图进行匹配来获取激光雷达里程计。作者使用滑动窗口来维护特征地图，以确保实时性能。最后，作者使用iSAM2 [33]在因子图中联合优化各种传感器的贡献。

这些贡献包括IMU预积分约束、偏振视觉/惯性里程计约束、磁力计航向约束、激光雷达里程计约束、光流里程计模块提供的速度和高度约束，以及回环闭合约束。状态估计被表述为最大后验估计。请注意，在本文中，偏振相机提供的RGB图像用于提高VIS的鲁棒性，而光流和磁力计传感器用于提高集成导航系统的定位精度和LIS的鲁棒性。

B. 偏振RGB图像的计算

灰度图像、DOP图像和AOP图像分别通过以下公式计算：使用偏振相机传感器（索尼IMX250MZR CMOS）采集原始偏振图像，大小为

，作为单通道灰度图像，如图2所示。频率为

。四个像素构成一个单元，偏振图像可以在四个不同方向

同时捕捉。

1. 灰度图像对于原始图像的每个单元，计算一个新的像素。该像素的灰度值

可以按如下方式获得：

、

、

和

分别表示光通过朝向

、

、

和

的偏振器时产生的灰度值。这些灰度图像的分辨率为

像素。

1. DOP图像对于原始图像中的每个单元，计算一个新的像素，其中该像素的偏振度由公式（2）计算。

和

分别表示场景光波通过朝向

、

、

和

的偏振器后的强度。DOP值d非线性映射到0到255之间的整数，以增强DOP图像的对比度。

在偏振度映射后，每个像素的灰度值被替换为

值，从而生成DOP图像。

3. AOP图像

---

对于原始图像中的每个单元，计算一个新的像素，其中该像素的偏振角由公式(4)-(5)计算。然后，AOP

线性映射到一个介于0和255之间的整数。

经过偏振角映射后，每个像素的灰度值被替换为

值，从而生成AOP图像。

计算单幅原始图像的灰度图像、DOP图像和AOP图像后，将这三个单通道图像合成为RGB图像。在该图像中，红色通道表示偏振度图像，绿色通道表示灰度图像，蓝色通道表示偏振角图像。

C. 偏振视觉/惯性系统

LPVIMO-SAM系统受LVI-SAM系统框架启发。作者引入了偏振RGB图像，并利用DOP和AOP图像来增强灰度图像的特征。在低纹理和低特征的复杂场景中，普通VIS系统可能丢失特征或无法匹配，而所提出的PVIS利用场景偏振来补充特征信息。这使得VIS系统能够在复杂环境中稳定高效地提取和跟踪特征。由于空间限制，VIS的设计遵循文献[7]中的方法。

激光雷达/惯性导航/磁力计/光学流系统

本文提出的LPVIMO-SAM框架中的LIS系统如图3所示。基于LVI-SAM中的LIS系统，引入了磁力计航向因子来修正航向误差，同时利用光学流测距模块推导出的速度因子和高度因子来约束系统的速度和Z轴误差。与PFLIO-SAM[34]中的因子图相比，本文中的因子图用磁力计航向因子约束替换了偏振航向因子约束，并额外添加了偏振视觉惯性里程计因子约束。

1. IMU预积分因子和LiDAR里程计因子：LIMOS系统基于LVISAM中的LIS系统构建。因此，这些因子的设计以及系统的设计可参考[5]。
2. 磁力计航向因子：引入磁力计以获取车辆在世界坐标系中的绝对航向，从而使得导航与定位系统的航向角能够直接与世界坐标系对齐。随后，计算磁力计航向角与系统优化航向角之间的差异，并设置阈值以过滤掉磁力计航向角的无效测量值。最后，将磁力计航向角作为航向观测因子，并将其插入因子图中。
3. 光流的速度因子和高度因子：由于空间限制，本部分的设计可以参考PFLIO-SAM论文[34]中的因子设计。

针对所提出的框架进行了一系列实验，以获取五个自采集数据集，分别命名为场景a至e。数据采集的传感器套件包括Velodyne VLP16激光雷达、IMX250MZR CMOS相机、WTGAHRS1惯性测量单元、光流测距模块以及JY-RM3100磁力计。轨迹真值由RTK提供（平面误差：

，高程误差：

）。作者将所提出的框架与几种开源解决方案进行了比较，包括LIO-SAM、FASTLIO2、LVI-SAM和FAST-LIVO。

五个数据集分别采集于中国理工大学博远楼下的操场、小型篮球场、排球场和大型篮球场。它们各自的特点如下：

1. 场景a：拥有丰富之字形轨迹的小篮球场。2) 场景b：拥有大量动态行人的小篮球场。3) 场景c：大型篮球场，环境存在LiDAR退化现象且特征Sparse。4) 场景d：小篮球场与排球场组合场景包含更多动态行人。5) 场景e：游乐场场景，环境存在LiDAR退化现象且特征Sparse。

表1 不同算法的轨迹精度（RMSE/m）比较

A. 激光雷达惯性系统比较

使用五个不同长度的场景数据集来验证所提出方法的有效性。在场景a中，引入了更多的锯齿形轨迹。在这种情况下，LIO-SAM和FAST-LIO2系统均表现出良好的定位性能。然而，在磁力计航向、光流模块提供的速度和高度约束下，所提出的LIOMOSAM相比LIO-SAM将轨迹精度提高了约65.7%。

在场景b、c和d中，轨迹长度被增加。由于误差累积，LIO-SAM系统在Z轴上出现了严重的漂移问题。随着LIO-SAM系统轨迹长度的增加，其RMSE持续上升。相比之下，FAST-LIO2的轨迹定位精度保持相对稳定。所提出的LIOMO-SAM通过使用光流来约束Z轴误差和速度误差，因此其平均轨迹定位精度比LIO-SAM高约62.9%。

在场景e中，受到LiDAR退化环境的影响，LIO-SAM系统的LiDAR扫描匹配失败，导致载体的定位要么持续停留在原地，要么出现严重漂移。LIOMO-SAM通过添加约束修正了整个系统，使系统能够在一段时间内有效运行。然而，随着误差的持续累积，LiDAR里程计失败的影響变得更为显著，最终LIOMO-SAM系统也出现了显著漂移，未能完成轨迹定位。相比之下，FAST-LIO2仍然能够实现定位，但RMSE值相对较大。

B. LiDAR/Inertial/Visual融合系统比较

为进一步提升系统的可靠性，将视觉惯性系统VIS引入到激光惯性系统LIS中。这一改进为LIS提供了更多约束条件，使得整个系统能够在激光雷达扫描匹配失败时仍能稳定运行并实现精确定位。针对不同的应用场景，当引入结合了VIS和LIS的LVI-SAM系统时，其在各场景中的定位精度均优于LIOSAM系统。显然，在场景e中LIOSAM系统失效的情况下，LVI-SAM系统仍能保持稳定运行并持续输出可靠的定位信息。从这些对比结果可以看出，LVI-SAM系统在稳定性和可靠性方面优于LIOSAM系统。由于FAST-LIVO系统采用的算法提供极化RGB图像，该系统在某些场景中会出现定位漂移。然而，在场景c中，FAST-LIVO系统能够稳定运行，其定位精度略高于LVI-SAM系统，展现了其优势。请注意，后续所有提及的LPVIMO-SAM均指在严苛条件下的结果。严苛条件表示角点选择质量水平为0.9-0.95。

图4展示了不同场景下的轨迹。场景a和b比较了各种算法在锯齿形轨迹下的鲁棒性。结果表明，所提出的LPVIMOSAM方法在这些场景中表现出优异的稳定性。场景c基于场景b扩展了路径长度，所有算法均保持了良好性能。尽管场景c和d的路径长度相似，但场景d的轨迹更为复杂，导致FAST-LIVO系统在后续操作中发生漂移。LVI-SAM系统虽然能够在恶劣条件下完全运行，但在轨迹定位中也经历了显著的漂移。场景d选择了一个LiDAR严重退化且在操场上运行距离更长的环境，所有方法均产生了相当大的误差。FAST-LIVO在坐标(-40, 0)处开始漂移。FAST-LIO2能够稳定运行，但其起始节点位于X轴-30m至-20m和Y轴40m至50m之间，表明其误差大于EVO计算出的误差。LVI-SAM系统在恶劣条件下继续经历大幅漂移。总体而言，所提出的LPVIMOSAM在收集的场景中展现了强大的鲁棒性和定位精度。

为了更深入地评估定位精度，图4展示了场景d中的轨迹对比，图5呈现了该场景下七种方法（LIO-SAM、FAST-LIO2、LIOMO-SAM、FAST-LIVO、LVISAM、LVI-SAM（Harsh）、LPVIMO-SAM）的位置估计误差。

在计算误差时，所有轨迹的第一个节点都与真实值精确对齐。如图5所示，LIO-SAM的误差集中在

轴上。FAST-LIO2在

和

轴上的误差高于所提出的LPVIMO-SAM。FAST-LIVO的轨迹由于所选图像的影响而漂移。与LIO-SAM相比，LVI-SAM校正了一些

轴误差，但LVI-SAM（Harsh）由于特征不足而整体漂移。新提出的LIOMO-SAM和LPVIMO-SAM表现优异，所有方向的误差在接近零的小范围内波动，并实现了最优的整体轨迹定位误差。

为全面展示基于场景偏振信息计算得到的DOP和AOP图像在灰度图像上的特征增强效果，作者设置了LVI-SAM（Harsh）和LPVIMO的角点选择质量水平系数

将SAM调整为0.9-0.95以模拟低纹理场景中的恶劣条件。如表1和图6所示，在大多数场景中，由于LVI-SAM（Harsh）提取的特征数量较少，VIS误差显著，导致系统漂移甚至完全失效，无法进行轨迹定位。相比之下，如图6所示，通过引入本文中的场景极化信息，并利用DOP和AOP图像有效提取极化特征，LPVIMO-SAM系统即使在低纹理和低特征场景中也能稳定运行。此外，由于LIOMO-SAM子系统的定位精度较高，整个导航系统兼具高稳定性和高定位精度优势。其定位精度比正常运行的LVI-SAM提高了约36.8%，并且高于FAST-LIO2和FAST-LIVO系统。然而，LPVIMO-SAM系统基于因子图架构，其运行速度低于基于滤波的FAST-LIO2和FAST-LIVO系统。输出节点数量仅为FAST-LIO2的50%左右，为FAST-LIVO的16.8%。尽管如此，总体而言，LPVIMO-SAM系统在定位精度和稳定性方面表现出色。

参考

[1]. LPVIMO-SAM: Tightly-coupled LiDAR/Polarization Vision/Inertial/Magnetometer/Optical Flow Odometry via Smoothing and Mapping

点击上方卡片，关注「AI视界引擎」公众号