一种完全分布式的点线协同视觉惯性导航系统

在本文中，我们提出了一种完全分布式的点线协同视觉惯性导航系统。我们通过蒙特卡罗模拟和真实环境数据集，在稠密特征或稀疏特征环境下将所提出的算法与其他四种算法进行了比较。所有结果表明，我们的PL-CVIO优于独立的MSCKF和CVIO。

01   摘要

稀疏特征环境是几何计算机视觉（CV）算法的主要挑战之一。在许多人工场景中，通常存在稀疏特征情况，而线条可以被视为点特征的补充。本文提出了一种利用点和线特征的多机器人协同视觉惯性导航系统（VINS），通过在多状态约束Kalman滤波器（MSCKF）框架内使用协方差交集（CI）更新，每个机器人不仅利用自身的点和线测量数据，还利用邻近机器人观察到的共同点和共同线特征进行约束，采用最近点表示对线特征进行参数化和更新。该算法（PL-CVIO）经过蒙特卡罗模拟和真实环境数据集广泛验证，在稀疏特征和稠密特征环境中均表现出优于独立MSCKF以及我们先前CVIO工作成果的结果。

02   简介

近年来，SLAM技术受到了广泛关注，已经成为许多机器人和计算机视觉应用的核心技术，例如增强现实/虚拟现实、自动驾驶和机器人导航等。在GPS信号受限的环境中，通过使用低成本、轻便的车载摄像头和惯性测量单元（IMU），视觉惯性导航系统（VINS）及其相关算法已经获得了广泛的认可。不过事实上，多个机器人能够比单个机器人能够更有效地完成任务并实现更高的精度。但对于多机器人群体，有一个关键问题就是如何最好地利用环境信息和其他机器人的信息。

在人工环境中，线可以被视为对点的良好补充，特别是在稀疏特征环境中只能提取到少量点特征的情况下。在VINS中，有两种主要方法来处理点和线：间接（基于特征）和直接方法。具体而言，间接方法通过提取特征描述符并沿着序列进行匹配来预处理图像流。间接方法是通过最小化几何误差来优化系统。直接方法跳过了特征提取步骤，并直接使用行像素来优化光度误差。相比较，直接方法非常高效，但需要假设亮度恒定（忽略曝光变化），而实际环境中曝光会发生较大变化。

在先前的基于特征的VINS文献中，解决方案可以大致分为两类：基于滤波器的方法和基于图的方法。基于滤波器方法中最具代表性的是多状态约束卡尔曼滤波（MSCKF）,它通过使用相同特征点的测量值形成了一个多约束更新。另一种领先技术是紧耦合单目图像VIO（VINS-Mono），它引入了鲁棒初始化和非线性优化. 此外，还有一些同时使用点特征和线特征的VINS算法。其中，点线视觉惯性里程计（PL-VIO） 是对VINS-Mono 的扩展，它可以在滑动窗口内优化点和线特征的重投影误差。PL-SLAM  提出了一个基于ORB-SLAM 的点线SLAM框架。还有研究者设计了适用于快门相机的Plucker 表示下的直线特征和提出了两种直线三角测量算法，以及对三种不同直线表示形式(Plucker, Quaternion, Closest Point) 及其相应可观测性进行深入分析.然而，以上所有方法都是集中在单个机器人情况下进行研究。

协作式VINS（C-VINS）的一个优势在于多个机器人共享共同特征，从而引入更多共同特征的几何约束。具体来说，群组中的每个机器人不仅观察其自己的测量值，还会收集来自多机器人群组的测量值。通过利用共同特征约束，机器人应用更新以改善定位性能。也存在一些集中式多机器人解决方案，但它们通常需要昂贵的计算和通信成本，分布式算法在这方面有着一些优势。最近，研究者采用M-Space表示不同类型特征提出了分布式点线协作SLAM（CSLAM）算法,但由于重复使用相同信息导致估计一致性无法保证.在 [Localization of a group of communicating vehicles by state exchange] 中,群组中每个机器人处理自己可获得的测量，并且只在特定时间步内与通信范围内其他机器人融合估计和协方差.DOOR-SLAM 提出了一个完全分布式CSLAM算法,包含姿态图优化模型和类似于[Data-efficient decentralized visual SLAM] 的数据高效分布式SLAM前端.文章 [Distributed maximum a posteriori estimation for multi-robot cooperativelocalization]提出了使用最大后验概率（MAP）进行完全分布式算法.我们之前的工作CVIO , 利用协方差交叉(CI)更新提供了一个完全分布式合作算法并可以保证一致性,但稀疏特征环境未被考虑进去.

在本文中，我们提出了一种利用点特征和线特征的完全分布式多机器人姿态估计算法。每个机器人不仅利用自己的点和线测量，还相互进行合作（见图1）。特别是在稀疏特征环境中，也就是缺乏稳健的地标，可以通过整合每个机器人独立获取到的点和线特征，并利用协方差交叉更新来更精确地估计它们的位置。为此，我们开发了PL-CVIO算法，并将其应用于最先进的OpenVINS系统中，该系统采用单目摄像头-IMU架构。通过在蒙特卡罗模拟和现实环境下进行测试，结果表明，在稀疏特征和稠密特征环境下，我们的算法都表现出更精确的定位能力。

03问题概述

点线协同式视觉-惯性里程计的目标是在全局坐标系 中跟踪每个机器人的3D姿态，其中= 1, · · · ,n。与单个机器人不同，多个机器人可以相互共享共同特征。本文利用常见的点和线特征来提高定位精度。

A. 视觉-惯性里程计状态向量

为了执行PL-CVIO，每个机器人的状态向量被定义为：

B. 动态系统模型

对于每个机器人，IMU的线性加速度和角速度的测量被建模为：

C. 点-线测量模型

在稀疏特征环境中，线条是点的良好补充。因此，在本文中我们考虑了点和线测量。机器人的点测量可以描述为：

D. 独立点和线特征更新

为了进行独立的点或线特征更新，将对每个机器人应用标准的MSCKF更新。具体来说，我们会收集当前滑动窗口内所有的点和线测量，通过堆叠一个点或一条线的测量，我们可以利用IMU姿势的估计来三角化这个点特征或线特征。为简化符号表示，令表示一个点特征或一条线特征，并且机器人的测量残差可以被线性化为：

E. 共同点和线特征更新

需要注意，相邻的机器人可能会观察到共同的点或线特征。因此，我们将进一步利用相邻机器人之间的点和线特征约束来提高定位精度。机器人之间可以通信以共享信息。

Robot 及其邻近机器人将对共同特征应用线性化和左零空间投影。与第II-D节类似，机器人将使用进行EKF更新。然而，机器人不会放弃，而是利用邻近机器人的共享信息。它将构建一个新的残差系统，该系统依赖于与自身和邻近机器人相关联的顶部部分堆叠在一起的共同点或线特征。

04模拟和实验

在本节中，我们利用蒙特卡罗模拟和真实环境数据集验证了协作情况下常见线特征对定位精度的提高以及独立情况下线特征对精度的改善。我们将在两种不同环境下通过表I比较我们的PL-CVIO算法与先前工作。稀疏特征场景包含少量特征，而稠密特征场景则包含足够多的特征。如表I所示，PVIO代表独立MSCKF算法，PLVIO代表独立点线MSCKF算法，P-CVIO代表我们之前的CVIO工作，IPLCP-CVIO是指不仅利用每个机器人的独立点、线特征还收集邻近机器人的共享点特性的算法，并且PL-CVIO使用了本文介绍的独立和共享点线功能。

A. 蒙特卡罗模拟

在蒙特卡罗模拟中，我们利用了一个由三个机器人组成的小组。该小组中的机器人0按照数据集的真实轨迹运动，而机器人1和机器人2的轨迹则是通过向真实轨迹添加位置和方向偏移来创建。之后，如果当前帧中点或线测量数量低于阈值，则生成3D特征及其相应的2D测量。然后，收集并利用来自一个机器人共同特征以及邻近机器人共有特征的约束来更新当前状态。

稀疏特征和稠密特征环境通过提取不同数量的点特征进行划分。在稠密特征环境中，每帧的点特征数量为150，线特征数量为50。对于稀疏特征环境，我们将点特征数量减少到50。对于这两种环境，我们使用了First-Estimation
Jacobian(FEJ)和在线相机-IMU校准。在运行了30次蒙特卡罗迭代后，在稠密特征或稀疏特征环境的Udel Gore数据集下，相对姿态误差（ROE）和相对位置误差（RPE）的统计数据分别如图2和图3所示。可以看到，我们的PL-CVIO算法在两种环境中都优于其他所有算法。特别是在稀疏特征环境下，我们发现共同线可以明显降低ROE和RPE（如图3中的蓝色和红色条形图所示）。此外，有趣的发现是，如果每帧只观察到有限数量的点，PL-VIO将优于P-CVIO。在此情况下，共同点特征的数量也有限，因此仅依赖于共同点特征的协同方法P-CVIO可用资源有限。相比之下，进一步利用线特征的方法PL-VIO和PL-CVIO表现出更好的性能，而PL-CVIO则取得了最佳性能，因为它不仅利用了点和线特征，还利用了与邻近点的协作。

此外，我们还在所有的EuRoC V1数据集中对我们的PL-CVIO算法进行了模拟，并将其与P-VIO和P-CVIO在稀疏特征和稠密特征环境下进行了比较。

每个机器人的方向和位置的均方根误差（RMSE）以及每种算法在各个环境中的平均RMSE都记录在表II中。RMSE结果显示，在所有模拟场景中，我们的PL-CVIO算法优于P-CVIO和P-VIO。特别是在稀疏特征环境中，PL-CVIO显著改善了姿态和位置的RMSE。

B. 实验

在现实环境的实验中，每个机器人的位置和方向都初始化为地面真值。我们使用FAST[37]从每个帧中提取点特征，并通过ORB与8点RANSAC算法进行跟踪以跨越帧或与其他机器人的点观测匹配。同时，利用LSD提取线段，并使用LBD进行跟踪。此外，我们还添加了一些异常值剔除策略，以删除满足以下条件的线段：(1)
LBD距离大于50；(2) 线段长度小于50像素；(3) 线段起点到该线的距离小于0.1或大于100；(4) 当应用SVD对该线进行三角测量时，线条视差太小会导致奇异性。

我们在TUM
Visual Inertial Dataset Rooms 1, 3和5中评估了我们的PL-CVIO算法，其中IMU频率为200Hz，相机频率为20Hz。我们加载同一房间的所有三个数据集，并分别使用三台机器人运行所有五种算法。此外，我们提取不同数量的点特征来模拟稀疏特征和稠密特征环境。因此，在图4中展示了我们的PL-CVIO算法与其他四种算法在稠密特征环境下的实验结果，并在图5中展示了稀疏特征环境下的实验结果。同时，在表III中展示了利用不同算法在TUM数据集上每个机器人方向和位置的均方根误差（RMSE）。从ROE/RPE和RMSE结果可以明显看出，线特征可以提高P-VIO的准确性，而共享点-线功能则可以改善P-CVIO的性能。另外，在稀疏特征场景中通过比较P-VIO和PL-VIO以及P-CVIO和IPL-CP-CVIO可见线条显著改善了性能,如表III所示.最终,在所有实验案例中,我们的PL-CVIO算法均优于其他四种算法。

05   总结

在本文中，我们提出了一种完全分布式的点线协同视觉惯性导航系统。我们通过蒙特卡罗模拟和真实环境数据集，在稠密特征或稀疏特征环境下将所提出的算法与其他四种算法进行了比较。所有结果表明，我们的PL-CVIO优于独立的MSCKF和CVIO。此外，我们验证了线特征可以改善独立情况下定位的准确性，并且共享线特征在协作情况下表现更好。