基于改进关键帧选取策略的快速PL-SLAM算法

陈孟元，丁陵梅，张玉坤

1 引言

近年来，同时定位与地图构建（Simultaneous Local‑ization And Mapping，SLAM）技术是计算机视觉和机器人领域的一大研究热点，其核心在于提高机器人认知自身位姿以及周围环境几何结构的准确性［1~3］. 根据机器人使用传感器的不同，SLAM 主要分为激光SLAM 和视觉SLAM（visual SLAM，vSLAM）两大类. 激光发生器在成本、功耗、体积和质量等方面的劣势限制了激光SLAM的应用，使得性价比较高的vSLAM 逐渐成为自主导航与感知的主流［4，5］.

目前较为成熟的vSLAM 系统有ORB-SLAM（Ori‑ented FAST and Rotated BRIEF SLAM）［6］和LSD-SLAM（Large Scale Direct SLAM）［7］等，前者利用角点附近领域信息计算特征描述子，完成特征匹配，继而基于特征点的数据关联构造优化问题求解状态变量. 后者主要基于光度不变假设，使用光流法实现相机位姿跟踪. 基于特征点的vSLAM 系统为视觉SLAM 算法研究提供了较为完整的实现框架，但其易受环境纹理特征稀疏、运动过快或曲线运动等情况的影响，导致定位精度较低，位姿估计困难，甚至跟踪丢失［8］.Neira 等人［9］首次将直线特征融合到基于扩展卡尔曼滤波的单目SLAM 算法中，通过对直线特征的端点进行参数化表示，进而构建2D地图，但该系统的定位性能受制于线特征提取精度不高等因素. 为提高线特征的提取效果，Von 等人［10］提出了LSD（Line Segment Detector）线段检测器算法，该方法利用区域增长算法在形成的像素梯度场中获得线段支持域，通过错误报警数更新矩形线段的支持域，输出了亚像素级别的线特征.Zhang等人［11］针对线特征端点位置具有不确定性，导致线特征匹配准确率较低的问题，提出了二进制的LBD（Line Band Descriptor）线特征描述符，该算法将线特征支持域划分为若干条带，并利用划分的条带描述提取的线特征. 文献［12］提出了第一个较为完整的基于线特征的双目SLAM 系统，该系统框架基于3D 线特征，可完成地图构建及闭环检测任务；Zuo 等人［13］组合使用LSD 和LBD 对ORB-SLAM 的双目结构进行了改进，提出一种鲁棒的点线特征结合的双目SLAM 系统，该系统继承了文献［12］中线特征的3D表示方法，并引入普吕克坐标进行位姿优化，但是该系统在构造误差函数时，将点、线特征的检测结果简单相加，直接将线特征匹配的高误差引入系统，降低了数据关联的准确性. Gomez-Ojeda 等人［14］开源了PL-SLAM算法，该系统广泛适用于不同的场景，尤其是点特征稀疏或图像特征分布不均的场景. 但该算法在所有线程（位姿跟踪、局部建图、闭环检测）中同时使用点线特征，存在数据冗余的同时增大了特征误匹配的风险，且该算法在曲线运动尤其是拐弯时未关注关键帧的漏选问题，存在定位精度不高，闭环准确率较低等问题.Mur-Artal等人［15］为提高特殊场景下相机位姿跟踪的鲁棒性，采取尽可能快的插入新的关键帧的策略. 该方法虽通过增大关键帧的选取密度提高曲线运动的定位精度，但大量冗余关键帧将增大后端优化的计算负担.

综上所述，现有的PL-SLAM 算法缺乏对场景纹理稠密程度的识别能力，在SLAM 各线程中同时使用点线特征造成特征计算冗余，实时性不足，以及曲线运动时存在漏选关键帧等问题，本文提出一种基于改进关键帧选取策略的快速PL-SLAM 算法（Improved key‑frame extraction strategy-based Fast PL-SLAM algorithm，IFPL-SLAM），解决了现有PL-SLAM 算法在特殊场景环境下运行的不足. 在公开的KITTI 数据集和TUM 数据集中对本文算法进行测试. 测试结果表明IFPL-SLAM算法与ORB-SLAM2 算法、PL-SLAM 算法相比，在实时性，定位精度等方面有较大优势，表现出了良好的构图能力.

2 系统整体框架

本文提出的IFPL-SLAM 算法在PL-SLAM 算法的基础上引入了基于信息熵引导的位姿跟踪决策和逆向索引关键帧选取策略，其系统整体框架如图1所示.

图1 IFPL-SLAM算法框架

位姿跟踪阶段，IFPL-SLAM 算法为解决纹理稠密场景下各线程同时使用点线特征造成特征计算冗余，实时性不足的问题，优先对系统输入的图像序列提取点特征，而后通过基于信息熵引导的位姿跟踪决策判定是否调用线特征，使得算法对场景纹理稠密情况具有识别能力，提高了系统的实时性. 关键帧选取与闭环检测阶段，使用逆向索引关键帧选取策略，补充在曲线运动中漏选的关键帧，提高系统的定位精度和闭环检测的准确率.

3 点线特征结合的位姿估计策略

IFPL-SLAM 算法分别采用ORB（Oriented FAST and Rotated BRIEF）算法［16］和LSD 算法对场景中点特征、线特征进行提取，为避免在纹理稠密场景下点线特征同时使用造成特征计算冗余，本文提出一种基于信息熵引导的位姿跟踪决策，使用信息熵评价优先提取的特征点，并依据评价结果决策点线特征的融合使用方式，提高系统的实时性. 同时，为克服LSD 线特征提取时存在的长线特征被过度分割为短线特征，为后续特征匹配造成误匹配干扰等问题，本文对LSD 算法提取出的线特征进行条件筛选和拼接，提高特征匹配的准确率.

3.1 基于条件筛选的线特征提取与匹配

LSD作为一种优良的局部直线特征提取算法，其优点是能在线性时间内得出亚像素级别精度的线特征检测结果．但缺点也很明显，如计算耗时. 因此，本文对LSD算法提取的线特征进行条件筛选，包括拼接被过度分割的线特征和去除冗余短线特征，减少冗余匹配的计算耗时和提高线特征匹配的准确率. 其具体步骤如下.

步骤1 机器人在实验场景中探索环境，采集当前位置的图像信息，计算图像像素梯度，包括方向与幅值，计算方式如式（1）~（3）所示.

其中，gx(x，y)、gy(x，y) 为像素点在x轴和y轴方向上的梯度，i(x，y)表示图像中(x，y)位置处像素点的灰度值. 在形成的像素梯度场中利用区域增长算法获得线段支持域，随后对该支持域进行矩形估计，得到备选的线特征.

步骤2 依据错误报警数（NFA）更新矩形线段支持域，输出符合标准的LSD线特征.

其中，阈值ε为矩形中像素点密度的评估量. 使用LBD描述算子对输出的线特征进行描述，并将提取的线特征存入集合中，集合表示第i帧图像中共有n条线特征，表示第i帧图像中的第n条线特征.

①计算当前帧图像fc上线段长度大于长度阈值λ的线特征数目，记为Nλ，记集合

②当集合Nλ中的线特征数目大于δ（δ适应于场景纹理的整体稀疏程度）时，保留集合中前δ条线特征，删除中其他冗余线特征.

③若集合Nλ中的线特征数目小于δ，则通过断线拼接方法拼接被过度分割的长线特征并同步更新和［17，18］. 当Nλ≥δ时停止，返回步骤3②. 否则，说明图像fc中场景纹理较为稀疏，可能相机正处于纹理稀疏的区域或相机抖动导致图像质量不佳，此时保留中的所有线特征并输出. 其流程图如图2所示.

图2 线特征按条件筛选流程图

步骤4 本文将冗余的线特征剔除后，利用直线特征的外观和几何约束进行有效的直线匹配求解相机位姿.

图3（a）、3（c）为LSD 线特征提取效果图，由于图像中物体本身的特性和光线等原因，存在较多短线特征和少数被过度分割的长线特征. 图3（b）、3（d）为条件筛选的线特征提取效果图，图中可以看出通过本文提出的条件筛选方法剔除了图像中冗余短线特征和部分重复提取的线特征，使得提取的线特征在图像中分布较为清晰，为后续线特征匹配提供了良好的基础.

图3 线特征提取效果对比图

3.2 基于信息熵引导的位姿跟踪决策

机器人获取视觉场景图像序列后，为避免图像纹理稠密场景下，同时提取点线特征进行位姿估计造成特征冗余，本文提出一种基于信息熵引导的位姿跟踪决策. 该跟踪决策优先提取图像中的点特征，并通过信息熵计算点特征的信息量，当点特征信息量不足时，调用图像中的线特征进行辅助位姿估计，基于信息熵引导的位姿跟踪决策框架图如图4所示.

图4 基于信息熵引导的位姿跟踪决策框架图

信息熵是对随机离散信息的一种量化度量，能够直接表示图像中含有的信息量［19，20］. 由于图像中提取的特征点分布具有不确定性，因此本文通过信息熵来计算优先提取的点特征含有的信息量. 其计算规则如式（5）所示.其中，P(xi，yi)表示第i个特征点在图像中(xi，yi)处出现的概率.n为图像中特征点的个数.HX为图像中特征点的信息熵. 信息熵值越大，图像中提取的特征点携带的信息量越大，反之，说明图像中特征点携带的信息量较少.

当点特征的信息熵HX高于设定阈值ξ时（ξ为经验值，由多次实验估计而来），表明该图像上的点特征可满足算法进行位姿估计和跟踪，即此时不需调用环境中的线特征. 而当图像中的场景纹理特征变稀疏时，点特征提取难度增大，数量减少或分布不均，此时信息熵HX低于设定阈值ξ，为保证有足够的环境信息参与到位姿估计中选择调用图像中的线特征辅助位姿估计，避免位姿跟踪丢失. 此外，有选择地加入线特征位姿估计相对于传统的点线融合视觉里程计算法，从根源上避免了图像特征的冗余提取和计算，缓解了计算资源的紧张和因大量计算带来的实时性不足等问题. 算法1所示为基于信息熵引导的位姿跟踪决策算法.

4 关键帧选取与闭环检测

4.1 逆向索引关键帧选取策略

关键帧选取对SLAM 算法的定位精度和建图效率有着重要作用，后端优化和回环检测都是基于选取的关键帧进行的. 传统的关键帧选取方法在曲线运动拐点附近易出现漏选关键帧等问题. 使得整体算法缺乏克服机器人曲线运动条件下累计误差较大，准确定位困难，以及回环误差修正不完全的能力. 因此本文提出一种逆向索引关键帧选取策略.

逆向索引关键帧选取策略示意图如图5所示，机器人沿指示方向行驶，获取连续的图像帧数据，首先确定第一帧图像为第一个关键帧并将其位置定为世界坐标系的起点. 通过对极几何方法估计帧间的位姿变化.机器人直线运动阶段旋转变化量Δθ变化较小，运动变化主要体现在位移变化量Δp上，因此机器人直线运动阶段依照位移阈值选取关键帧，即当Δp超过位移阈值τ时选取关键帧，对应于图5 中的顺向索引. 当机器人曲线运动，尤其是发生转弯时，机器人可在短时间、短距离范围内发生较大的行驶角度变化，若未将关键位置处的图像帧设为关键帧，造成关键位置关键帧遗漏，将直接影响后端的回环检测及累计误差修正，继而影响全局一致性轨迹图的构建，因此，机器人曲线运动时，结合角度阈值进行关键帧选取. 具体过程为：计算当前帧fc与上一关键帧kfj之间的旋转变化量Δθjc，当Δθjc大于顺向角度阈值γ1时，记当前关键帧为新的关键帧，记为kfc，同时以当前关键帧为起点双向索引，其中逆向索引是指，估计kfc与介于kfc与kfc-1之间所有的普通帧之间的逆向旋转变化量Δθci，当Δθci大于逆向角度阈值γ2时，补充对应的普通帧fi为关键帧，插入关键帧库，该过程不中断关键帧的顺向索引，即继续帧间位姿估计，依据位移阈值τ和角度阈值γ1选取新的关键帧.在实际操作中γ1和γ2的设定受环境纹理稀疏程度的影响，同时，为了避免关键帧的冗余，一般设置γ1>γ2.

图5 逆向索引关键帧选取策略示意图

Δpjc和Δθjc表示当前帧fc相对于上一关键帧kfj的位移和角度变化量. 帧间位姿变化矩阵用对极几何表示为式（6）. 图像中的点、线特征经过提取与匹配之后，得到如图6 所示的点特征对极几何约束和线特征的三角化算法.

图6 点特征对极几何约束和线特征的三角化算法

以点特征为例，空间一点P=[X，Y，Z]T在两帧图像中的投影为p1和p2，由针孔相机模型可得像素点p1和p2的像素位置为：

其中，s1、s2为尺度因子，K为相机内参矩阵，R21和t21分别为I2相对于I1的旋转和平移矩阵. 可由基础矩阵或本质矩阵求解恢复相机运动得到. 旋转和平移矩阵得出之后将三维向量投影到二维平面，继而可求得本算法所需的任意两帧之间的位移变化量Δp和旋转变化量Δθ.

新关键帧选取方法的数学表达式如式（7）所示. 两类阈值是判别关键帧的主要依据，当新关键帧是依据旋转变化量确定的时，启动逆向索引关键帧选取策略，避免关键位置遗漏. 逆向索引关键帧选取策略流程如算法2所示.

4.2 融合点线特征的闭环检测

当插入新的关键帧之后，利用词袋模型［21］中的词包向量在地图中搜索与之具有相同词汇的所有关键帧，并通过点线特征匹配确定闭环. 在计算相似性得分时，按照闭环候选帧中的点线比例进行加权：

5 实验结果与分析

本文实验所用的电脑软硬件配置：CPU 为Inter Core i5-9400F，6 核处理器，主频2.9 GHz，内存8 GB，系统为Ubuntu18.04. 为了验证本文算法的有效性，分别使用KITTI 公开数据集［22］和TUM［23］公开数据集对本文所提策略进行验证.

长期以来由于投入资金不足，大多数县乡河道，特别是河道沿岸的县城、重要集镇和粮食生产基地的已建防洪排涝设施不足，遇到常遇洪水就可能造成较大洪涝灾害。一些河道因水土流失严重、侵占河道的问题，致使河道萎缩，行洪排涝能力降低，对所在地区防洪排涝安全构成威胁。

5.1 点线特征提取实验

图7 所示为ORB-SLAM2、PL-SLAM 和IFPL-SLAM 3种算法在TUM 数据集上的特征提取效果对比图. 表1所示为对应3 种算法的特征提取时间与匹配准确率对比结果. 其中，图7（a）~7（f）所示场景纹理信息丰富，点线特征稠密. 该6 幅图的特征提取对比可知，稠密场景下3种算法均能提取到丰富的特征信息，而IFPL-SLAM算法与PL-SLAM 算法相比，由于IFPL-SLAM 算法采用了基于信息熵引导的位姿跟踪决策，通过计算点特征的信息熵得出该图像中提取的点特征已能满足相机位姿求解，因此，本文算法在该图像帧未进行线特征提取，节省了线特征的处理时间. 使得本文算法的单帧处理时间与PL-SLAM 算法相比减少了16.0%. 图7（h）~7（j）所示场景的纹理较为稀疏，仅提取点特征的ORBSLAM2 算法由于缺乏数目充足且均匀分布的点特征进行特征匹配，导致匹配准确率较低.IFPL-SLAM 算法克服了稀疏场景中点特征不足的弊端，针对稀疏场景点特征不足的情况及时调用环境中的线特征，并对提取的线特征进行条件筛选，剔除了冗余线特征，整体提升了该算法的特征匹配准确率. 使得本文算法的匹配准确率与ORB-SLAM2相比提高了7.8%.

表1 TUM数据集下3种算法特征匹配准确率和特征提取时间对比

5.2 KITTI数据集评估

本文选取KITTI 数据集进行评估，如图8（a）~8（d）分别为ORB-SLAM2、PL-SLAM 和IFPL-SLAM 3 种算法在KITTI 4 个序列下获取的轨迹图对比. 表2 为对应的3种算法在4个序列中运行时获取的对比数据.01序列和10序列较为简单，3种算法均实现了不同精准程度的跟踪，其中IFPL-SLAM 算法的轨迹更加接近真实轨迹.图8（c）所示为3 种算法在08 序列中获取的轨迹图，该序列在真实轨迹坐标为（375，400）位置附近时，场景纹理信息较为稀疏.ORB-SLAM2 算法由于在稀疏场景中提取的点特征较少导致跟踪丢失. 而IFPL-SLAM 和PL-SLAM 算法凭借线特征的补充跟踪完成了对整个环境的建图. 图8（d）所示02序列是一个带闭环的图像序列，对比的3 种算法均通过闭环检测修正累计误差. 从图8（c）和8（d）中的局部放大图中可以看出，在拐弯和曲线运动过程中，由于IFPL-SLAM 算法提出的逆向索引关键帧选取策略补充了更多关键位置的关键帧，提高了算法的定位精度，使得IFPL-SLAM 的绝对轨迹误差相比于PL-SLAM 算法缩小了23.4%，相比于ORBSLAM2 缩小了43.0%. 可见本文算法在定位精度方面与ORB-SLAM2和PL-SLAM算法相比有较大优势.

图7 3种算法对不同稠密程度场景的特征提取对比

表2 3种算法在KITTI数据集4个序列下的结果对比

5.3 TUM数据集评估

本文进一步使用TUM 数据集进行评估，TUM 数据集主要源于室内环境，实验中选取纹理较为稀疏的fr3/large/cabinet 序列、旋转度较大的fr2/360-hemisphere 序列、具有闭环场景的fr1/plant 与fr2/xyz 序列对本文算法进行验证. 图9 分别为ORB-SLAM2、PL-SLAM 和IFPLSLAM 3种算法在不同序列下构建的轨迹图.

图8 KITTI数据集序列下的轨迹图

图9 TUM数据集序列下的轨迹图

从图9（a）可以看出，由于场景为稀疏环境，ORBSLAM2 算法的跟踪效果较差，且在运行时由于提取的点特征无法满足匹配要求导致跟踪丢失. 而IFPLSLAM 算法和PL-SLAM 算法通过点线特征结合进行跟踪，能够完成全局一致性建图. 从图9（b）中可以看出机器人在蓝色矩形区域内出现了多次剧烈的拐弯导致ORB-SLAM2 和PL-SLAM 最终在（1.344，0）附近发生跟踪丢失，而IFPL-SLAM 算法的逆向索引关键帧选取策略补充了拐弯处漏选的关键帧，因此能够较好地完成弯道轨迹的跟踪并提升整个环境的建图效果. 图9（c）和9（d）是对闭环场景轨迹的跟踪效果，2 种场景下的3种算法均因具有闭环检测环节完成了全局地图的构建，由于IFPL-SLAM 算法对提取的线特征进行了条件筛选且引入了逆向索引关键帧选取策略使得本文算法获取的轨迹图更加接近真实轨迹.

表3 为3 种算法在不同序列下的平移误差和旋转误差对比. 从表中数据可知，IFPL-SLAM 算法的平移误差与PL-SLAM 相比减少了21.3%，与ORB-SLAM2 相比减少了43.1%. 同时，IFPL-SLAM算法的旋转误差与PLSLAM 相比减少了17.2%，与ORB-SLAM2 相比减少了39.3%.

表3 3种算法在TUM数据集下的平移误差和旋转误差对比

图10 TUM数据下的准确率－召回率对比图

图11所示为3种算法在TUM 数据集下的运行时间对比图，从图中可以看出本文算法的运行时间与PLSLAM 相比减少了22.1%，与ORB-SLAM2 算法相比，由于本文算法增加了对场景纹理稠密程度的识别能力、线特征的提取与筛选占用了一部分运行时间，使得本文算法运行时间较ORB-SLAM2 有所增加，但仍可满足实时性需求.

图11 3种算法在TUM数据集下的运行时间对比图

6 结论

本文提出了一种IFPL-SLAM 算法，该算法具有以下优点.（1）提出一种基于信息熵引导的位姿跟踪决策，解决了PL-SLAM 在稠密场景下同时使用点线特征（进行位姿跟踪）造成特征计算冗余的问题，提高了系统的实时性.（2）针对LSD 线特征提取时存在的长线特征被过度分割为短线特征，对LSD 算法提取出的线特征进行按条件筛选和拼接，剔除冗余短线特征，提高了特征匹配的准确率.（3）通过引入逆向索引关键帧选取策略解决了在非直线运动条件下漏选关键帧的问题，提高了算法的闭环准确率和定位精度. 所提算法在公开的KITTI 数据集和TUM 数据集上进行测试，结果表明，所提算法与ORB-SLAM2 和PL-SLAM 算法相比，在实时性和定位精度方面具有一定的优势，体现出了良好的构图能力. 然而本文算法受动态障碍物影响较大，下一步将在本文的研究基础上，融合IMU、激光雷达等较精确的传感器提高机器人遇到动态障碍物的鲁棒性，使得本文所提方法的位姿估计能力更加精确.