夏琳琳，沈 冉，迟德儒，崔家硕，蒙 跃

（东北电力大学 自动化工程学院，吉林 132012）

作为智能机器人的核心技术，自主导航为移动机器人实现各项高级功能（如人机互动、多机协同等）奠定了基础，自主导航能力也逐渐成为衡量移动机器人性能的重要指标[1,2]。传统的惯性导航系统、全球卫星导航系统很难独立完成相关任务[3]，而基于视觉的同时定位与地图创建（Simultaneous Localization and Mapping，SLAM）则既能实现高精度的导航定位，又可以实时构建周围环境的地图，为自主导航提供了新的解决方案。

经过三十余年的发展，视觉SLAM 已经逐步趋于成熟，一批开源算法相继问世，表1总结了近年经典的视觉SLAM 研究方案。这些方案主要分为两类：一类是基于图像特征点的间接法（特征点法），一类是基于图像灰度变化的直接法。特征点法依靠人工设计的局部图像特征，通过检测、匹配与追踪前后两帧图像中的特征点，来获取相机的位姿；直接法采用最小化光度误差来寻求图像中像素间的对应关系，避免了特征提取和匹配，计算效率优于特征点法，且可适用于特征缺失（弱纹理或重复纹理）的环境。

表1 经典开源视觉SLAM 算法Tab.1 Classic open-source visual SLAM algorithms

近年，视觉SLAM 已转向为以非线性优化理论为主导的批量状态估计，逐步形成了前端实时跟踪相机位姿、后端优化历史相机位姿和环境地图的两级结构。研究者一边在陆续开源的相对成熟的算法框架中获得研究上的极大便利，一边也在积极探索并提出众多工程性的解决方案。这其中，越来越多的研究者偏向于视觉SLAM 前端，即视觉里程计（Visual Odoemtry，VO）的研究。在已有的代表性成果方面，比较著名的有R.Wang与J.Engel等人提出的基于单目或双目相机的稀疏直接里程计DSO（Direct Sparse Odometry）[8,9]及C.Forster 等人提出的融合特征点法和稀疏直接法的视觉里程计SVO（Semi-direct VO）[10]。

上述VO 改进方案旨在克服相机在短时、较高速运动情况下出现的运动模糊、累积漂移及定位失败等问题。为满足移动机器人长航时精确定位和短时快速运动下稳定导航的任务需求，将视觉信息与惯性导航进行数据融合的视觉惯性里程计（Visual Inertial Odometry，VIO）已成为SLAM 工业化应用的主要方向。早期的研究者沿用机器人学中的滤波理论融合惯性测量单元（Inertial Measurement Units，IMU）与相机信息，以处理视觉SLAM 的并行状态估计问题：多状态约束卡尔曼滤波器（Multi-State Constraint Kalman Filter，MSCKF）主要通过将特征点转化为相机位姿间的几何约束，首次实现了视觉-惯性紧耦合算法[11]。文献[12]中的ROVIO（Robust VIO）算法则将图像光度误差作为Kalman 滤波器观测更新项，利用多级图像块跟踪状态，采用紧耦合方式联合估计IMU 偏差与相机的外参。由于ROVIO 采用地标参数化的逆深度估计，故不需要明显的单目迟延初始化过程。

自2013年，随着稀疏矩阵和非线性优化理论的逐步完善[13,14]，更多的学者采用非线性优化的方式来求解SLAM 问题—如S.Leutenegge 等人[15]提出的关键帧机制下的非线性优化惯性视觉里程计OKVIS（采用图像特征点检测），其通过构建联合优化IMU 误差项与地标重投影误差项的非线性函数、限制进入关键帧窗口中的状态变量规模，提升了SLAM过程的实时性。近期，香港科技大学的沈劭劼团队[16]开源了VINS-Mono 算法（前端提取并跟踪图像Fast 角点），其通过高效的初始化、紧耦合非线性优化方法得到高精度的VIO，并利用回环检测、全局位姿图优化等机制使VINS-Mono 具有良好的鲁棒性，与OKVIS 相比定位精度提高了一倍。

综上所述，目前已有的SLAM 方案主要采用相对成熟的特征点法进行图像追踪。这一方法在短时高速运动或弱纹理环境中的应用仍面临计算耗时、实时性有限等挑战，为了满足机器人载体在上述环境中的导航、定位需求，本文基于VINS-Mono 的算法框架，结合融合点、线特征的VIO 研究，提出了一种在弱纹理环境中基于光流-线特征的单目视觉-惯性SLAM 算法（Particle-Line Visual Inertial SLAM,PL-VINS）。算法首先提取图像中的Shi-Tomas 角点和LSD（Line Segment Detection）线特征，在视觉初始化的基础上，通过视觉、惯性联合初始化过程得到系统初始时刻的状态估计值；其次，构建PL-VINS 的后端优化目标函数及滑动窗口边缘化策略，通过非线性优化求解载体的位姿；在此基础上，为应对随增量定位过程不断增长的累积误差，建立回环检测和重定位机制，并通过全局位姿优化保证载体轨迹和地图的全局一致性。

1 系统框架搭建

本文以紧耦合方式，利用非线性优化方法实现视觉与惯性信息的深度融合，算法包含五部分，分别为：测量数据预处理、系统初始化、后端滑动窗口模型优化、回环检测与重定位及全局位姿图优化。PL-VINS系统算法框架如图1所示。

图1 PL-VINS 系统算法框架Fig.1 The algorithmic framework of PLP-VINS

● 测量数据预处理：分为图像处理和IMU 数据处理。图像处理时，分别提取图像中Shi-Tomas 角点及LSD线特征，利用金字塔LK（Lucas-Kanade）光流法跟踪相邻帧的特征点，通过随机采样一致性去除异常点，将有效特征点存储到图像序列中，并导入后端进行优化求解；而对于图像中检测到的LSD 线特征则利用LBD（Line Band Descriptor）描述子将其与前一帧中线段进行匹配。针对IMU 数据则进行预积分处理，从而得到当前时刻的速度、位姿。

● 系统初始化：单目视觉传感器无法直接获得准确的图像尺度信息，为此，本文将视觉与惯性联合初始化以获取尺度信息。先利用视觉SFM（Struct From Motion）求解两帧图像间的位姿及3D 点逆深度，随后通过IMU 预积分模型所产生的偏移误差去修正陀螺仪数据，并根据其与视觉运动信息的关系，构建优化函数求解出初始的速度、尺度、重力及偏置等。

● 后端滑动窗口模型优化：采用基于滑动窗口模型的非线性估计器，根据视觉约束、IMU 约束以及回环约束构建联合优化函数并求解出滑动窗口内所有帧间位置、速度、旋转及偏置等。

● 回环检测与重定位：通过DBoW 词袋模型和BRIEF 描述子进行回环检测，建立局部滑动窗口与回环候选帧之间的链接。如存在回环，则利用重定位过程维持当前滑动窗口与过去时刻的位姿图对齐，并将全部回环的位姿作为常量，利用所有IMU 测量值、局部视觉测量以及从回环中提取的特征对应值，共同优化滑动窗口，从而降低系统的累积误差及计算量。

● 全局位姿图优化：重定位之后局部滑动窗口与过去时刻位姿对齐，而视觉惯性信息可以提供横滚角和俯仰角数据，故只存在 4 个自由度（Degrees-of-Freedom，DOF）误差（即三轴位置误差和航向角误差）。只需在位姿图中添加关键帧并对其4DOF 进行优化，即可保证全局轨迹的一致性。

2 视觉-惯性信息融合方法

2.1 系统初始化

初始化是为了获取系统在后端优化所需的必要参数和载体初始时刻的状态初值，主要包括陀螺仪偏置、重力加速度、绝对尺度以及初始时刻前几个关键帧的位姿、速度和三维路标点的位置。其中，绝对尺度和重力加速度在系统运行时几乎不会发生较大的变化。PL-VINS 系统初始化主要分为两步：一是纯视觉单目初始化，利用视觉SFM 求解出初始时刻内关键帧的位姿信息以及三维路标点的位置信息；二是利用视觉-惯性松耦合联合优化出尺度、重力加速度、相机速度、陀螺仪的零偏信息。

2.2 基于滑动窗口的后端非线性优化

本文采用的是基于滑动窗口模型的非线性优化方法，即保证优化变量的个数维持在一定范围，通过滑动窗口动态地增加或移除优化变量，只让当前一段时间内的关键帧数据参与到位姿解算过程。

若将滑动窗口中第i时刻完整的状态向量定义为：

本文将线特征信息引入到视觉-惯性SLAM 中，故目标优化函数包含四类残差项，分别为边缘化的先验残差项、IMU 测量残差项、视觉重投影残差项（包括点特征重投影误差和线重投影误差）及回环检测残差项。优化目标函数形式如下：

式(3)中，B为窗口内IMU 测量集合，D和L分别为图像帧中至少两次观测的点特征和线特征的集合。代表边缘化后的先验信息，Hp是先前优化信息的信息矩阵，为IMU 预积分噪声项的协方差，、为视觉观测的噪声协方差；ρ为抑制离群点的鲁棒核函数。本文采用柯西核函数CauchyLoss 作为鲁棒核函数，即

采用合适的鲁棒核函数可以保证优化过程的平稳性，削弱异常值对状态变量优化过程的影响。需要特别强调的是，回环检测部分的残差只有在出现视觉回环且需要重定位时才将第v帧的位姿和相关特征点作为视觉约束项参与到后端非线性优化的整体目标函数中，并固定第v帧的参数用以优化窗口内其他参数。

本文的PL-VINS 系统采取双向边缘化策略来控制滑动窗口内状态变量，具体策略是：当次新帧为关键帧时，边缘化最旧帧，但将最旧帧的点线路标点和相关联的IMU 数据转化为先验信息，参与优化目标函数，即本小节提到的先验残差｛rp,Hp｝；当次新帧为非关键帧时，则直接舍掉次新帧和其对应的视觉测量值，但保留其相关联的IMU 测量值，从而保证IMU 预积分的连贯性。边缘化过程如图2所示。

图2 边缘化策略示意图Fig.2 Diagrammatic representation of marginalization strategy

2.3 回环检测与重定位

回环检测是SLAM 后端的重要一环。PL-VINS 采用BRIEF 描述子的DBoW2 词袋进行回环检测，由于系统前端提取的Shi-Tomas 角点数量有限（通常在70个左右），很难满足检测需要。为此，本文对非线性优化后的新关键帧重新提取500个FAST角点用于回环检测，同时对所有新、旧角点进行BRIEF 描述。随后，依托词袋模型计算当前帧的相似度分数，并与关键帧数据库中所有历史帧进行比对。在此基础上，进行回环一致性检测，评分大于0.015 则认为是回环候选帧。在检测到回环后，本文根据BRIEF 描述子对新、旧角点进行邻域匹配，并对匹配点进行随机一致性采样以去除匹配异常点。当匹配点的索引值大于50，则认定该候选帧为正确的回环帧。当检测到当前帧与历史帧（设为第v 帧）存在回环关系后，进行紧耦合重定位。

将回环帧的位姿和相关路标点作为视觉约束项，加入到后端非线性优化目标函数中，见式(4)。

式(4)中，ϑ为回环检测到特征集合，(i,v) 为回环帧v中观测到的第i个特征，与其他残差项不同的是，回环帧的位姿将通过下一节中介绍的位姿图获得。但如果是第一次重定位，则以过去时刻的VO 估计的位姿为当前回环帧的位姿，并将其作为优化过程的常值。

2.4 全局位姿图优化

重定位之后，局部滑动窗口移动与历史位姿对齐，进行全局图优化则是为了确保基于重定位结果对历史位姿进行全局一致性匹配。由于PL-VINS 系统中载体的水平姿态（横滚角及俯仰角）完全可测，因此，只有三轴位置和航向角这4个自由度存在累积漂移，需进行位姿图优化。当关键帧从滑动窗口中被边缘化之后，会被添加进位姿图中，该帧会作为位姿图中的一个顶点，通过下述的两类边与其他顶点相连接：

① 序列边

经过前端VIO 解得的两帧间的相对位姿为：

其中，i、j分别代表边缘化的关键帧和历史帧，、分别为两帧之间的相对位置和相对航向。第i帧和第j帧的残差可以表示为

② 回环边

对于存在回环关系的两帧，回环边只包含4DOF 的相对位姿，可通过重定位的结果得到。所有序列边和回环边整体的优化目标函数可表示为

其中，ζ代表序列边，τ代表回环边，ρ则采用Huber核函数，定义见式(8)。至此系统完成了全局位姿优化过程。

式中δ为控制误差降到多小时变为平方误差的超参数，当Huber 损失s在|s|δ≤时，等价为均方误差，否则等价为绝对误差。

3 实验与分析

本文针对PL-VINS 系统开展了基于公开数据集EuRoc 的性能评估实验，对其导航定位精度、鲁棒性等进行仿真实验分析与评价。

3.1 仿真实验环境及实验平台

图3 EuRoc 数据采集硬件系统Fig.3 EuRoc data acquisition hardware system

EuRoc 数据集的采集载体是无人机AscTec Firefly，其所测视觉及惯性数据来自人为装配的惯性视觉传感器；其轨迹真值由两个运动捕捉系统进行辅助测量，分别是Vicon 动作捕捉系统及Leica 激光追踪器。前者采集六旋翼飞行器的6DOF 位姿变换，后者负责三维位置追踪。该EuRoc 数据采集硬件系统如图3所示。

PL-VINS 算法运行的硬件设备为DELL Inspiron 15-3542 型笔记本计算机，CPU 为Intel i5-4210U@1.7GHZ×4，所用操作系统为 Ubuntu16.04LTS，PL-VINS 的整体架构搭载在机器人操作系统ROS（版本为Kinetic1.12.14）上。PL-VINS 系统以C++语言开发，系统架构参考开源算法VINS-Mono。程序主要依托四个开源库：计算机视觉库OpenCV3.3.1、矩阵运算库3.3.1、非线性优化求解库CeresSlover 以及词袋模型库DboW2。地图可视化部分采用的是ROS 自带的Rviz三维可视化工具。

3.2 PL-VINS 仿真实验与分析

3.2.1 点线特征

为展现PL-VINS 系统运行过程中对点、线特征的提取和跟踪情况，本文截取了运行中的两幅追踪图像，如图4所示。

图4 点线特征跟踪的效果Fig.4 Point and line feature tracking performances

图4中颜色越红代表跟踪的时间越长，图中存在一定量的红色线段，而且是点特征所无法有效跟踪的部分，由此可见点、线融合的特征跟踪可以使SLAM 算法获得更丰富、更有效的特征信息。

3.2.2 导航定位精度

实验选用的EuRoC 数据集主要为MH_01_easy 运动场景序列。鉴于PL-VINS 是单目视觉，本实验仅采用双目摄像机左相机拍摄图像。在同等软硬件条件下，独立运行开源算法 VINS-Mono 和本文提出的PL-VINS，将二者结果进行对比，评价PL-VINS 系统的导航定位精度。

图5为PL-VINS 运行时通过Rviz 可视化界面观测到的载体飞行轨迹。图中左上部显示的是回环检测情况；左下部是对当前帧提取出点、线特征，进行跟踪显示的情况，其中，点、线越红则说明该特征追踪的时间越长，属于良好的路标点；图的右侧则实时展现真实轨迹与估计轨迹之间的差异情况。

图5 PL-VINS 系统在Rviz 中的可视化界面Fig.5 Visual interface of PL-VINS in Rviz

图6为PL-VINS 系统在MH_01_easy 序列上的绝对位姿误差（APE）。读取颜色条量化结果可知，平均误差在10 cm 左右，精度上相对良好。

图6 PL-VINS 在MH_01_easy 序列上的三维绝对位姿误差Fig.6 3D absolute pose error of PL-VINS on MH_01_easy sequence

图7 MH_01_easy 在X-Z 轴向投影的轨迹误差Fig.7 MH_01_easy trajectory error in X-Z axial projection

为进一步细化轨迹误差，将图6中的轨迹投影到X-Z 轴所在的二维平面进行细致观察，如图7所示。从图中可以看出，当载体突然转向或是快速运动时，造成了PL-VINS 估算的位姿出现偏差，即误差水平平均在40 cm 以内，在平缓运动时误差可以缩小到1.6 cm，整体来看，PL-VINS 可以较为鲁棒地完成位姿跟踪任务。

图8为三轴位置误差，蓝线为系统输出随时间变化的轨迹，虚线为轨迹真值，两条线重合会显示蓝色。可以看出，图中大部分的预测值与真值重合，说明整体上系统输出的信息较为准确。同时可知，主要误差出现在Z 轴，这是由于相机在此时发生了跟踪较少特征点的模糊运动，而IMU 受偏置和噪声影响亦输出了不精确的导航参数，二者此时无法实现有效功能互补，导致精度下降，但由于全局优化的作用，误差在整体上并未发散。

图8 三轴位置误差Fig.8 Three-axis position error

图9为PL-VINS 系统与开源VINS-Mono 系统的导航误差统计特性对比图，量化指标包括均方根误差、误差中值、误差均值、误差极值及误差平方和。如图9（a），PL-VINS 在均方根误差（控制在25 cm 以内）、误差中值及误差均值上的表现明显优于VINS-Mono，但在误差极值表现上不如VINS-Mono，究其原因，主要是线特征的引入虽然降低了累积误差，但却导致了误差的趋向多极化，即线特征带来的误差形式与点特征并不一致。图9（b）给出了PL-VINS 与VINS-Mono 的绝对位姿误差柱状统计特性比较，不难发现，在整体上，不论是误差分布范围还是误差分布趋势，PL-VINS 都明显优于VINS-Mono。

图9 PL-VINS 与VINS-mono 在MH_01_easy 序列上的误差统计特性比较Fig.9 Error statistic property comparisons between PL-VINS and VINS-Mono on MH_01_easy sequence

进一步选取两个不同难度（medium、difficult）的运动场景序列进行实验，对所设计的PL-VINS 系统的鲁棒性进行验证。对比全局轨迹误差及APE 各项参数指标，结果分别如图10、图11所示。

图10 MH_03_medium 序列情形Fig.10 MH_03_medium sequence case

图11 MH_05_medium 序列情形Fig.11 MH_05_medium sequence case

根据上述结果图可知，PL-VINS 在不同难度数据集上均有良好的表现，特别是在MH_03_medium 序列上，PL-VINS 展现出较VINS-Mono 全面优异的性能。但随着数据集运行难度的上升，PL-VINS 与VINS-Mono 之间的精度差距逐渐减小，以至于在MH_05_difffcult 序列上，两者的位姿估算精度几乎相当。根据MH_05_difffcult 的场景特点—载体快速运动、场景昏暗，可以初步认为在快速运动且光照不利的条件下，PL-VINS 受到了很大的挑战，这主要是由于PL-VINS 采用了对光照变化比较敏感的光流跟踪法，且快速运动本身不利于点、线追踪。但即便在上述快速、非理想光照的天然不利条件下，PL-VINS 依然与VINS-Mono 精度相当，达到了本次设计的要求。

根据表2及表3的实验数据，可得到如下结论：PL-VINS 将线特征引入到VINS 中，可有效提升系统导航的定位精度（APE 均方根统计值均低于VINS-Mono），且在快速运动及昏暗场景等不良条件下均有良好表现。

表2 EuRoc 数据集上PL-VINS 绝对位姿误差的统计特性（单位：米）Tab.2 The absolute pose error statistic property of PL-VINS in EuRoc datasets（Unit:m）

表3 VINS-Mono 与PL-VINS 在EuRoc 数据集上的APE 均方根误差比较（单位：米）Tab.3 APE mean square error comparison of VINS-Mono and PL-VINS in EuRoc datasets（Unit:m）

4 结 论

本文提出一种基于光流-线特征的单目视觉-惯性SLAM 算法PL-VINS，有效抑制弱纹理环境下视觉SLAM 运行不稳定及长航时累积误差问题。通过将回环检测误差、惯性误差及点线视觉重投影误差整合，构建目标优化函数，以及制定特定的边缘化策略，实现视觉SLAM 的后端非线性优化；通过采用BRIEF描述子的DBoW2 词袋进行回环检测并进行4DOF 的位姿图优化，保证系统长期运行时全局轨迹与地图的一致性。

通过开源数据集EuRoc 进行PL-VINS 性能评估测试，可以得到如下结论：

（1）将线段特征提取、Shi-Tomas 角点提取及光流法相结合是可行的，算法能够提取到更多的环境特征，在走廊等弱纹理环境下可以提高其对环境的适应性，增强系统的鲁棒性；

（2）在点-线SLAM 中引入回环检测及重定位环节，并将回环检测残差加入到优化目标函数中可有效提高算法的定位精度，确保了长航时全局轨迹的一致性；

（3）所设计的PL-VINS 算法在导航定位精度上要高于VINS-Mono，且增强了VINS 在弱纹理环境下的适应性，即在快速运动或昏暗场景等不利的条件下，PL-VINS 仍能正常工作且定位精度与VINS-Mono 相当，这为后续视觉SLAM 研究提供了一定的技术支持与模型参考。