一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法技术

本发明专利技术公开了一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法。本方法为：1)对每张图片提取ORB特征；2)利用相邻图片的特征匹配跟踪相机姿态；3)对跟踪丢失的图片执行重定位与重跟踪策略；4)若重定位成功执行，则估计当前相机的姿态，且停止执行重跟踪策略并删除其产生的临时变量；5)若重跟踪策略成功执行，则产生一条新的轨迹；6)对产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；8)定位系统结束时，对所述轨迹进行筛选，得到一条姿态正确的轨迹。本发明专利技术的优势在于能在快速移动、遮挡、纹理不足、光照变化的条件下，定位完整的相机轨迹。

A method for simultaneous localization and map building of monocular vision robot based on re tracking strategy

The invention discloses a method for synchronously locating and mapping a monocular vision robot based on the re tracking strategy. The method is as follows: 1) ORB feature extraction for each picture; 2) features using neighboring images matching tracking camera attitude; 3) implementation of relocation of the tracking lost pictures and heavy tracking strategy; 4) if the relocation is successfully executed, the current estimate of the camera's pose, implementation of tracking strategy and delete the temporary variable produce and stop; 5) if the tracking strategy succeeds, it generates a new trace; 6) the number of generated path to determine if the number exceeds the threshold, the initial trajectory will be eliminated; 7) for each key frame loop detection, detection after the success of track fusion; 8) positioning system at the end of the path selection, a correct attitude trajectory. The advantage of the invention is that the complete camera trajectory can be located in the condition of rapid moving, occlusion, insufficient texture and illumination change.

【技术实现步骤摘要】
一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法
本专利技术属于基于单摄像头的移动机器人自主定位与导航领域，特别涉及一种基于重跟踪策略的单目视觉同步定位与地图构建方法。
技术介绍
随着计算机技术和人工智能的发展，机器人的研究得到越来越多的关注和投入，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。同步定位与地图构建(simultaneouslocalizationandmapping,SLAM)问题是指移动机器人在未知环境中，利用自身的传感器(如激光雷达、里程计、摄像头、超声波等)创建一个与环境相一致的地图，并同时利用该地图对机器人的位置和姿态进行估计。该问题被认为能否真正实现机器人自主导航的关键问题。近年来，随着计算机视觉技术的发展以及计算机运算能力的增强，出现了大量利用摄像机作为传感器的视觉SLAM方法。视觉传感器具有信息量大、灵活性高、成本低等优点，这是其他传感器所无法比拟的。单目视觉SLAM仅使用一个摄像头作为传感器完成同步定位与地图创建操作，具有系统结构简单、成本低且易实现等优点。因此，基于单目视觉SLAM技术具有广阔的应用前景。现有的解决单目SLAM的方法主要分为基于概率框架的单目视觉SLAM方法和非概率框架的单目视觉SLAM方法。基于概率框架的单目视觉SLAM方法主要通过递归的贝叶斯滤波方法估计相机姿态和地图特征的空间位置，常用的解决方法有基于扩展卡尔曼滤波器(extendedKalmanfilter)或粒子滤波器(particlefilter)。非概率框架的单目视觉SLAM系统目前主要是基于关键帧和稀疏捆绑调整等优化算法实现，常用的解决方法有直接法或基于特征的方法。非概率框架的单目视觉SLAM系统要求相邻两帧的基线较短(smallbaseline)，以便系统能够估计每帧的姿态。当跟踪丢失(trackinglost)，即当前帧的姿态无法被正确估计时，系统采用重定位策略，只有当机器人回到曾经经过的地点时，SLAM系统才能继续定位相机的位置并重建周围环境。这种策略的缺点在于从跟踪丢失到重定位成功之间，相机的姿态和地图无法重建出来。
技术实现思路
针对现有技术存在的技术问题，本专利技术的目的是提供一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法；本专利技术致力于解决传统方法中由于使用重定位策略导致跟踪丢失后部分相机姿态和环境无法估计与建模的问题，创新性地提出了一种重跟踪策略，提高了相机姿态估计的成功率以及地图重建的完整率。本专利技术采用的技术方案如下：一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，其步骤为：1)输入摄像头捕获的场景图像，对每张图片提取ORB特征；2)利用相邻图片的特征匹配跟踪相机姿态并估计特征点的深度信息；因特征匹配失败而导致相机姿态无法被估计定义为跟踪丢失；3)对所述跟踪丢失的图片执行重定位与重跟踪策略；所述重跟踪策略采用局部初始化算法；4)若所述重定位成功执行，则估计当前相机的姿态，且停止执行所述重跟踪策略并删除其产生的临时变量；5)若所述重跟踪成功执行，则产生一条新的轨迹；6)对所述产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；8)定位系统结束时，对所述轨迹进行筛选，得到一条姿态正确的轨迹。上述方法对跟踪丢失这种情况采用了重定位与重跟踪(局部初始化)两种策略。下面进一步具体说明本专利技术的
技术实现思路
：1.局部初始化本专利技术的局部初始化算法改进于ORB-SLAM(参考文献：R.Mur-Artal,J.M.MMontiel,andJ.D.Tardos.ORB-SLAM:aversatileandaccuratemonocularSLAMsystem.IEEETransactionsonRobotics,2015)的自动地图初始化算法。当系统检测到跟踪丢失时，局部初始化的过程如下：a)查找初始匹配点对：提取当前帧的ORB特征，搜索当前帧与参考帧(首次迭代时，以跟踪丢失前的最后一个关键帧为参考帧)的特征点匹配个数。若匹配数量大于设定阈值，则执行b)；否则，将当前帧设置为参考帧，重新执行a)。b)模型选择：采用RANSAC(RandomSampleConsensus，随机抽样一致性)策略，分别用八点法与四点法估计基本矩阵F(Foundamentalmatrix)与单应矩阵H(Homographymatrix)，并计算相应的重投影误差，选择误差小的矩阵作为系统模型。c)运动估计与地图构建：利用上述b)中选择的模型，计算相机的姿态以及构建环境地图。2.轨迹修正与融合Loopclosure即轨迹闭环，是指相机捕获的当前场景与先前走过的场景一样，即相机回到曾经经过的地方。利用该闭环，可以大大消除累计误差。具体步骤如下：a)计算当前关键帧c与闭环关键帧l的相似变换Sc,l，获取累计误差：其中，sc,l为正实数，表明尺度因子；Rc,l与tc,l分别为旋转矩阵与平移矩阵，Sim(3)为相似变换群。b)用Sc,l修正当前关键帧的姿态Tc,w：其中w表示世界坐标系，c表示当前关键帧，l表示闭环关键帧，Tl,w为闭环关键帧l的姿态，为修正后的当前关键帧的姿态。c)修正当前关键帧所在轨迹的其余关键帧。该步骤分为两种情况：一、若当前关键帧与闭环关键帧属于同一条轨迹，则修正当前关键帧的所有邻接关键帧，该过程与ORB-SLAM的方法一致；二、若当前关键帧与闭环关键帧属于不同的轨迹，则按从后向前的顺序修正当前轨迹上的所有关键帧，并将当前轨迹融合进闭环轨迹。该过程分两步执行，首先计算当前轨迹上相邻关键帧的相对姿态：其中σ＝(1,2,...,n-1),n为当前轨迹的关键帧数量；是Tc-σ,w的逆矩阵，Tc-σ,w是第c-σ帧在世界坐标系w的姿态。其次，利用计算出的相对姿态从后向前修正当前轨迹剩余关键帧的姿态：其中σ＝(1,2,...,n-1),n为当前轨迹的关键帧数量；是ΔTc-σ+1,c-σ的逆矩阵d)将当前轨迹的标签更改为闭环关键帧所在轨迹的标签。3.轨迹删除实际应用中，遮挡、抖动、纹理不足等状况可能使跟踪丢失的情况频繁出现，进而使SLAM系统产生多条轨迹。然而，SLAM系统只需要输出一条准确的相机轨迹，因此需要删除多余的相机轨迹。本专利技术的有益效果是：本专利技术提供了一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，能够解决传统方法中由于使用重定位策略导致跟踪丢失后部分相机姿态和环境无法估计与建模的问题，通过重跟踪策略提高了相机姿态估计的成功率以及地图重建的完整率。本专利技术的优势在于能在快速移动、遮挡、纹理不足、光照变化的条件下，定位完整的相机轨迹。附图说明图1.本专利技术方法的总体框架图；图2.本专利技术方法中生成新轨迹以及修正与融合轨迹的示意图。具体实施方式下面通过具体实施例和附图，对本专利技术做进一步说明。参考图1，本专利技术流程图的具体设计如下：1.输入摄像头捕获的场景图像：本专利技术以30fps的速率处理普通数码摄像机采集的图像。需要注意的是，由于纯旋转运动没有时差，不能估计特征点的深度，因此要求系统开始运行时摄像机有一定的水平运动，且运动速度不宜太快，以便于完成系统的初始化。2.判断是否跟踪丢失若通过上一帧图像无法估计出本文档来自技高网...

【技术保护点】
一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，其步骤为：1)输入摄像头捕获的场景图像，对每张图片提取ORB特征；2)利用相邻图片的特征匹配跟踪相机姿态；3)在跟踪丢失时，对跟踪丢失的图片执行重定位与重跟踪策略；所述重跟踪策略采用局部初始化算法；4)若所述重定位成功执行，则估计当前相机的姿态，且停止执行所述重跟踪策略并删除其产生的临时变量；5)若所述重跟踪策略成功执行，则产生一条新的轨迹；6)对产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；8)对轨迹进行筛选，删除冗余轨迹，得到一条姿态正确的轨迹。

【技术特征摘要】
1.一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法，其步骤为：1)输入摄像头捕获的场景图像，对每张图片提取ORB特征；2)利用相邻图片的特征匹配跟踪相机姿态；3)在跟踪丢失时，对跟踪丢失的图片执行重定位与重跟踪策略；所述重跟踪策略采用局部初始化算法；4)若所述重定位成功执行，则估计当前相机的姿态，且停止执行所述重跟踪策略并删除其产生的临时变量；5)若所述重跟踪策略成功执行，则产生一条新的轨迹；6)对产生的轨迹数量进行判定，若数量超过阈值，则将最初始的轨迹剔除；7)对每个关键帧进行闭环检测，检测成功后进行轨迹融合；8)对轨迹进行筛选，删除冗余轨迹，得到一条姿态正确的轨迹。2.如权利要求1所述的方法，其特征在于，其具体执行流程为：a)输入摄像头捕获的场景图像；b)判断是否跟踪丢失，若是则执行步骤c)，否则执行步骤d)；c)重定位当前图像，若成功，则删除临时变量，执行步骤d)；否则执行步骤e)；d)提取ORB特征并估计相机姿态；执行步骤g)；e)局部初始化相机轨迹，若成功，执行步骤f)，否则执行步骤g)；f)判断轨迹数量，若超过给定阈值，先删除标签最小的轨迹，再从当前位置生成新的轨迹；否则直接从当前位置生成新的轨迹；g)检测闭环，若形成闭环，则修正并融合轨迹；h)判断系统状态，若系统即将结束，则删除冗余轨迹；否则返回步骤a)。3.如权利要求1或2所述的方法，其特征在于，所述局部初始化包括以下步骤：a)查找初始匹配点对：提取当前帧的ORB特征，搜索当前...

【专利技术属性】
技术研发人员：刘宏，黄伟波，宋章军，张国栋，杨冰，吴观明，
申请(专利权)人：北京大学深圳研究生院，深圳市银星智能科技股份有限公司，
类型：发明
国别省市：广东,44