一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质

本发明属于移动机器人同步定位，尤其涉及一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质。

背景技术：

1、矿山存在环境复杂、空间狭小和绝大部分是易燃、易爆、有毒、有腐蚀的危险品等环境因素。目前矿山的巡视和检修工作大部分都由人工完成，而矿山巡检机器人的出现，它可以用于替代人工进行安全检查、监测以及采集矿山中的信息等操作，使得矿山巡检更加先进、安全地进行矿山巡检。

2、为使得煤矿巡检机器人能够实现无人化、自动化运行，现有文件中公开了一种煤矿机器人用智能识别系统，该文件主要包括巡检端、云端及审核端；本发明，通过视觉及声音融合的方式实时检测变电所、水泵房内设备的运行状态，自动识别和记录设备显示屏上仪表显示的数值、开关状态、指示灯状态，搭载升降云台可对不同位置的目标进行识别，同时具备温湿度数据、烟雾、多种气体浓度参数采集等功能，将数字化的图像、声音和准确的数据实时分析并预警，实现巡检机器人的功能多样化。

3、但是，现有技术中，面对煤矿井下信号弱、光线差、gps定位无法使用等复杂多变的工作环境，目前大部分煤矿巡检机器人采用激光雷达来实现自动运行，由于煤矿井下光线不够均匀，使得视觉传感器采集的数据鲁棒性较差，故只有单一的视觉传感器无法满足煤矿井下精确的同步定位与地图构建，造成煤矿巡检机器人在执行巡检过程中，无法有效的进行作业，无法满足巡检要求。

技术实现思路

1、本发明提供一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法，方法实现对煤矿巡检机器人的定位，使得煤矿巡检机器人同步定位与地图更加精准、可靠。

2、方法包括：

3、s101：定义煤矿巡检机器人的功能模块；

4、s102：构建实现煤矿巡检机器人移动定位的地图，并基于地图分别建立世界坐标系、双目相机坐标系、imu坐标系、成像坐标系以及像素坐标系；构建imu坐标系与世界坐标系之间的转换矩阵；

5、还构建世界坐标系、双目相机坐标系与成像坐标系之间的转换矩阵以及构建imu坐标系与双目相机坐标系之间的转换矩阵；

6、s103：将双目相机和imu模块采集的原始数据传输到工控机中进行预处理，使imu模块采集的数据同双目视觉数据进行融合优化；

7、s104：对双目相机和imu模块进行初始化，并将双目视觉数据与imu数据对齐；

8、s105：构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据；

9、s106：配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。

10、进一步需要说明的是，所述s102还包括：

11、以煤矿巡检机器人启动时的位置为原点，建立世界坐标系ow-xw-yw-zw；

12、以双目相机的左目相机中心为基准，建立双目相机坐标系oc-xc-yc-zc；

13、将煤矿巡检机器人车头的朝向为x轴正方向，垂直于地面向上为z轴正方向，垂直于x轴向右为y轴正方向，建立imu坐标系ob-xb-yb-zb；

14、基于imu坐标系与双目相机坐标系之间的外参标定，使得imu数据通过平移旋转到双目相机坐标系，最终使imu坐标系与双目相机坐标系的数据构成关联关系。

15、进一步需要说明的是，所述s103中的预处理方式包括对imu数据进行预积分处理，具体步骤为：

16、s1031：建立imu测量模型与运动学模型；

17、建立加速度计和陀螺仪得到的测量值与真值之间的误差模型：

18、

19、其中，和分别表示加速度计和陀螺仪的测量值，a(t)和ω(t)分别表示两者的真值，ba(t)和bg(t)分别表示加速度计和陀螺仪的偏置，σa(t)和σg(t)分别表示二者的噪声项，σa(t)和σg(t)服从零均值的高斯分布；

20、用t表示采样时刻，对imu模块的运动学模型离散化后进行欧拉积分，得到如下方式：

21、

22、δt为imu模块两次相邻测量之间的时间间隔，exp()为李代数so(3)到李群so(3)的指数映射，σgd(t)和σad(t)分别代表σg(t)和σa(t)的离散形式；

23、s1032：对imu模块预积分处理；

24、在第i帧和第j帧图像之间有多个imu数据时，则将imu数据的第一帧与第i帧图像关键帧对齐，将imu数据的最后一帧与第j帧图像关键帧对齐，对齐之后，对这两个连续时间间隔为δt的图像关键帧之间的imu数据进行积分处理，预积分使得每项与起始状态无关，将参考坐标系从世界坐标系调整为第k个关键帧bk时刻，有：

25、

26、δrij的含义为从i帧时刻到j帧时刻旋转的预积分增量；

27、的含义为i帧时刻世界坐标系到imu坐标系的旋转矩阵的转置；

28、的含义为j帧时刻世界坐标系到imu坐标系的旋转矩阵；

29、的含义为k帧时刻陀螺仪的偏置；

30、的含义为k帧时刻陀螺仪噪声项的离散形式；

31、δvij的含义为从i帧时刻到j帧时刻线速度的预积分增量；

32、vj的含义为j帧时刻的线速度；

33、vi的含义为i帧时刻的线速度；

34、gw的含义为世界坐标下的重力加速度；

35、δrik的含义为i帧时刻到k帧时刻旋转的增量；

36、的含义为k帧时刻imu坐标系下的加速度；

37、的含义为k帧时刻加速度计的偏置；

38、的含义为k帧时刻加速度计噪声项的离散形式；

39、δpij的含义为i帧时刻到j帧时刻平移的预积分增量；

40、pj的含义为j帧时刻的平移量；

41、pi的含义为i帧时刻的平移量；

42、δvik的含义为i帧时刻到k帧时刻线速度的增量；

43、的含义为k帧时刻世界坐标系下的加速度；

44、的含义为k帧时刻陀螺仪的偏置；

45、i的含义为双目相机采集图像的第i帧时刻；

46、j的含义为双目相机采集图像的第j帧时刻；

47、k的含义为双目相机采集图像的第k帧时刻；

48、进一步需要说明的是，所述s103中，还对双目相机采集的图像数据进行特征提取与位姿解算处理，具体步骤为：

49、提取oriented fast关键点和steered brief描述子，并采用图像金字塔来确保特征点的尺度不变性，最后使用灰度质心法来计算关键点的主方向，统一将像素旋转到所述主方向，其中，灰度质心法求关键点旋转角度的计算方式如下：定义区域图像的矩为：

50、

51、其中，p，q取0或1，i(x，y)为像素坐标(x，y)处图像的灰度值，mpq表示图像的矩；

52、从图像圆形区域形心o指向质心c的方向向量即可为关键点的主方向，故关键点的旋转角度记为：θ＝arctan2(cy，cx)＝arctan2(m01，m10)；

53、方法中还对位姿进行解算，具体为：

54、由三维空间点的匹配关系，根据位姿变换关系得：pw＝rp′w+t；

55、其中pw和p′w分别表示前后两个图像中观测到的三维空间点。

56、首先计算两组三维空间点的质心位置，用p和p′表示，得：

57、

58、

59、再计算两组三维空间点的去质心坐标，用qw和q′w表示，得：

60、qw＝pw-p

61、q′w＝p′w-p′

62、然后将旋转参数作为优化变量计算旋转矩阵r，构建最小二乘问题r*得：

63、

64、最后由计算出的旋转矩阵r，计算平移矩阵t得：

65、t＝p-rp′。

66、进一步需要说明的是，所述s104还包括：通过双目相机和imu之间的旋转约束初始化陀螺仪偏置bias；通过双目相机和imu模块之间的平移约束来估计速度和重力方向；最后将双目相机坐标系的变量转化到世界坐标系下，将轨迹对齐到世界坐标系下；对齐之后在滑动窗口内采用非线性优化的方法进行数据融合。

67、进一步需要说明的是，所述s105还包括：

68、s1051：将待优化的状态量定义为：

69、

70、

71、λi＝[x，y，z]t，i∈[0，m]

72、滑动窗口内存在n个关键帧，x为状态向量，其中，λi为观测到的地图点，xi为关键帧对应的系统状态量，rbc为双目相机到imu的旋转外参；xk为滑动窗口内第k帧的状态量，imu预积分得到的旋转项为速度项为位移项为陀螺仪偏置为ba、加速度计偏置为ba；为双目相机到imu模块的外参。

73、s1052：构建imu约束，将imu两帧之间的位置、速度、旋转、加速度偏置bias以及陀螺仪偏置bias的变化量之差定义为imu残差；

74、s1053：构建双目视觉约束，像素重投影造成视觉残差，将首次投影在第i个相机坐标系中的第m个路标点的像素坐标值，变换至第j个坐标系中，变换后的像素坐标值与直接观测的像素坐标值构成残差具体如下：

75、

76、xcj，ycj，zcj的含义为第j个相机坐标系三维空间点的坐标(xcj，ycj，zcj)；

77、ucj，vcj的含义为第j个相机坐标系直接观测到的二维像素坐标(ucj，vcj)。

78、s1054：将先验信息、双目视觉约束信息和imu约束信息构成整体的目标函数，通过非线性优化的方法优化特征点在图像中的重投影误差和imu的测量误差。

79、进一步需要说明的是，测量误差的目标函数为：

80、

81、rp-jpx的含义为滑动窗口中边缘化的先验信息误差；

82、的含义为滑动窗口中imu的测量误差；

83、的含义为imu预积分产生的噪声协方差；

84、的含义为滑动窗口中的视觉观测误差；

85、的含义为视觉观测产生的噪声协方差；

86、c的含义为当前滑动窗口中一对匹配良好的至少观察到两次的特征点对。

87、进一步需要说明的是，所述s106还包括：使用oriented fast关键点进行特征检测，使用steered brief描述子来构建地图的单词，并利用dbow3词袋模型来进行存储；

88、具体包括：

89、设地图中所有的特征数量为α1，特征为xi的数量为αi1，那么单词的idf为：

90、设地图n中特征xi有αi2，则单词的tf为：

91、则权重为：ηi＝tfi×idfi；

92、地图n中每个特征的权重计算出来后，地图n中所有单词构成词袋模型vn，vn为：vn＝{(x1，η1)，(x2，η2)，...，(xn，ηn)}；

93、地图m和地图n的相似程度可用l1范数求解，如下式所示：

94、

95、xn的含义为地图n中的某一个特征；

96、ηn的含义为地图n中某个特征的权重；

97、vni的含义为地图n中i个特征构成的词袋模型；

98、vmi的含义为地图m中i个特征构成的的词袋模型。

99、本发明还提供一种终端机，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现所述基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法的步骤。

100、本发明还提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法的步骤。

101、从以上技术方案可以看出，本发明具有以下优点：

102、本发明提供的基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法可以实现多信息融合及同步定位。通过非线性优化的方法融合两种传感器的数据，解决了imu长时间运行产生漂移和双目视觉实时性差、不稳定的问题。本发明还在煤矿巡检机器人中配置了闭环检测模块，并构建关键帧数据库，使得煤矿巡检机器人移动到曾经到过的位置后，可以对当前帧数据与词袋模型中的关键帧数据进行匹配，进行全局优化，从而消除累计轨迹误差和地图误差，提高系统定位与建图精度，提升系统的鲁棒性。