一种基于激光视觉融合的防冲钻孔机器人同步定位与地图构建方法

本发明涉及一种同步定位与地图构建方法，具体为一种基于激光与视觉融合的防冲钻孔机器人同步定位与地图构建方法，属于井下防冲钻孔机器人。

背景技术：

1、防冲钻孔机器人是煤炭井下预防冲击地压事故的关键设备，但是目前，井下防冲钻孔机器人仍需要人工进行现场操控，卸压作业需要人员进入危险区域，卸压过程中容易诱发冲击地压灾害，对施工人员的生命安全造成严重威胁，不仅会影响煤矿的安全生产，而且会造成十分惨重的人员伤亡和经济损失。

2、同步定位与地图构建方法是移动机器人实现自主移动的必要前提，但目前基于激光雷达的同步定位与地图构建方法存在需要用大量的变量来求解、在退化场景中扫描匹配会失败等缺点；基于视觉的同步定位与地图构建方法存在视觉特征点过少会失效等缺点。

技术实现思路

1、本发明的目的就在于为了解决上述至少一个技术问题而提供一种基于激光视觉融合的防冲钻孔机器人同步定位与地图构建方法，能够提高系统的鲁棒性和准确性、提高定位的精度、改善地图的质量以及精度，为移动机器人自主导航提供扎实前提基础。

2、本发明通过以下技术方案来实现上述目的：一种基于激光视觉融合的防冲钻孔机器人同步定位与地图构建方法，包括防冲钻孔机器人，所述防冲钻孔机器人包括深度相机、激光雷达、履带式移动底盘、旋转平台、下液压支架、虎钳、马达、上液压支架、钻杆箱、上杆平台以及履带底盘顶板；

3、所述深度相机和激光雷达均安装在履带底盘顶板的一端，所述履带底盘顶板下方安装有履带式移动底盘，所述深度相机和激光雷达获取防冲钻孔机器人周边环境信息进行位姿估计与地图构建，基于构建好的地图和位姿数据通过路径规划算法给予目标点即机器人通过从地图中获取的位姿信息发送至plc，通过该plc控制液压马达驱动履带式移动底盘进行移动，所述履带底盘顶板上安装有旋转平台，所述下液压支架和上液压支架分别支撑在巷道地面与巷道上顶层进行机器人车体固定，且所述旋转平台上安装有虎钳、马达以及钻杆箱，所述钻杆箱内的钻杆伸出后通过上杆平台输送至虎钳夹紧，并由马达驱动钻杆转动；

4、所述防冲钻孔机器人同步定位与地图构建的方法包括：

5、部分一，通过视觉激光里程计构建新的描述子，并求解出对应两帧图像对应描述子间的位姿变换关系：

6、包含三大模块：投影描述子模块、特征匹配模块以及位姿估计模块；将去除畸变后的激光点云投影在相机平面上，构建新的特征描述子，运用视觉里程计的方法匹配计算对应两帧图像下的位姿变化矩阵，将其作为激光惯性里程计在帧间匹配的位姿变换初值；

7、部分二，通过激光惯性里程计消除激光雷达的运动畸变，对点云进行特征提取与匹配，并求解出当前激光雷达的位姿：

8、包含点云畸变消除模块、线面特征提取模块以及点云匹配模块；运用imu预积分对激光雷达数据进行预处理，消除激光点云的畸变，对去除畸变后的激光点云的线、面特征进行提取，在获得点云的线、面特征后，匹配相邻帧之间的线、面特征，通过视觉激光里程计所提供的位姿变换初值以及lm法，求解最小化距离代价函数得到当前激光雷达的位姿；

9、部分三，通过匹配历史关键帧进行回环检测：

10、通过里程计获得帧间有效位姿估计后，筛选出有有效关键帧作为局部地图，运动kd-tree的方法将其存储，将存储的历史帧和当前关键帧进行匹配，当两帧之间的特征点之间满足一定的阈值关系这形成回环，并对回环进行优化，以实现回环检测功能；

11、部分四，通过位姿图优化的方法对回环检测以及里程计位姿估计的结果进行优化，最终输出位姿数据和地图：

12、通过里程计对位姿有效估计后仍然存在误差，采用位姿图优化的方法对回环检测以及里程计位姿估计的结果进行优化，最后输出位姿数据和地图。

13、作为本发明再进一步的方案：部分一中投影描述子模块包括：采用bruef描述子将去除运动畸变后的激光雷达点云对应到相机平面中进行特征描述，其步骤如下：

14、①以激光雷达点云在相机平台下的坐标pc所对应的像素点为中心，取其领域大小为d×d的集合ρ；

15、②在该集合ρ选取一对点对(x,y)，其对应像素值分别为ρ(x)和ρ(y)，比对两者像素值大小通过下述的公式进行二进制赋值：

16、

17、③在集合ρ中随机选取n对像素点，多次运用步骤②进行二进制赋值，最终将该集合构建为一个二进制编码，这个编码则是该特征点的特征描述子，可由以下n维二进制向量表示：

18、

19、作为本发明再进一步的方案：部分一中特征匹配模块包括：通过该特征点所对应特征描述子的编码特征，采用bruteforce算法将相邻两帧图像中表示同一特征的特征点相匹配构成相对应的特征点对，其步骤如下：

20、①随机从特征集合与中抽取四组不共线的像素坐标，计算对应的单应矩阵f,如下所示：

21、

22、②通过单应矩阵f测试所有两集合间的特征点，并构建最小化投影误差函数，如下所示，若ξ小于设定的阈值，则将特征点对加入到内点集合i中；

23、

24、③若内点集合i中的特征点对个数大于最优内点集合ib,则更新ib＝i，并且更新迭代次数，若迭代的次数小于所设定的阈值，则进行迭代循环，若迭代的次数大于所设定的阈值，则退出循环并输出内点集合中的特征点对。

25、作为本发明再进一步的方案：部分一中位姿估计模块包括：将特征匹配后的两帧图像中对应匹配的相同描述子运用对极几何的原理，求解相机在两帧图像对应位姿变换，并通过相机与激光雷达所标定外参，进一步估计激光雷达的相对位姿变换。

26、作为本发明再进一步的方案：部分二中点云畸变消除模块包括：运用imu预积分对激光雷达数据进行预处理，并且运用imu测量值，反向推演插值一个完整激光雷达帧中的每一激光点的准确距离从而消除激光点云的畸变。

27、作为本发明再进一步的方案：部分二中线面特征提取模块包括：通过以下公式计算特征点的平滑程度，将特征不明显的激光雷达点云数据进行忽略并提取出环境轮廓中的线、面特征，通过设定平滑度阈值来判定特征点为线特征点亦或是面特征点，当计算出的平滑度小于平滑度阈值时，则判定为面特征点；当计算出的平滑度大于平滑度阈值时，则判定为线特征点。

28、

29、作为本发明再进一步的方案：部分二中点云匹配模块包括：采用icp点云帧间匹配算法，求解出两帧激光点云配对关系未知情况下的帧与帧间的激光雷达位姿变换，其步骤如下：

30、①构建线条特征点距离函数和面特征点距离函数如下所示：

31、

32、

33、②通过激光雷达相邻帧之间的位姿变换关系，可以获得相邻两帧之间线、面特征点的位姿变换关系，如下所示：

34、

35、③采用lm算法求解出线条特征点距离函数和面特征点距离函数的最小值，即可求解出位姿变换矩阵

36、作为本发明再进一步的方案：部分三中包括：有效关键帧选取、kd-tree的构建和回环检测；

37、有效关键帧选取步骤如下：

38、②判定当前k-1(k≥1)帧是否为第一帧，若为第一帧执行步骤②；

39、②由点云帧间匹配算得相邻帧特征点云集合dk和dk-1间的位姿变换矩阵计算特征点云dk与经投影变换后的特征点云相同的线面特征点个数占dk-1特征点个数的比例。

40、③若相同特征点个数比例小于70％，则将第k帧点云作为有效关键帧，放入kd-tree中。若大于等于70％，则继续进行帧间匹配。并将下一帧第k+1点云通过投影在上一关键帧k-1的坐标下，即重复执行步骤②，直至其相同特征点个数比例小于70％；

41、kd-tree的构建步骤如下：

42、①求取多个维度数据的方差，选择方差最大的维度记为k；

43、②以该维度集合数据中的中位数作为分界线d，以分界线对该集合进行划分为两个集合，同时将其作为树节点存储起来；

44、③对所得两集合重复进行步骤②的工作，以公式对分界线d进行更新，直至所有子集合不可再分，则将该集合作为子节点进行存储；

45、回环检测分为判断是否形成回环和回环优化两大部分：

46、其中，判断是否形成回环步骤如下：

47、①设定最近关键帧个数阈值kvpt＝30，当历史关键帧个数达到30个以上时则进行步骤②，否则循环步骤①；

48、②依据激光雷达里程计对本体位姿估计的结果，当出现关键帧与历史关键帧中所对应位姿所处位置小于l/30(m)时则进行步骤③，否则返回步骤①；

49、③运用scantomap的方法对关键帧与历史关键帧对应存储在kd-tree中的局部地图的特征点进行匹配。若两者间的线、面特征点距离d小于阈值0.05(m)时判断形成了回环。若并未小于阈值，则返回步骤①；

50、其中，回环优化采用位姿图优化的方法对回环进行优化。

51、本发明的有益效果是：

52、1、与传统单一传感器的同步定位与地图构建方法比较，基于激光视觉融合的防冲钻孔机器人同步定位与地图构建方法可以通过两个传感器的优势互相弥补对方的缺点，最终获得更好的地图和位姿，提高系统的鲁棒性和准确性、提高定位的精度、改善地图的质量以及精度；

53、2、本发明具有通用性，可以适用于不同种类、不同型号的传感器；

54、3、本发明采用激光雷达和相机相融合，弥补了基于单一传感器的同步定位与地图构建方法所存在的缺陷，大大提高了系统的安全性；

55、4、本发明可适应煤矿井下环境，为防冲钻孔机器人自主导航系统提供的理论基础。