基于点线特征的单目视觉惯性里程计方法

技术领域

本发明涉及同步定位与建图(SLAM)领域，尤其是基于点线特征的单目视觉惯性里程计方法。

背景技术

同时定位与建图(simultaneous positioning and mapping，SLAM)是指机器人在未知环境中，通过对环境的检测，完成自身的定位及并同时构建周围地图的过程。近十年中，SLAM技术和相关理论取得了长足的发展。由于纯视觉的SLAM方法存在无法在图像纹理少的地方工作，快速运动时会导致图像模糊的问题，而惯性传感器在相机运动时可获取加速度和角速度的信息。因此两种测量方法存在互补性，惯性测量单元可以补充视觉信息的不足，同时视觉信息可以矫正惯性传感器长时间工作而产生的漂移问题。因此将视觉与惯性信息进行组合定位是一个十分具有应用前景的技术，具有很强的研究价值。

目前，基于影像点特征的视觉SLAM技术一直是研究的热点内容，算法比较成熟。基于点特征的SLAM算法利用影像上特征点为基础，能够实时的进行特征跟踪、构图、闭环检测，完成同时定位与制图的全过程，是目前视觉SLAM领域最为领先的算法之一。由于点特征容易受到光照、噪声的干扰，且构建出的三维点地图比较稀疏，无法表达真实的场景结构，在点特征不够丰富的区域，又容易跟踪失败，因此能代表环境结构的线特征和面特征也逐渐引起了研究者们的关注，特别是在人造建筑比较多的区域，比如街道和室内环境等。相比较点特征而言，线特征受环境影响比较小且同时能够更好地表达更高语义的环境信息。因此，基于结构线的SLAM算法也是研究者们的热点研究对象。

由于特征提取和匹配会消耗较多的计算量，因此基于影像像素的视觉SLAM也逐渐引起了研究者们的关注，基于影像像素的视觉SLAM算法直接使用图像灰度信息进行影像跟踪，无需特征提取和描述，直接使用图像的梯度进行跟踪和优化，这在特征较少的区域可以增强视觉SLAM的连续性，但其完全依靠图像像素，对于光照变化较为强烈的区域效果比较差，且仅依靠图像像素梯度，计算得到的定位和构图精度也相对比较低。

因此，本发明提出一种基于点线特征的视觉惯性里程计方法，在前端分别提取点、线特征并进行匹配，采用紧耦合非线性优化的视觉惯性方法，并利用点线特征构建共同构建地图。

发明内容

本发明的目的是提出一种在特征点较少的情况下仍然保持较高精度的里程计方法，同时能够综合视觉图像帧中的特征点、线信息及IMU信息，以提高SLAM的鲁棒性和精确度。

系统由传感器设备、前端、回环检测、后端与建图四部分组成，如图1所示。

其中，传感器设备为单目摄像头和IMU，单目摄像头用来采集周围图像，IMU用来采集机器人移动信息。前端将传感器的数据抽象成适用于估计的模型。回环检测判断机器人是否经过已知的位置。而后端接受不同时刻前端测量的位姿和回环检测的信息并对它们进行优化，从而得到全局一致的轨迹。建图则是根据状态估计得到的轨迹建立与任务要求相对应的地图。

本发明所提出的基于点线特征的单目视觉惯导SLAM方法是在上述系统的基础上来实现的，具体实现步骤如下：

步骤1；传感器设备中单目摄像头采集当前视觉图像帧，IMU采集机器人移动信息。

步骤2，针对移动机器人平台上相机获取的视频帧影像数据，提取每一影像帧上的特征点和特征线，具体包括如下子步骤；

步骤2.1，对于特征点的提取，首先提取Harris角点，该特征点具有良好的旋转不变性和抗噪性；

步骤2.2，对于特征线的提取，采用LSD(linesegmentdetector)算法并计算LBD描述子。

步骤3，根据获取的特征点和特征线，进行特征点和特征线跟踪，并进行关键帧选取，具体包括如下子步骤：

步骤3.1，根据步骤2中获取的特征点，利用LK光流跟踪法进行特征点跟踪，获取较好的跟踪特征点对；

步骤3.2，根据步骤2中获取的特征线，利用LBD描述子并结合几何约束进行结构线跟踪，获取较理想的跟踪特征线对；

步骤3.3，利用当前的跟踪特征点对和跟踪结构线对，对当前的点、线特征重投影到当前帧，计算投影误差；

步骤3.4，对IMU的测量值进行IMU预积分,利用SFM进行单目视觉估计滑窗内所有帧的位姿以及路标点的逆深度,再通过视觉与IMU预积分融合对齐,紧耦合初始化求解初始化参数；

步骤3.5，根据步骤3.1中的跟踪特征点对和步骤3.3中优化的影像姿态，判断当前影像帧是否选取为关键帧，如果判断不是关键帧，返回到步骤2重新读取新的影像帧，如果判断是关键帧，则将其送入步骤4进行处理；

步骤4，根据获取的特征点和结构线的跟踪信息，进行周围环境空间点和空间线制图以及平台定位优化；

步骤5，判断平台运动轨迹是否形成闭环，获取正确的闭环关键帧，对全局的影像姿态和地图进行整体优化；

步骤6，输出全局地图。

与现有技术相比，本发明在特征点较少的情况下具有更好的鲁棒性和更高的精度。由于本专利采用点线特征提取代替传统视觉测量中的目标物体轮廓或单纯点特征，利用直线特征加工精度高、鲁棒性好、抗噪能力强以及信息量大便于优化等优点提高类圆柱体轴线求取精度，同时由于点、线特征个体之间不需准确定位、组合特征求取位姿精度不会因装配带来的误差有所下降，使得对分体式目标也同样具有较高的测量精度。因此本发明能够更好地适应生产环境下各种场景，具有更好的稳定性。实验结果表明，本发明在定位精度上也有所提升。

附图说明

图1为系统架构图。

图2为系统流程图。

图3为线特征重投影误差示意图。

图4为本发明一实施例中提取点线特征及匹配的示意图。

图5为为本发明一实施例中轨迹检测示意图。

具体实施方式

为了使本发明的目的、发明内容及优点更加清楚明白，以下结合附图对本发明作进一步的详细说明。实施案例的具体步骤如下：

步骤1：接收摄像头输入的当前视觉图像帧。

步骤2：提取所述当前视觉图像帧的特征点和特征线。

其中，特征点指的是在所述摄像设备所处的环境中，以点的形式存在的环境元素；特征线指的是在所述摄像设备所处的环境中，以线的形式存在的环境元素。

步骤2.1:首先对获取的视频帧建立金字塔，然后分别在金字塔图像上进行影像分块，获取一定窗口大小的影像区域，然后利用现有的特征点提取算法在每个分块里分别进行特征点提取和描述，考虑到Harris特征点提取算法速度快、特征点多，本发明选择的特征点表达为Harris特征点；

步骤2.2:对视频帧影像进行高斯滤波，然后使用LSD算子进行结构线提取；

步骤2.3:针对提取了特征点和结构线以后的影像进行分块处理，分别将特征点和结构线划分到不同的影像区域中，结构线的划分参考依据为影像的中心点。

步骤3:根据获取的特征点和结构线，进行特征点和结构线跟踪，并进行关键帧选取；

步骤3.1:根据步骤2中获取的特征点，利用特征描述符距离进行特征点跟踪，获取较好的跟踪特征点对；

步骤3.1.1:利用当前影像上的特征点，判断是否有候选匹配帧，如果有选取的候选匹配帧，则在其上开辟适当大小的影像窗口，获取其中的特征点作为候选匹配点，否则选取当前帧的上一帧作为候选匹配帧，进行相应候选匹配点的选取；

步骤3.1.2:计算当前影像上的特征点和候选匹配点的描述符距离，对特征点根据描述符距离按照升序排序，取前百分之六十作为最佳匹配点；

步骤3.1.3:利用RANSAC对获取的最佳匹配点进行错误点滤除；

步骤3.1.4:利用当前帧影像的姿态和候选匹配帧的姿态对最佳匹配点进行投影，计算其反投影误差；

步骤3.2:根据步骤2中获取的结构线，利用结构线的参数(中点位置、线的长短、角度方向)进行结构线跟踪，获取较理想的跟踪结构线对；

步骤3.2.1:利用当前影像上的结构线的中心，在待匹配关键帧上，开辟适当大小的影像窗口，获取结构线中心落在区域内的结构线作为候选匹配线；

步骤3.2.2:计算当前影像上的结构线和候选匹配线的参数差别，如线的长短、角度方向等，并利用当前帧的姿态和待匹配关键帧的姿态进行三维空间线重建，通过观测对应的三维空间线段的重叠度来进行结构线匹配；

步骤3.2.3:利用多张影像上的同名线段，通过对比空间3D线段的角度和位置相似性，选取最终较为相似的空间线段的匹配影像线对作为候选影像匹配线对，接着对候选影像匹配线对进行反投影，投影到相应的多张可视影像上，在各个影像上分别计算空间三维线段的投影线与影像上相应原有二维线段的误差，若大于一定的阈值，通常选取3个像素大小，则进行去除；

步骤3.3:利用当前的跟踪特征点对和跟踪结构线对，对当前的影像姿态进行初步优化；

通过使用Ceres优化库，即可对影像的初始姿态进行调整，直到函数优化能量项最小，这样即可得到的最优的影像姿态。

步骤3.4:根据优化的影像姿态和较为准确的匹配点对，判断当前影像帧是不是选取为关键帧。

步骤3.4.1:根据步骤3.1与步骤3.3分别求得的跟踪点对与姿态，重建三维空间点，判断是否新建三维空间点90％以上与之前关键帧生成的三维空间点重复，如果是，则设置为非关键帧，重新读取新的影像帧，回到步骤2,重新进行特征点、线跟踪与关键帧的判断，否则作为候选关键帧进入步骤3.4；

步骤4.1:计算与当前关键帧有共视关键帧的影像关键帧组。利用当前关键帧上二维匹配点重建的空间三维点，计算其他关键帧可看到对应三维点的数目来进行选取，并降序排序当肖LI影像关键桢组；

步骤4.2:当前关键帧与共视关键帧组里的关键帧进行特征点和结构线匹配，并滤除可视图像较少的三维点；

步骤4.2.1:计算当前关键帧与共视关键帧的特征点描述符距离，利用现有方法词典对特征点进行分层，然后在对应层上进行特征点匹配；

步骤4.2.2:计算当前关键帧与其共视关键帧的结构线的参数距离，进行结构线匹配，方法与步骤3.2相同；

步骤4.2.3:计算三维空间点的可视关键帧数目，将三维点投影到影像上，如果落在影像范围内，则认为该关键帧为候选可视关键帧，然后在此关键帧上寻找与当前三维点相似性比较高的特征点，如果找到，则认为该关键帧为真正关键帧，计算三维点的真正关键帧数与可视关键帧数两者的比值，如果小于一定的阈值(本算法采用0.8作为阈值)，则将该三维点进行滤出；

步骤4.3:根据步骤4.2中新增加的特征匹配点和匹配线，生成三维空间点和对应的三维空间线；

步骤4.3.1:利用三角测量原理与关键帧的姿态，生成新的空间三维点，并计算反投影误差，去除错误匹配点对；

步骤4.3.2:利用关键帧的姿态和匹配的结构线，生成空间三维线，通过空间线的相似性去除错误匹配线；

步骤4.4:利用生成三维空间点和对应的三维空间线，对当前的影像姿态进行进一步优化，该步骤参考步骤3.3；

步骤4.5:针对优化后的影像关键帧，若其对应的空间三维点里有90％的三维点被其他共视关键帧所看到，对该关键帧进行滤除。

步骤5:进行回环检测，即判断运动平台是否运动了之前已经运动到的区域，如果发现到了之前运动区域，则运动轨迹形成闭环，即可利用该闭环去除大规模范围内的影像漂移误差。

步骤6：输出全局地图。