基于视觉的煤矿用坑道钻机机器人自动装卸钻杆定位方法

本发明涉及机器人控制，特别涉及一种基于视觉的煤矿用坑道钻机机器人自动装卸钻杆定位方法。

背景技术：

1、目前，在煤矿井下瓦斯抽采活动中，电液控制自动化钻机已经实现了远程遥控、电液控制、自动装卸钻杆，并在部分煤矿投入小批量使用。随着煤矿自动化钻机的推广应用，钻杆输送装置的重要性也逐渐显现。钻杆机械手作为较常见的钻杆输送装置，对于减轻现场劳动强度、提高安全性具有至关重要的作用。但是，由于煤矿井下钻机工况特殊，钻孔现场环境恶劣，大多数工业机器人采用离线编程或示教的方式来执行钻杆装卸任务，当钻机的位置发生变化时，机器人无法适应新的场景，便会无法正常工作。

2、钻机自动化装卸钻杆可以有效提供装卸效率、减轻人工劳动强度、提高钻进安全性。当前的自动装卸钻杆装置中，对装卸位置的定位方法主要有机械结构定位方法、多传感器定位方法与视觉定位方法三种。

3、机械结构定位方法是主要依靠机械结构和接近开关实现钻杆的定点取放。机械结构定位方法装卸钻杆方法一般利用多级传动机构设计，经过多次传递完成，机构庞大复杂，在完成既定功能的同时，存在因机械磨损和系统累计误差导致的系统精度降低问题。同时，机械结构定位方法的自动化与智能化程度较低。

4、多传感器定位方法是使用距离传感器、倾角传感器等，通过建立模型的方式计算出钻杆装卸位置。而现有的定位方法高度依赖模型建立精度，在因此后期使用中，会导致精度变化大、可靠性低的问题。

5、基于视觉的定位方法具有响应速度快、精度高和非接触式的特点，对环境的适应能力高，能够有效解决因系统累计误差、模型变化等造成的精度下降问题，同时能够大幅提高系统的自动化与智能化程度。虽然目前已经存在视觉辅助钻杆定位方法，但在系统精度与环境适应性上仍存在不足，无法真正落地于煤矿抽采过程。

技术实现思路

1、本发明旨在解决现有视觉辅助钻杆定位方法在系统精度以及环境适应性上较差导致的无法真正落地于煤矿抽采过程的技术问题。为了解决该技术问题，本发明采取的技术方案是：基于视觉定位钻机自动装卸钻杆，采用单目2d相机与合作靶目标，在无需高精度模型的情况下，通过图像精准引导机器人至钻机钻杆装卸夹持器位置。

2、为了实现上述目的，本发明提供了一种基于视觉的煤矿用坑道钻机机器人自动装卸钻杆定位方法，包括以下步骤：

3、s1：对相机进行标定、并进行机器人手眼标定，得到手眼矩阵；

4、s2：示教相机拍照点、钻杆放置点，采集包含合作靶的图像作为期望图像，根据机器人末端姿态、手眼矩阵、期望图像信息，将相机拍照点与合作靶之间的相对姿态设定为固定约束条件，并建立基于视觉的装卸钻杆的定位模型；

5、s3：机器人视觉系统控制机器人移动，使满足相机拍照点和合作靶之间的固定约束条件；

6、s4：根据机器人移动后的姿态、钻杆放置点与相机拍照点的变换关系，计算出钻杆放置位置，并控制机器人关节驱动机器人至夹持器位置，放置或抓取钻杆。

7、进一步地，步骤s1具体包括：

8、s11：对二维相机进行标定，根据相机采集到的多组包含合作靶的图像，建立三维空间的物体在二维相机平面的映射关系，计算出相机的内参信息；

9、s12：将相机固定于机器人末端，通过移动不同的位姿，并记录图像，计算出机器人末端法兰与相机中心的变换关系，即手眼矩阵etc。

10、进一步地，步骤s2具体包括：

11、s21：通过机器人示教器，移动携带钻杆的机器人至钻机夹持卡槽中，记录为钻杆放置点；

12、s22：移动机器人至钻机上方，记录为相机拍照点；

13、s23：采集包含合作靶的图像作为期望图像；

14、s24：根据机器人末端姿态、手眼矩阵、期望图像信息，将相机拍照点与合作靶之间的相对姿态设定为固定约束条件，建立基于视觉的装卸钻杆的定位模型。

15、进一步地，步骤s3具体包括：

16、s31：根据基于视觉的装卸钻杆的定位模型，根据相机内参信息和p3p算法求解机器人装卸钻杆过程中采集的图像中合作靶在相机坐标系下的变换矩阵初始设定期望图像中合作靶与初始相机之间的相对姿态变换矩阵为即系统固定约定条件；根据采集图像与设定期望图像中获取的相对合作靶的相对姿态，依据坐标系间变换关系得到当前相机坐标系与理想固定约束下的相机坐标系间的变换矩阵

17、

18、其中，表示合作靶在当前相机坐标系下的变换矩阵，表示合作靶与期望相机拍照点即相机坐标系间的固定约束条件；

19、s32：根据钻机装卸系统中机器人基坐标系、机械臂末端坐标系、相机坐标系、目标合作靶坐标系和夹持器坐标系的系间坐标系变换框架，得到当前姿态的机器人末端相机坐标系的位姿矩阵

20、

21、s33：依据当前相机位姿与期望相机位姿之间的变换矩阵得到基于当前机器人位置的合作靶识别与定位的变换矩阵完整变换路径：

22、

23、其中是通过机器人示教器获得的机器人末端坐标系与机器人基坐标系之间的变换矩阵，etc是通过手眼标定算法获得的手眼矩阵，为合作靶位姿与期望相机位姿之间的变换矩阵；

24、s34：通过合作靶识别与定位的变换矩阵计算出的机器人末端相机的下一步姿态，使当前状态的相机拍照位置与合作靶图像之间的相对姿态满足初始相机拍照点与初始合作靶目标之间的固定约束条件，其中下一步姿态的机器人末端姿态变换矩阵为：

25、

26、进一步地，步骤s4具体包括：

27、s41：完成相机拍照位置与合作靶图像之间的相对姿态满足初始相机拍照点与初始合作靶目标之间精确定位后，根据手动示教获得的相机拍照点和钻杆放置点得到变换矩阵得到钻杆放置位置在机器人世界坐标系下的描述，即钻杆放置点位：

28、

29、其中bth表示钻杆放置位置在机器人基坐标系下的变换矩阵，表示基于当前位置的合作靶识别与定位的变换矩阵，表示手眼矩阵的逆变换矩阵，表示钻杆放置点在机器人末端坐标系下的变换矩阵。

30、此外，为实现上述目的，本发明还提供了一种基于视觉的煤矿用坑道钻机机器人自动装卸钻杆定位装置，包括以下单元：

31、标定单元，用于对相机进行标定、并进行机器人手眼标定，得到手眼矩阵；

32、约束单元，用于示教相机拍照点、钻杆放置点，采集包含合作靶的图像作为期望图像，根据机器人末端姿态、手眼矩阵、期望图像信息，将相机拍照点与合作靶之间的相对姿态设定为固定约束条件，并建立基于视觉的装卸钻杆的定位模型；

33、控制单元，用于通过机器人视觉系统控制机器人移动，使满足相机拍照点和合作靶之间的固定约束条件；

34、计算单元，用于根据机器人移动后的姿态、钻杆放置点与相机拍照点的变换关系，计算出钻杆放置位置，并控制机器人关节驱动机器人至夹持器位置，放置或抓取钻杆。

35、此外，为了实现上述目的，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现所述的煤矿用坑道钻机机器人自动装卸钻杆定位方法的步骤。

36、此外，为实现上述目的，本发明还提供了一种存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现所述的煤矿用坑道钻机机器人自动装卸钻杆定位方法的步骤。

37、本发明提供的技术方案具有以下有益效果：

38、本发明方法在进行基于视觉的装卸钻杆的定位模型建立后，利用合作靶目标与钻杆装卸夹持器之间位置固有关系不变的原理，通过图像处理技术与机器人坐标系转换，最小化相机与合作靶目标的期望相对位姿与实际相对位姿误差，达到计算钻杆装卸夹持器理想位置的目的，并且，在实际钻进过程中，钻机的自身姿态发生旋转、升降、移动等因素发生改变，无需重复示教，仍然能够自主完成精准定位。