一种基于RGB-D相机的改进ORB-SLAM2算法地图构建方法

本发明涉及地图构建，具体为一种基于rgb-d相机的改进orb-slam2算法地图构建方法。

背景技术：

1、随着机器人技术的迅猛发展，移动机器人在制造业、工业以及服务业等各个领域的应用越来越广泛，同时定位与建图(simultaneous localization and mapping,slam)技术是移动机器人实现自主路径规划的核心[1-5]。slam技术按照具体搭载的传感器类型分成激光slam和视觉slam[6-9]，精度较高的雷达传感器价格昂贵且功耗较高，而搭载相机作为传感器的slam方案相比起来具有设备成本低、体积小等优点，可以提供鲁棒性更好的视觉图像识别信息，因此视觉slam以更高的性价比逐渐成为研究者们的主流方案[10-12]。

2、2014年，endres团队[13]通过使用低成本的rgb-d相机获取视觉信息，设计完成了rgb-d slamv2算法并利用深度相机的特性构建稠密点云地图。2015年，mur-artal等[14]人首次提出了全新的基于功能的单目同时定位和建图系统orb-slam算法，在ptam算法的基础上增加了地图的预处理初始化和回环检测等模块，具有实时性高和良好的鲁棒性，显著提升了构建地图的准确性。2017年，mur-artal团队[15]为了增加使用相机的多样性，在orb-slam方案的基础上又提出了orb-slam2算法，在原来的回环检测环节后增加了全局ba环节，用光速流差法进一步优化了后端，支持单目、双目和rgb-d等多种相机模式，另外还提出分格提取特征点的方法，增加系统的鲁棒性。

3、与其他的视觉slam算法方案相比，orb-slam2算法具有定位精确度高、优越的实时性、系统框架清晰、支持多种相机模式等特点，受到广大学者的喜爱并成为主流的视觉slam方案之一。qin等[16]人针对rgb-d深度orb-slam2视觉里程计的累积误差导致相机跟踪丢失和轨迹漂移等问题，改进了视觉里程计来优化累积误差，提出了一种自适应阈值算法从图像中提取特征点，所提方法具有较好的鲁棒性和较高的图像匹配精度；niu等[17]人针对orb-slam2只能通过orb特征提取解决相机姿势来构建地图的问题，提出了采用基于光强的直接方法求解相机姿态并构建稠密地图，大大提高了场景的处理速度和重建密度；zhang等[18]人为了以更低的成本快速获得准确的室内三维稠密地图，提出了一种改进的orb-slam2稠密地图优化算法，所构建的地图可直接用于导航，精度更高，跟踪时间短，内存更小；luo等[19]人针对运动场景及图像纹理信息不丰富的情况下slam系统无法定位建图的问题，提出了一种基于信息熵和锐化处理的改进orb-slam2算法，相对轨迹误差相比原系统降低了36.1％，绝对轨迹误差降低了45.1％。

4、针对传统orb-slam2算法在动态场景下运动速度过高产生机身抖动或在转角拐弯过快的情况下，会由于关键帧信息过少导致跟踪定位丢失、相机轨迹产生误差以及构建的稀疏点云地图不能直接应用于导航和路径规划等问题，提出了一种基于rgb-d相机的改进关键帧选取的orb-slam2算法。

技术实现思路

1、针对现有技术的不足，本发明公开了一种基于rgb-d相机的改进orb-slam2算法地图构建方法，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明提供如下技术方案：一种基于rgb-d相机的改进orb-slam2算法地图构建方法，包括以下步骤：

3、s1、首先从rgb-d相机tum获取的图像，并在传统orb-slam2框架的基础上对相邻的图像帧中新增帧间相对运动量对关键帧进行筛选；

4、s2、同时新增特征点跟踪进行判定，从而提高关键帧选取的准确率；

5、s3、然后在原本系统三线程的基础上添加稠密点云建图线程；将关键帧位姿信息通过相机成像模型计算出对应关键帧点云的深度信息，将改进关键帧选取后的orb-slam2算法构建出稠密点云地图并转换压缩成八叉树地图。

6、优选的，所述帧间相对运动量包括平移转换因数和转角因数。

7、优选的，所述转角因数为主要变化量，所述平移转换因数为次要变化量，具体的计算如下：

8、假设视觉里程计获取得到两组点云特征点p＝{p2,p2,…,pn}和p'＝{p'2,p'2,…,p'n}，并且点云特征点p和p'中的每个点都通过下标一一进行数据关联；在没有噪声的情况下，点云特征点p和p'之间满足p'＝r·pi+t；在考虑噪声的情况下，通过构建最小二乘问题解出(r,t)，如下式(1)所示：

9、

10、将点云点云特征点p和p'去质心后，再构建最小二乘问题，结果是等价的，如下式(2)所示，式中为两个加法项，让每个项都最小化的同时整体也就随之变小；

11、

12、其中，

13、代入式(2)中对式(1)进行化简得：

14、

15、并对式(3)中第一个加法项展开化简：

16、

17、将式(4)中与r无关的项去除，化简后得：

18、

19、令利用svd进行分解得到svd(m)＝usvt；其中s为奇异值构成的对角矩阵，当m为满秩时，从而求得r＝uvt；再将求出的r代入式(3)的第二个加法项得到其中r为当前帧与上一个关键帧之间的帧间转角矩阵，t为二者的平移向量；

20、通过上述推导得到的r和t计算出帧间相对运动量d，并在系统中设定阈值q，如式(6)所示：

21、d＝(α+β)||δt||+γmin(2π-||r||,||r||)         (6)

22、其中，α+β为综合平移转换因数，α为相机的左右平移转换因数，β为相机的前后平移转换因数；γ为转角因数，它的大小变化与转角的变化为指数关系，即转角变大，γ变大，并且α+β与γ无相关性，在帧间相对运动的过程中，γ为主要变化量，α+β为次要变化量，当转角变化量较大时，帧间相对运动量由γmin(2π-||r||,||r||)决定，相机转角的取值范围为0～90°，转角大于90°时，相机会出现跟踪丢失。

23、优选的，将计算得到的帧间相对运动量d与预设的阈值q进行作比较：

24、当d＜q时，framerefkey≠framecur；其中，framecur表示当前帧，framerefkey表示参考待选关键帧；

25、当d≥q时，将当前帧判定为参考待选关键帧，即framerefkey＝framecur，并进行特征点跟踪匹配计算，把上一时刻的参考待选关键帧与当前的参考待选关键帧代入式(7)中计算图像帧的相关匹配性：

26、tracking(framerefkey-1,framerefkey)＞ε      (7)

27、式中的tracking函数计算出上一时刻的参考待选关键帧与当前的参考待选关键帧的特征点跟踪数量，ε为可调的特征点匹配度参数；

28、当上式条件满足时，说明当前的参考待选关键帧特征点匹配度高，符合关键帧的相关性要求，随之将当前的参考待选关键帧赋值为关键帧并插入关键帧，即framekey＝framerefkey，framekey表示关键帧。

29、优选的，相机里程计通过tracking线程采集当前图像帧的信息，其中包括图像特征点、深度信息以及位姿信息，系统在执行插入关键帧选取流程前首先要判断前置条件是否满足：系统距离上一次重定位较远，同时不在定位模式下和全局闭环中，并且图像帧的内点数大于预设的最小值。

30、优选的，所述原本系统三线程包括追踪线程、局部建图线程以及闭环线程：

31、追踪线程：从相机tum获取图像，并输出每帧图像的相机位姿信息用于定位，将当前帧图像与一系列关键帧图像进行快速匹配或是重定位进行初始位姿预测，通过使用pnp的ransac迭代剔除误匹配，优化当前帧的位姿信息，最后在相邻的图像帧中新增转角因数和平移转换因数等对关键帧进行筛选，结合特征点跟踪点判定挑选新的关键帧；

32、局部建图线程：将追踪线程挑选的关键帧存放在缓冲区内，并不断从缓冲区中取出并插入地图中，对近期的地图点进行筛选，重建新的地图点放入局部ba中优化，最后再一次对关键帧进行筛选，剔除冗余的关键帧，保证地图中关键帧的鲁棒性；

33、闭环线程：局部建图线程中处理后的每个关键帧都要送入闭环线程，通过回环检测并对全局地图进行回环修正；闭环线程主要包括回环检测的查询数据库和计算转移矩阵，以及回环修正中的位姿图优化和回环融合。

34、优选的，稠密点云地图的构建，具体包括以下：

35、对于相机采集的每一帧关键帧信息，通过相机物理成像模型对齐深度图像信息和彩色图像信息，并结合相机本身的内参信息得到每一关键帧的图像像素点信息；首先定义已知像素点的坐标为x＝[u,v,1]，其在对应空间的三维点坐标为xi＝[xi,yi,zi]，根据针孔成像模型得到：

36、

37、其中，η为深度信息与实际距离的比例关系，k为相机标定后的内参数值，分别用fx,fy,cx,cy表示为式(9)，r为旋转矩阵，t为平移向量；

38、

39、接着结合深度信息计算得到每一关键帧的图像点云数据信息：

40、

41、其中，d表示实际空间下的测量距离，m＝[mx,my,mz]为图像的点云数据点坐标，得到每一个像素点对应的点云数据坐标并构成稠密点云地图，由于稠密点云地图的存储规模过大，占据过多内存的同时也无法直接用于导航，便在此基础上引入八叉树地图。

42、优选的，构建八叉树地图，具体包括通过用概率对数值(log-odds)的形式表示节点被占据的情况，如下式所示：

43、

44、其中，x∈(0,1)，l∈r为概率对数值，那么当节点占据的可能性越高时，l值越大，节点未被占据的可能性越大时，l值越小，设该节点为m，相应的观测数值为z，那么从0时刻到t时刻节点的概率对数值可表示为l(m|z1:t)，以此类推t+1时刻的可如下式表示：

45、l(m|z1:t+1)＝l(m|z1:t-1)+l(m|zt)    (12)

46、则推算出各个节点对应时刻的对数概率值，并将所得的概率值结合深度信息对八叉树地图进行更新，通过不断维护八叉树地图处理运动物体的拖影，实现实时构建八叉树地图。

47、优选的，在八叉树中的每一个节点均存储占据信息，且通过用0表示节点未被占据，1表示全部被占据，另外由于外部干扰条件的影响，会造成部分节点信息在未被占据和占据往返跳动，甚至会出现未知的状态。

48、与现有技术相比，本发明的有益效果：

49、1、本发明在传统orb-slam2算法的基础上，在相邻的图像帧中添加了帧间相对运动量对关键帧进行筛选，并增加了特征点跟踪进一步提升关键帧选取的准确度，在tum数据集下的实验验证表明，在系统实时性没有较大改变的前提下，改进关键帧选取的orb-slam2算法相较于原算法最大平均轨迹误差减少了57.8％，有效提高了跟踪定位的精度。

50、2、本发明在原系统三线程的基础上添加了稠密点云地图和八叉树地图线程，将改进关键帧选取后的orb-slam2算法构建出稠密点云地图并转换压缩成八叉树地图，构建出的八叉树地图能以较低的内存量进行存储并且可以直接用于机器人的导航规划。