一种将软管运输至洞孔中的方法、系统、设备及介质与流程

本发明涉及将软管运输至洞孔中的相关，尤其是涉及一种将软管运输至洞孔中的方法、系统、设备及介质。

背景技术：

1、目前，在使用乳化炸药进行爆破时，主要通过人工直接控制或遥控操控机械臂间接控制使得炸药软管对准深入至炸药填充洞孔中，该过程具有较高的安全隐患且人工遥控效率不高。

技术实现思路

1、本发明旨在至少解决现有技术中存在的技术问题。为此，本发明提出一种将软管运输至洞孔中的方法、系统、设备及介质，能够提升作业效率，减少安全隐患。

2、本发明的第一方面，提供了一种将软管运输至洞孔中的方法，包括如下步骤：

3、获取第一位置的第一洞孔彩色图像、第一位置的点云数据、深度相机的内参和深度相机的畸变系数，其中，所述第一洞孔彩色图像为所述深度相机在所述第一位置拍摄的图像，所述第一位置为与待检测洞孔的距离值为预先设置的第一距离值的位置，所述第一位置的点云数据为所述深度相机在所述第一位置拍摄的点云数据；

4、将所述第一洞孔彩色图像输入训练好的yolov5模型进行第一次彩色图像识别，得到第一检测结果；

5、根据所述第一检测结果、所述深度相机的内参和所述深度相机的畸变系数进行计算，得到第一检测结果对应的顶点坐标，并将所述第一检测结果对应的顶点坐标进行求平均计算，得到几何中心点坐标；根据所述几何中心点坐标与所述第一位置的点云数据进行计算，得到所述待检测洞孔的第一法向量；

6、根据所述几何中心点坐标与所述待检测洞孔的第一法向量控制机械臂朝待检测洞孔移动，以使得所述机械臂位于第二位置，并获取第二洞孔彩色图像与第二位置的点云数据，其中，所述第二洞孔彩色图像为所述深度相机在所述第二位置拍摄的图像，所述第二位置的点云数据为所述深度相机在所述第二位置拍摄的点云数据；

7、将所述第二洞孔彩色图像输入所述训练好的yolov5模型进行第二次彩色图像识别，得到第二检测结果；

8、将所述第二检测结果进行投影变换，得到投影变换结果；并比较所述投影变换结果与所述第一检测结果，得到待检测洞孔的坐标；

9、根据所述待检测洞孔的坐标与所述第二位置的点云数据进行计算，得到所述待检测洞孔的第二法向量；

10、根据所述待检测洞孔的坐标与所述待检测洞孔的第二法向量控制所述机械臂将软管运输至所述待检测洞孔中。

11、根据本发明的实施例，至少具有如下技术效果：

12、本方法通过获取第一位置的第一洞孔彩色图像、第一位置的点云数据、深度相机的内参和深度相机的畸变系数，其中，第一洞孔彩色图像为深度相机在第一位置拍摄的图像，第一位置为与待检测洞孔的距离值为预先设置的第一距离值的位置，第一位置的点云数据为深度相机在第一位置拍摄的点云数据，将第一洞孔彩色图像输入训练好的yolov5模型进行第一次彩色图像识别，得到第一检测结果，根据第一检测结果、深度相机的内参和深度相机的畸变系数进行计算，得到第一检测结果对应的顶点坐标，并将第一检测结果对应的顶点坐标进行求平均计算，得到几何中心点坐标；根据几何中心点坐标与第一位置的点云数据进行计算，得到待检测洞孔的第一法向量，根据几何中心点坐标与待检测洞孔的第一法向量控制机械臂朝待检测洞孔移动，以使得机械臂位于第二位置，并获取第二洞孔彩色图像与第二位置的点云数据，其中，第二洞孔彩色图像为深度相机在第二位置拍摄的图像，第二位置的点云数据为深度相机在第二位置拍摄的点云数据，将第二洞孔彩色图像输入训练好的yolov5模型进行第二次彩色图像识别，得到第二检测结果，将第二检测结果进行投影变换，得到投影变换结果；并比较投影变换结果与第一检测结果，得到待检测洞孔的坐标，根据待检测洞孔的坐标与第二位置的点云数据进行计算，得到待检测洞孔的第二法向量，根据待检测洞孔的坐标与待检测洞孔的第二法向量控制机械臂将软管运输至待检测洞孔中，能够提升作业效率，减少安全隐患。

13、根据本发明的一些实施例，通过如下方式训练yolov5模型：

14、获取初始洞口图像样本集，并对所述初始洞口图像样本集中的所有初始洞口图像进行标注，得到洞口图像训练样本集，其中，所述标注的矩形标签内接所述初始洞口图像中的待检测洞孔；

15、构建初始yolov5模型，根据所述洞口图像训练样本集训练所述初始yolov5模型，得到所述训练好的yolov5模型。

16、根据本发明的一些实施例，所述根据所述第一检测结果、所述深度相机的内参和所述深度相机的畸变系数进行计算，得到第一检测结果对应的顶点坐标，并将所述第一检测结果对应的顶点坐标进行求平均计算，得到几何中心点坐标，包括：

17、获取第一位置的第一深度图像与所述第一深度图像的深度值，其中，所述第一深度图像为所述深度相机在所述第一位置拍摄的图像；

18、对所述第一检测结果的洞孔进行分布式编码，得到洞孔检测结果；

19、根据所述洞孔检测结果计算每个洞孔的四个像素顶点；

20、根据所述深度相机的内参、所述深度相机的畸变系数和所述第一深度图像的深度值将所述四个像素顶点转换到图像坐标系，得到所述四个像素顶点的相机坐标系的坐标，并将所述四个像素顶点的相机坐标系的坐标进行求平均计算，得到相机坐标系的几何中心点坐标，其中，所述根据所述深度相机的内参、所述深度相机的畸变系数和所述第一深度图像的深度值将所述四个像素顶点转换到图像坐标系，得到所述四个像素顶点的相机坐标系的坐标的计算公式为：

21、ximg＝(xpixel-ux)/fx

22、yimg＝(ypixel-vy)/fy

23、

24、ximg_undis＝ximg*(1+k1*r2+k2*r4+k3*r6)+2*p1*ximg*yimg+p2*(r2+2*ximg*ximg)

25、yimg_undis＝yimg*(1+k1*r2+k2*r4+k3*r6)+2*p2*ximg*yimg+p1*(r2+2*yimg*yimg)

26、zcamera＝zimg

27、xcamera＝zcamera*ximg_undis

28、ycamera＝zcamera*yimg_undis

29、其中，fx为深度相机的内参，fy为深度相机的内参，ux为深度相机的内参，vy为深度相机的内参，k1为深度相机的畸变系数，k2为深度相机的畸变系数，k3为深度相机的畸变系数，p1为深度相机的畸变系数，p2为深度相机的畸变系数，xpixel为像素坐标系下的点横坐标，ypixel为像素坐标系下的点纵坐标，ximg为图像坐标系下的点横坐标，yimg为图像坐标系下的点纵坐标，r为距离，ximg为图像坐标系下的去畸变后点横坐标，yimg为图像坐标系下的去畸变后点纵坐标，zimg为像素点深度值，xcamera为相机坐标系下的点横坐标，ycamera为相机坐标系下的点纵坐标，zcamera为相机坐标系下的点深度值；

30、获取第一位置的机械臂末端坐标系到相机坐标系的第一旋转矩阵与第一平移矩阵，获取第一位置的相机坐标系到机械臂末端坐标系的第二旋转矩阵与第二平移矩阵，获取第一位置的机械臂末端到机械臂平台的第三旋转矩阵与第三平移矩阵，获取第一位置的平台坐标系到机械臂末端坐标系的第四旋转矩阵与第四平移矩阵；

31、根据所述第一旋转矩阵、所述第一平移矩阵、所述第二旋转矩阵、所述第二平移矩阵、所述第三旋转矩阵、所述第三平移矩阵、所述第四旋转矩阵、所述第四平移矩阵将所述相机坐标系的几何中心点对应的相机坐标系的坐标通过转换公式投影至机械臂平台坐标系下，得到所述几何中心点坐标，其中，所述转换公式为:

32、

33、其中，为坐标系o2的坐标，为坐标系o1的坐标，为坐标系o1到坐标系o2的旋转矩阵，为坐标系o1到坐标系o2的平移矩阵。

34、根据本发明的一些实施例，所述将所述第二检测结果进行投影变换，得到投影变换结果；并比较所述投影变换结果与所述第一检测结果，得到待检测洞孔的坐标，包括：

35、获取第二位置的第二深度图像与所述第二深度图像的深度值，其中，所述第二深度图像为所述深度相机在所述第二位置拍摄的图像；

36、根据所述第二检测结果、所述深度相机的内参、所述深度相机的畸变系数和所述第二深度图像的深度值进行计算，得到第二检测结果对应的顶点坐标；

37、根据所述第一旋转矩阵、所述第一平移矩阵、所述第二旋转矩阵、所述第二平移矩阵、所述第三旋转矩阵、所述第三平移矩阵、所述第四旋转矩阵、所述第四平移矩阵将所述第二检测结果对应的顶点坐标通过转换公式投影至所述相机坐标系下，得到第一位置下相机的空间坐标；

38、根据所述第一位置下相机的空间坐标进行相机的投影变换，得到待检测洞孔在第一洞孔彩色图像中的投影集合；

39、寻找在所述待检测洞孔在第一洞孔彩色图像中的投影集合和所述第一检测结果中第一个交并比大于0的待检测洞孔，得到第一个待检测洞孔，并获取所述第一个待检测洞孔的坐标。

40、根据本发明的一些实施例，所述根据所述待检测洞孔的坐标与所述第二位置的点云数据进行计算，得到所述待检测洞孔的第二法向量，包括：

41、获取第二位置的相机坐标系到机械臂末端坐标系的第二旋转矩阵与第二平移矩阵，获取第二位置的机械臂末端到机械臂平台的第三旋转矩阵与第三平移矩阵；

42、将所述第二位置的点云数据进行透视变换投影至像素坐标系下，得到点云数据的像素坐标；

43、提取所述点云数据中像素坐标满足且的点，得到第一点云数据，其中，α为常数，(u`，v`)为点云数据投影得到的像素坐标，为待检测洞孔的坐标、宽和高；

44、滤除所述第一点云数据中的点云，得到第二点云数据；

45、对所述第二点云数据的点云采用ransac算法进行平面提取，得到平面法向量；

46、根据所述第二位置的相机坐标系到机械臂末端坐标系的第二旋转矩阵与第二平移矩阵以及所述第二位置的机械臂末端到机械臂平台的第三旋转矩阵与第三平移矩阵将所述平面法向量通过转换公式投影至所述机械臂平台坐标系下，得到所述待检测洞孔的第二法向量。

47、根据本发明的一些实施例，所述根据所述洞孔检测结果计算每个洞孔的四个像素顶点的计算公式为与其中，为第n个洞孔的横坐标，为第n个洞孔的纵坐标，为第n个洞孔的像素级宽，为第n个洞孔的像素级高。

48、根据本发明的一些实施例，所述将软管运输至洞孔中的方法还包括：

49、获取第一检测结果在图像像素坐标系中的中心以及宽、高；

50、若所述待检测洞孔在第一洞孔彩色图像中的投影集合和所述第一检测结果中存在交并比等于0的洞孔，则判断所述交并比等于0的洞孔的第一检测结果中的像素坐标投影是否在所述第一检测结果在图像像素坐标系中的宽与高的范围内：若所述交并比等于0的洞孔的第一检测结果中的像素坐标投影在所述第一检测结果在图像像素坐标系中的宽与高的范围内，则将所述交并比等于0的洞孔加入到所述第一检测结果中，若所述交并比等于0的洞孔的第一检测结果中的像素坐标投影超过所述第一检测结果在图像像素坐标系中的宽与高的范围，则忽视所述交并比等于0的洞孔。

51、本发明的第二方面，提供一种将软管运输至洞孔中的系统，所述将软管运输至洞孔中的系统包括：

52、数据获取模块，用于获取第一位置的第一洞孔彩色图像、深度相机的内参和深度相机的畸变系数，其中，所述第一洞孔彩色图像为所述深度相机在所述第一位置拍摄的图像，所述第一位置为与待检测洞孔的距离值为预先设置的第一距离值的位置；

53、第一次彩色图像识别模块，用于将所述第一洞孔彩色图像输入训练好的yolov5模型进行第一次彩色图像识别，得到第一检测结果；

54、第一检测结果计算模块，用于根据所述第一检测结果、所述深度相机的内参和所述深度相机的畸变系数进行计算，得到第一检测结果对应的顶点坐标，并将所述第一检测结果对应的顶点坐标进行求平均计算，得到几何中心点坐标；

55、第二位置数据获取模块，用于根据所述几何中心点坐标控制机械臂朝待检测洞孔移动，以使得所述机械臂位于第二位置，并获取第二洞孔彩色图像与第二位置的点云数据，其中，所述第二洞孔彩色图像为所述深度相机在所述第二位置拍摄的图像，所述第二位置的点云数据为所述深度相机在所述第二位置拍摄的点云数据；

56、第二检测模块，用于将所述第二洞孔彩色图像输入所述训练好的yolov5模型进行第二次彩色图像识别，得到第二检测结果；

57、待检测洞孔坐标获取模块，用于将所述第二检测结果进行投影变换，得到投影变换结果；并比较所述投影变换结果与所述第一检测结果，得到待检测洞孔的坐标；

58、法向量计算模块，用于根据所述待检测洞孔的坐标与所述第二位置的点云数据进行计算，得到所述待检测洞孔的法向量；

59、洞孔作业模块，用于根据所述待检测洞孔的坐标与所述待检测洞孔的法向量控制所述机械臂将软管运输至所述待检测洞孔中。

60、本系统通过获取第一位置的第一洞孔彩色图像、第一位置的点云数据、深度相机的内参和深度相机的畸变系数，其中，第一洞孔彩色图像为深度相机在第一位置拍摄的图像，第一位置为与待检测洞孔的距离值为预先设置的第一距离值的位置，第一位置的点云数据为深度相机在第一位置拍摄的点云数据，将第一洞孔彩色图像输入训练好的yolov5模型进行第一次彩色图像识别，得到第一检测结果，根据第一检测结果、深度相机的内参和深度相机的畸变系数进行计算，得到第一检测结果对应的顶点坐标，并将第一检测结果对应的顶点坐标进行求平均计算，得到几何中心点坐标；根据几何中心点坐标与第一位置的点云数据进行计算，得到待检测洞孔的第一法向量，根据几何中心点坐标与待检测洞孔的第一法向量控制机械臂朝待检测洞孔移动，以使得机械臂位于第二位置，并获取第二洞孔彩色图像与第二位置的点云数据，其中，第二洞孔彩色图像为深度相机在第二位置拍摄的图像，第二位置的点云数据为深度相机在第二位置拍摄的点云数据，将第二洞孔彩色图像输入训练好的yolov5模型进行第二次彩色图像识别，得到第二检测结果，将第二检测结果进行投影变换，得到投影变换结果；并比较投影变换结果与第一检测结果，得到待检测洞孔的坐标，根据待检测洞孔的坐标与第二位置的点云数据进行计算，得到待检测洞孔的第二法向量，根据待检测洞孔的坐标与待检测洞孔的第二法向量控制机械臂将软管运输至待检测洞孔中，能够提升作业效率，减少安全隐患。

61、本发明的第三方面，提供了一种将软管运输至洞孔中的电子设备，包括至少一个控制处理器和用于与所述至少一个控制处理器通信连接的存储器；所述存储器存储有可被所述至少一个控制处理器执行的指令，所述指令被所述至少一个控制处理器执行，以使所述至少一个控制处理器能够执行上述的将软管运输至洞孔中的方法。

62、本发明的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行上述的将软管运输至洞孔中的方法。

63、需要注意的是，本发明的第二方面至第四方面与现有技术之间的有益效果与上述的一种将软管运输至洞孔中的系统与现有技术之间的有益效果相同，此处不再细述。

64、本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。