一种点线特征融合的双目同时定位与地图构建方法

蒋 林， 夏旭洪， 韩 璐， 邱存勇， 张 泰， 宋 杰

(1.西南石油大学电气信息学院，成都 610500；2.中国石油华北油田通信有限公司，任丘 062550)

同时定位与地图构建技术(simultaneous localization and mapping，SLAM)最早起源于机器人领域，是移动机器人实现自主导航和定位的核心技术之一。如今更是被用于增强现实/虚拟现实(AR/VR)、无人机、自动驾驶等领域。SLAM技术主要用来解决两个核心问题：一是构建未知环境的模型，二是对机器人进行定位[1]。近年来,随着计算机视觉的发展和视觉传感器的广泛应用，SLAM问题的解决方案也越来越多样化。陈超等[2]对基于Kinect视觉传感器和激光传感器信息融合的移动机器人进行了研究。将Kinect视觉传感器获取的深度图像转换为三维点云，通过过滤点云、投影及转化为激光扫描数据等操作，与激光传感器的扫描数据进行数据级的信息融合，并使用Gmapping算法进行二维地图构建，但该方法只能构建二维环境地图，且激光传感器价格昂贵。

目前，主流的纯视觉SLAM方法大致可以分为三类[3]:基于滤波器、基于关键帧优化和基于直接跟踪法。其中，基于关键帧优化的视觉SLAM通常需要进行特征点的提取与匹配，根据特征点的匹配关系计算相机运动，最后运行集束调整(bundle adjustment， BA)对相机位姿和三维路标进行优化，因此对环境特征的丰富程度和图像质量十分敏感。然而，在低纹理环境中通常很难找到足够数量的可靠点特征，导致跟踪失败。在人们生活环境中的建筑物、街道、走廊、办公室等都具有明显的线条特征,因此基于线特征的视觉SLAM算法逐渐受到关注。Zuo等[4]在ORB-SLAM2的基础上设计了一种点线综合特征的SLAM方法，并推导了后端优化过程中残差的雅可比矩阵。Lu等[5]设计了一种融合点和线特征的RGB-D视觉里程计，该算法从RGB-D数据中提取3D点和线。还证明了融合这两种类型的特征在运动估计中产生的不确定性小于在其工作中单独使用任何特征类型。但该方法不具备SLAM中重要的回环检测与校正环节，不是一个完整的SLAM系统。

为此，设计一种基于点、线特征融合的双目SLAM方法。采用基于关键帧优化的框架，改进了跟踪线程，并利用点线特征共同构建环境地图。从而改善定位精度提升鲁棒性。基于特征提取的SLAM算法中，一般特征提取和跟踪是顺序执行的，即完成特征提取后依次进行匹配、跟踪、位姿估计。特征提取会耗费较多时间，从而影响算法实时性。分离出特征提取线程，即对当前帧进行运动估计的同时特征提取线程继续进行下一帧的特征提取。最终实验表明，本文方法具备较好的实时性。

1 特征提取与运动估计

视觉SLAM的前端，主要完成特征的提取与匹配，并实时进行位姿求解，得到粗略的运动轨迹。

1.1 特征提取与匹配

选用ORB(oriented FAST and rotated BRIEF)算子来提取特征点，该算子具有良好的旋转不变性、尺度不变性和抗噪能力，其描述符的二进制性质可以实现快速、高效的特征匹配。为了确保特征点的均匀分布，对图像进行分块，根据小区域图像的纹理丰富程度设置不同阈值，分别提取ORB特征。进行双目匹配时为了尽可能的剔除局外点，采用如下策略。①左图与右图对应点互为最佳匹配;②匹配点的描述子距离必须小于设定阈值;③最优匹配与次优匹配的描述子距离比小于0.5。

线特征的提取算法采用LSD(line segment detector)算法[6]并计算了每条线段的LBD(line band discriptor)[7]描述子。LSD算法提取速度快、不需要调整额外的参数，并且每条线段都具有主方向，在进行线段匹配时可以排除一些误匹配对。在双目匹配与帧间匹配过程中，对线段特征采取类似点特征的处理方式以确保线段匹配的有效性。

1.2 空间直线参数化

空间直线具有四个自由度，不像空间点可以很容易利用四维齐次坐标表示，使用过参数化的表示方法会带来多余的自由度及约束条件。设空间直线经过点X1=(x1,y1,z1,r1)和X2=(x2,y2,z2,r2)，在如图1所示的Plücker坐标表示下，空间直线L可以用一个六维向量来描述：

(1)

(2)

式(2)中：Rcw和tcw分别表示坐标系的旋转和平移变换;Lw和Lc分别为直线在世界坐标系与相机坐标系下的坐标表示；nc和vc为相机坐标系下构成Plücker坐标的两个向量。

图1 Plücker坐标表示Fig.1 The Plücker line coordinates

由于Plücker坐标表示方式存在约束v⊥n，不利于后端的非线性优化过程，若直接进行优化，则在优化过后无法保证该约束条件。Bartoli等[8]提出了一种最小参数正交表示方法，通过对Plücker坐标的正交三角分解(QR分解)得到:

(3)

1.3 运动估计

SLAM系统中，根据参考帧与当前帧之间的特征匹配关系，最小化特征的重投影误差来求解相机的运动。记由参考帧到当前帧的位姿变换矩阵为T，对应的李代数为ξ，则特征点的重投影误差可描述为

(4)

(5)

此时参考帧到当前帧的位姿变换ξ*可由ξ的最大似然估计给出。假设所有的观测数据均受0均值的高斯噪声所影响，此时对ξ的最大似然估计ξ*等价于求解以下的最小二乘问题。

(6)

式(6)中：Ni、Nj分别代表参考帧到当前帧的点和线特征的匹配数量;ΩΔpi、ΩΔlj分别表示点和线特征观测的协方差矩阵;ρ为鲁棒核函数。重投影误差Δp和Δl相对于位姿增量δξ的雅可比矩阵如式(7)、式(8)所示[4]:

(7)

(8)

式中：pc和lc分别为点特征与线特征在像素坐标系下的表示;Lc为线特征在相机坐标系下的Plücker坐标表示。

为了尽可能的剔除匹配异常值对位姿估计的影响，迭代结束后，将误差大于阈值σ的全部局外点剔除，对所有剩下的内点重新进行位姿估计，得到的结果作为运动估计的最终结果。

运动估计完成后采用文献[9]的方法选取关键帧。记上一关键帧为i，当前帧为i+u，计算α:

(9)

其中，

h(ξ)=3[1+ln(2π)+0.5ln|Ωξ|]

(10)

式中：Ωξ为运动估计的协方差矩阵，当α>0.9时将当前帧选取为关键帧。

2 地图构建与优化

利用视觉里程计进行运动估计时仅估计了两个相邻时间上的帧间运动，没有考虑与其他关键帧之间的联系，其误差较大且存在累计误差。对局部地图进行优化可以减小误差，检测到闭环后可以用闭环约束来降低系统的累计误差。

2.1 局部地图构建与优化

局部地图包括当前关键帧和共视图中与当前关键帧直接相连的关键帧，以及这些关键帧所观测到的三维点和线的集合。插入新的关键帧后，运行集束调整对局部地图进行优化。记ψ为所有需要被优化的变量，包含相机位姿，特征点以及空间直线的三维坐标。通过最小化关键帧和路标的重投影误差，来提炼最优的相机位姿和三维路标信息，即：

(11)

式(11)中：Kl为局部地图中关键帧的集合；Pl为局部地图中三维路标点的集合；Ll为局部地图中空间线段的集合;eij为第j个路标点在第i帧的投影误差;eik为第k条空间线段在第i帧的投影误差;ρ为鲁棒核函数。

其中误差项关于位姿的雅克比矩阵同式(7)、式(8)，点特征重投影误差关于3维世界坐标pw的雅可比矩阵为

(12)

线特征重投影误差关于空间直线正交增量δθ的雅可比矩阵为

(13)

式(13)中:Lw为线特征在世界坐标系下的Plücker坐标表示。

BA优化的复杂度将随关键帧数目增加而增大，从而导致运算时间加长，占用内存更大。因此，如果某一关键帧的所有特征的90%能够被不少于3个关键帧所观测到，那么将该关键帧从地图中删除。同时，如果地图中的某一特征被少于3个关键帧所观测到，则认为该特征不够稳定，也将删除该特征。

2.2 闭环检测

当插入新的关键帧后，分别利用点、线视觉词典[10]将其转换为词包向量vp、vl。利用词包向量在地图中搜索与之具有相同词汇的所有关键帧{Ki}，并分别计算点、线的相似性分数{spi,sli}，最后对分数进行加权得到最终的相似性评分：

si=λspi+(1-λ)sli

(14)

采取ORB-SLAM2[11]相似的策略寻找闭环关键帧。成功检测到闭环后，则对当前关键帧和闭环帧进行特征匹配，并求解当前帧与闭环帧之间的位姿变换。

2.3 位姿图的建立与优化

通常地图中点线特征的数量要远大于关键帧的数量，因此同时对所有的路标和位姿进行BA优化是相当耗时的。而位姿图只考虑位姿信息，可以在极短的时间内完成位姿优化。系统闭环校正前后的对比如图2所示。

图2 有无闭环校正的对比实验效果Fig.2 Comparison of experimental results with or without closed-loop correction

位姿图中将关键帧位姿作为顶点，关键帧之间相对位姿变换ΔTij(对应的李代数为ξij)作为边。顶点用李群表示为T1,T2,…,Tn，对应的李代数表示为ξ1,ξ2,…,ξn。两个顶点之间相连的边用李群表示为

(15)

式(15)对应李代数的表达为

(16)

由于误差的存在，两个节点之间相对运动的估计值与真实值的误差eij可以定义为

(17)

记K为所有位姿节点的集合，则总体目标函数为

(18)

通过求解式(18)可以对整个运动过程中的相机位姿进行全局优化，有效降低系统的累积误差。

3 实验结果及分析

为了验证所提方法的有效性，首先在KITTI[12]数据集上进行实验，该数据集通过自主驾驶车搭载多种传感器进行采集，包括64线激光雷达、GPS(global positioning system)/IMU(inertial measurement unit)组合导航系统以及两个灰度相机和两个彩色相机，高精度的组合导航系统可以作为系统的真值。

与ORB-SLAM2的双目算法对比实验如图3所示，其中所用计算机CPU为Inter Core i7，主频2.2 GHz，内存为8 G。

图3 部分序列估计轨迹对比Fig.3 Comparison of partial sequence estimation trajectories

将本文方法与ORB-SLAM2的双目算法关键帧位姿之间的相对误差(RMSE)[13]进行对比分析，在KITTI数据集下不同序列的相对平移误差(trel/m)和相对旋转误差(Rrel/°)，如表1所示。

表1 算法在KITTI数据集上的误差比较Table 1 Comparison of errors of algorithms on KITTI datasets

续表1

序列ORB-SLAM2本文方法平移误差/m旋转误差/(°)平移误差/m旋转误差/(°)071.150.531.350.69087.421.357.891.77096.881.149.581.25102.900.847.601.54

图3(a)、图3(c)、图3(d)、图3(e)均包含了不同数目的闭环，而图3(b)、图3(f)则为不包含闭环。其中数据集中序列00、05[图3(a)、图3(c)]包含闭环数目较多，具有大量闭环约束因而误差也较小；序列07、09[图3(d)、图3(e)]只包含一个闭环，序列07的相对平移误差为1.35 m，序列09的相对平移误差为9.58 m。然而，序列03、10[图3(b)、图3(f)]不包含闭环，其相对平移误差也仅为1.38、7.60 m。由图3可知，无论是在不包含闭环、包含少量闭环以及包含较多闭环的情况下，本文方法均能较好的完成定位与建图的任务。能获得更丰富的环境信息，在部分数据集上定位精度有所提升，具备较高可靠性。最后评估相同数据集下系统各线程的处理时间，实验结果如图4～图6所示。

图4 KITTI00序列跟踪线程时间对比Fig.4 Time comparison of the KITTI00 sequence tracking thread

由图4可知，在KITTI数据集00序列的测试中ORB-SLAM2方法的跟踪线程平均每帧约为110 ms，本文算法在分离特征提取线程后跟踪线程平均每帧为78 ms。

图5 KITTI00序列局部建图线程时间对比Fig.5 Time comparison of the KITTI00 sequence local mapping thread

图5为局部建图线程时间对比。由图5可知，本文算法在局部建图线程过程中的动态响应更快，较ORB-SLAM2方法的响应时间短130 ms左右。

图6 KITTI00序列闭环线程时间对比Fig.6 Time comparison of the KITTI00 sequence closed loop thread

图6为闭环线程时间对比，由于闭环检测过程中加入了线段特征，所以闭环线程时间明显高于ORB-SLAM2，但闭环检测几乎不影响前端线程的实时性。

图7 关键帧数量对比Fig.7 Number comparison of key frames

图7为部分数据集下关键帧数量对比。本文方法关键帧数量约为ORB-SLAM2的70%左右，在关键帧存储上占用的内存更少。

实验结果表明，双目SLAM算法能够成功完成SLAM任务，该方法在保证定位精度的同时能够得到更为丰富的环境地图信息，通过改进跟踪线程，提升了系统的帧率，保证了算法的实时性。

4 结论

提出一种点线特征融合的双目SLAM算法，算法采用点和线段两种特征共同进行位姿估计，并构建点线地图模型。通过从跟踪线程中分离特征提取线程，提高了系统的帧率。基于KITTI数据集实验结果验证了该算法满足实时性要求，具备较强的鲁棒性，并能够较准确地进行定位与构图。后续研究中希望引入IMU传感器，利用IMU数据与相机数据进行融合，进一步提升系统鲁棒性，使系统适应更多复杂环境。