基于点线特征视觉惯性融合的机器人SLAM 算法

王立玲，朱旭阳，马 东，王洪瑞

（1.河北大学电子信息工程学院，保定 071002；2.河北大学机器人技术研究中心，保定 071002）

同时定位与地图构建（Simultaneous Localization and Mapping,SLAM）指机器人通过自身传感器摄像头等对其周围未知环境连续感知，构建环境地图信息，并自主完成自身定位[1]。近年来，很多实时基于特征的SLAM 算法逐渐被提出，并有了很大的进步，例如，文献[2]中的单目视觉惯性融合状态估计器（VINS-Mono）算法在前端视觉里程计中采用精确的初始化信息，使用特征点方法并紧密融合惯性传感器（Inertial Measurement Unit,IMU）进行机器人位姿估计。基于ORB-SLAM（Oriented Brief-SLAM）算法提出采用点和线特征的实时单目视觉 SLAM 系统（PL-SLAM）[3]实现视觉里程计从连续三帧图像信息中检测线特征，利用历史帧信息估计运动，定位效果更鲁棒。结合点和线段特征的半直接法单目视觉里程计（PL-SVO）[4]，在光照条件下具有较好性能，将特征法与直接法结合，是半直接法点线特征融合的代表。采用点和线特征单目视觉里程计与惯性紧耦合算法（PL-VIO）[5]，将视觉里程计与IMU 融合估计机器人位置信息，通过最小化成本函数来优化状态，该方法在机器人定位上取得了不错的效果。基于实时优化的点和线特征的单目惯性融合方法（PL-VINS）[6]，通过研究参数调整和长度抑制策略，提出了一种改进线特征（LSD）提取算法，改进算法运行速度至少是传统LSD 的三倍，提升了系统运行速度。基于点线面三类特征融合的视觉SLAM 方法（PLP-SLAM）[7],实现三维地图中两条共面的线段特征作为面特征来采集环境信息，利用更多特征信息来定位。但是目前的采用线特征视觉里程计定位算法中线段提取与匹配精度较差，存在特征扎堆及误匹配现象，在弱纹理环境容易出现机器人定位失败情况。

弱纹理环境[8]如存在大面积白墙、天花板的室内及楼道等特征较为单一的环境，不能提取足够图像特征点，机器人定位失败情况常常发生。本文针对目前的视觉惯性融合（VI-SLAM）系统在场景纹理缺失环境下特征提取效果较差，容易出现机器人定位失败情况，提出了一种基于点线特征视觉里程计与惯性传感器相融合的SLAM 算法（A SLAM Algorithm for Visual Inertial Navigation based on ORB and LSD,OL-VISLAM 算法）。

本文主要研究内容包括：

1) 视觉里程计部分，对视觉图像信息提取点线特征，改进边缘检测（Canny）算法，采用自适应加权提取点线特征并使用普吕克坐标法表示线段[9]，直线特征初始化及空间变换更具有鲁棒性。

2) 针对特征提取容易出现特征堆积造成误匹配现象，在构建图像金字塔时点线特征提取引入四叉树管理特征法，实现点线特征均匀提取；提取预处理阶段，使用随机抽样一致（Random Sample Consensus,RANSAC）算法筛选出割裂线段并进行合并。

3) 针对视觉里程计由于纹理信息不足或噪声干扰图像信息陷入局部极小值[10]，无法确保足够精确初始估计的问题，将视觉点线特征信息与IMU 紧耦合实现鲁棒估计[11]，并引入点线特征闭环检测，有效减轻帧间累积误差造成的轨迹漂移。

1 算法结构流程

本文所提算法在ORB-SLAM3 算法（An Accurate Open-Source Library for Visual,Visual–Inertial and Multimap Slam）结构基础上引入了视觉线特征信息，点线特征融合IMU 用于机器人定位。本文系统有几个模块，包括传感器提取图像信息和IMU 信息、前端视觉里程计、后端优化、视觉点线特征联合闭环检测和地图构建。整个系统的框图如图1 所示。

图1 算法框架Fig.1 Algorithm frame diagram

其中视觉里程计采用点线特征紧密融合IMU 来估计机器人轨迹，为视觉惯性数据联合优化过程提供良好初始参照。建图部分由3D 地图中反投影的关键帧和关键帧所对应的地图点、地图线组成，采用光束平差法进行局部位姿优化。最后进行点线特征回环检测，并实时优化更新视觉信息、IMU 状态信息，鲁棒估计机器人位置信息从而进行地图构建。

2 SLAM 问题描述

2.1 机器人运动状态模型

机器人在三维世界运动时，SLAM 问题是一个状态估计问题，其系统状态方程定义为：

其中，Xr（k）=[x r（k）,y r（k）,θr（k）]T表示机器人在世界坐标系中的位姿方程，根据笛卡尔右手坐标系定义x r（k）为世界坐标系水平方向坐标值，yr（k）为垂直方向坐标值，θr（k）为水平方向与垂直方向所成夹角。M（k）为视觉里程计提取点线特征得到的环境路标方程。

假设机器人在k-1时刻机器人操作系统（Robot Operating System,ROS）节点控制输入量为线速度v(k-1)和角速度ω(k-1)，相邻两帧时间间隔为ΔT，则机器人在k时刻运动方程为：

理想条件下机器人所处三维环境中路标点均为静止状态，设机器人系统SLAM 过程中采集到的三维环境路标状态方程为：

综合运动方程和运动中三维世界环境路标方程，机器人系统状态模型为：

其中，X(k-1)为k-1时刻系统状态方程；q(k-1)为机器人运动时产生的高斯白噪声，其均值为0，协方差为Q(k-1)。

2.2 线特征表示

本文视觉里程计提取线特征后，采用普吕克坐标法（Plücker）表示线段特征[12]，该方法使用法向量和方向向量的形式在线特征初始化和进行空间变换时很方便。三维空间中直线L在Plücker 坐标系下表示为L=(nT,dT)T∈R6（包括方向向量n∈R3，垂直向量d∈R3）。直线上截取两个端点p1=(x1,y1,z1,ω1)T和p2=(x2,y2,z2,ω2)T，则直线可由式(5)表示（这里直线表示定义是8 维的，后面实际使用过程中为6 维）：

设世界坐标系直线Lw转换为相机坐标系下Lc的信息变换矩阵为，其中Rcw为世界坐标系到相机坐标系的旋转变换矩阵，t cw为世界坐标系到相机坐标系的平移变换矩阵。线特征投影矩阵Kt将相机坐标Lc转换到线段像素平面lc。定义Plücker 线特征坐标变换为：

图2 Plücker 坐标法表示线段Fig.2 Plücker coordinate method to represent line segments

2.3 线特征的提取

针对视觉里程计传统线特征检测算法特征提取出现线特征割裂和位置偏差，而且在边缘密集处会产生误检，不能鲁棒地用于机器人位姿估计。本文改进传统线段检测算法（Line Segment Detector,LSD），针对线段割裂、相交问题，引用线段距离、角度差异并结合描述子方向将梯度相近像素点合并，采用加权提取点线特征并使用普吕克坐标表示线段信息，引入描述子距离、方向信息合并线段特征有效解决线段割裂问题。改进的线段特征提取效果如图3 所示。

图3 改进的线特征提取Fig.3 Improved line feature extraction

3 点线特征视觉惯性融合SLAM 系统

3.1 特征提取与处理

视觉里程计特征提取部分，针对特征堆积问题，利用图像特征四叉树分割算法对图像进行分块提取，然后划分点线节点以实现特征均匀提取，使得每个节点中保存的特征点和特征线性能是该节点所有特征中最好的。经过密集筛选有效减轻特征堆积问题，减少算法计算量并提高了特征匹配精度。其中，四叉树算法特征存储思路如图4 所示。

图4 四叉树法管理点线特征Fig.4 Quadtree method to manage point line features

3.2 剔除特征误匹配

在图像点线特征提取阶段，通过构造图像金字塔并均匀提取点线特征，使用LK 光流法（Lucas Kanade Method）匹配跟踪特征点和特征线[14]。针对图像角点、边缘密集提取特征时会产生误匹配，使用RANSAC算法过滤匹配点线特征异常值，进行点线特征随机抽样一致性过滤，有效减轻误匹配，筛选出线特征提取出现的割裂线段并进行合并，减少系统耗时并消除点线特征误匹配。

线特征过滤匹配通过寻找一个最佳单应矩阵H（Homography）来消除误匹配，定位算法计算线特征单应矩阵H中的8 个参数，至少包含机器人采集2 组匹配直线端点对，得到式(8)所示方程：

其中，[x,y,1]表示相机采集到线段端点的位置，[x',y',1]为三维世界线段端点位置，尺度参数为s。本文利用这个提取模型测试点线特征数据，并计算线段投影误差，若对应代价函数最小，则此模型为系统最优模型。代价函数定义为：

3.3 修正线特征重投影误差

本文对线段特征进行参数化，传统线特征由空间上两个端点位置描述。但三维空间映射线段重投影误差不能仅重投影空间线的端点位置来计算，因为机器人运动过程中端点可能会沿线特征方向位移或因运动采集图像信息时被遮挡[15]。因此，本算法通过重投影3D 直线端点与视觉里程计检测到的线段之间点到线距离，并引入线特征描述子方向、角度差异来计算线段重投影误差。定义第ith帧和第jth帧机器人视觉传感器观察到三维直线段l的重投影误差为：

其中，π表示投影平面，Si,k和Ei,k是世界坐标系中采集到线段端点，li,k为机器人运动时在第kth帧中视觉检测到线段ith的归一化线系数，线段方程的归一化线系数为：

3.4 单目视觉点线信息与IMU 融合

机器人沿着视觉图像梯度方向运动时，很容易由于图像自身的非凸性或噪声干扰陷入局部极小值中，无法确保足够精确初始估计。因此，本文将IMU 惯性信息与视觉点线特征紧耦合来定位，IMU 姿态解算深度信息为点线特征位姿估计提供较为精准的初值信息和位置参照[13]，并实时校准点线特征三角化深度信息。其中视觉惯性地图合并原理如图5 所示。

图5 地图点线与IMU 融合Fig.5 Fusion of map points,lines and IMU

机器人估计了新的点线特征关键帧和匹配地图点、线相对位置，使用局部窗口来保存运动位置信息。在窗口集中搜索点线数据关联，点线特征位置识别成功后，在点线关键帧Ka、活动地图Ma和机器人轨迹构建地图集Mm中存储空间匹配关键帧Km数据关联时，使用对齐变换Tam进行三维地图点线合并操作。如果局部点线地图构建成功，对齐变换Tma∈SE（3）将地图Ma包含在融合窗口中。如果点线特征活跃地图并未成熟，将使用有效的Tma∈Sim（3）与地图Ma进行特征对齐，实现视觉惯性融合。其中，SE(3)为相机位姿变换三维特殊欧式群，Sim(3)为三维相似变换矩阵。

3.5 点线联合闭环检测

闭环检测有效减少机器人位姿估计中视觉信息漂移误差，视觉点线特征长期匹配使用位姿图优化来重置机器人轨迹漂移和闭环校正。对局部地图中点、线关键帧之间位姿信息，通过优化状态图进行连续性校正实现闭环检测。本算法基于循环闭环检测关键点和关键线应用词袋模型法[16]，一旦选择关键帧，ORB 描述子和LBD 描述符将用于计算单词向量。对于点线关键帧sk，定义sk=ωpsp+ωl sl，其中：

式中，sp、sl分别为点特征、线特征对应的关键帧，np和nl分别为视觉提取关键点、线的数量，dp、dl分别为关键点、关键线的x、y坐标标准偏差，ω p和ωl分别为词袋模型中单词向量的权重系数。将关键点线在并行线程中使用完整的全局光束平差法（Bundle Adjustment,BA）优化，优化机器人运动位姿、3D 点、3D 线和IMU 偏差，实现系统闭环检测。

4 实验结果及分析

本文主要展示本算法使用点线特征紧耦合IMU在弱纹理环境中对机器人SLAM 系统的积极作用。为验证本算法定位有效性，通过公开数据集EuRoC 和真实弱纹理环境性能评估实验，对系统定位精度、鲁棒性等进行分析与评估。所有实验在64 位Ubuntu1804操作系统上搭建ROS 环境中进行。

4.1 弱纹理环境下特征均匀提取与匹配实验

针对单目视觉里程计提取点线特征采用基于四叉树管理特征法实现特征提取均匀化，减轻特征堆积问题，实现鲁棒定位。数据集及真实实验弱纹理环境点线特征提取效果如图6 所示，在结构较为单一的白墙、天花板、平滑桌面等弱纹理环境中特征提取效果良好。

图6 弱纹理环境中点线特征均匀提取Fig.6 The uniform extraction of point and line features in weak texture environment

此外，特征匹配通过RANSAC 算法过滤点线匹配异常值，剔除局部特征密集区域，实现精准位置估计。改进前后的线特征匹配效果如图7 所示，传统的线特征提取算法易出现特征扎堆及存在误匹配如图7(a)所示，图中弱纹理的白色机器人外壳头部线特征匹配到了肩颈部。改进的线特征匹配加入了过滤误匹配机制，减小计算量的同时实现精确线特征匹配。

图7 线段匹配Fig.7 Line segment matching

4.2 EuRoC 数据集评估

为验证本算法定位效果鲁棒性，本文使用EuRoC数据集进行多组对比实验。将本文算法与传统视觉SLAM 算法通过绝对轨迹均方根误差(Root Mean Square Error,RMSE)进行对比，定量分析本算法的性能特性，对比结果如表1 所示。

表1 绝对轨迹误差（单位：米）Tab.1 Absolute trajectory error（Unit: m）

表1 表明本算法在不同数据集取得较好效果，并且大多数情况下精确度优于传统算法，点线特征紧耦合IMU 在一定程度上提升位姿估计鲁棒性，满足实际定位需求。数据集中存在大面积弱纹理环境如图6(a)，像MH01（Machine Hall 01）数据集中机器设备平滑的表面、管道表面，Vicon Room 数据集中室内简约的家具如单调特征的门、柜子、垫子等点特征提取较为稀疏。数据集实验表明，通过提取线段特征可以提取大量有效信息从而更加直观描述弱纹理环境的几何结构，提高后续视觉里程计定位精度。数据集评估体现弱纹理环境增加线特征实现高鲁棒定位效果。

为验证本文算法OL-VISLAM 相较于传统定位算法稳定性，将本文定位算法与改进前的ORB-SLAM3及经典算法采用EuRoC 中MH01 数据集进行定位，效果轨迹对比如图8 所示。

图8 定位精度对比Fig.8 Positioning accuracy comparison

如图8 红色箭头标注，本文OL-VISLAM 算法应用线特征在弱纹理环境中具有更优的稳定性与鲁棒性，实验结果表明机器人转弯过程中采用本算法相比于传统算法实现了更加精准的定位。图中本算法位姿估计轨迹与真实轨迹基本一致，满足SLAM 需求。

采用大量弱纹理环境的MH05（Machine Hall 05）、V1_03（Vicon Room 03）数据集验证算法位姿估计精度，生成的轨迹及三维线特征地图信息如图9 所示。其中线特征提取效率良好，直线特征建图效果鲁棒。

图9 线特征建图Fig.9 Line feature mapping

在一定灯光干扰、快速移动的参数摄动下，进行SLAM 系统特征提取效果鲁棒性对比实验，实验结果如表2 所示。在室内灯光干扰情况下，对比实验中改进后线特征提取鲁棒性提高了22.70%，快速移动对比实验中改进后线特征提取鲁棒性提高了18.72%，大型弱纹理场景中线特征提取鲁棒性提高了12.94%。

表2 线特征提取鲁棒性对比Tab.2 Comparison of robustness of line feature extraction

4.3 弱纹理场景实验

为验证本算法实时性与鲁棒性，采用移动机器人平台XF_ROBOT_UCAR01（讯飞研发机器人平台），如图10 所示，CPU 为四核ARM Cortex_A57 MPCore处理器，机器人平台采用帧率为30 Hz、焦距为2.8 mm单目相机，并装载姿态检测模块等传感器。

图10 实验平台Fig.10 Experimental platform

为分析基于点线特征视觉里程计在弱纹理环境中机器人定位的性能，设定机器人运行速度为0.3 m/s，旋转速度为0.32 rad/s，在大面积白墙、天花板等弱纹理环境进行视觉里程计定位精度测评，将只采用点特征的视觉里程计（Visual Odometer,VO）与改进前后点线特征视觉里程计（Visual Odometer with point and line feature,PLVO）及是否引入回环进行对比实验。

SLAM 精度测评使用绝对位置误差（Absolute Position Error,APE）用于整体评估定位轨迹全局一致性。设机器人运动过程中动作捕捉仪采集到的轨迹坐标信息为真实值，分别利用算法在改进前和改进后的定位轨迹APE 的均值进行对比，对比结果如表3 所示。

表3 改进前后特征匹配时间与精度对比Tab.3 Comparison of feature matching time and accuracy before and after improvement

表3 表明，视觉里程计使用点线特征可以有效提高算法定位精度，且改进后的点、线特征匹配减少特征堆积、降低图像匹配时间的同时具有良好效果。表3 中APE 均值表示定位算法得到的轨迹坐标值在允许误差[13]范围内与动作捕捉仪得到的真实轨迹的对比。传统PLVO（传统ORB-SLAM3 算法引入传统LSD 线特征检测）与改进后未引入回环检测的数据进行对比分析，改进后未引入回环的两帧图像点线特征匹配算法节约19.2%的耗时，传统PLVO 与改进后引入回环检测算法的APE 均值进行对比分析，整个系统定位精度提高约63.64%，验证了算法可行性。

未引入闭环检测条件下，特征匹配算法改进前后对比效果如图11(a)，存在误匹配时机器人大幅度转向定位失败，剔除误匹配后定位精度显著提高。改进后点线视觉里程计定位效果如图11(b)，弱纹理环境中，开环点线视觉里程计优于传统只采用点特征定位，定位效果更精准，验证算法可行性。

图11 视觉里程计定位对比实验Fig.11 The comparative positioning experiment of visual odometry

为分析视觉里程计点线特征融合IMU 方法在弱纹理环境中机器人定位性能，依据上述实验条件，在大面积白墙、天花板等弱纹理环境室内进行定位实验。分别进行纯视觉里程计的点特征、点线特征、点和IMU 融合与本文算法进行对比实验如图12(a)所示，本算法与真实轨迹对比如图12(b)所示，结果表明本文算法在弱纹理环境定位性能提升较为明显，定位鲁棒性高。

在室内弱纹理环境下，本文算法与真实轨迹基本一致，在10 cm 的允许误差范围内[13]，对本算法定位数据和动作捕捉仪得到的真实机器人位置数据进行对比得到APE 结果，如图12(c)所示，在室内定位实验中，本算法APE 的均值为0.0315 m，传统算法APE的均值为0.0729 m，相较于传统ORB-SLAM3 算法定位精度提高了56.8%。大型弱纹理环境定位实验如图12(d)所示，本算法APE 的均值为0.0336 m，传统算法APE 的均值为0.0756 m，相较于传统ORB-SLAM3算法，定位精度提高了55.6%。弱纹理环境中特征匹配鲁棒性高，能够满足实际机器人定位需求。

图12 定位效果对比实验Fig.12 Comparative experiment of positioning effect

5 结论

本文提出一种基于点线视觉特征与惯性传感器融合的机器人SLAM 算法，有效提高弱纹理环境下系统定位精度和鲁棒性。通过四叉树法均匀提取图像点线特征构建视觉里程计，并与IMU 紧耦合从而实现精准定位。所提算法在标准数据集和机器人平台中分别进行测试和实验，结果表明，改进后线特征提取相较于传统线特征提取鲁棒性提高了12.94%，在大型弱纹理环境下，该方法相较于传统ORB-SLAM3 算法，绝对定位精度提高了55.6%，有效提高了机器人定位精确度和鲁棒性，且能够满足机器人实时定位的需求，为后续机器人避障导航提供了有力保障。