题目：单目视觉-惯性融合自动导航
摘要
自主微型飞行器（MAV）具有成本和机动性优势，使其成为航空摄影，监视和搜索和救援等应用的理想机器人平台。随着平台缩小，MAV变得更能够在受限环境中运行，但它也引入了显着的大小和有效载荷约束。 由于其重量轻，占地面积小，单视觉惯性导航系统（VINS）仅由惯性测量单元（IMU）和摄像机组成，在这种情况下成为最合适的传感器套件。事实上，它是最小的传感器套件，允许具有足够环境意识的自主飞行。 在本文中，我们展示了使用单目VINS实现可靠的在线自主导航是可能的。我们的系统建立在配备有鱼眼摄像头，低成本IMU和异构机载计算资源的定制四旋翼测试平台上。我们系统的主干是一个高度精确的基于优化的单目视觉惯性状态估计器，具有在线初始化和自我外部校准。 基于GPU的基于单一密集映射模块，基于估计的姿势，提供广角态态感知。最后，直接在增量式三维地图上运行的在线轨迹规划器可确保在杂乱的环境中进行安全导航。 提供了广泛的实验结果来验证各个系统模块以及室内和室外环境中的整体性能。
关键词
空中机器人，建图，规划，位置估计
1.简介
微型飞行器（MAV）是低成本，小尺寸和出色移动性的理想机器人平台，适用于室内和室外环境中的各种应用。 由于MAV的快速动态和非视距操作的可能性，空中机器人必须能够自主导航。
可靠的状态估计，环境映射和避障是自主导航最重要的模块。 全球导航卫星系统（GNSS）为状态估计提供了一个直接的解决方案，使大量自主微型飞行器的户外应用成功商业化。 但是，GNSS不提供任何障碍信息。 它们也不适用于需要MAV移动性优势的狭窄，杂乱或室内环境中的操作。
在狭窄环境中操作需要小平台。 然而，较小的平台通常具有紧凑的尺寸，重量和功率（SWaP）限制，限制了它们携带有源但重的传感器（例如雷达或激光雷达）的能力。 由于基线长度不足，小平台的尺寸也限制了其承载立体声或多摄像机系统的能力。 随着平台变小，单目视觉惯性系统（VINS）仅由低成本惯性测量单元（IMU）和摄像头组成，由于其重量轻和占地面积小，成为唯一可行的传感器设置。 实际上，在消费电子设备上广泛使用的单眼VINS是最小的传感器套件，允许具有足够环境意识的自主飞行。
尽管具有出色的SWaP性能，但在使用单眼VINS进行自主导航时仍存在重大挑战。使用单个摄像机无法直接观察距离，并且要从单目VINS获得公制测量值，需要对环境中的特征进行可靠跟踪，以及具有足够加速度和角速度的运动。来自IMU和相机的测量值被融合在一起以获得状态的度量估计。这是一个脆弱且高度非线性的过程，需要宽视场（FOV）摄像机进行特征跟踪，精确的摄像机-IMU外部校准和良好的估计器初始化。密集3D地图的使用对于确保检测稀疏特征未覆盖的障碍物是必要的。为此，通过每像素多视图三角测量，离群值去除和深度图融合来生成密集深度图需要高度精确的姿态估计以及强大的计算平台。为了关闭感知 - 动作循环，我们还需要轨迹规划和反馈控制模块，这些模块在重建的密集环境表示上运行以保证安全性。
在本文中，我们提出了一个完整的系统解决方案来解决上述所有挑战。 系统级的贡献证明了使用最小单目VINS和完全机载处理的可行性，以通过未知混乱的环境实现安全导航。 从硬件到软件，每个系统模块的决策均来自仔细的工程考虑和折衷。我们选择使用具有IMU触发的硬件同步功能的单(?)目鱼眼摄像头来提供具有精确时间戳的广角观测。 我们使用一组功能强大的异构机载计算平台来并行化计算要求较高的每像素图像操作。
在软件模块层面，我们的主要贡献在于合适实时算法的选择，或者从我们以前的工作中得到改进。我们的算法包的主干是一个高精度，紧耦合，基于优化的单目视觉 - 惯性状态估计器，支持鱼眼相机，在线初始化和摄像机 - IMU外部校准。基于平面扫描的多视点深度估计模块结合半全局深度平滑，为障碍物检测提供实时密集深度图像。使用概率截断的带符号距离函数（TSDF）融合将深度图像融合到全局3D图中。最后，基于梯度的轨迹规划模块直接在重建的3D地图上运行，通过混乱的3D环境提供无碰撞轨迹。通过使用标准多旋翼控制器执行期望的轨迹来闭合感知 - 行动循环。据我们所知，我们是第一个使用这种最小传感器套件实现此类导航功能的人。这项工作指出了一个非常小（直径小于10厘米）的飞行器的自主方向。
本文的其余部分分为八个部分。 系统级相关工作将在第2节中简要讨论。第3节概述了我们的系统在硬件和软件体系结构中的情况。 第4节讨论我们如何使用单眼鱼眼相机。 状态估计，密集3D映射和轨迹规划模块分别在第5节，第6节和第7节中给出。 在线实验结果（第8节）验证各个模块以及集成系统的性能。 我们在室内和室外环境中展示自主导航。 第9节总结了这项工作，并指出了未来改进的可能方向，我们的目标是将我们的框架扩展到更高速度，更低功耗，更小尺寸和更低成本的系统。
2.相关工作
在不可使用GNSS的室内和室外环境中，有一系列关于自主飞行的学术工作。 当我们考虑对整个系统有贡献的单个模块时，该集合变得更加广泛。 在本节中，我们仅回顾使用低成本视觉传感模式的最相关的系统级工作，并将关于各个模块的讨论留给相应的部分。
基于立体视觉的方法已被广泛研究用于航空导航。参考文献1，使用三个板载计算单元，FPGA用于实时立体匹配，Gumstix用于实时嵌入式处理，Intel Core2Duo用于视觉测距，映射和规划。通过使用由Octomap library2提供的概率体素图，路径规划器被设计为提供到指定航路点的无障碍路径。松散耦合的视觉 - 惯性扩展卡尔曼滤波器（EKF）用于状态估计。然而，由于立体深度估计的空间基线要求，很难使平台小型化。同样，参考文献3给出了另一个基于视觉的映射和规划框架，其映射模块在其外部运行。还使用板外处理来实现循环闭合模块。然而，除了几个前视立体相机之外，另一个向下看的相机用于基于光流的状态估计。然而，在我们的工作中，我们展示了如何只有一台摄像机足以实现所有导航功能。
已经提出了使用各种传感器放置和计算基础结构的基于单目视觉的方法。结合SVO4和REMODE5，设计了一种重量轻，成本低的飞行平台，可以自动飞行并实时同时进行密集的三维重建。该平台配备有向下摄像头，用于状态估计和映射，这不足以在杂乱环境中进行障碍物检测。此外，参考文献6将映射模块卸载到地面站，这会导致高延迟到不适合在线导航的点。通过仅使用包含足够传感器的智能手机，包括IMU和单目相机，参考文献7在GPS拒绝环境中实现自主飞行。但是，在这项工作中没有开发出映射或避障模块。与我们的系统配置最相似的是参考文献8，其中前视单目视觉系统用于闭环自主导航。然而，使用半稠密方法的深度估计模块仍然是非车载的9。
在参考文献10中，作者提出了具有多个传感器的双层融合框架，旨在在诸如船载环境的挑战性环境中开发鲁棒的自主导航系统。 使用冗余传感器套件，参考文献10主要致力于提供安全路径，并在极端环境中提供令人信服的强大导航实验。 然而，出于不同的目标，我们考虑建立一个具有最小传感的敏捷飞行平台。
在固定翼飞行器上也可以实现视觉导航。 基于IMU和平面激光，参考文献11提出了一种高斯粒子滤波器的新颖扩展来解决定位问题。 通过展示具有挑战性的室内环境实验，作者对已知室内环境中的高速飞行进行了深思熟虑。
另一个研究方向涉及使用基于学习的技术进行end-to-end的导航动作生成。 代表作品包括参考文献12。 尽管是一个很有希望的方向，但基于学习的方法尚未达到复杂未知环境中安全飞行的成熟水平。
3. 系统概述
我们现在介绍我们的四旋翼实验测试台的硬件设计和软件架构。 我们满足传感器选择，传感器同步，计算能力分配，监控，调试工具，实验接口和模块化系统设计的要求。 我们将解释每个系统设计决策背后的基本原理以及系统模块的描述。
3.1 硬件结构
我们的目标是设计一个模块化，紧凑，耐用，易于构建的机器人测试台。 我们的机载系统由两个主要模块组成：感知核心和四旋翼机架。 感知核心集成了车载电脑和传感器，四旋翼机架由机械结构，电机，螺旋桨和电池组成。
3.1.1 感知核心
感知核心是一个可拆卸的模块，可容纳所有计算单元和传感模式。 它可以独立测试状态估计和映射模块，并可与其他机械框架一起使用。
我们使用两台电脑组成强大的异构计算平台（图2）。 其中一款采用双核英特尔i7-5500U处理器，运行频率高达3.00 GHz，配备8 GB内存和256 GB固态硬盘，功耗约为15W。 另一款车载计算机是NVIDIA TX1，它由载板上的核心组装而成。 TX1上的256个NVIDIA CUDA GPU内核使其特别适用于并行计算深度图像和TSDF融合。 TX1配备了四核ARM Cortex-A57处理器和4 GB内存，功耗约为15 W. 有关感知内核中每个组件的更多详细信息可以在表1中找到。两个机载计算机使用以太网电缆连接。 两台计算机之间的通信通过使用ROS基础设施完成。（计算单元的成本好高，这东西我毕设不一定能用）
我们的板载计算资源涵盖了大多数广泛使用的开发平台：x86，ARM和GPU，使其能够高度灵活地开发高级算法。 它显着缩短了开发周期，因为研究人员不需要在代码级优化上花费过多的时间。 然而，我们强调，如果通过数字比较，我们的机载资源只与Qualcomm SnapDragon 835等先进的手机处理器相媲美。这意味着使用感知核心开发的算法可以转移到低功耗移动设备 计算平台经过充分的代码级优化。
机载摄像头和IMU也包含在感知核心中。我们使用具有752×480分辨率的前视矩阵视频蓝光CTX-MLC200w全局快门摄像头。 它配备了220度超广角鱼眼镜头。 DJI A3飞行控制器同时用作IMU和姿态稳定控制器。由于在杂乱的室内环境中缺乏可靠性，我们禁用了A3中的GPS和磁力仪功能。
高性能的视频融合需要准确的时间戳。 在我们的系统中，这是通过硬件同步实现的，其中个人图像捕捉由DJI A3飞行控制器的同步信号触发。 更具体地说，我们在相机的硬件实时控制器中使用触发模型。 DJI Onboard SDK提供了一个界面，用于生成一束脉冲信号和相应的IMU测量值。 请注意，IMU以比摄像机更高的频率运行，并且如果IMU测量对应于图像，则会标记IMU测量。 只需一根附加电缆即可构成硬件同步设置。
3.1.2 四旋翼结构
四旋翼机架由机械结构，电机，螺旋桨和电源模块组成。 框架采用碳纤维管和铝制部件制造，轻巧耐用。 整个系统如图1所示。总重量，包括感知核心，是1.8公斤。 Tip-to-tip距离为33 cm。我们使用DJI E310电机和DJI Takyon Z425-M ESC作为升降系统。 我们使用DJI Phantom 4 Pro智能电池提供约15分钟的飞行时间。 DJI Lightbridge 2用于在iPad上实现远程桌面功能。
3.2 软件结构
我们系统的软件架构如图3所示。利用ROS作为通信中间件，我们在两台板载计算机之间分配计算负载，其原理是在NVIDIA TX1上执行密集像素操作。 两个车载电脑使用以太网连接。
3.2.1 算法流水线
在Mini i7计算机上，100 Hz IMU测量以及20 Hz同步鱼眼图像融合在视觉惯性状态估计器（第5节）中，以获得密集映射，轨迹规划和姿态的姿态，速度和姿态，以及 反馈控制。 估算器输出为100 Hz，这足以实现灵活的空中机器人的反馈控制。 为了利用最初针对针孔图像设计的运动立体算法，我们使用感兴趣区域（ROI）提取模块（第4.2节）将鱼眼图像裁剪成两个针孔图像以覆盖180度水平FOV。 然后将针孔图像与估计的姿势捆绑在一起并发送到TX1以进行深度估计。
（分成两块：1.i7-提取roi，估计状态
2.tx1-深度估计）
利用NVIDIA TX1的GPU主板，使用具有半全局平滑的密集多视图运动立体声以10Hz计算深度图像（第6节）。 使用GPU加速截断符号距离函数（TSDF）融合将深度图像转换为全局地图，也以10 Hz运行。
全局地图被发送回Mini i7计算机以辅助轨迹规划（第7节），其中生成引导机器人朝向用户指定目标的无碰撞时间参数化轨迹。 使用DJI A3飞行控制器中的姿态和推力控制接口执行轨迹。
与许多依赖向下摄像头进行状态估计的现有系统不同，我们的系统仅使用一个摄像头来满足所有传感需求。 我们通过在线室内和室外实验（第8节）展示我们的最小传感设置足以实现完全自主导航。
3.2.2 远程监视和调试接口
除了机载软件，我们还利用DJI Lightbridge 2实现了一个iOS应用程序，用于远程监控和调试。 Lightbridge直接将Mini i7计算机的HDMI输出流式传输到iPad Pro。 这使我们即使在机器人飞行时也可以实时查看桌面。 我们使用Lightbridge中的数据透明传输协议将虚拟键盘和鼠标输入从iPad传输到机载计算机。 通过这种方式，我们可以直接使用iOS应用程序在机载计算机上运行可视化工具，如rviz。 它甚至使我们能够在四旋翼飞行器的同时调试和重新编译机载代码。 此设置独立于任何外部基础设施，这使其非常便于现场测试。
4. 鱼眼相机
鱼眼摄像机提供的超宽视野是围绕障碍物安全导航的关键，但它还伴随着大量的镜头失真，必须对其进行建模和校正。 在本节中，我们将讨论如何建模鱼眼，以及必要的预处理步骤，以便能够使用鱼眼图像进行视觉惯性状态估计和密集深度估计。
4.1 鱼眼相机模型
我们使用参考文献14,15中提出的统一球体模型模拟鱼眼相机。 如图4(a)所示，相机投影模型是一个函数πc：R3→Ω，这个函数模拟了3d点及其在2d图像平面上的位置之间的关系。反投影模型πc-1：Ω→R3是它的逆。在该模型中，图像中的3D点pc∈R3首先被投影为单位球面上的点pc'，然后变换成2D图像平面上的uc。镜头失真在πc(⋅)中处理。 这可以在数学上描述为
反投影模型是pc'=πc-1(uc)，可以将2D图像平面上的点映射回单位球。
为了在单眼视觉惯性状态估计器（第5节）中利用鱼眼摄像机的覆盖感知范围，我们直接在单位球面上定义图像测量残差。 更多细节将在第5.5.3节中给出。
4.2 ROI提取
多视角稠密深度估计的过程可以通过使用一维扫描线搜索加速。 然而，存在显着的透镜畸变使得难以直接将这些加速技术应用于鱼眼图像。 我们注意到，对于主要经历水平运动的空中机器人而言，大的水平FOV是确保安全的最关键方式。为此，我们引入了一个感兴趣区域（ROI）提取模块，将鱼眼图像转换为两个无失真的针孔图像，覆盖180度水平视场。
我们考虑使用虚拟理想针孔相机来捕捉无失真图像。 根据应用需求手动选择虚拟摄像机的FOV，从中可以唯一确定虚拟摄像机的投影函数πv(·)。 虚拟摄像机相对于鱼眼摄像机图象(Rvc)的方向也由用户选择，如图4(b)所示。 对于虚拟图像中的像素uv，我们有
一旦我们在鱼眼图像中获得相应的像素位置，我们使用子像素插值来确定强度值。
我们在系统中使用的两个ROI的图示如图4(b)所示，其中相应的鱼眼视图标记在图4(c)中。 从ROI获得的两个无失真针孔图像如图4(d)所示。 两个针孔图像与物理光轴成±45度。虽然理论上我们只需要将每个针孔图像的水平FOV设置为90度，但我们将两个虚拟图像的FOV设置为实际实现的120度。 为了给中心区域提供冗余。
5. 单目视觉惯性状态估计
状态估计是自主飞行的核心。 建图和路径规划需要高精度的度量状态（位置，方向和速度）。 在本节中，我们提出了一种基于单目紧耦合滑动窗口的视觉惯性状态估计器，它为整个系统提供准确的状态。 由于单目视觉惯性融合是一种脆弱且高度非线性的过程，因此需要稳定的特征跟踪和良好的估计器初始化。 在下文中，我们将详细介绍测量预处理，稳健估计器初始化过程以及紧耦合优化框架。 这是我们以前的工作16,17的延续。
5.1 相关工作
最近，我们已经看到了很多关于视觉惯性估计的工作，无论是单目摄像机18-20，立体摄像机21还是RGB-D摄像机22.基于框架，我们可以将VINS分为两类：一类是 基于过滤的算法18-20,23-26，而另一个是基于图优化或基于束调整的方法16,21,27.基于过滤的算法通常被认为在计算上更有效。 主要缺点是早期固定线性化点可能导致次优结果。通过重复线性化过去状态，基于图优化的方法可以实现更好的性能，同时它们需要更多的计算资源。 从数学上讲，这些方法是相同的，但是以不同的形式实现。
对于单目视觉惯性融合，需要对系统进行精确初始化，但单目摄像机不能直接提供度量测量。我们早期的工作16,17,28提出了一种基于优化的线性估计器初始化方法。然而，由于不能在原始投影公式中对传感器噪声进行建模，因此在特征深度分布在整个范围（例如，室外环境）中的环境中失败。此外，我们早期的工作并未考虑陀螺仪偏差。 后来参考文献29中引入了另一种封闭形式的解决方案。 在参考文献30中提出了对这种封闭形式解决方案的修订。 这种封闭形式的解决方案对噪声传感器数据很敏感，因为作者无法在不同的持续时间内为不同的惯性积分建模测量噪声。参考文献31中，提出了一种基于SVO32的重新初始化和故障恢复算法。在这种方法中，首先使用惯性测量来稳定MAV的姿态，然后启动SVO以稳定位置。 MSF26松散地融合了视觉和惯性信息。
处理IMU测量的一种有效技术是预积分，其通过相对运动约束的重新参数化避免重复积分。 该算法首先在参考文献33中提出。 在我们之前的工作16中提出了考虑流形不确定性传播的方法。在参考文献34中提出了进一步改进IMU偏差并将它们与完整的SLAM框架相结合的建议。
在本文中，为了实现准确的实时估计，我们使用基于紧密耦合优化的单目视觉惯性框架和强大的初始化。 我们的VINS是一个稀疏且基于特征的框架，它提取和跟踪特征，最大限度地减少了后端的视觉和惯性几何误差。
5.2 滑动窗口公式
我们在固定的时间间隔内进行IMU和相机测量以进行状态估计。 由于使用多视图约束，这提供了良好的准确性。 它也享有恒定的时间复杂性。 如图5所示，固定窗口中包含多个IMU和摄像机测量。 滑动窗口中的完整状态向量定义为（为简化演示而忽略转置）
其中xk是第k帧的状态，它包含世界坐标系下的位置、速度、旋转以及body坐标系下的陀螺仪偏转、加速度偏转。
在这里，我们将IMU坐标系视为body坐标系。 世界坐标系与重力垂直的有关。 在初始化过程中解决重力矢量后，将设置世界坐标系。 在整个估算框架中，我们使用四元数来表示外部参数旋转xcb，其包含从相机到IMU的旋转和平移。 n是滑动窗口中关键帧的数量，m是滑动窗口中的特征点数量，λi是第i个特征点在单位球面上的第一次观察的反向深度。
5.2 测量预处理
我们的状态估计框架中存在两种测量。 一个是图像，另一个是IMU测量。 在将这两种测量结合到估计器之前进行预处理。 我们在预处理步骤中跟踪和检测图像中的特征。 同时，IMU测量是预先整合的的。 请注意，IMU测量不仅受噪声影响，还受偏差影响。 因此，我们特别考虑IMU预积分和以下优化过程中的偏差。
5.3.1 特征处理前端
对于每个新图像，现有特征由KLT稀疏光流算法35跟踪。 同时，检测到新角点特征以保持每个图像中最少特征（100-300）。 通过在两个相邻要素之间设置最小20-30像素的间隔，检测器强制实现特征的均匀分布。 在进入外点拒绝之前，使用鱼眼摄像机模型将特征投影到单位球体。 外点拒绝只是由RANSAC执行。
在此步骤中也使用了关键帧(Keyframes)。 我们有两个关键帧选择标准：其中一个是平均视差——如果跟踪要素的平均视差超出某个阈值，我们会将此图像视为关键帧( 请注意，不仅平移而且旋转都会导致视差; 但是，在仅旋转运动中不能对特征进行三角测量。 为了避免这种情况，我们在计算视差时使用IMU传播结果来补偿旋转。)； 另一个标准是跟踪质量， 如果跟踪特征的数量低于某个阈值，我们也会将新帧视为关键帧。
5.3.2 IMU预积分
给定两个时刻k和k + 1对应于图像第bk，bk+1帧，我们可以预积分局部帧bk中的线性加速度和角速度
在上述公式中，⊕代表四元数乘法操作符。可以看出，预积分部分可以仅用[k，k+1]内的IMU测量获得，因为它与状态(位置和速度)无关。
5.4 初始化
由于紧耦合视觉 - 惯性优化是非线性的，我们需要一个良好的初始猜测来引导整个系统。我们不能直接使用IMU预积分结果作为初始猜测，因为我们不知道初始加速度和速度。 此外，IMU测量受到未知偏差的影响，尤其是陀螺仪偏差，这将极大地影响估计。
我们采用松耦合传感器融合方法来获得初始值。 我们发现仅视觉SLAM或Motion from Motion（SfM）具有良好的初始化特性。 在大多数情况下，仅视觉系统可以通过相对运动方法的派生初始猜测来自我引导，例如八点法36，五点法37，齐次和基本矩阵。 通过仅视觉SfM和IMU度量预积分信息之间的松散耦合对齐，我们可以粗略地恢复尺度，重力，速度和陀螺仪偏差。 这足以引导非线性系统。
首先，我们在滑动窗口中获得成比例位置和特征点位置。其次，我们将视觉和IMU信息对齐以校准IMU陀螺仪偏差，尺度，速度和重力矢量。 虽然第一步是非线性的，但它可以自我引导并且非常稳定。 第二步是线性形式。
在初始步骤中需要相机和IMU之间的外部参数xcb。 注意，我们这里只需要对外部参数(安装关系)进行初步猜测，并且将在非线性优化中进行准确的估计。
5.4.1 Up-to-scale visual SfM in sliding window
在此步骤中，我们尝试在窗口内构建仅视觉结构（按比例缩放相机位置和特征点位置，可以理解为归一化坐标）。
我们首先在滑动窗口中选择包含足够满足视差要求的特征点数的两个关键帧。 接下来，我们使用五点法37来恢复这两帧之间的相对旋转和按比例的平移，然后我们确定平移的尺度并对这两帧中观察到的特征进行三角测量。 基于这些三角测量特征，执行Perspective-n-Point（PnP）方法以估计滑动窗口中的所有帧位置。
然后应用全局完整的Bundle Adjustment38以最小化所有帧中重投影误差。
公式中的s为未知的尺度，将在后文讨论。
5.4.2 视觉-惯性对齐
IMU偏差初始化：
假设在现在的窗口中IMU陀螺仪的偏差bg。考虑窗口中的两个连续帧bk，bk+1，我们从视觉框架中得到两个相关的旋转qbkb0，qbk+1b0，以及从IMU预积分得到的旋转传播结果γbk+1bk。我们通过最小化这两个之间的误差来估计陀螺仪偏差(最小化视觉求出的旋转与陀螺预积分出来的结果间误差)：
其中B代表窗口中的IMU测量值。 通过求解这个最小二乘问题，我们可以得到bg的估计。 然后我们就利用bg更新αbk+1bk，βbk+1bk。至于加速度计偏差，在初始化过程中很难解决，因为当我们同时求解gb0时，需要足够的旋转运动来区分加速度计偏差和重力。 然而，给出粗略的初始猜测是足够的，因为我们将在初始化之后不断改进偏差。 因此，我们在初始化步骤中将加速度计偏差ba视为零。
速度，重力矢量和度量标准初始化（不太懂，得看一下IMU初始化）
我们定义了我们想要估计的变量
其中s是尺度参数，它将视觉与IMU测量隐式提供的实际度量标度对齐。 基于牛顿运动学，我们可以得到以下形式的等式：
注意其中的Δtk的意义是两个连续关键帧之间的时间间隔。 通过解决最小二乘问题(10)我们可以得到每个local坐标系中的速度，以及视觉坐标系中的重力矢量(·)b0，还有尺度参数。来自视觉的平移分量pbi将被缩放到单位度量。我们将所有变量从坐标系(·)b0旋转到世界坐标系(·)w，其中重力矢量是垂直的。 此时，初始化过程完成，这些度量值将被馈送到紧耦合非线性视觉惯性估计器。
5.5 视觉惯性定位紧密耦合
在状态初始化之后，我们继续使用滑动窗口非线性估计器来进行高精度状态估计。 通过在非线性优化框架中包含IMU偏差校准，这是我们早期工作16,17的扩展。
5.5.1 公式
我们最小化所有测量残差的Mahalanobis范数的总和，以获得最大的后验估计a：
5.5.2 IMU测量模型
根据运动学理论，预积分IMU的测量残差可以定义为
其中[·]xyz提取四元数q的向量部分，是error-state的近似值。
5.5.3 相机测量模型
为了利用大型FOV摄像机的优势，我们在参考文献中39提出了与鱼眼相机型号相匹配的在单位球面上定义摄像机测量残差。 如图6所示，第j个图像中的第l个特征的相机残差被定义为
5.5.4 边缘化(Marginalization)，理解为舍弃?
为了限制基于图优化的方法的计算复杂性，结合了边缘化。 我们选择性地将IMU状态xk边缘化并从滑动窗口中获取λl，同时将对应于边缘化状态的测量值转换为先验状态。
在我们以前的工作16,17我们使用了双向边缘化有选择地删除基于场景的视差测试最近的或旧的状态。 如果是关键帧，则将新帧添加到滑动窗口，并且最旧的帧状态通过其相应?(corresponding)的测量被边缘化。 否则，如果出现非关键帧，我们会将第二个最新帧边缘化。 这种边缘化方案可以在窗口中保持空间关键帧，同时限制预积分的IMU测量的不确定性。
然而，我们之前策略的一个缺点是边缘化第二个最新帧会产生密集的先验，这将破坏系统的稀疏性，增加计算复杂性。为了避免这种情况，我们稍微修改了边缘化第二个最新框架的策略。 当非关键帧到来时，我们丢弃视觉测量并保持IMU测量值对应于窗口中的第二个最新帧，而不是边缘化所有测量值。尽管我们放弃了一些视觉测量，但这些测量并不重要，因为它们对应于小范围运动，这不影响视觉。一个潜在的缺点是我们延长了预积分的时间间隔，这将导致滑动窗口中的协方差变大。 但是，这种策略可以保持系统的稀疏性，使其在计算受限的平台上高效工作。
我们基于边缘化来构建新的先验。 边缘化使用Schur补集进行。 直观地，通过边缘化，保留了被移除状态的重要信息，并且计算复杂性受到限制。
5.6 用于反馈控制的IMU传播(?propagation)
请注意，IMU测量的速度远远高于视觉测量。 我们的非线性优化估计器的频率受到视觉测量的限制。 为了有利于实时控制的性能，估计器的输出直接传播最新的IMU测量值，这些测量值用作控制环路中的高频反馈。(翻译有问题，To benefit the performance for real-time control, the outputs of the estimator are directly propagated with the newest IMU measurements, which serves as the high-frequency feedback in the control loop.)
6. 单目稠密地图
作为自主四旋翼导航的核心，建图模块提供了重建周围环境的感知能力。 这种重建是围绕障碍物安全有效导航的支柱。 本工作中使用的方法是继续我们的初步结果40。我们使用来自单目的多个图像的时间成本聚合( ?Temporal cost aggregation)来消除基线限制。 这之后是为无纹理环境去除异常值和深度传播进行半整体平滑。 我们利用NVIDIA TX1上的GPU，并使用CUDA实现我们的算法。 使用截断的有符号距离场(TSDF)融合局部深度图像，以提供直接用于轨迹规划的全局地图。
6.1 相关工作(RGB-D，双目缺点，半稠密建图缺点)
在稠密建图领域存在广泛的学术工作，并且使用各种各样的传感器，例如RGB-D相机，立体相机和单目相机。 在RGB-D相机设置中，KinectFusion41同时解决了定位和建图问题。 它使用迭代最近点（ICP）来解决相机姿态，然后通过TSDF融合重建全局地图。 参考文献42,43中提出了使用RGB-D传感器的自主飞行。 利用由Kinect构造的已知网格图，作者使用直接半模跟踪进行姿态估计。 然而，RGB-D传感器有其自身的局限性，严重限制了它们在室外环境中的使用。(受光照影响严重，因为深度是由结构光取得的)。
空间立体视觉相机是另一种流行的传感器配置。 参考文献44中提出了半全局匹配立体方法。通过结合像素成本(?pixelwise cost)和平滑约束，作者提出了一种全局能量函数来求解视差图并通过动态规划优化其近似版本。 如参考文献45中所提出的，可以使用基于机器学习的方法来进一步改善立体匹配成本。空间立体声很容易受到校准问题的影响，因为它取决于已知的相机外部参数。 此外，基于立体视觉的方法的基线要求从根本上限制了其对小规模平台的能力。
对于单目，参考文献46提供了一种自校准方法，可以在束调整框架中共同解决相机的内在3D标志点以及相机姿势。 作者通过新颖的匹配成本定义在小运动条件下生成精细深度图像，该定义通过一组图像获取候选深度之间的强度和梯度差异。 该算法显示出令人印象深刻的深度图像，但每次深度图像估计的时间消耗（10分钟）对于实时四旋翼应用是不可接受的。
DTAM47结合了数据项和正则化项，形成全局能量函数。 通过使用原始对偶(?primal-dual)方法，DTAM通过total variation有效地优化了问题。 类似的工作REMODE5，也使用原始对偶方法，并通过总变差优化问题结合了贝叶斯估计。 但是，它们都需要桌面级(desktop-grade)GPU才能实时使用。 半稠密建图(Semidense mapping)9是另一项研究。 在基于特征的稀疏映射和基于像素的密集映射之间，半稠密方法选择具有高梯度的像素，这使得整个框架在CPU上实时运行。 然而，这种像素密度不足以进行安全导航。
6.2 深度估计
如4.2节中所讨论和图4（b）所示，从鱼眼图像中提取两个无失真虚拟针孔图像，以提供180度水平FOV。 这两个图像在完全相同的算法管道中处理建立稠密地图。 为简洁起见，我们仅为其中一个提供算法流程。
我们的深度估计是根据关键帧进行的。 对于每个关键帧Ir，多个测量帧Im用于深度更新。任何时候只存在一个关键帧，并使用距离度量来切换关键帧。 如果该帧与当前关键帧之间的姿势转换大于距离阈值ζ，则帧被视为新的关键帧(图7)
受平面扫描算法48,49的启发，我们为关键帧中的每个像素采样不同深度的多个候选虚拟平面。 这些平面垂直于规范视点的z轴。 关键帧中的每个像素首先投影到虚拟平面，然后反投影到测量(measurement frame)帧。 通过测量强度差异，我们获得每个深度中每个像素的相似性成本(similarity cost)。 我们收集所有相似性并把它们放在一个3D量(?volume)里。
当存在多个测量图像时，我们可以获得每个测量图像的相似成本量。 由于所有成本量都以参考图像Ir为条件，因此我们可能会将所有成本量汇总到一个成本量中，以降低其对噪声和异常值的敏感度。 使用GPU并行实现可以加速此过程。 直观地，对于每个像素，其实际深度是对应于聚合成本量中的最小相似性成本的深度。
然后应用半全局平滑44来进行无纹理区域的异常值去除和深度插值。 抛物线拟合用作深度细化的最后一步。
6.2.1 关键帧选择(Keyframe selection)
设置距离阈值的动机是确保我们选择的每个关键帧都有足够的信息来提供可靠的深度图像。
在我们之前的工作40中，关键帧由VINS框架自动选择。 但是，存在两个限制：
a. 正如我们在5.3.1节中提到的，VINS中的视差检查是选择具有足够视差的关键帧。 但是，不能单独配置此关键帧标准以满足稠密地图的要求。
b. 为了识别附近的深度，扫描平面被预先参数化，这意味着深度感知的范围是固定的。 但我们检查VINS中的平均视差，计算远离相机的特征，并减少平均视差。 考虑到大规模环境的情况，关键帧的度量距离将变得非常大，使得由于近深度像素的溢出而观察不到附近的障碍物。
作为改进，在这项工作中，我们使用两个阈值来控制关键帧选择。 当我们没有足够的深度感知信息时，第一个阈值ι用于对抗悬停。 如果平移距离小于第一个阈值，我们将只进行聚合步骤。第二个阈值ζ针对关键帧选择。 如前所述，此关键帧阈值为适应最近的采样平面配置。 根据经验，我们将此阈值处理得足够大，以生成具有足够深度提取信息的关键帧，但又小到足以避免遮挡和近平面的深度不可观察性(足够小不可以理解)。
6.2.2 GPU加速的时间成本聚合(GPU-accelerated temporal cost aggregation)
我们采用反向深度λk对平面L进行采样，其中k属于[1,L-1]。第K个深度平面(enumerated plane)的深度是dk
绝对差之和(SAD)用于计算相似性成本。 应用固定尺寸的3×3窗口。 因此，成本可以表示为
通过首先将关键帧中的像素u'反投影到由候选平面定义的3D点，然后将该3D点重新投影到测量帧上来找到测量帧中的像素位置。 对于关键帧中的每个像素u，我们对其相邻像素重复采用该过程来计算相似性成本，如公式(16)所示。 通过聚合来自在不同时间捕获的多个测量图像的相似性成本来计算成本量ESAD。为了增加浮点像素位置um的光强精度，使用双线性插值。 这将在使用纹理存储器(texture memory)的GPU计算期间自动发生，其不消耗任何额外的计算资源。
6.2.3 半全局优化
时间成本聚合(Temporal cost aggregation)旨在通过使用多个测量来降低图像噪声的灵敏度。 但是，简单的winnertakes-all策略都不会得到可靠的结果，因为重复图案很容易发生错误。 此外，在无纹理区域中，cost volume将包含不同深度之间的类似cost，这导致深度估计的模糊性。 为此，我们的深度估计器采用了基于平滑的优化方法。
由于我们使用枚举平面(?enumerated planes)来表示深度图像，因此深度优化问题现在变成平面索引优化问题。 将K表示为我们尝试优化的平面索引地图。 将像素成本(?pixelwise cost)和平滑约束组合的全局能量函数E(K)表示为
其中Ω是关键帧的图像域，Ku是一个在像素u索引下的enumerated plane(可能是u对应深度的同深度图像)。T[·]为指标函数，若内部为真则为1，否则为0。每个像素的能量函数包括三种项：一个直接从成本量(cost volume)中提取的数据项和两个正则化项。第一正则化项P1对所列举的深度差仅为1的相邻像素进行能量惩罚。第二正则化项P2对较大的深度差给出较高的惩罚。
由于全局最小化是一个无法在多项式时间(?polynomial time)内求解的NP完全问题，因此在参考文献44中提出了半全局匹配（SGM）。在这项工作中，我们使用四路SGM，它在速度和精度之间进行了很好的权衡。
因为最后一项minjLr(u-r,j)小于前两项之和，因此它用于减小数值，当ur不变时它也不变，这保证了Lr(u,K)不受影响。因此，该问题被简化为沿r方向的一维问题。 通过使用GPU可以并行求解近似成本函数。 伪代码在算法1中示出，并且SGM计算的直观说明在图8中示出。
6.2.4 深度图像的后处理
在半全局优化之后，可以使用简单的winner-takes-all策略通过选择每个像素的最低成本S(k)来获得深度。 然而，由于深度是在离散设置下采样的，我们使用抛物线插值来获得更好的深度，这是两个深度样本之间的极值点，
其中a，b，c是抛物线的系数，Ku是平面索引(像素u对应的平面)，其在像素u处达到最低成本，Ku*是极值点处的插值平面索引(index)。 因此，可以通过du计算像素u的估计深度。
这里有两个标准用于消除异常值。 首先，对于具有一些逆深度采样的像素，我们无法计算相似性成本，因为在该反深度处的测量帧中投影的相应像素是未知的。对于那些像素u，我们在相应的反深度的3D cost volume 中将其标记为未定义，并且稍后在SGM步骤中将Lr设置为零以消除缺失成本的影响。 最后，输出深度图像处的那些像素被设置为未定义。 其次，如果它不是拟合抛物线的绝对最小值，我们会降低深度。 具体而言，必须忽略位于第0或第(L-1)平面的最小成本; 在这种情况下，像素的深度将设置为未定义。 此外，如果两个相邻平面的成本之和小于最小成本的两倍，则相应的像素也将被设置为未定义。（该部分将深度图像中的一些异常像素设置为未定义）
6.3 全局稠密地图
我们用uncertainty-aware truncated signed distance field(TSDF)法融合所有相机不同位置对应的深度图。用开源CHISEL TSDF50实现，6.3.2节改进了不确定感知深度融合(uncertainty-aware depth fusion)，6.3.3节改进了算法并行化。
6.3.1 TSDF
整个环境可以建模成一个三维立体符号距离场。 带符号距离函数Φ(x)：ℝ3→ℝ是体素x到其最近曲面的距离，如果体素在曲面和相机之间则为正，否则为负。 由于我们只对重建曲面感兴趣，因此我们使用截断的带符号距离字段：
为了获得更好的内存效率，CHISEL使用两级混合数据结构。 第一级块是空间散列映射，由Nv3体素的固定网格组成。 从增长的堆动态分配块并基于其整数坐标在空间3D哈希映射中索引。 第二级是尺寸为L×L×L的3D立方体体素，其中L是度量中的映射分辨率。 草图如图9所示。
CHISEL提供了两种深度扫描融合方案。 由于我们在深度图像估计中使用虚拟针孔相机，因此基于camera frustum的投影映射比光线投射更有效。 camera frustum是一个轴对齐的边界框，它本质上取决于相机，我们倾向于融合最近的平面和最远的平面。 我们首先检查与camera frustum相交的每个块，如果是第一次检查，我们分配块。 然后，将在更新过程中枚举块中的每个体素。(这一段不太能看懂)
在这种投影建图方法中，每个体素将被投影到相机平面上并与来自深度图像的深度值进行比较。 我们将差异表示为ex∈ℝ+; 只有当差异在截断距离[-τ，τ]内的，体素才会被更新，否则它将被忽略。 体素更新算法在算法2中详述。
6.3.2 不确定性融合(?Uncertainty-aware fusion)
由于远程视觉深度估计通常伴随着更大的深度不确定性，我们通过使用动态截断距离Ψ(d)来编码不确定性。 将σλ定义为逆深度λ中的深度测量的标准偏差。 然后可以使用线性化来近似深度测量的标准偏差d：
其中σλ设定为λmin，这是逆深度场中的最小值。 因此，截断函数是(33)，其中Struncation是一些常量截断比例。 因此，截断距离是动态的，并且随着深度观察变得更能够增加。
6.3.3 并行化
注意，整个TSDF融合计算复杂度近似等于体素更新复杂度，其与体素计数成比例。 每个体素的更新过程是独立的，这允许我们通过多线程加速过程。我们还将地图更新和发布过程分离，以保证最新的轨迹规划地图的可用性。 一旦深度图像（可以来自两个ROI中的任何一个）到来，地图更新就会运行，而地图publisher会以10 Hz的恒定频率将地图输出到轨迹规划。 我们使用互斥锁来保证共享内存不会冲突。
6.3.4 渲染可视化地图
虽然自主导航不需要视觉上吸引人的地图，但我们仍然提供了一个离线运行的地图渲染模块，以实现可视化目的。 Marching Cube算法51用于地图渲染。对于每八个体素，我们将它们的中心形成立方体，并且每个中心点的带符号距离场(SDF)值近似等于其体素的SDF值。 考虑每个SDF值的符号。 整个立方体有256种不同的可能构成，这意味着有256种不同的方法将立方体分成几个三角形网格(见图9)。 样本重建结果在图10中可视化。
7.轨迹规划
暂未看之后内容
8. 实验结果
