视觉惯性导航系统的研究和开发

1、引言

多年来，惯性导航系统(INS)[1,2]被广泛用于估计传感平台(如自动驾驶车辆)的6自由度姿态(位置和方向)，特别是在GPS信号缺乏的环境，如水下，室内，城市峡谷或在其他星球上。大多数INS依赖于一个6轴惯性测量单元(IMU)，它测量平台的局部线性加速度和角速度，而平台与IMU是刚性连接的。随着近年来硬件设计和制造的进步，低成本的轻量化微电子机械(MEMS) IMU已经普遍存在[3-5]，这使得高精度定位成为可能。移动设备[6]和微型飞行器(MAVS)[7-111]，在广泛的新兴应用领域中有着巨大的影响[12,13]，从移动增强现实(AR)到虚拟现实(VR)[14]再到自动驾驶[15,16]。不幸的是，高速率IMU测量的简单集成被噪声和偏置所破坏。往往导致长期导航下姿态估计的不可靠。尽管存在一种高端战术级IMU，但对于广泛部署来说，它的成本仍然高得令人望而却步。另一方面，体积小、重量轻、能效高的相机可提供丰富的环境信息，并作为INS的辅助来源，产生视觉惯性导航系统(VINS)。

虽然这一问题具有挑战性，因为缺乏能够减少随时间累积的运动漂移的全球信息(如果使用低成本、低质量的传感器，这种情况甚至会加剧)。在过去的十年里，VINS吸引了大量的关注。到目前为止。许多VINS算法可用于视觉惯性SLAM[17]和视觉惯性测程(VIO)，如扩展卡尔曼滤波器(EKF)[17,18. 20 - 22]，无迹卡尔曼滤波UKF[23 - 25]，批处理或增量平滑[26,27]，以及基于滑动窗口的方法 [28-31]。在这些方法中，基于EKF的方法因其效率高而仍然很受欢迎。例如，作为移动设备上最先进的VINS解决方案，Tango[32]项目(或ARCore[12])似乎使用EKF来融合用于运动跟踪的视觉和惯性测量。然而，近年来预积分技术的进步也使得在基于图优化的方法高效地融合了高频IMU测量 [29, 30, 33–35]。

显然，VINS技术正在兴起，主要是由于移动感知和导航应用的高要求，这在该领域产生了大量的文献。然而，据我们所知，目前还没有关于VINS的当代文献综述，尽管最近有关于SLAM的广泛调研[16,36]，但并不专门研究VINS。这使得学术界和工业界的研究人员和工程师很难有效地找到和理解与他们的兴趣相关的最重要的工作，而这些工作我们多年来在处理这个问题时都经历过。因此，我们正努力填补这一缺口：(1)提供一个简洁(由于空间限制)但完整的有关VINS进行状态估计的关键方面的回顾，(2)提供了我们对最重要的相关工作的理解，（3）对尚待解决的挑战的开放的讨论。这是由于希望(至少)帮助研究者/工程师跟随和理解最先进的VINS算法和系统，从而更有效地加速我们整个研究领域的VINS研究和开发。

2、状态估计

A.基于滤波的vs.基于优化的估计

Mourikis和Roumeliotis [18]开发了最早成功的VINS算法之一，称为多态约束卡尔曼滤波器（MSCKF），后来应用于航天器下降和着陆[21]以及快速无人机自主飞行[43]。这种方法使用基于四元数的惯性动力学[37]进行状态传播，与有效的EKF更新紧密结合。具体而言，不是将通过相机图像检测和跟踪的特征添加到状态向量，而是将它们的视觉方位测量值投影到特征雅可比矩阵的零空间（即线性边缘化[44]），从而保留仅与状态向量中与随机克隆的相机位姿[45]关联的运动约束。在通过移除共同估计可能成百上千个点特征的需要来降低计算量的同时，该操作阻止了特征在以后的非线性测量的重新线性化，从而产生性能的近似恶化。

标准MSCKF [18]最近已经沿着不同的方向进行了扩展和改进。特别是，通过利用我们以前的工作[46-49]中提出的基于可观性的方法，已经开发了不同的可观测性约束（OC）-MSCKF算法，通过强制线性化VINS的正确可观察性来改善滤波器的一致性[19,38-40,50-52]。MSCKF的平方根逆版本，即平方根逆滑动窗口滤波器（SR-ISWF）[6,53]，以提高计算效率和数值稳定性，使VINS能够在计算资源有限的移动设备上运行，同时不牺牲估计精度。我们已经引入了最佳状态约束（OSC）-EKF [54,55]，其首先在滑动窗口中最佳地提取关于相对相机位姿的视觉测量中包含的所有信息，然后在EKF更新中使用这些推断的相对测量。最近采用（右）不变卡尔曼滤波器[56]来改善滤波器一致性[25,57-60]，以及用于机器人中心设备中VINS的（迭代）EKF [22,61-63]。另一方面，在EKF框架中，除了点之外的不同几何特征也被用于改善VINS性能，例如[64-68]中使用的线特征和[69-72]中的平面特征。此外，基于MSCKF的VINS也扩展到使用具有不准确时间同步的卷帘门相机[64,73]，RGBD摄像机[69,74]，多个摄像机[53,75,76]和多个IMU [77]。虽然基于滤波的VINS已显示出高精度状态估计，但它们在理论上受到限制; 也就是说，非线性测量（12）在处理之前必须具有一次性线性化，可能将大的线性化误差引入估计器并降低性能。

相比之下，批量优化方法解决了一组测量中的非线性最小二乘（束调整或BA[78]）问题，允许通过重新线性化[79,80]减少误差，但计算成本高。Indelman等[27]使用因子图来表示VINS问题，然后类似于iSAM[81,82]逐步解决它。为了在应用于VINS时实现恒定的处理时间，通常将有界滑动窗口中的近期状态视为有效的优化变量，同时边缘化过去的状态和测量[28,31,83-85]。特别是Leutenegger等人[28]引入了一种基于关键帧的优化方法（即OKVIS），其中一组非连续的过去相机姿态和一系列近期惯性状态，与惯性测量相关联，被用于非线性优化以进行精确的轨迹估计。Qin，Li和Shen [31]最近提出了一种基于优化的单目VINS，它可以在非实时线程中包含闭环，而我们最近的VINS [86]能够在一个仅有线性复杂度线程中有效地利用闭环。

B.紧耦合vs.松耦合传感器融合

VINS融合视觉和惯导有多种方案，可以广义分为松耦合和紧耦合两种。具体来说，松耦合融合，无论是滤波还是基于优化的估计，分别处理视觉和惯导的运动约束，然后融合这些约束(例如 [27,87-91])。虽然该方法具有较高的计算效率，但视觉约束和惯导约束的解耦会导致信息丢失。相比之下，紧耦合的方法直接融合视觉和惯性测量在同一过程，从而实现更高的精度(例如，[18,28,34,40,85,92])。

C.VIOvs.SLAM

通过联合估计传感器平台的位置和周围环境特征，SLAM估计容易实现加入闭环约束实现有界定位误差，这在过去三十年中也出现了重大研究成果[16,36,93,94]。VINS可以被视为SLAM的一个实例(使用特定的视觉和惯性传感器)，在广义上包括视觉惯性(VI)-SLAM[28,33,85]和视觉-惯导里程计(VIO)[18,22,39,40,52,95]。前者联合估计由环境特征位置和相机/IMU姿态共同构成状态向量，而后者状态向量不包含环境特征，但仍然利用视觉测量在相机/IMU姿态之间施加运动约束。一般来说，加入建图(从而形成闭环)，VI-SLAM从特征地图和可能回环检测中获得了更好的精度，同时比VIO带来更高的计算复杂度。尽管已经提出了不同的方法来解决这个问题[18、21、28、85、96、97]。然而，VIO估计器本质上是里程计(航迹推算)方法，除非某些全局信息(例如GPS或先验地图)或对以前位置的约束(例如，使用闭环检测)。许多方法利用不同关键帧的特征观测来限制在轨迹上的漂移[28,98]。大多数都有一个双线程系统，该系统优化小窗口的局部关键帧和限制短时漂移，而后台进程优化一个长期稀疏位图，其中包含强制实现长期一致性的闭环约束[31,83,99,100]。例如，VINS-Mono[31,100]在局部滑动窗口和全局批处理优化中都使用了闭环约束。具体地说，在局部优化过程中，关键帧的特征观测提供了隐式的环闭约束，其中通过假设关键帧的姿态是理想的(从而将它们从优化中去处)，问题变得很小。

特别是，在VINS中是否通过地图[83、101、102]和/或位置识别[103 108]实行闭环检测是VIO和SLAM之间的关键区别之一。虽然利用闭环来实现有界误差的VINS是必要的，但也面临着由于无法在不做出不一致假设的情况下保持计算效率的挑战，例如将关键帧的姿势视为正确，或重用信息。为此，[109]提出了一种混合估计器，利用MSCKF进行实时局部估计，并在闭环检测时触发全局BA。在滤波器等待BA完成的额外计算期间内，允许一致性重新线性化和闭环约束。最近，Lynen等人[110]开发了一种基于大规模地图的VINS，该VINS使用包含特征位置及其不确定性的压缩先验地图，并使用先验地图中的特征匹配来约束全局估计。DuToit等[102]利用Schmidt KF[111]的思想，开发了一个Cholesky-Schmidt EKF，该EKF使用了一个具有完全不确定性的先验地图，并放松了地图特征与当前状态变量之间的所有关联;而我们最新的SchmidtMSCKF[86]将闭环集成在一个线程中。此外，最近的point-line VIO[67]将边缘关键点的三维位置作为闭环的真值，但这可能会引发不一致性。

D.直接vs.间接的视觉处理

视觉处理过程是任何VINS的关键部分之一，负责将密集的图像数据转换为运动约束，并将其纳入估计问题中。其算法根据使用的视觉残差模型可分为直接算法和间接算法。间接方法[18,28,30,40,51]被视为经典技术，它提取和跟踪环境中的点特征，同时在估计过程中使用几何重投影约束。当前最先进的间接视觉SLAM的一个例子是ORB-SLAM2[83,112]，它使用来自3D特征点对应的信息来执行基于图形的相机姿态优化。

相比之下，直接方法[96,113,115]在公式中利用原始像素强度，并允许包含更大比例的可用图像信息。LSDSLAM[114]是一个最先进的直接视觉SLAM的例子，它基于最小化相机关键帧之间的强度误差来优化它们之间的转换。注意，这种方法还优化了一个包含关键帧约束的分离的图，以允许合并高信息量的闭环来纠正长轨迹上的漂移。这项工作后来从单目相机扩展到立体和全方位相机，以提高精度[116,117]。其他流行的直接方法包括[118]和[119]，它们以紧耦合的方式估计关键帧深度，并提供低的漂移结果。最近直接方法在VINS上的应用受到了关注，因为即使在低纹理环境中，它们也能够鲁棒跟踪动态运动。例如，Bloesch等人[61,62]使用基于patch的直接方法提供迭代EKF更新;Usenko等[96]提出了一种基于离散预积分和直接图像对齐的滑动窗口VINS; Ling, Liu, and Shen [9] 和 Eckenhoff, Geneva, and Huang[115]将不同IMU预积分的直接图像对齐集成到动态运动估计中[34,35,85]。

由于光度一致性假设，直接图像对齐需要良好的初始猜测和较高的帧率，而间接视觉跟踪在提取和匹配特征时需要额外的计算资源。然而，由于间接方法的成熟和鲁棒性，它在实际应用中得到了更广泛的应用，但是直接方法在非结构场景中具有潜力。

E.惯导预积分

Lupton和Sukkarieh[33]首先提出了IMU预积分，这是标准惯性测量积分的一种计算上有效的替代方案，它将惯性测量动力学的离散积分与局部参考系统相结合，从而避免了在每个优化步骤里重新积分状态动力学的需要。虽然这解决了计算复杂性问题，但由于在方向表示中使用欧拉角，该方法存在奇点。为了提高这种预积分的稳定性，在[29,34]中引入了流形上的表示，其在SO（3）流形上呈现了无奇点定向表示，将IMU预积分结合到基于图优化的VINS中。

虽然Shen，Michael和Kumar [85]引入了连续状态下的预积分。但他们在不提供封闭形式解决方案的情况下对测量动态进行离散采样，这与从连续时间的角度预积分的理论完整性方面存在显著的区别。与先前方法中使用的预积分测量和协方差计算的离散近似相比，在我们先前的工作[30,35]中，我们得出了测量和协方差预积分方程的闭合形式解，并且这些解决方案提高了在高动态运动的情况下离散方法的精度。

F.状态初始化

提供准确的初始状态估计的鲁棒、快速初始化对于手持实时VINS估计器至关重要，其通常以线性闭合形式求解[7,84,120-124]。特别是，Martinelli [123]引入了单眼视觉惯性初始化问题的闭合解决方案，后来扩展到陀螺仪偏差校准。这些方法无法模拟惯性积分中的不确定性，因为它们依赖于长时间内IMU测量的双重积分。费斯勒等人[127]在松散耦合的估计框架内开发了基于SVO [113]的重新初始化和故障恢复算法，但需要额外的向下距离传感器来恢复尺度。Mur-Artal和Tard'os [83]在他们的ORBSLAM [112]上引入了一个高度（约10秒）的初始化器，它利用已知ORB-SLAM的关键帧的视觉惯导全BA计算初始尺度，重力方向，速度和IMU偏差。在[7,84]中，最近提出了一种线性方法用于无噪声情况，通过利用短期IMU（陀螺仪）预积分获得的相对旋转但不对陀螺仪偏差建模。当观察的是远处的视觉特征时，这在现实世界中并不可靠。

3、传感器标定

在融合来自不同传感器的测量结果时，必须高精度地确定空间和时间传感器校准参数。特别是，我们应该准确地知道相机和IMU之间的刚体转换，以便正确地融合从它们的测量中提取的运动信息。此外，由于不正确的硬件触发，传输延迟和时钟同步误差，每个传感器的带时间戳的传感数据可能不一致，因此，视觉和惯性测量之间的时间线错位（时间偏移）可能会发生。因此，校准这些时间偏移很关键。传感器校准空间和时间参数的问题已成为许多最近VINS研究工作的主题[42,128-132]。例如，Mirzaei和Roumeliotis [42]在相机和IMU之间开发了基于EKF的空间校准。进行校准参数的非线性可观测性分析[133]以表明这些参数在随机运动下是可观察的。同样，Jones和Soatto [128]基于无法区分的轨迹分析检查了相机和IMU空间校准的识别能力，并在嵌入式平台上开发了基于滤波器的在线校准。Kelly和Sukhatme[129]通过类似ICP的匹配方法对齐这两个传感器的旋转曲线，解决了相机和IMU之间的刚体转换问题。

许多这些研究工作都集中在需要额外校准辅助工具的离线校准流程上[42,130,134-136]。特别是，作为最先进的方法之一，Kalibr校准工具箱[130,134]使用传感器轨迹的连续时间基函数表示[137]来校准多个传感器的内参和外参。由于该B样条表示允许直接计算预期的局部角速度和局部线性加速度，因此预期惯性读数和测量惯性读数之间的差异用作批量优化公式中的误差。离线校准的缺点是每次重新配置传感器套件时都必须执行此操作。例如，如果移除传感器进行维护并返回，则放置误差可能导致性能不佳，需要进行时间的重新校准。

相反，在线校准方法在传感器套件的每个操作期间估计校准参数，从而使得它们在这种情况下更加鲁棒并且更容易使用。Kim，Shin和Kweon [138]通过将惯性读数从IMU帧转换为第二帧，重新制定了IMU预积分[33-35]。这允许IMU和其他传感器（包括其他IMU）之间的校准，但不包括时间校准，并且依赖于计算陀螺仪测量的角加速度。Li和Mourikis[131]在移动设备上使用的滤波器框架中，在单个IMU相机对之间同时校准空间和时间外参，后来扩展到包括相机和IMU的内参[139]。Qin和Shen [132]扩展了他们之前关于基于批量的单目VINS [31]的工作，通过插入图像平面上的特征位置来包括相机和IMU之间的时间偏移。施奈德等人[140]提出了利用信息量最大的运动的可观察性在线校准。虽然我们最近还分析了时空校准的简并运动[141]，但尚未完全理解如何对内参进行最优建模并同时校准它们与外参[142,143]。

4、可观性分析

系统可观测性在一致状态估计的设计中起着重要作用[49]，它检查可用测量提供的信息是否足以估计状态/参数而没有模糊[133,144,145]。当系统可观测时，能观性矩阵是可逆的，这也与Fisher信息（或协方差）矩阵密切相关[146,147]。由于该矩阵描述了在测量中可用的信息，通过研究其零空间，我们可以获得有关估计器应获取信息的状态空间方向的见解。。在我们之前的工作[46-48,52,146,148-150]中，我们是第一个为机器人定位问题设计可观察性约束（OC）一致估计器的团队。从那时起，VINS的可观察性分析（例如，[19,38,151,152]）成为了一个重要的研究方向。

特别是，VINS非线性可观测性分析被研究者使用不同的非线性系统分析技术进行了研究。例如，Jones和Soatto [128]以及Hernandez，Tsotsos和Soatto [153]对系统的难以区分的轨迹[154]从可观察性的角度进行了检验。通过采用[155]中的连续对称概念，Martinelli [122]分析得出了VINS的闭合形式解，并确定了IMU偏差，3D速度，全局滚动和俯仰角是可观测的。他还研究了退化运动[156]，最小可用传感器[157]，协同VIO [126]和未知输入[158,159]对系统可观测性的影响。基于李导数和可观测矩阵秩检验[133]，Hesch等。[51]分析表明，单目VINS有4个不可观察的方向，对应于全球偏航和外感受传感器的全球位置。Guo和Roumeliotis[69]将这种方法扩展到RGBD相机辅助INS，如果点和平面测量都可用，则保留相同的不可观测方向。根据类似的想法，在[74,129,160]中，分析研究了IMU相机（单目，RGBD）校准的可观测性，这表明在给定通用运动的情况下IMU和相机之间的外参平移分量是可观测的。另外，在[161,162]中，具有来自水平平面的向下看的相机测量点特征的系统被示出具有传感器的可观测的全局z位置。

实际上，VINS估计器通常建立在线性化系统上，实际上更重要的是对线性化VINS进行可观测性分析。特别地，线性化VINS系统的可观测性矩阵[41,163]具有零空间（即，不可观察的子空间），其理想地跨越全局平移和绕重力矢量的全局旋转四个方向[19,38,40,51] ]。在为VINS设计非线性估计器时，我们希望估计器使用的系统模型具有由这些方向跨越的不可观测子空间。然而，如[19,38-40,51]所示，标准EKF不是这种情况。特别是，标准EKF线性化系统在当前状态估计时使系统和测量函数线性化，具有三个不可观测的子空间，而不是四个维度。这意味着滤波器从可用测量中获得不存在的信息，从而导致不一致。为了解决这个问题，采用了第一个估计雅可比（FEJ）思想[47]来提高MSCKF一致性[19,40]，OC方法[48]用于开发OC-VINS[38,39,50] ]。我们最近还开发了robocentricVIO（R-VIO）[22,63]，它保留了与线性化点无关的适当的可观察性。

随着惯性和视觉传感器变得无处不在，视觉惯性导航系统（VINS）已经进行了大量的研究工作，并在过去十年中取得了巨大的进步，在实践中促进了越来越多的创新应用。作为SLAM问题的一个特例，VINS研究人员在SLAM之上迅速建立了丰富的文献资料[36]。鉴于该领域发表的论文越来越多，了解最新技术水平变得越来越难（尤其是从业者）。此外，由于特定的传感器特性，在不了解文献中现有方法的优缺点的情况下从头开发VINS算法并非易事（注意每种方法都有其特定的关注点，并不一定解决VINS估计的所有方面）。所有这些都激励我们对VINS进行研究，据我们所知，遗憾的是，这些研究在现有文献中是缺乏，因此对于正在研究这个问题的研究人员/工程师来说应该是一个有用的参考。基于我们在该领域的重要工作，我们通过关注构建VINS算法的关键方面，包括状态估计，传感器校准和可观察性分析，尽量使这篇论文简洁而完整。

虽然过去十年中VINS取得了重大进展，但仍有许多挑战需要应对，下面我们列举一些可供讨论的问题：

•持久定位：虽然目前的VINS能够提供准确的3D运动跟踪，但是仅限于在小规模友好的环境中，它们不够鲁棒，不适合长期，大规模，安全关键的部署，例如自动驾驶，部分由于资源限制[95,97,164]。因此，要求即使在具有挑战性的条件（例如不良照明和运动）中也能够实现持久性VINS，例如通过有效地集成闭环或构建和利用新的地图。

•语义定位和建图：虽然点，线和平面[151,165]等几何特征主要用于当前的VINS进行定位，但这些手动提取的特征可能不适用于导航，并且能够通过深度学习最新优势，了解VINS的最佳功能[166]。此外，最近的一些研究努力试图赋予VINS对环境的语义理解[167-170]，这只是零星地探索但具有很大的潜力。

•高维对象跟踪：在动态复杂环境中导航时，除了高精度定位外，通常还需要检测，表示和跟踪在同一空间中实时共存的移动对象，例如3D对象跟踪自主导航[92,171,172]。

•分布式协作VINS：虽然已在[126,173]中初步研究了合作VINS，但开发实时分布式VINS仍然具有挑战性，例如，用于众包操作。最近关于合作绘图的工作[174,175]可能会对如何解决这个问题提供一些启示。

•对不同辅助传感器的扩展：虽然在许多应用中，光学相机被视为INS的理想辅助源，但其他辅助传感器可能更适合某些环境和运动，例如，声学声纳可能在水下使用[176]；低成本的轻量级激光雷达可以在环境中更好地工作，例如在光照条件差的情况下[71,177]；以及事件相机[178,179]可以更好地捕捉动态运动[180,181]。在这个方向上，我们应该研究使用不同辅助源的深入VINS扩展，以便应用于我们手上的工作。

审核编辑：郭婷