Extended VINS-Mono: 大规模户外环境进行绝对和相对车辆定位的系统性方法（IROS2021）

Extended VINS-Mono: 一个在大规模户外环境中进行绝对和相对车辆定位的系统性方法

单位：卡内基梅隆大学

本文作者：Maple|来源：3D视觉工坊

针对问题：

针对实际场景中动态因素、大规模定位以及不同需求下定位策略选择存在的问题。

提出方法：

提出融合KF-INS、GNSS与VINS-Mono多定位方法融合框架；

提出基于DNN的修正滤波的方法，以暂时筛选掉潜在动态物体内的所有特征点方法；

提出基于速度观测约束的VINS初始化方法。

达到效果：

扩展的VINS-Mono在穿越美国匹兹堡的高速公路、隧道、桥梁、城市峡谷和郊区的25公里行程中产生了明显的改进和卓越的性能。我们的融合方法还在一个给定的坐标系中提供了多种选择，具有不同的精度、速率和延迟属性。

Abstract

我们提出了一种被称为扩展的VINS-Mono的系统方法，利用VINS-Mono这种最先进的单目视觉-惯导相对定位方法，针对大规模户外道路环境中的实际车辆定位。我们提出的融合方法将多种独立的定位方法联系在一起，并在所需的坐标系中提供多种（预测）状态估计，以满足不同的精度、速率和延迟要求。我们用绝对定位方法（如GNSS）和相对定位方法（如基于卡尔曼滤波的INS）来扩展VINSMono，为导航/路线提供全局状态估计，为规划/控制提供局部状态估计。此外，扩展的VINSMono解决了VINS-Mono在大规模户外道路环境中使用时的两个重要缺点。首先，在一个几乎是平面的路面上的运动将使VINS-Mono无法观测到尺度。其次，动态场景中的移动物体会降低精确度。我们通过用速度读数来扩展VINS-Mono的（重新）初始化过程，并引入一个速度因子来处理VINS-Mono的尺度估计问题。一种带有基于DNN的物体检测掩膜的动态特征点过滤方法可以处理动态环境，并重新收集停放的汽车等静止物体的特征点。与VINS-Mono相比，在穿越匹兹堡的高速公路、隧道、市区和郊区的25公里行程中，使用扩展的VINS-Mono获得了更好的全局准确性。因此，扩展的VINS-Mono可用于动态道路环境中可靠而准确的绝对定位。我们还评估了多种定位方法在全局坐标系中的多个（预测）状态估计的准确性、定位率和延迟。因此，我们的融合方法能够满足智能车辆上各种任务的不同定位要求。

Main Contributions

本文的主要贡献是采用系统化的方法，将多种独立的定位方法联系起来，提供具有不同精度水平、速率和延迟的多种状态估计，以满足各种任务的定位要求。这种系统化的关联被设计为通用和可扩展的。

此外，在大规模户外车辆环境中使用VINS-Mono面临两个关键问题。

1)在一个几乎是平面的路面上的运动使得尺度不可观。

2)在动态道路场景中，许多检测和跟踪的特征点都在移动物体上，而且许多车辆可能在同一方向上移动。

为解决第一个问题，直接使用车辆速度读数而不进行融合，并做出类似二维平面假设的假设来恢复VINS-Mono中的尺度观测。我们提出了一种新的初始化方法，对原有的初始化方法进行了扩展，以有效和可靠地初始化车辆状态。

为了处理第二个问题，我们使用基于DNN的物体检测方法，用轻量级和快速的DNN来检测潜在的移动物体，如车辆和行人，以掩盖其上的任何特征点。我们提出了一种修正滤波的方法，以暂时筛选掉掩膜内的所有特征点，并重新收集停放的汽车等静止物体上的特征点。

多种定位方法的系统关联

本节介绍了我们的融合方法，将VINS-Mono与绝对定位方法GNSS和相对定位方法KF-based INS进行系统性的关联。由于本文中使用的所有定位方法都是在三维直角坐标系中进行定位的，因此我们只讨论这种类型的坐标系的关联。然而，我们的融合方法可以被推广，以支持不同类型的坐标系之间的关联。此外，在我们的系统方法中，绝对定位表示具有固定原点的定位方法，而相对定位表示具有（潜在的）动态原点的定位方法。因此，我们的系统方法旨在获得和更新绝对/相对定位方法的多个坐标系之间的动态转换关系。

1.问题定义和符号声明

每种定位方法都在自己的坐标系中输出状态估计值。如图2所示，GNSS绝对定位方法在以G表示的全局坐标系UTM中输出全局状态估计值{gt}，VINS-Mono相对定位方法在以X表示的局部坐标系中输出局部状态估计值{xt}，INS相对定位方法在以Y表示的另一个局部坐标系中输出局部状态估计值{yt}。对于GNSS绝对定位方法，其全局坐标系的原点OG是预先定义的，并保持不变。对于相对定位方法VINS-Mono和INS，其局部坐标系的框架原点OX和OY在其初始化后动态定义，是其后续相对定位的基础。同时，定位结果是对某种定位方法的体心的状态估计。

2.转化关系计算

（1）轨迹对齐:

（2）基于卡尔曼滤波的INS的重置操作:

在我们的实现中，我们首先截断时间戳t之前的状态估计，然后用存储的IMU测量值重新传播时间戳t之后的状态估计。

3.松耦合(可选择)

针对室外大场景的EXTENDED VINS-MONO

1.VINS-MOON的速度读数

我们现在讨论VINS-Mono的扩展（使用速度读数进行重新初始化）。VINS-Mono的图优化中的速度因素是直截了当的，这里不做讨论。因为车辆的速度读数通常来自于后轴的中心，我们使用阿克曼模型将原始速度读数转化为安装在车辆前部的Camera-IMU系统。

我们首先对VINS-Mono的初始化过程做一个简要概述。其基本思想是通过解决一个线性最小二乘法问题来获得速度、重力矢量和尺度，从而使来自Camera的SfM与IMU的预积分结果相一致。

2.基于DNNs的特征点筛选方法

EXPERIMENTAL EVALUATION

1.实验系统平台和实验设置

我们在自主车辆平台凯迪拉克SRX和凯迪拉克CT6上评估我们的扩展VINS-Mono。

我们将一个全局快门相机（Flea3，1280×1024，30fps）和一个IMU（Memsense3030，9轴，800Hz）组成一个相机-IMU系统。工作频率为500Hz的轮速计提供速度读数，工作频率为5Hz的低成本GPS接收器ReachRTK提供全球位置，工作频率为500Hz的SRX的高成本Applanix POS LV和工作频率为200Hz的CT6的相对便宜的高级导航惯性导航系统（AdvNav）提供真值。我们从匹兹堡的25公里行程中收集数据，跨越高速公路、隧道、桥梁、城市峡谷和郊区。我们使用RTK模块作为绝对定位方法，在高速公路、桥梁和郊区环境中提供全球定位。然而，在城市峡谷中，我们使用基于全局对齐和缩放的地图匹配的单目ORB-SLAM2（定位模式）[26]作为绝对定位方法来提供全局位姿。为了故意给ORB-SLAM2引入绝对定位误差，地图是由三个月前收集的不同数据集生成的。一个基于KF的INS在100Hz下运行，通过两个投影提供相对和绝对定位，如图1s和3所示。在隧道中没有全局位姿，我们只能依靠VINSMono和基于KF的INS来提供全局状态估计。我们的融合方法以1Hz的速度更新变换关系。松耦合的融合被禁用，以专注于评估扩展的VINS-Mono，而没有来自GPS/RTK或ORB-SLAM2的优化。

2.评估

（1）初始化表现:

对于每种方法，我们重复了五次初始化实验，并在图7中给出了统计结果。我们的初始化方法在完成初始化后具有最长的有效时间Δt，并且明显优于其他方法，证明了其可靠性。由于需要额外的计算，它的初始化时间∆t成本比其他方法略长。然而，它仍然是可以接受的，可以在实践中使用。从箱形图中可以看出，我们的方法的∆t成本的离群值较长，比其他方法要少得多。这表明我们的方法有更高的这表明我们的方法在完成初始化方面有更高的成功率。结论是，我们的初始化方法在整个25公里的行程中都是有效的，因此可以为接下来的扩展VINS-Mono的状态估计提供可靠的初始状态。

（2）全局精度:

接下来，我们测量了所有定位方法对真值（GT）的偏移量（误差）（预测）全局状态估计。结果分为三个部分：表一为城市前，表三为城市峡谷闭环测试，表二为城市后。最长的前城市段是具有挑战性的，因为它包含一个没有任何全局状态估计的隧道，而且在从桥到公路的南岸段，可见卫星的数量通常低于6颗。对于Extended VINS-Mono来说，后城市段比较容易，因为RTK模块提供了一路有效的全球定位。城市峡谷环路测试比城市后段更难，因为来自ORB-SLAM2的全局定位及其故意引入的误差比来自RTK的全局定位要嘈杂得多。

在表I-III中，全局精度由全局误差的平均值μe和标准差σe来衡量（第二列组），对GT的偏移量的CDF（累积分布函数）范围为0.5m至3.0m（第三列组）。我们将VINS-Mono与绝对定位方法联系起来，从相对定位方法中获得预测的全局状态估计值，我们以独立的GT为参考，计算其偏移量（误差）进行评估。因此，"全局误差 "和 "对GT的偏移量（CDF）"的绝对值反映了多种相关定位方法的总体精度水平。然而，我们更关注的是绝对定位方法和相对定位方法之间的相对关系。我们的目标是证明相对定位方法的预测全局状态估计值应该比绝对定位方法的全局状态估计值具有类似或更高的总体精度水平。

[1]融合方法:

从表二和表三来看，扩展的VINS-Mono（第6行）和其相关的基于KF的INS在城市后段的表现比使用RTK和ORB-SLAM2在城市场景测试中的表现更好。这个实验表明，我们的融合方法将绝对定位方法的全局精度传播给了相对定位方法，并在相对定位的约束下提高了该投影全局状态的精度。对于城市前的路段，在进入隧道前，Extended VINS-Mono、相关的INS和RTK的总体精度与城市后的路段处于相似的水平。然而，在离开隧道后，扩展的VINS-Mono和相关的基于KF的INS的整体精度明显下降。在隧道内和前城市段的某些部分没有绝对的定位方法，相对定位方法的累积误差降低了它们的整体精度。在未来的工作中，我们将在隧道内加入其他互补的绝对定位技术来解决这个问题。

[2]速度系数（原始VINS-MOON与扩展VINS-MOON对比）:

在表I-III中，即使采用我们的融合方法，将局部状态估计动态地投影到全局坐标系中，VINS-Mono的全局精度（第1行）在所有三个路段中都非常低。这是因为在大规模的室外道路环境中，如果没有速度因素，VINS-Mono的尺度估计是不可靠的。这个缺点导致了随着时间的推移出现相当大的漂移，相反，扩展的VINS-Mono的全局精度（第2行）在只有速度因子的情况下得到了明显改善。

[3]动态特征点筛选:

尽管我们用图5中的例子证明了对静态物体的特征点重新收集，但从表一到表三中可以看出，它对整体精度的影响并不明显，正如人们所期望的那样。一个可能的原因是，与COVID19之前的水平相比，由于流量较少，数据集包含的动态对象较少。因此，"RANSAC First "方法（第3-4行）即使在去除一些内部掩码后，仍然有足够的特征点。另一个可能的原因是，VINS-Mono的深度估计首先来自于旧状态和新状态之间的三角测量，这是从IMU传播中初始化的。反过来，由 "无遮挡 "方法（第2行）接受的移动特征点在接下来的鲁棒化优化中会有一个可行的但错误的深度值。

估计延迟是指从最新的GT时间戳到最新的状态时间戳的时间间隔。负值表示定位方法有延迟，正值表示定位方法提供的值比GT早。从表一到表三，我们看到我们的融合方法中基于KF的INS提供了高速率和低延迟的绝对定位输出，在全局坐标系中具有可接受的整体精度水平。

最后，我们强调，表一至表三中所有具有不同精度、速率和延迟属性的输出都来自于我们的系统性关联。基于各种任务的预期要求，我们的系统方法可以提供相应的定位选择。

CONCLUDING REMARKS

我们提出了扩展的VINS-Mono，使VINS-Mono[4]能够同时提供相对和绝对的定位，用于大规模户外道路环境中的规划/控制和导航/路线。我们提出的融合方法系统地关联了多种相对/绝对定位方法，为智能车辆的各种任务提供多种状态估计选择。此外，我们使用车辆的速度读数来获得尺度指标，以实现可靠的（重新）初始化和与尺度相关的位置/速度估计。为了处理有移动车辆的动态环境，我们修改了传统的滤波程序，并利用DNNs的车辆和天空掩膜，去除动态但重新收集静态特征点。与VINS-Mono相比，扩展的VINS-Mono在穿越美国匹兹堡的高速公路、隧道、桥梁、城市峡谷和郊区的25公里行程中产生了明显的改进和卓越的性能。我们的融合方法还在一个给定的坐标系中提供了多种选择，具有不同的精度、速率和延迟属性。在未来，我们将重点评估Extended VINS-Mono内其他定位方法所提供的多种状态估计的可靠性。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)

3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进

4.国内首个面向工业级实战的点云处理课程

5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦

7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化

8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现