基于点线特征融合的实时视惯SLAM算法

摘 要：为了在光照不足和低纹理场景中实现移动机器人的高精度实时定位和建图，提出了一种基于视觉点线特征以及IMU特征融合的实时SLAM算法。首先通过跳跃路由策略和自适应阈值策略改进了EDlines算法，提高了线特征提取的质量，从而提高了特征跟踪的有效性。然后将视觉惯性特征紧耦合建立约束，通过滑动窗口和边缘化模型进行非线性优化，实现了高精度高实时性的状态估计。实验证明，所提算法在线特征提取的有效性方面优于传统的线段提取算法，同时SLAM系统的定位精度和鲁棒性均得到有效提升。

关键词：视觉同步定位与建图；特征提取；视觉惯性紧耦合；滑动窗口

中图分类号：TP242 文献标志码：A 文章编号：1001-3695（2024）10-018-3008-07

doi：10.19734/j.issn.1001-3695.2023.12.0636

Real-time visual-inertial SLAM algorithm based on point-line feature fusion

Wang Leia，Chen Shuaikuna，Qi Junyanb，Yuan Ruifuc

（a.School of Computer Science & Technology，b.School of Software，c.School of Energy Science & Engineering，Henan Polytechnic University，Jiaozuo Henan 454003，China）

Abstract：This paper introduced a real-time SLAM algorithm designed for mobile robots operating in environments characte-rized by low illumination and low texture.The algorithm leveraged the fusion of visual point-line features and IMU characteristics.It enhanced the EDlines algorithm by incorporating a leapfrog routing strategy and an adaptive threshold strategy，thereby improving the quality of line feature extraction and consequently enhancing feature tracking effectiveness.It established tight coupling constraints between visual and inertial features and employed a sliding window，along with a marginalization model，for nonlinear optimization.This enabled high-precision and real-time state estimation.Experimental results show that the proposed algorithm is superior effectiveness in online feature extraction when compared to traditional line segment extraction methods.Concurrently，the SLAM system achieves enhanced localization accuracy and robustness.

Key words：visual simultaneous localization and mapping（VSLAM）;feature extraction;visual-inertial tight coupling;sliding window

0 引言

同步定位与建图（simultaneous localization and mapping，SLAM）技术是机器人领域具有重要意义的技术之一。目前，SLAM方法可分为视觉和激光两大领域。虽然激光SLAM在理论、技术和实际应用上都比较成熟，但是二维激光SLAM只能获取环境中的单个平面信息，不适合在大型复杂场景中应用。而通过增加雷达线路数量来获取更多信息的方法会导致硬件成本的大幅增加。视觉传感器与激光雷达相比具有轻便、便宜的优点，且视觉传感器所构建的地图包含更多的信息。低功耗和低重量也使得在负载有限的移动平台上部署视觉传感器成为可能。

随着相关技术的快速发展，许多优秀的视觉SLAM（VSLAM）算法涌现出来。文献［1］提出了MonoSLAM，使用图像Shi-Tomasi角点，再利用EKF算法进行状态估计。其作为首个使用单目视觉传感器的实时SLAM方法，取得了不错的效果，使得VSLAM的研究流行起来。后来，基于关键帧的并行跟踪与建图方法（parallel tracking and mapping）［2］的提出，为VSLAM打开了基于非线性优化方向的突破，自此VSLAM的研究开始有了区分前端和后端的概念。该算法实现了并行处理任务，能够更好地进行全局优化，并且使得跟踪更加稳定。VSLAM根据前端对图像的处理方法，可分为特征点法和直接法。特征点法即通过提取关键子和计算描述子两步来确定图像特征。直接法通过最小化光度误差来直接确定点与点之间的关系，通过图像的像素灰度信息来计算相机运动［3］。但是直接法由于其原理特性，也存在算法非凸性及鲁棒性差等问题。因此在VSLAM前端部分，采用特征点法是研究工作的主流。特征点提取算法包括SIFT、SURF、Harris角点、ORB［4～7］等。在这些特征提取方法中，都只提取图像中的重要特征点，同时为了提高算法的实时性，特征点的分布是稀疏的。这使得VSLAM对光照更加敏感，在光照剧烈变化或相机高速运动的情况下，算法将无法有效提取到图像中的特征信息。例如，ORB-SLAM2［8］和DSO［9］均采用视觉特征点方法进行定位，在环境光照条件良好的情况下，能够达到良好的定位精度，然而在低纹理和光照条件差的情况下，这些系统表现较差甚至发生崩溃。

为了丰富SLAM技术的应用场景，研究者意图通过融合相机、激光雷达、惯性测量单元（IMU）和GPS等不同传感器以提高系统的精度和鲁棒性。其中，视觉传感器和IMU传感器的融合是十分有效的。视惯融合优秀的算法如ORB-SLAM3［10］，利用长期的数据相关性实现大场景的高精度定位。其快速准确的IMU初始化和多地图拼接技术使其能够在各种场景中实现鲁棒的实时操作，但这也需要较高的计算成本。2018年Qin等人［11］提出了VINS-Mono，采用视觉惯性紧耦合的方法建立视觉特征约束和IMU预积分约束，在前端部分采用Shi-Tomasi角点和LK光流法进行特征点提取。与传统的视觉SLAM算法相比，其定位精度得到了显著提升，但由于其前端采用光流法进行跟踪匹配，不提取特征描述符，当图像帧剧烈运动且模糊时，特征点会丢失，系统也极易受到光照影响。

为了解决上述问题，研究者关注到可通过多特征融合，包括提取点、线、灰度值等特征，获得多个特征映射元素，进而提高单一基于特征点的SLAM系统的准确性和鲁棒性。线特征具有良好的光照不变性和旋转不变性，可以对点特征进行更高结构层次的补充，对场景结构提供额外的约束条件。如Pumarola等人［12］提出的PL-SLAM，在ORB特征点的基础上添加了采用LSD线段提取算法［13］的线特征，提高了系统的精度和鲁棒性。2018年He等人［14］提出了PL-VIO，实现了点线特征融合以及视觉惯性融合。后来，Fu等人［15］在PL-VIO的基础上进行改进，将原先的线特征提取算法LSD进行了优化，调整一些隐藏参数并引进长度抑制策略，使得LSD的运行速度达到原来的三倍。赵伟博等人［16］提出了一种点线结合的视惯SLAM算法，在前端引入了改进的LSD线段提取算法来增加线特征，取得了不错的效果。此外，研究者还注意到基于深度学习的视觉惯性里程计具有明显的优势。与传统方法相比，神经网络完全依靠数据驱动训练和提取特征，能够将数据自身蕴涵的信息最大程度地挖掘并进行深度预测。2017年，Clark等人［17］首次提出融合了IMU信息的VINet，它利用CNN从相邻帧图像中提取出视觉特征，并使用LSTM网络来训练惯导特征模型。既减少了系统对手动同步和校准的依赖，同时也表现出了更强的鲁棒性。2021年，Zuo等人［18］提出了CodeVIO，做到了网络在实时单线程运行的同时，具有较强的泛化能力以及更高的计算效率。但是，限于深度学习的特性，多数系统无法很好地兼顾效率和性能，并且多数基于深度学习的视惯SLAM算法仍仅利用特征点作为前端输入，造成系统的鲁棒性较低。

目前，主流SLAM算法对于线特征处理的方法多为LSD算法。而LSD算法计算运行速度较慢，难以满足实时性要求。虽然EDLines［19］线段提取算法提取速度是LSD算法的近十倍，但对光照变化较为敏感，提取的线段存在较多不连续的分割短线，难以提供精确的有效线特征，加大了线特征匹配任务的难度，导致SLAM系统鲁棒性较差。针对以上问题，本文在VINS-Mono框架的基础上提出了一种具有实时性的基于点线特征的单目视觉惯性紧融合SLAM算法。算法在前端部分加入了改进的EDLines提取匹配算法来解决线段检测算法在复杂环境下线特征提取质量低的问题，提高了系统的实时性和鲁棒性。然后在后端利用滑动窗口将点、线特征和IMU特征进行紧耦合，并通过边缘化模型优化位姿，联合最小化多差目标函数，实现精准的状态估计和环境建图。

1 系统架构

本文系统采用点线视觉特征与IMU信息联合对位姿进行非线性优化的方法，实现了前端、后端优化和回环检测与全局优化三个线程。系统的整体结构如图1所示。

在前端视觉里程计部分，系统对相机和IMU采集的数据作预处理。其中包含并行处理原始图像，对其进行点特征和线特征的提取以及跟踪匹配。其中点特征的提取沿用VINS-Mono的Harris角点提取算法，利用KLT光流法跟踪特征点，并使用RANSAC算法剔除异常值。线特征的提取是基于改进的EDLines算法，通过跳跃路由策略和自适应阈值策略提高线特征提取算法在复杂环境中的有效性。然后利用LBD描述子［20］计算跟踪线特征，进行线段匹配。最后用几何约束算法剔除误匹配。此外，在前端部分的系统初始化过程中，主要是利用点、线以及IMU预积分［21］处理之后的信息优化求取系统的初始化参数。

之后，在后端对位姿进行非线性优化，将所有约束项构建为目标函数。主要工作包括通过对二维帧间的点、线特征对应进行三角化，重投影空间点和线。空间点和空间线分别由逆深度参数化和普吕克坐标参数化［22］。然后，基于滑动窗口，联合最小化残差函数，得到位姿、速度、空间点和空间线以及加速度和陀螺仪bias在内的最优状态向量。为了保持系统持续运行的精确性和实时性，当系统输入新的图像帧时，通过本文的边缘化模型滚动窗口中的最后一帧来固定保持窗口大小不变。

在后端进行局部优化之后，根据当前帧与最后帧之间的视差是否大于设定阈值，或根据跟踪的特征数量是否低于设定阈值来判断当前帧是否为关键帧。若确定为关键帧则会激活回环线程，根据文献［11］的理论所述，利用DBoW词袋模型来搜索并决定是否已经发生了回环。当检测到发生回环，系统将进行重定位，对所有位姿进行全局优化，以达到更高的精度。

2 前端视觉里程计

2.1 线特征提取算法

作为一种基于局部梯度方向的算法，EDLines算法生成的是一组干净的、连续的像素链［23］。本文基于EDLines所改进的线特征提取算法流程如图2所示。

算法的改进策略可分为以下两部分概括。

1）跳跃路由策略

EDLines算法作为一种局部分割检测方法，其局限性之一是梯度不连续会产生许多小的分割，在如阴影、遮挡等情况下会产生许多断裂的短线。在线段绘制的过程中，一条线段可能会被几个边缘的不连续所中断。这些间隔是图像中梯度方向改变的区域或者梯度幅度趋于零的区域。为了实现相同方向上存在像素断裂的短线段能够合并为长线段，获得更有效的线段检测结果，提出此策略。

根据文献［16］可知，算法在连接锚点的过程中，从任一锚点开始，利用相邻像素的梯度信息，通过梯度极值连接到下一个锚点。当像素延伸方向有变化时，仍优先按照当前方向进行延伸，同时将改变的方向存储在一个堆栈中。当像素延伸至图像边缘时，回溯堆栈中最后一次改变的方向并再次沿此方向延伸，如此递归直到堆栈中存储的方向都提取完成。算法流程如图3所示。

在延伸过程中，每当检测至像素断裂的位置时，即没有边缘像素或边缘方向与拟合线段不对齐区域，通过跳跃这段像素间隔的方法来判断是否需要保持像素当前的延伸方向。这一策略的前提有以下必要条件：

a）从锚点a开始，算法能够沿着边缘方向绘制至少J像素。将这组J像素称为扩展像素。由于算法使用5×5高斯平滑核降噪，任何1像素的不连续都会对至少5像素大小的邻域产生影响，因此可将扩展像素的初始参数设置为J∈［5，7，9］。

b）与线段L对齐的锚点像素a，距离当前像素为J像素，且在图像内部满足G［a］>0（即不是弱边缘像素）。

c）Llength>Jpixels，即绘制线段的总长大于扩展像素J。最后，通过计算像素梯度的自相关矩阵M来验证扩展像素与当前线段的方向是否对齐。M为扩展像素邻域的每侧方向各取1像素并对其梯度信息进行加权平均计算得出。若满足

λ1λ2≥TEigenExt，∠（v1，n）≤TAngleExt（1）

则可证明扩展像素沿当前线段方向连续延伸。其中：λ1和λ2（λ1>λ2）是M的特征值；∠（·，·）是角距离；v1是M的第一个特征向量；n是与线段正交的向量。当TEigenExt设为10，TAngleExt为10°时，则可认为扩展像素的延伸方向与线段结构的主导方向足够一致。

由此，算法不仅保证了像素延伸过程的连续性，还可解决图像中存在像素断裂的问题，从而保证一条有像素断裂的长线段不会被判定为多个短线。该策略做到了在即使有阴影、遮挡等复杂环境中，算法也能保证有效性和准确性。

2）自适应阈值策略

由于锚点像素大部分都是边缘元素，进而可形成边缘像素链，完成边缘特征检测。图像梯度计算公式如下：

gx（x，y）=I（x+1，y）-I（x，y）+I（x+1，y+1）-I（x，y+1）2（2）

gy（x，y）=I（x，y+1）-I（x，y）+I（x+1，y+1）-I（x+1，y）2（3）

g（x，y）=gx（x，y）2+gy（x，y）2angle（x，y）=arctangx（x，y）-gy（x，y）（4）

其中：I（x，y）表示输入图像于像素（x，y）处的强度;g（x，y）为梯度幅值;angle（x，y）为像素点方向的水平线夹角。

EDLines算法在选取锚点前，首先需要对梯度小于阈值ρ的像素进行筛除，但是将阈值设置为固定参数会降低算法在复杂场景的鲁棒性。因此，本文对EDLines算法的梯度阈值ρ进行自适应改进，通过自适应调整矩阵来适应像素点上的水平线、垂直线和斜线的梯度变化，从而使梯度阈值对不同方向上的梯度变化更敏感，提高边缘检测的准确性，矩阵如式（5）～（7）所示。

P1x，y=ρx，y+ρx，y+1+ρx，y-1+ρx-1，y+ρx+1，y+z5（5）

P2x，y=ρx，y+ρx+1，y+1+ρx-1，y-1+ρx-1，y+1+ρx+1，y-1+z5（6）

Px，y=min{P1x，y，P2x，y}（7）

其中：ρx，y是点（x，y）的像素梯度;z为噪声抑制常数，本文将其设定为锚点阈值的一半。

最终，选择两者中的较小值作为梯度阈值。完成这一步后，算法对提取的线段数量进行计数。若提取的线段数量低于自适应阈值的设定值，则会通过降低梯度阈值参数，再次进行线特征提取，以获得足够的有效线特征。这种自适应阈值提取方法能够更好地应对如光照变化明显、不足或弱纹理等环境情况，提高了线特征提取算法在复杂条件下的鲁棒性。

2.2 线特征描述及匹配

在改进的提取线特征的基础上，通过LBD描述子来对图像相邻帧中对应的线特征进行几何一致性评估。LBD描述子通过高斯下采样建立多尺度空间金字塔，对原始图像进行N次高斯降采样。尺度金字塔从底层向上分层依次是原始图像和经过高斯模糊算法降低分辨率的图像。使用本文算法对高斯分层采样的图像中线特征进行提取，得到一组线段序列。尺度空间中的所有线特征都有唯一标识，将其进行重组后，相关或相同的线段存储到同一个LineVec向量中。如此将整个金字塔提取完成，得到一组LineVec向量。

LBD描述子对每条线段进行标识，以线段为中心周围所建立一个矩阵的线支持域LSR。它以一组有相同宽度的频带Bj表示。计算每个频带的特征向量BDj，所有频带的特征向量构成初始LBD描述子：（BDT1，BDT2，…，BDTm）T。分别计算频带Bj及其相邻频带Bj-1和Bj+1的局部梯度g′=（g′d⊥，g′dL），然后求和。第K行像素的梯度分为四个部分，如下：

v1kj=λ∑g′d⊥>0g′d⊥，v2kj=λ∑g′d⊥<0g′d⊥v3kj=λ∑g′dL>0g′dL，v4kj=λ∑g′dL<0g′dL（8）

其中：高斯权重λ=fg（k）fl（k）。把所有行的梯度汇总，构成特征向量BDj对应的特征描述矩阵BDMj如下：

BDMj=v11jv12j…v1njv21jv22j…v2njv31jv32j…v3njv41jv42j…v4nj∈Euclid ExtraaBp4×n，n=2wj=1 or m3welse（9）

计算出BDMj的均值方差MTj与均值向量STj，得到特征向量BDj=（MTj，STj）T∈Euclid ExtraaBp8。LBD特征为

LBD=（MT1，ST1，MT2，ST2，…，MTm，STm）∈Euclid ExtraaBp8m（10）

得到LBD描述子后，检查LineVecs的一元几何属性和局部相似性以消除异常值。最后使用几何约束算法剔除误匹配线特征，提高匹配准确率。

2.3 线特征重投影残差模型

特征线的重投影误差可表示为归一化图像坐标系下线段端点到投影直线的距离。对于空间线段与其在世界坐标系下的正交表示：Euclid Math OneLAp=［n，v］T，通过式（1）将其转换到相机坐标系下后，再投影到成像平面上得到lcil。

l=l1l2l3=Euclid Math OneKApnc=fy000fx0-fycx-fxcyfxfync（11）

则线特征的重投影误差定义如式（12）所示。

rl（zciLl，Euclid Math OneXAp）=d（scil，lcil）d（ecil，lcil）（12）

其中：d（s，l）计算方法如式（13），特征线重投影误差示意如图4所示。

d（s，l）=sTll21+l22（13）

3 后端优化与回环

3.1 基于滑动窗口的非线性优化

本文系统为了在减少计算量的同时保证优化效果，在后端部分采用基于滑动窗口的非线性优化模型对位姿进行优化。定义滑动窗口的状态变量为

X=［xn，xn+1，…，xn+N，λm，λm+1，…，λm+M，Euclid Math OneOApo，Euclid Math OneOApo+1，…，Euclid Math OneOApo+O］T

xi=［pwbi，qwbi，vwi，bbia，bbig］T，i∈［n，n+N］（14）

其中：xi为摄像头在滑动窗口中的状态;pwbi为世界坐标系下的IMU的位置;qwbi是IMU的方向信息;vwi是IMU的速度;bbia是IMU载体坐标系下的加速度计偏置;bbig是陀螺仪偏置;λk为空间特征点的逆深度;Euclid Math OneOApl是世界坐标系下线特征的正交表达式。式（15）表示为将构建的目标函数最小化，以此来优化滑动窗口内的全部状态变量：

minEuclid Math OneXApρk（‖rp－JpEuclid Math OneXAp‖2Σp）+∑i∈Bρk（‖rb（zbibi+1，Euclid Math OneXAp）‖2Σbibi+1）+∑（i，j）∈Fρh（‖rf（zcifj，Euclid Math OneXAp）‖2Σcifj）+∑（i，l）∈Lρh（‖rl（zciEuclid Math OneLApi，Euclid Math OneXAp）‖2Σcifi）（15）

其中：rb（zbibi+1，Euclid Math OneXAp）是状态xi与xi+1之间的IMU测量残差数据；B是滑动窗口内所有IMU预积分值的集合；rf（zcifj，Euclid Math OneXAp）与rl（zciEuclid Math OneLApi，Euclid Math OneXAp）分别表示点和线特征的重投影残差；rp和Jp是被边缘化一帧之后的先验信息；Jp是优化后得到的海信矩阵的先验雅可比矩阵；ρk为优化后的柯西鲁棒核函数；ρh为Huber核函数［24］；F和L分别是观测到的点、线特征集合。

3.2 边缘化模型

主流基于优化方法的VIO系统都会使用滑窗与边缘化的方式减少优化参数，降低系统的整体复杂度，提高状态估计的效率和精确性。因此，边缘化的过程即为将联合概率分布分解为边缘概率分布和条件概率分布的过程。本文所提边缘化模型可以保持后端滑动窗口的大小，在滑动窗口前，进行边缘化操作，提高系统运行的实时性和鲁棒性。通过边缘化去除掉滑动窗口内的最旧帧或者次新帧，在以约束项的形式保留此帧和窗口内其余帧的约束关系的同时，不再冗余计算该帧的位姿与相关路标点，以达到对状态变量的优化效果。这样可以应对当移动机器人进行退化运动时，既拥有相关的历史约束信息求解，又可达到全局约束优化的目标。本文的边缘化模型将对系统中的每一图像帧都进行优化以确保系统的精度和性能。

将边缘化的状态变量设为Euclid Math OneXApa，保留变量设为Euclid Math OneXApb，可以将HδX=b表示为

ΛaΛbΛTbΛcδEuclid Math OneXApaδEuclid Math OneXApb=babb（16）

利用Schur补方法推导出式（17），即可计算得出变量δEuclid Math OneXApb的先验信息。其中ΛTbΛ-1aΛb为Λa在Λb中的Schur项。则被保留下来的变量δEuclid Math OneXApb的先验信息如式（18）所示。

ΛaΛb0Λc-ΛTbΛ-1aΛbδEuclid Math OneXApaδEuclid Math OneXApb=babb-ΛTbΛ-1aba（17）

（Λc-ΛTbΛ-1aΛb）δEuclid Math OneXApb=bb-ΛTbΛ-1aba（18）

在边缘化过程中，相机和路标点之间对应的关系模型如图5所示。当Xp1被边缘化后，此时被边缘化的变量式为δEuclid Math OneXApa=δx6×1P1。在此之后，Xm1、Xm2和Xm3之间将建立起新的约束，Xm1与Xp2也将建立起新约束。此时H矩阵对应于Λc-ΛTbΛ-1aΛb。接下来以相似的过程继续边缘化Xm1。

3.3 回环检测和全局优化

回环检测与全局优化是为了系统能为了得到全局一致的位姿估计。回环检测能够给除了相邻图像帧以外的更加久远的约束，它提供了当前数据与所有历史数据的关联，能够提高系统长时间运行的精确性和鲁棒性。

本文对完成后端优化后的关键帧进行回环检测：首先从图像上提取一些的新的角点进行描述；接着从关键帧数据库中检测当前帧对应的闭环帧，在检测到闭环后，当前帧会产生一个闭环约束，将此约束项构建到目标函数中，非线性优化求解得到相对位姿关系；然后对滑窗内的所有帧进行调整，完成快速重定位。在重定位之后，闭环优化使得过去的位姿信息能够被加载到全局地图中。

4 实验与评估

本文实验平台为64位Linux操作系统，CPU为Intel Core i5-8300H；核心数为4，内存为8 GB。为验证改进算法以及系统的有效性，实验分为两部分。首先将本文改进的线特征提取匹配算法与LSD算法和EDLines算法进行对比来评估其性能。然后在公开数据集上评估本文算法和VINS-Mono、ORB-SLAM3、PL-VIO以及PL-VINS算法的性能。其中，VINS-MONO和ORB-SLAM3是目前最先进的视觉惯性算法，在前端仅使用点特征进行跟踪。PL-VIO算法是一种基于点-线特征的视觉惯性里程计，其前端部分是利用改进的LSD算法提取线特征，能够达到很好的效果。而PL-VINS算法是在VINS-mono和PL-VIO的基础上改进的优秀视惯SLAM算法，在前端利用改进的LSD线特征提取算法，提升了系统实时性和定位精度。通过与这两种算法的比较，不仅证明了本文线特征对全局轨迹估计结果的性能提升，也证明了本文所提出的单目点线视惯SLAM算法的精确性和鲁棒性。

4.1 线特征提取实验

首先在EuRoC数据集中的MH_04_difficult和MH_05_difficult序列上实验本文改进的线特征提取算法。该序列MAV运动快速且场景黑暗，纹理效果也有好有坏，能够很好地检测线段提取算法的有效性和鲁棒性。由图6、7可知，在场景黑暗，纹理不足的环境下，本文算法与LSD算法相比提取的特征线数目基本相当，比EDLines算法提取数目多，而提取线段的平均长度（平均线长=线段长度总和/线段总数目）比这两种算法都要大，这说明本文算法在光照和纹理不足的场景中仍能提取到足够数量的有效长线段用于约束场景结构，从而进行更好的位姿估计。这对系统的鲁棒性有很大提升。

本节实验结果证明，本文算法在提取出的线特征数量上以及线特征的长度上均有所进步，实验对比如图8、9所示。因此，本文的线特征提取算法在黑暗、弱纹理的复杂环境中所提取的线段质量优于传统的线段提取算法，对系统的后端优化以及数据融合均有所帮助。

4.2 系统精度评估

在EuRoC公开数据集上进行测试，验证本文系统的有效性和精度。数据集序列提供了轨迹的真实值用以量化轨迹误差，且根据微型飞行器和手持相机移动的复杂程度以及环境光照、纹理等条件，序列分为简单、中等和困难三个等级。本节实验所选序列对于系统的前端特征的提取和后端优化模块的鲁棒性都具有很大的考验。尤其在光照变化剧烈或纹理信息差的场景中，线特征的有效提取将会给系统的精度提升提供较大帮助。表1给出了以APE（absolute pose error）为指标进行定量分析的结果，并将不同SLAM系统进行对比，可以直观地展示三种算法的轨迹精度。

由表1可知，在这些算法中，本文算法均保持了最好的性能。与VINS-mono和ORB-SLAM3相比，本文算法的RMSE在所选的MH序列上精度分别平均提升了47.8%和37.7%，在VR序列上与PL-VINS性能较为相近。因为MH序列是飞行器在光照条件较差且低纹理场景中采集，故证明了改进的线特征提取匹配算法在此类复杂场景中的有效性。VR序列为手持相机在室内采集，点、线特征较繁杂，且含有大量的无规则高速晃动，证明了本文系统的精度和鲁棒性都有所提升。从表中不难得出结论，本文算法对于这些场景精度都很高，在所选VR序列中，比同样基于点线特征的PL-VINS平均精度提升了259%，表现更好，鲁棒性更高。

图10更为直观地显示了所选序列的地真轨迹（虚线）和本文算法获得的轨迹（实线）的对比。它们分别是MH_03_medium、MH_05_difficult、V1_02_medium和V2_03_difficult的绝对轨迹误差结果。图中误差大小的变化情况由小到大，以深蓝色到红色的渐变色带为代表。通过结果图可以看出，本文算法的稳定性较强，整体轨迹误差持续控制在较低范围。且当相机传感器运动方向突然加速或转向，或光线变化较大区域时，系统误差相对来说会增大，但仍能稳定控制在较小范围。系统整体可以较好地完成位姿精准估计的任务。

如图11所示，以V2_02_medium序列的轨迹结果为例可以看出，本文算法的估计轨迹与真实轨迹的贴合度在几个算法中最好。在相机传感器遭遇大幅度快速变向，运动模糊以至纹理效果很差的情况时，也能较好地贴合真实轨迹。对比来说VINS-mono与PL-VINS算法与真实轨迹的整体贴合程度不如本文算法。

图12为不同算法分别在x、y和z轴的轨迹误差对比以及x、y、z轴的细节。可以看出，虽然整体的轨迹误差几种算法的效果都相差较小，但是本文算法是误差控制以及与轨迹真值的贴合度是最好的，且能够做到在算法初始化阶段就将轨迹误差控制在最低范围并持续平稳保持直到系统运行结束。

图13和14展示了系统运行过程中的实时APE和绝对位姿误差分布范围情况。可以看出，本文算法的精度整体上优于两种对比算法。在系统运行的整个过程中，本文算法比其余两种对比算法的误差控制更低，分布更为集中，波动更小。VINS-mono由于仅使用特征点而造成在运动模糊、纹理低且光照变化大的复杂环境中时精度会产生较大波动。而PL-VINS和本文算法添加了线特征，因此整体波动稳定且更低。得益于本文的改进的线特征提取匹配算法，所提取的线特征效果更好，特征有效性更强。由实验结果可知，由于本文的后端非线性优化模块和边缘化模型，本文算法比PL-VINS的误差控制效果更加稳定，精度更高。系统运行的稳定性相对于两种对比算法而言有了较大的提升，做到了精度和鲁棒性的同步提高。

5 结束语

本文提出了一种基于VINS-mono所改进的，融合IMU数据和点、线特征的视觉惯性实时SLAM算法。得益于改进线特征提取算法以及边缘化模型，本文算法达到了更优秀的系统位姿优化效果，提高了SLAM算法整体的精度和鲁棒性。在系统前端，本文采用基于LBD描述子的改进的EDLines线特征匹配算法，通过引入跳跃路由策略和自适应阈值策略提高了线特征提取的质量与精确性，为系统提供了更有效的图像特征，提高了后端优化的精度和鲁棒性。通过公开数据集表明，在光照条件差、弱纹理的复杂环境下，本文算法与VINS-mono和PL-VINS等SLAM算法相比，定位精度更高，鲁棒性更强。在后续的工作中，计划进一步简化线特征提取和匹配算法以达到更好的效果，继续优化后端模块，提高算法的性能，并引入特征加权策略以达到更好的后端优化效果，进而将其应用到机器人路径规划与导航中。

参考文献：

［1］Andrew J D，Ian D R，Nicholas D M，et al.MonoSLAM：real-time single camera SLAM［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.

［2］Klein G，Murray D.Parallel tracking and mapping for small AR workspaces［C］//Proc of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.Piscataway，NJ：IEEE Press，2007：225-234.

［3］Forster C，Zhang Zichao，Gassner M，et al.SVO：semidirect visual odometry for monocular and multicamera systems［J］.IEEE Trans on Robotics，2017，33（2）：249-265.

［4］Lowe D G.Distinctive image features from scale-invariant keypoints［J］.International Journal of Computer Vision，2004，60（2）：91-110.

［5］Bay H，Tuytelaars T，Van Gool L.SURF：speeded up robust features［J］.Computer Vision and Image Understanding，2008，110（3）：346-359.

［6］Harris C，Stephens M.A combined corner and edge detector［C］//Proc of the 4th Alvey Vision Conference.Piscataway，NJ：IEEE Press，1988：147-152.

［7］Rublee E，Rabaud V，Konolige K，et al.ORB：an efficient alternative to SIFT or SURF［C］//Proc of International Conference on Computer Vision.Piscataway，NJ：IEEE Press，2011：2564-2571.

［8］Mur-Artal R，Tardós J D.ORB-SLAM2：an open-source SLAM system for monocular，stereo and RGB-D cameras［J］.IEEE Trans on Robotics，2017，33（5）：1255-1262.

［9］Engel J，Koltun V，Cremers D.Direct sparse odometry［J］.IEEE Trans on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.

［10］Carlos C，Richard E，Juan J G R，et al.ORB-SLAM3：an accurate open-source library for visual，visual-Inertial，and multimap SLAM［J］.IEEE Trans on Robotics，2021，37（6）：1874-1890.

［11］Qin Tong，Li Peiliang，Shen Shaojie.VINS-mono：a robust and versatile monocular visual-inertial state estimator［J］.IEEE Trans on Robotics，2018，34（4）：1004-1020.

［12］Pumarola A，Vakhitov A，Agudo A，et al.PL-SLAM：real-time mono-cular visual SLAM with points and lines［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2017：4503-4508.

［13］Gioi R G V，Jakubowicz J，Morel J W，et al.LSD：a fast line segment detector with a false detection control［J］.IEE Trans on Pattern Analysis and Machine Intelligence，2010，32（4）：722-732.

［14］He Yijia，Zhao Ji，Guo Yue，et al.PL-VIO：tightly-coupled monocular visual-inertial odometry using point and line features［J］.Sensors，2018，18（4）：1159.

［15］Fu Qiang，Wang Jialong，Yu Hongshan，et al.PL-VINS：real-time monocular visual-inertial slam with point and line features［EB/OL］.（2022-04-15）.https：//arxiv.org/abs/2009.07462.

［16］赵伟博，田军委，王沁，等.基于点线特征融合的视觉惯性SLAM算法［J］.计算机应用研究，2023，40（2）：445-449.（Zhao Weibo，Tian Junwei，Wang Qin，et al.Visual inertial SLAM algorithm based on point-line feature fusion［J］.Application Research of Compu-ters，2023，40（2）：445-449.）

［17］Clark R，Wang Sen，Wen Hongkai，et al.VINET：visual-inertial odo-metry as a sequence-to-sequence learning problem［C］//Proc of AAAI Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2017：3995-4001.

［18］Zuo Xing Xing，Merrill N，Li Wei，et al.CodeVIO：visual-inertial odometry with learned optimizable dense depth［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2021：14382-14388.

［19］Cuneyt A，Cihan T.EDLines：a real-time line segment detector with a false detection control［J］.Pattern Recognition Letters，2011，23（13）：1633-1642.

［20］Zhang Lilian，Koch R.An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency［J］.Journal of Visual Communication and Image Representation，2013，24（7）：794-805.

［21］Elaziz M A，Heidari A A，Fujita H，et al.A competitive chain-based Harris hawks optimizer for global optimization and multi-level image thresholding problems［J］.Applied Soft Computing，2020，95：106347.

［22］Bartoli A，Sturm P.The 3D line motion matrix and alignment of line reconstructions［J］.International Journal of Computer Vision，2004，57（3）：159-178.

［23］付煜，郑爽，别桐，等.融合点线特征的视觉惯性SLAM算法［J］.计算机应用研究，2022，39（2）：349-355.（Fu Yu，Zheng Shuang，Bie Tong，et al.Visual inertial SLAM algorithm fusing point and line feature［J］.Application Research of Computers，2022，39（2）：349-355.）

［24］Rainer K，Giorgio G，Hauke S，et al.g2o：a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607-3613.