CN107992098B - 控制工作臂末端输药管对准炮孔的方法和设备 - Google Patents

Abstract

本发明提供一种控制工作臂末端输药管对准炮孔的方法和设备，所述方法包括：检测岩壁倾斜角度；根据所述倾斜角度确定工作臂的每个关节的第一转动角度调整量及工作臂的伸缩副的第一伸缩调整量；控制工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动；获取设置在工作臂末端的摄像机摄取的图像；根据对所述图像进行图像分析确定炮孔在所述图像中的位置；检测工作臂末端与岩壁之间的垂直距离；根据所述位置以及所述垂直距离确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量；控制工作臂按照所述第二转动角度调整量和第二伸缩调整量进行运动。根据本发明可提高工作臂的对孔精度以及装药的安全性和效率。

Description

控制工作臂末端输药管对准炮孔的方法和设备

技术领域

本发明涉及采矿领域。更具体地讲，涉及一种控制工作臂末端输药管对准炮孔的方法和设备。

背景技术

在井下矿山、隧道工程的爆破行业中，采用先进的工业炸药装药装备可大幅度减少现场作业人员的数量，降低工人劳动强度及提高作业人员的安全保障。民爆行业作为高危行业，安全至关重要。随着地下矿无人开采技术的发展，能够在井下高效可靠地进行自动化、智能化装药的混装车将越来越受到青睐。

目前井下炮孔的输药管的对准操作，主要还是采用人工站在吊兰中进行辅助对准或者采用人工视距内遥控多自由度工作臂对准炮孔的方式。第一种作业方式的危险性及劳动强度都较高，第二种方式的效率及安全性不能有效保证。

发明内容

本发明的目的在于提供一种控制工作臂末端输药管对准炮孔的方法和设备，以解决现有的炮口对准方式的效率和安全性较低的问题。

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

可选地，确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量步骤包括：根据炮孔在所述图像中的位置以及工作臂末端与岩壁之间的垂直距离确定炮孔在摄像机坐标系中的三维坐标；将炮孔在摄像机坐标系中的三维坐标进行坐标变换得到炮孔在工作臂工作坐标系中的三维坐标；根据炮孔在工作臂工作坐标系中的三维坐标确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量。

可选地，检测炮孔所在的岩壁相对于工作臂末端的倾斜角度的步骤包括：获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。

可选地，确定炮孔在图像中的位置的步骤包括：提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

可选地，还包括：在控制工作臂运动的过程中，实时地获取工作臂末端与岩壁之间的距离；当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩副的伸缩调整量。

可选地，所述测距传感器为激光测距传感器、红外测距传感器或雷达测距传感器。

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

可选地，确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量步骤包括：根据炮孔在所述图像中的位置以及工作臂末端与岩壁之间的垂直距离确定炮孔在摄像机坐标系中的三维坐标；将炮孔在摄像机坐标系中的三维坐标进行坐标变换得到炮孔在工作臂工作坐标系中的三维坐标；根据炮孔在工作臂工作坐标系中的三维坐标确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量。

可选地，检测炮孔所在的岩壁相对于工作臂末端的倾斜角度的步骤包括：获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。

可选地，确定炮孔在图像中的位置的步骤包括：提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

可选地，处理器还被配置为：在控制工作臂运动的过程中，实时地获取工作臂末端与岩壁之间的距离；当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩副的伸缩调整量。

本发明的另一方面提供一种计算机可读存储介质，存储有当被处理器执行时使得处理器执行如上所述的控制工作臂末端输药管对准炮孔的方法的计算机程序。

根据本发明的实施例的控制工作臂末端输药管对准炮孔的方法和设备，通过设置在工作臂末端的摄像机以及测距传感器来进行多自由度的工作臂的自动寻孔工作，提高了工作臂的对孔精度，并且减少了现场的操作人员的数量，提高了装药的安全性和效率。

将在接下来的描述中部分阐述本发明另外的方面和/或优点，还有一部分通过描述将是清楚的，或者可以经过本发明的实施而得知。

附图说明

通过下面结合附图进行的详细描述，本发明的上述和其它目的、特点和优点将会变得更加清楚，其中：

图1是示出根据本发明的实施例的控制工作臂末端输药管对准炮孔的方法的流程图；

图2是示出根据本发明的实施例的工作臂末端的结构示意图。

具体实施方式

下面参照附图详细描述本发明的实施例。

图1是示出根据本发明的实施例的控制工作臂末端输药管对准炮孔的方法的流程图。这里的工作臂末端输药管对准炮孔是指工作臂末端的输药管与炮孔的轴心对齐，且输药管的末端将要接近炮孔。例如，输药管的末端离所述炮孔的距离为10cm左右。

在步骤S10，检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度。

这里，该工作臂可包括多节自由臂，工作臂的末端臂是指位于工作臂最末端的自由臂。该倾斜角度是指岩壁所在的平面与工作臂的末端臂之间的夹角。

可在工作臂末端(即末端臂的末端)设置相应的传感器来检测该倾斜角度。

作为示例，可在工作臂末端的表面设置至少三个测距传感器，用于检测工作臂末端与岩壁之间的至少三个距离。至少三个测距传感器可均匀地布置在工作臂末端的表面上。这里，优选为在工作臂末端的表面设置三个以上的测距传感器，以便在测距时进行冗余剔除失真数据。该测距传感器可以是各种可测量距离的传感器。例如该测距传感器可以是激光测距传感器、红外测距传感器或雷达测距传感器等。在测量距离时，至少三个激光测距传感器均沿着该工作臂最末端的自由臂的长度方向放射激光。

图2是示出根据本发明的实施例的工作臂末端的结构示意图。如图2所示，在工作臂末端的表面上均匀布置了4个测距传感器201，此外，工作臂末端的表面上布置了后续即将详细介绍的摄像机202以及输药管203。

在步骤S10中，获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。具体说来，由于三个点可确定一个平面，因此可根据至少三个距离确定岩壁所在的平面，进而可确定岩壁所在的平面与工作臂的末端臂之间的夹角。

在步骤S20，根据所述倾斜角度确定工作臂的每个关节的第一转动角度调整量及工作臂的伸缩副的第一伸缩调整量，其中，工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动后，工作臂的末端臂将与岩壁垂直。在步骤S30，控制工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动，以使末端臂与岩壁垂直。也就是说，控制工作臂的每个关节按照所述第一转动角度调整量和控制伸缩副按照所述第一伸缩调整量进行运动，以使末端臂与岩壁垂直。

所述工作臂为多自由度工作臂，即所述工作臂包括多个关节。每个关节的第一转动角度调整值为每个关节的角度调整值。工作臂可包括一个或多个伸缩副。在工作臂包括多个伸缩副时，每个伸缩副对应一个第一伸缩调整量。

在步骤S30，控制工作臂进行运动的目标为使末端臂与岩壁垂直(即倾斜角度为90度)，而根据岩壁与末端臂之间的夹角(即上述倾斜角度)，可确定末端臂在长度方向的目标方向，从而可根据该目标方向以及各关节当前的转动角度以及各伸缩副当前的长度来确定工作臂的每个关节的第一转动角度调整量和伸缩副的第一伸缩调整量。这里，优选地，为了减少调整工作量，可优先确定工作臂的最末端的关节的第一关节角度转动量，在最末端的关节的第一关节角度转动量不能使末端臂的长度方向达到目标方向的情况下，再确定其他关节的第一关节角度转动量。

在步骤S40，获取设置在工作臂末端的摄像机摄取的图像。

如图2所示，所述摄像机202可设置在工作臂的末端臂的末端的表面上的任意位置，优选为设置在至少三个测距传感器201的中间。

该摄像机摄取的图像中包括了炮孔的图像。

在步骤S50，根据对所述图像进行图像分析确定炮孔在所述图像中的位置。

这里，可对图像进行各种图像分析以识别炮孔。

作为示例，可利用炮孔对光线反射较少导致的炮孔的图像的灰度较小，以及炮孔的形状是类圆形的特点，提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

在步骤S60，检测工作臂末端与岩壁之间的垂直距离。

这里，可通过设置在工作臂末端的至少三个测距传感器中的一个或多个测距传感器来获取工作臂末端与岩壁之间的垂直距离。

由于已在步骤S30调节各关节的角度使工作臂末端与岩壁垂直，因此，测距传感器中检测到的工作臂末端与岩壁之间的距离就是工作臂末端与岩壁之间的垂直距离。这里可将多个测距传感器检测到的工作臂末端与岩壁之间的多个距离的平均值作为该垂直距离，或者将多个距离中的中值作为该垂直距离。

在步骤S70，根据所述位置以及所述垂直距离确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。工作臂按照所述第二转动角度调整量和第二伸缩调整量进行运动后，工作臂末端的输药导管将对准所述炮孔。在步骤S80，控制工作臂按照所述第二转动角度和第二伸缩调整量进行运动，以使工作臂末端的输药导管对准所述炮孔。也就是说，控制工作臂的每个关节按照所述第二转动角度调整量和控制伸缩副按照所述第二伸缩调整量进行运动，以使工作臂末端的输药导管对准所述炮孔。

这里，根据炮孔在图像中的位置以及工作臂末端与岩壁之间的垂直距离，可以确定炮孔相对于工作臂末端的相对位置，将该相对位置作为工作臂末端的目标位置，根据目标位置以及各关节当前的转动角度以及各关节对应的自由臂当前的长度可确定每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。

作为示例，可根据炮孔在所述图像中的位置以及工作臂末端与岩壁之间的垂直距离确定炮孔在摄像机坐标系中的三维坐标；将炮孔在摄像机坐标系中的三维坐标进行坐标变换得到炮孔在工作臂工作坐标系中的三维坐标；根据炮孔在工作臂工作坐标系中的三维坐标确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。也就是说，将炮孔在工作臂工作坐标系中的三维坐标作为工作臂末端的目标位置，根据目标位置以及各关节当前的转动角度以及各关节对应的自由臂当前的长度确定每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。

作为一个优选的示例，在控制工作臂运动的过程中，为了防止工作臂末端碰到岩壁造成部件损害，可实时地获取工作臂末端与岩壁之间的距离；当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩副的伸缩调整量。

可通过设置在工作臂末端的至少三个测距传感器中的一个或多个测距传感器来获取工作臂末端与岩壁之间的距离。例如，可将多个测距传感器检测到的工作臂末端与岩壁之间的多个距离的平均值作为该距离，或者将多个距离中的中值作为该距离。

当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新获取摄像机摄取的图像，确定炮孔在摄像机摄取的图像中的位置，以及重新检测工作臂末端与岩壁之间的垂直距离，以及重新根据炮孔在图像中的位置以及垂直距离确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量，并控制工作臂按照重新确定的第二转动角度调整量和第二伸缩调整量进行运动。

可以理解，步骤S50和S60可同时进行，并且，由于工作臂在不停地运动，岩壁相对于工作臂的末端臂的倾斜角度以及炮孔在工作臂工作坐标系中的三维坐标可能会不停地变化，因此，本方法中的各个步骤都是实时进行的。根据本发明的实施例还提供一种控制工作臂末端输药管对准炮孔的设备。该设备包括处理器。

处理器被配置为检测炮孔所在的岩壁相对于工作臂的末端臂的倾斜角度。

这里，该工作臂可包括多节自由臂，工作臂的末端臂是指位于工作臂最末端的自由臂。该倾斜角度是指岩壁所在的平面与工作臂的末端臂之间的夹角。

可在工作臂末端(即末端臂的末端)设置相应的传感器来检测该倾斜角度。

作为示例，可在工作臂末端的表面设置至少三个测距传感器，用于检测工作臂末端与岩壁之间的至少三个距离。至少三个测距传感器可均匀地布置在工作臂末端的表面上。这里，优选为在工作臂末端的表面设置三个以上的测距传感器，以便在测距时进行冗余剔除失真数据。该测距传感器可以是各种可测量距离的传感器。例如该测距传感器可以是激光测距传感器、红外测距传感器或雷达测距传感器等。在测量距离时，至少三个激光测距传感器均沿着该工作臂最末端的自由臂的长度方向放射激光。

图2是示出根据本发明的实施例的工作臂末端的结构示意图。如图2所示，在工作臂末端的表面上均匀布置了4个测距传感器201，此外，工作臂末端的表面上布置了后续即将详细介绍的摄像机202以及输药管203。

处理器被配置为获取设置在工作臂末端的至少三个测距传感器测量的工作臂末端与岩壁之间的至少三个距离；确定差距最小的三个距离，根据所述三个距离确定岩壁相对于工作臂的末端臂的倾斜角度。具体说来，由于三个点可确定一个平面，因此可根据至少三个距离确定岩壁所在的平面，进而可确定岩壁所在的平面与工作臂的末端臂之间的夹角。

处理器被配置为根据所述倾斜角度确定工作臂的每个关节的第一转动角度调整量及工作臂的伸缩副的第一伸缩调整量，其中，工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动后，工作臂的末端臂将与岩壁垂直。处理器被配置为控制工作臂按照所述第一转动角度调整量和所述第一伸缩调整量进行运动，以使末端臂与岩壁垂直。也就是说，控制工作臂的每个关节按照所述第一转动角度调整量和控制伸缩副按照所述第一伸缩调整量进行运动，以使末端臂与岩壁垂直。

所述工作臂为多自由度工作臂，即所述工作臂包括多个关节。每个关节的第一转动角度调整值为每个关节的角度调整值。工作臂可包括一个或多个伸缩副。在工作臂包括多个伸缩副时，每个伸缩副对应一个第一伸缩调整量。

处理器被配置为控制工作臂进行运动的目标为使末端臂与岩壁垂直(即倾斜角度为90度)，而根据岩壁与末端臂之间的夹角(即上述倾斜角度)，可确定末端臂在长度方向的目标方向，从而可根据该目标方向以及各关节当前的转动角度以及各伸缩副当前的长度来确定工作臂的每个关节的第一转动角度调整量和伸缩副的第一伸缩调整量。这里，优选地，为了减少调整工作量，可优先确定工作臂的最末端的关节的第一关节角度转动量，在最末端的关节的第一关节角度转动量不能使末端臂的长度方向达到目标方向的情况下，再确定其他关节的第一关节角度转动量。

处理器被配置为获取设置在工作臂末端的摄像机摄取的图像。

如图2所示，所述摄像机202可设置在工作臂的末端臂的末端的表面上的任意位置，优选为设置在至少三个测距传感器201的中间。

该摄像机摄取的图像中包括了炮孔的图像。

处理器被配置为根据对所述图像进行图像分析确定炮孔在所述图像中的位置。

这里，可对图像进行各种图像分析以识别炮孔。

作为示例，可利用炮孔对光线反射较少导致的炮孔的图像的灰度较小，以及炮孔的形状是类圆形的特点，提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

处理器被配置为检测工作臂末端与岩壁之间的垂直距离。

这里，可通过设置在工作臂末端的至少三个测距传感器中的一个或多个测距传感器来获取工作臂末端与岩壁之间的垂直距离。

由于之前已调节各关节的角度使工作臂末端与岩壁垂直，因此，测距传感器中检测到的工作臂末端与岩壁之间的距离就是工作臂末端与岩壁之间的垂直距离。这里可将多个测距传感器检测到的工作臂末端与岩壁之间的多个距离的平均值作为该垂直距离，或者将多个距离中的中值作为该垂直距离。

处理器被配置为根据所述位置以及所述垂直距离确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。工作臂按照所述第二转动角度调整量和第二伸缩调整量进行运动后，工作臂末端的输药导管对准所述炮孔。处理器被配置为控制工作臂按照所述第二转动角度和第二伸缩调整量进行运动，以使工作臂末端的输药导管对准所述炮孔。也就是说，控制工作臂的每个关节按照所述第二转动角度调整量和控制伸缩副按照所述第二伸缩调整量进行运动，以使工作臂末端的输药导管对准所述炮孔。

这里，根据炮孔在图像中的位置以及工作臂末端与岩壁之间的垂直距离，可以确定炮孔相对于工作臂末端的相对位置，将该相对位置作为工作臂末端的目标位置，根据目标位置以及各关节当前的转动角度以及各关节对应的自由臂当前的长度可确定每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。

作为示例，可根据炮孔在所述图像中的位置以及工作臂末端与岩壁之间的垂直距离确定炮孔在摄像机坐标系中的三维坐标；将炮孔在摄像机坐标系中的三维坐标进行坐标变换得到炮孔在工作臂工作坐标系中的三维坐标；根据炮孔在工作臂工作坐标系中的三维坐标确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。也就是说，将炮孔在工作臂工作坐标系中的三维坐标作为工作臂末端的目标位置，根据目标位置以及各关节当前的转动角度以及各关节对应的自由臂当前的长度确定每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量。

作为一个优选的示例，在控制工作臂运动的过程中，为了防止工作臂末端碰到岩壁造成部件损害，可实时地获取工作臂末端与岩壁之间的距离；当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩调整量。

可通过设置在工作臂末端的至少三个测距传感器中的一个或多个测距传感器来获取工作臂末端与岩壁之间的距离。例如，可将多个测距传感器检测到的工作臂末端与岩壁之间的多个距离的平均值作为该距离，或者将多个距离中的中值作为该距离。

当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新获取摄像机摄取的图像，确定炮孔在摄像机摄取的图像中的位置，以及重新检测工作臂末端与岩壁之间的垂直距离，以及重新根据炮孔在图像中的位置以及垂直距离确定工作臂的每个关节的第二转动角度调整量和伸缩副的第二伸缩调整量，并控制工作臂按照重新确定的第二转动角度调整量和第二伸缩调整量进行运动。

根据本发明的实施例的控制工作臂末端输药管对准炮孔的方法和设备，通过设置在工作臂末端的摄像机以及测距传感器来进行工作臂的自动寻孔工作，提高了工作臂的对孔精度，并且减少了现场的操作人员的数量，提高了装药的安全性和效率。

根据本发明的实施例还提供一种计算机可读存储介质。该计算机可读存储介质存储有当被处理器执行时使得处理器执行如上所述的控制工作臂末端输药管对准炮孔的方法的计算机程序。

尽管已经参照其示例性实施例具体显示和描述了本发明，但是本领域的技术人员应该理解，在不脱离权利要求所限定的本发明的精神和范围的情况下，可以对其进行形式和细节上的各种改变。

Claims (10)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

7.根据权利要求6所述的控制工作臂末端输药管对准炮孔的设备，其特征在于，确定工作臂的每个关节的第二转动角度调整量和工作臂的伸缩副的第二伸缩调整量步骤包括：

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

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

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

8.根据权利要求6所述的控制工作臂末端输药管对准炮孔的设备，其特征在于，确定炮孔在图像中的位置的步骤包括：提取所述图像的灰度特征和形状特征，将所述图像中的所述灰度特征小于预定阈值以及形状特征为类似圆形的区域确定为炮孔在图像中的位置。

9.根据权利要求6所述的控制工作臂末端输药管对准炮孔的设备，其特征在于，处理器还被配置为：在控制工作臂运动的过程中，实时地获取工作臂末端与岩壁之间的距离；当工作臂末端与岩壁之间的距离小于或等于预定距离时，控制所述工作臂停止运动，并重新确定每个关节的转动角度调整量和伸缩副的伸缩调整量。

10.一种计算机可读存储介质，存储有当被处理器执行时使得处理器执行如权利要求1至5中任意一项所述的控制工作臂末端输药管对准炮孔的方法的计算机程序。

Images