多相机多地图视觉惯性定位：系统、验证与数据集

基于地图的定位对机器人自主运动至关重要，因为它提供实时的位置信息反馈。然而现有的视觉惯性导航系统（VINS）和同步定位与地图构建系统（SLAM）无法直接集成到机器人的控制回路中。尽管VINS可以提供高频率的位置估计，但其在长期运行中会产生漂移。而SLAM通过环路闭合校正消除漂移，但这是非因果的后处理过程。在实际控制中，不可能用未来的信息来更新当前的姿态。此外，现有的SLAM评估系统在对齐整个轨迹后测量精度，忽略了里程计起始帧与真实值帧之间的变换误差。

为解决这些问题，我们提出了一种多相机多地图视觉惯性定位系统，能够为机器人控制回路提供实时、因果且无漂移的位置反馈。此外，我们分析了基于地图的定位系统的误差组成，并提出了一套适合衡量因果定位性能的评估指标。为了验证我们的系统，我们设计了一种多相机IMU硬件设置，并收集了一个长期挑战性的校园数据集。实验结果表明，所提出系统具有更高的实时定位精度。为了促进社区发展，我们已将系统和数据集开源。

基于地图的视觉惯性定位（VILO）是指通过结合预构建地图的观测数据，获取实时的状态估计。在实际应用中，VILO对于自主机器人具有重要意义，因为从机器人控制的角度来看，VILO能够提供实时、因果且无漂移的反馈，可以集成到位置控制回路中。然而，无论是视觉惯性导航系统（VINS）还是同步定位与地图构建（SLAM）系统，都无法满足这一需求。尽管VINS提供实时的姿态估计，但随着时间推移，其会累积漂移；而SLAM通过环路闭合校正消除漂移，因此提供的无漂移姿态会有较大的时间延迟，无法作为控制的因果反馈。

目前，VILO的一个典型应用是基于高精地图（HD Map）的自动驾驶系统。然而，这种系统依赖于大量预定义的语义元素，如交通灯和车道线，在其他应用场景（例如物流或非结构化区域）中无法适用。因此，能否构建一个通用的VILO系统来输出实时、因果且无漂移的姿态估计成为一个关键问题。我们从以下三个方面探讨解决方案：

1. 系统：尽管已有的系统（如VinsMono、ORB-SLAM、Maplab等）支持利用地图进行定位，但在应用于具有长期变化的大规模环境时，仍面临以下问题：

• 需要在全局一致的地图上进行定位。

• 在多子地图环境下，这些系统要么需要将多个子地图离线合并为一个全局地图，要么需要在线融合多个子地图。

• 经典PnP算法结合RANSAC用于剔除外点，但在长期环境变化下的特征匹配中仍存在高外点率的情况。

2. 评估：现有VINS和SLAM的评估指标可能无法全面衡量定位性能。例如，绝对轨迹误差（ATE）和相对位姿误差（RPE）需要将VINS轨迹与真实全局轨迹对齐后再进行评估，忽略了从VINS帧到地图帧的变换误差。

3. 数据集：评估VILO系统需要满足两项要求：

• 数据集需要在相同地点多次采集，构建多会话数据集。

• 数据集需包含多种长期外观和结构变化。

为了解决上述问题，我们提出了一种多相机多地图的VILO系统，其主要创新点包括：

• 该系统能够在线灵活融合多个独立地图，并提供实时、因果且无漂移的位置估计。

• 提出了基于IMU辅助的多相机最小解法，并嵌入RANSAC框架以增强对长期变化中高外点率的鲁棒性。

• 设计了一种多传感器硬件设置，收集了一个覆盖265,000平方米、总轨迹长度超过55公里、包含长期外观和结构变化的校园数据集。

所提出的VILO系统框架如图1所示。在系统设计中，特别关注用户实际使用VILO系统时的需求，并为此定制了相应模块以满足这些需求。

• 地图构建模式：从用户角度来看，它包括数据采集和数据处理两个阶段。由于用户通常缺乏专业的地图构建知识，为避免数据质量差导致建图失败，需要在采集阶段及时反馈采集数据的质量。因此，我们开发了一种在线建图模块，能够快速生成初步地图并及时提供反馈。随后，在获得高质量数据后，离线建图模块会进一步优化3D重建结果，为定位模式提供支持。

• 定位模式：用户主要需要在预构建的多地图中获得实时定位结果。此外，由于本地计算平台计算能力有限，实际应用中要求计算效率高。为满足这些需求，我们开发了初始化模块和在线匹配模块，用于获取地图观测数据，并将其传递至轻量化的基于地图的VINS模块，从而实现一致、精确的实时状态估计 。

图 1：所提出的 VILO 系统框架概览。在建图模式下，系统接收实时多传感器数据输入至在线建图模块进行初始地图构建，并向用户提供建图质量反馈，以便及时调整数据采集策略。随后，离线建图模块执行两阶段的高精度地图构建，并支持密集重建输出。在定位模式下，系统利用建图模式中构建的单个或多个独立地图，进行基于地图的一致性、准确且实时的状态估计。具体细节详见第 III 节。LCD：回环检测，SFM：基于结构的运动分析，BA：捆绑调整。

地图构建

1. 在线视觉建图模块

在线建图模块的整体流程如图3所示。我们采用多相机SLAM模块实时运行，并提供反馈以指导用户的采集行为。

图3.在线建图流程

• 多相机SLAM：该模块主要分为多相机视觉惯性里程计（VIO）前端、环路闭合检测和后端优化。为实现鲁棒的前端估计，我们采用OpenVINS，一个基于扩展卡尔曼滤波（EKF）的模块化平台。尽管过滤方法速度快，其精度受到线性化误差的限制。为解决这一问题，我们集成了环路检测和位姿图优化。

• 数据质量反馈：在数据采集过程中，过快的运动可能导致图像模糊或相邻图像之间的视角差异过大，进而影响地图构建的质量。我们通过前端算法的失败案例或高估计速度作为反馈，指导用户重新采集数据。

2. 离线视觉建图模块

传统的多相机视觉建图方法将所有相机固定在统一框架下，结合IMU提升系统功能，但这种方法易受外参校准和时间同步误差影响。我们提出了一种两阶段的结构化从运动（SfM）方法来解决这些问题：

• 阶段1：利用增量式SfM算法（如COLMAP）生成初始地图，并使用VINS估计的轨迹作为先验信息。

• 阶段2：进行全局优化，结合重投影误差和多相机外参约束，提升精度。

3.外部数据源建图模块

在地图构建过程中，许多采集设备配备了LiDAR或GPS等传感器，这些数据具有绝对尺度信息，可提升大规模定位任务的准确性。我们支持融合这些传感器信息以构建更精确的地图。

定位

1. 初始化模块

定位模式的目标是实现一致、准确和实时的状态估计。为此，在操作开始时，我们设计了一个初始化模块，用于将当前视觉-IMU观测与预构建地图进行关联，计算当前相机相对于地图帧的位姿。这一过程的关键在于确保初始化的鲁棒性和精确性。

• 鲁棒性要求：在实际应用中，初始化的成功与否对后续的定位精度具有关键影响。由于机器人在长期运行中可能出现显著的外观和视角变化，因此需要一种能够适应高外点率的确定性算法。

• 高效确定性算法：我们提出了一种基于最小解法的确定性收敛算法，能够在极高外点率（高达95%）下实现鲁棒的初始化。

2. 在线匹配模块

初始化完成后，为了在运行过程中持续减小里程计漂移，我们设计了在线匹配模块，专注于实时高效地获取地图观测数据。

• 采样框架：通过将最小解法嵌入到RANSAC框架中，我们提出了一种基于2点匹配的RANSAC方法，减少了计算模型所需的最小匹配点数量，从而显著提高了效率和鲁棒性。

• 多相机融合：我们开发了多相机最小解法（MC2P），结合来自多个视角的观测数据，提升内点数量，进一步增强鲁棒性和精度。

3. 基于地图的VINS模块

有了良好的初始化和在线匹配观测结果，关键在于如何将这些信息有效地用于实时定位。我们提出了一种基于滤波的框架，能够高效融合多个独立地图的观测数据。

• 系统状态扩展：在原有VINS状态的基础上，增加了地图观测变量和历史关键帧信息，用于优化当前估计。

• 观测函数设计：结合本地特征和地图特征的观测信息，通过滤波方法实现对机器人位姿的实时估计。

为验证所提出的 VILO 系统的有效性，我们需要一个包含环绕多摄像头图像、IMU 和外部传感器数据（如 LiDAR 和 GPS）的数据集，同时应包含具有挑战性的长期变化。然而，如表 III 所示，目前公开可用的数据集难以满足这些要求。需要注意的是，尽管 NCLT 数据集拥有丰富的传感器数据，其六个摄像头视场不重叠，缺乏立体图像，无法验证立体视觉里程计算法。因此，我们组建了一个硬件平台，用于采集具有长期变化的校园多摄像头数据集。具体而言，我们设计了一个多传感器同步与数据采集系统，利用该系统采集了包含长期变化的数据集。系统的核心是一种同步模块，能够实现激光、摄像头、IMU 和惯性导航系统（INS）等传感器的硬件同步，并具有良好的扩展性。在此基础上，我们在车辆上构建了一个多传感器数据采集平台，并在校园环境中通过车辆行驶采集了不同条件下的道路数据。

硬件平台配置

如图 4 所示，我们设计了一个多传感器硬件平台，并将其安装在车辆上以进行数据采集。传感器的详细参数见表 IV。具体而言，平台前部安装了 4 个摄像头，包括 2 个彩色摄像头组成的彩色立体对，以及 2 个灰度摄像头组成的灰度立体对，每对的基线约为 50cm。彩色立体摄像头分辨率较高但帧率较低，适用于车辆检测、行人检测等感知相关研究。灰度立体摄像头分辨率较低但帧率较高，可用于 SLAM、视觉定位等研究。此外，平台在左右两侧和后侧各安装了一台鱼眼摄像头，提供宽广视角以确保车辆周围的完整覆盖，并与灰度立体摄像头结合用于视觉定位研究。平台顶部安装了一台 32 通道激光雷达，用于覆盖周围环境，支持基于激光的定位研究。左右两侧各安装了一台 16 通道激光扫描仪，用于覆盖车辆两侧，适合环境感知研究。

图 4：多传感器数据采集车辆

B. 硬件同步设计

同步模块的任务是维护统一的时钟源，并根据此时钟源为不同传感器的数据打上时间戳。同步模块需支持常见的定位感知传感器，如摄像头、LiDAR、IMU、GPS 和 INS。此外，为确保同步质量，应尽量通过硬件而非软件实现。因此，我们设计了一个以 MCU 为核心的同步模块，如图 5 所示。模块的时钟源来自高频率 IMU，并将其脉冲信号分配给不同的低频传感器。为适应室内和室外两种场景，设计了两套方案：室外方案利用 GPS 信号并支持同步的 INS 设备，而室内方案无需 GPS 信号。模块中的传感器选择列于表 V。

数据采集

图 6：数据采集车辆中不同传感器数据的示例

在浙江大学紫金港校区，利用该车辆进行了约 9 个月的数据采集。采集数据示例如图 6 所示。彩色立体摄像头的广视角有助于检测行人和车辆等前方环境。激光雷达覆盖车辆周围，顶部激光获取全方位结构信息，侧面激光弥补顶部激光导致的近距离稀疏数据问题，提高了障碍物检测能力。为评估算法在长时间运行下的鲁棒性，车辆多次在不同时间重复相同路线，采集到适合长期研究的定位与感知数据，如图 7 所示。此外，为增加采集数据的多样性，采用了如图 8 所示的多种轨迹，包括环形、矩形和线性运动等。

图 7：数据采集车辆在同一地点但不同时间拍摄的图像示例。

实验结果

图 10：使用 Ep 建图进行的地图精度评估。黑色点云为真实值，彩色点云对应不同算法的结果，颜色表示重建误差。

图 12：使用我们方法的密集重建结果。不同颜色的箭头表示不同的观察方向。虚线框显示从对应颜色箭头方向观察到的密集重建结果，实线框显示相应的真实场景。

图 14：不同算法识别的匹配内点示意图。