RailLoMer-V：适用于铁轨系统的多传感器融合SLAM（RAL 2022）

3D视觉工坊

发布于 2023-04-29 10:39:23

4850

发布于 2023-04-29 10:39:23

文章被收录于专栏：3D视觉从入门到精通

论文阅读：Rail Vehicle Localization and Mapping With LiDAR-Vision-Inertial-GNSS Fusion(RAL 2022)

Motivation

监测铁路安全运营需要精确的轨道车辆定位和长期的铁路环境。国内现行铁道系统的定位策略仍以轨旁系统为主，不仅实时性和准确率都差，而且需要大量的前期投资和后续维护。

虽然已经有基于GNSS和odometry的研究，但是这类方法缺失了对于环境的感知信息，并且会受到GNSS环境的限制。所以加入诸如雷达和相机的感知进行辅助融合是一个可行的方向，但是在铁路系统加入lidar和相机存在几个问题：

一是运动长时间受到约束，轨道车辆被限制沿平面轨迹移动，导致 IMU存在非观测方向的虚假信息增益。这种问题会导致许多VIO的大规模漂移。
二是铁路系统信息重复，主要可观察的特征是重复的铁轨和悬架夹具，这对于基于特征跟踪的方法具有挑战性。
三是铁路系统基本无回环，现有的SLAM 通常使用地标描述符来检测重访问的地点，并在检测到的循环上纠正累积的漂移。铁路系统没有回环，所以需要更加低漂移的位姿估计。

Contribution

这篇文章提出了一个框架，基于滑窗因子图将 LiDAR、IMU、轨道车辆车轮里程计、相机和 GNSS 紧密融合。实验结果表明，融合后的系统对于大规模铁路环境定位足够准确，并且对铁路上的长时间倒退具有鲁棒性。
充分利用传感器测量的几何信息，利用提取的铁轨和消失点的平面约束来提高系统精度和鲁棒性。实验证明这些特征有效地限制了具有重复结构的区域的高度和旋转误差。
实验中所使用的数据集时长跨度一年多，涵盖各种规模、天气和铁路状况。

Content

问题描述

多传感器融合的状态估计问题本质上是MAP问题，定义当前时刻列车状态如下：

为了保证实时性，滑窗基于关键帧建立，对于滑窗内的关键帧，优化问题表述如下：

其中是通过舒尔补给出的先验运动因子，是通过IMU/轮式里程计给出的预积分，是lidar约束，是平面约束，是重投影约束，是线段约束，是灭点约束，是GNSS约束。

2.系统框图

虚线框表示因子图结果。给定输入的 LiDAR 点云和单目图像，提取每张图像的点、线和灭点，并跟踪图像帧，然后通过分配的深度信息校正比例。此外，以铁轨平面作为平面约束，检测提取铁轨。所有测量都与滑窗因子图进行后续的联合优化。关于滑窗因子图中的各类因子介绍如下：

A、IMU/轮式里程计预积分因子

铁轨的轮式里程计模型可以表述为：

其中，表示为通过随机游走建模的轮式里程计的尺度因子。

然后通过同步采集的陀螺仪和里程表输出来实现姿态估计，连续两帧之间的位移可以表述为：

(是0均值高斯噪声)

因此，在帧K和帧K+1之间的离散形式的IMU/轮式里程计信息

可以表示为：

那么，最终可以得到关于IMU/轮式里程计的预积分残差

：

其中，

表示IMU和轮式里程计之间的位移，用于表示从四元数中取最后四个元素。

B、lidar残差

因为铁路速度过快，所以首先通过IMU/轮式里程计的增量运动模型来线性矫正lidar的点云畸变，并且去除距离过近的点云点。

然后基于LOAM的特征提取方式来基于曲率提取角点和面点，并且基于这两类点进行帧间的扫描配准。那么lidar残差可以表述为：

(LOAM的两个残差，不赘述)

另外，作者注意到，具有有限 FoV 的仅 LiDAR 里程计对由轨道和轨道道岔的接头引起的振动过度敏感，可能会出现pitch方向的误差。因为两条铁轨在转弯处高度不同，在随后的直线铁路中，仅 LiDAR 的里程计也会保持roll的发散。针对这个问题，利用LEGO-LOAM的分段地面约束方法，可以有效地限制roll和pitch角度的漂移，但是LEGO-LOAM的基于角度地面提取方法并不适用于铁轨系统，所以可以通过铁轨平面提取方法来确定地面。

C、和视觉相关的残差

与具有任意运动的 UAV 或 UGV 不同，轨道车辆仅限于高度受限的运动，比如速度长时间恒定，加速度恒定，以及没有旋转。在恒定加速度或不旋转的情况下，会引入局部不可观察的 IMU 偏差，生成病态甚至秩不足的信息矩阵，并导致 VIO 的显着尺度漂移。

因此，在初始化阶段利用LIO的 IMU 偏差，并将深度信息与图像关键帧上提取的特征点相关联从而进一步矫正漂移。

在尺度矫正的前提下，平移精度可以显着提高。但是旋转误差仍然是不可避免的，因此需要引入两个额外的结构约束来进一步优化。

首先基于LSD进行线段分割，

是投影到图像平面的线特征，是线特征的中点，那么线特征的重投影残差可以表述为：

但是LSD提取的大多是大量的短线特征（灌木树叶、阳光或树木的早午餐）。这些特征很难跟踪，因为其中一些可能会在下一帧中消失。因此将图像分辨率调整为 720 × 480，并设置最小线特征长度和宽度阈值以提取主要特征。

旋转漂移可以通过灭点进行有效的约束，因此可以利用灭点来约束相机方向。灭点基于平行线的交点进行检测。也就是说，平行线的方向决定了灭点。与一般的室内或室外环境不同，在单个帧中可以提取多个灭点，对于大多数铁路场景只能找到一个灭点，如下图。

假设是检测到的灭点在第k帧的角度，那么对应的灭点残差可以表述为：

D、GNSS残差

当估计的位姿协方差大于GNSS 协方差时，添加 GNSS 因子。仅依赖于 GNSS 协方差进行判断有时不可信，并且可能会产生模糊或不连续的建图结果。因此使用增量噪声对 GNSS 测量进行建模，GNSS残差可以定义为：

其中，

表示从GNSS接收天线到IMU的位姿，因为铁路的 4G 通信质量不一致，仅将单点定位 (SPP) 结果视为输入。

3、地图管理

为了减少频繁的 GNSS 辅助优化导致的建图模糊，提出了一种基于子图的两阶段map2map配准，首先基于局部优化创建子图，然后利用 GNSS 信息进行子图到子图的配准正态分布变换（NDT）。在实际操作过程中，每个子图维护30个关键帧。

4、实验

A、硬件

在两条铁路上对各种维修车辆进行了一系列实验：一个是普通高速列车的货运铁路，另一个是高速列车的载人运输铁路。根据铁路上的安全原则，前者只能在白天进行，后者只能在午夜进行。

数据集的详细情况如下表，真值由 MPSTNAV M39 GNSS/INS 组合导航系统的后处理结果保存（带有来自千寻 SI 的 RTK 校正）。

Baseline采用R2LIVE、FAST-LIO2、LiLi-OM、Lio-Livox 3 和 VINS-Mono。对于消融研究，将 RailLoMer-V w/o GNSS、RailLoMer-V w/o ODO 和 RM-LVI 定义为无 GNSS 因素、无里程计因素和仅 LiDAR 视觉惯性部分。

B、评测

1）Benchmark指标

所提出的系统可以实现所有序列的分米级精度。其他方法的性能并不理想，特别是对于长时间的任务。对于小规模测试，不正确初始化的重力矢量加速了垂直误差的累积。由于两条轨道在转弯时高度不同，因此无法正确初始化重力矢量，并且所选方法在没有额外约束的情况下都无法正常工作。对于大规模测试，仅视觉方法在恒定速度下表现出糟糕的性能。

2）关于运动方向的鲁棒性

与无人地面车辆（UGV）或自动车辆在收集数据时始终向前移动不同，轨道车辆还包括长时间的向后运动。HQ-Long，一个仅向后运动的序列，可以用来研究运动方向的影响。如上表和下图所示，基于滤波器的算法 R2LIVE 和 FAST-LIO2 在这个序列上的 6-DoF 中展现了巨大的误差。然而，基于优化的算法对这种影响不敏感

3）关于退化情况的鲁棒性

高速铁路隧道采用光滑的人造墙、重复的铁轨和悬垂线夹。这些地区是 SLAM 最困难的场景之一，数据集中的隧道由三个连续的隧道组成（最长的 1.7 公里）。在里程表和 GNSS 的帮助下，可以很好地消除纵向发散和累积误差，所提出的系统可以保持准确的轨迹。相反，其他方法要么“停止”，要么“倒退”到严重退化的场景。量化结果见上表的CH-Tunnel。

4）天气的影响

常速铁路数据集在夏季收集，而高速铁路数据集在冬季收集。由于铁轨在冬季午夜变得湿滑，车轮打滑是不可避免的，所提出的 RailLoMer-V 系统在冬季的性能降低了 30%。

5）消融实验

可以看出，里程计可以很好地约束退化时的纵向位移。在 CH-Tunnel 中，不带 GNSS 的 RailLoMer-V 比不带 ODO 的 RailLoMer-V 具有更好的性能。然而，对于特征丰富的地区，里程计的贡献并不明显，并且 RM-LVI 可以得到与 RailLoMer-V w/o GNSS 的其他序列相似的精度。量化结果在上面的表格。

6）高精度地图构建

所提出的方法可以构建铁路环境的大比例尺地图。实时建图如下图所示，其中与卫星图像清晰且匹配良好的结果表明提出的方法具有较高的精度。