[发明专利]控制工作臂末端输药管对准炮孔的方法和设备

权利要求书

1.一种控制工作臂末端输药管对准炮孔的方法，其特征在于，包括：

检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度；

根据所述倾斜角度确定工作臂的每个关节的第一转动角度调整量及工作臂的伸缩副的第一伸缩调整量，其中，工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动后，工作臂的末端臂将与岩壁垂直；

控制工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动；

获取设置在工作臂末端的摄像机摄取的图像；

根据对所述图像进行图像分析确定炮孔在所述图像中的位置；

检测工作臂末端与岩壁之间的垂直距离；

根据所述位置以及所述垂直距离确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量，其中，工作臂按照所述第二转动角度调整量和所述第二伸缩调整量进行运动后，工作臂末端的输药管将对准所述炮孔；

控制工作臂按照所述第二转动角度调整量和第二伸缩调整量进行运动；

检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度的步骤包括：

获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；

确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。

2.根据权利要求1所述的方法，其特征在于，确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量步骤包括：

根据炮孔在所述图像中的位置以及工作臂末端与岩壁之间的垂直距离确定炮孔在摄像机坐标系中的三维坐标；

将炮孔在摄像机坐标系中的三维坐标进行坐标变换得到炮孔在工作臂工作坐标系中的三维坐标；

根据炮孔在工作臂工作坐标系中的三维坐标确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量。

3.根据权利要求1所述的方法，其特征在于，确定炮孔在图像中的位置的步骤包括：提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

4.根据权利要求1所述的方法，其特征在于，还包括：

在控制工作臂运动的过程中，实时地获取工作臂末端与岩壁之间的距离；

当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩副的伸缩调整量。

5.根据权利要求1所述的方法，其特征在于，所述测距传感器为激光测距传感器、红外测距传感器或雷达测距传感器。

6.一种控制工作臂末端输药管对准炮孔的设备，其特征在于，包括：

处理器，被配置为：检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度；根据所述倾斜角度确定工作臂的每个关节的第一转动角度调整量及工作臂的伸缩副的第一伸缩调整量，其中，工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动后，工作臂的末端臂将与岩壁垂直；控制工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动；获取设置在工作臂末端的摄像机摄取的图像；根据对所述图像进行图像分析确定炮孔在所述图像中的位置；检测工作臂末端与岩壁之间的垂直距离；根据所述位置以及所述垂直距离确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量，其中，工作臂按照所述第二转动角度调整量和所述第二伸缩调整量进行运动后，工作臂末端的输药管将对准所述炮孔；控制工作臂按照所述第二转动角度调整量和第二伸缩调整量进行运动；

检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度的步骤包括：

获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；

确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。