本发明涉及一种爆破孔装药自动寻孔方法、装置和系统，其中方法包括：控制平台根据预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据所述相对三维坐标和相对位姿信息，对机械臂各关节的伸缩长度和转动角度进行解算、调控，使得装药车机械臂上的输药软管能精确对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
1.一种爆破孔装药自动寻孔方法，其特征在于，包括：
控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
所述控制平台接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
所述控制平台根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
2.根据权利要求1所述的方法，其特征在于，所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，包括：所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述爆破孔相对于所述接收器的三维坐标和位姿信息；
所述控制平台根据所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述接收器的三维坐标和位姿信息；
所述控制平台根据所述爆破孔相对于所述接收器的三维坐标和位姿信息，以及所述机械臂相对于所述接收器的三维坐标和位姿信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息。
3.根据权利要求1或2所述的方法，其特征在于，所述爆破孔口孔标发射器的数量为至少两个；所述臂端发射器的数量为至少两个；所述信号接收器的数量为至少四个。
4.根据权利要求3所述的方法，其特征在于，所述爆破孔的数量为多个；
所述信号接收器位于各爆破孔圆截面法向汇聚方向的四周。
5.根据权利要求1所述的方法，其特征在于，所述控制平台根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，包括：所述控制平台获取所述机械臂中各个关节的伸缩长度、转动角度和运动范围信息；
所述控制平台根据所述各个关节的伸缩长度、转动角度和运动范围信息，以及所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，推算所述机械臂轴线与所述爆破孔法线重合的情况下，所述各个关节的目标伸缩长度和目标转动角度；
所述控制平台根据所述各个关节的目标伸缩长度和目标转动角度，对所述各个关节进行控制，以便所述装药车机械臂上的输药软管能够对准所述爆破孔。
6.根据权利要求1所述的方法，其特征在于，所述控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数之前，还包括：所述控制平台向所述孔标发射器、所述臂端发射器和所述信号接收器发送启动信号，以使所述孔标发射器和所述臂端发射器向所述信号接收器发送探测信号。
7.一种爆破孔装药自动寻孔装置，其特征在于，包括：
接收模块，用于接收预置在爆破孔周围的信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
所述接收模块，还用于接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
计算模块，用于根据所述孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
解算调控模块，用于根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
8.根据权利要求7所述的装置，其特征在于，所述计算模块，具体用于，根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述爆破孔相对于所述接收器的三维坐标和位姿信息；
根据所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述接收器的三维坐标和位姿信息；
根据所述爆破孔相对于所述接收器的三维坐标和位姿信息，以及所述机械臂相对于所述接收器的三维坐标和位姿信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息。
9.根据权利要求7或8所述的装置，其特征在于，所述爆破孔口孔标发射器的数量为至少两个；所述臂端发射器的数量为至少两个；所述信号接收器的数量为至少四个。
10.根据权利要求7所述的装置，其特征在于，所述解算调控模块，具体用于，获取所述机械臂中各个关节的伸缩长度、转动角度和运动范围信息；
根据所述各个关节的伸缩长度、转动角度和运动范围信息，以及所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，推算所述机械臂轴线与所述爆破孔法线重合的情况下，所述各个关节的目标伸缩长度和目标转动角度；
根据所述各个关节的目标伸缩长度和目标转动角度，对所述各个关节进行控制，以便所述装药车机械臂上的输药软管能够对准所述爆破孔。
11.根据权利要求7所述的装置，其特征在于，还包括：发送模块；
所述发送模块用于，在所述接收模块接收预置在爆破孔周围的信号接收器所发送的第一组参数之前，向所述孔标发射器、所述臂端发射器和所述信号接收器发送启动信号，以使所述孔标发射器和所述臂端发射器向所述信号接收器发送探测信号。
12.一种爆破孔装药自动寻孔系统，其特征在于，包括：孔标发射器、臂端发射器、信号接收器、如权利要求7-11任一项所述的爆破孔装药自动寻孔装置；
所述爆破孔装药自动寻孔装置，用于接收预置在爆破孔周围的所述信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
所述爆破孔装药自动寻孔装置，还用于接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的所述臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
所述爆破孔装药自动寻孔装置，还用于根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，机械臂前端的所述臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
所述爆破孔装药自动寻孔装置，还用于根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
一种爆破孔装药自动寻孔方法、装置和系统
技术领域
[0001] 本发明涉及一种炸药装填爆破孔装药自动寻孔技术领域，尤其涉及一种爆破孔装药自动寻孔方法、装置和系统。
背景技术
[0002] 目前，国内地下矿山爆破开采多采用无底柱分段崩落的方法来进行矿山爆破，矿山爆破的爆破孔多为上仰扇形布置，爆破孔的深度最深达到30多米，爆破孔分布的作业面凸凹不平，且不规则，炸药装填的作业面视线差、狭窄潮湿，且有片顶冒帮的安全风险，目前广泛采用的人工装药爆破方式，劳动强度大，作业人员多，作业效率低，本质安全性差。国外NORMET等公司已成功研制了自动送管系统，即采用地下装药车自动送管技术进行自动送管，但在向爆破孔送管前，仍需人工辅助对爆破孔进行定位，作业的安全性和效率仍未得到完全改善。
发明内容
[0003] 本发明提供一种爆破孔装药自动寻孔方法、装置和系统，用于解决现有技术中劳动强度大，作业人员多，作业效率低，且安全性差的问题。
[0004] 本发明的第一个方面是提供一种爆破孔装药自动寻孔方法，包括：
[0005] 控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
[0006] 所述控制平台接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
[0007] 所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
[0008] 所述控制平台根据所述机械臂相对于所述爆破孔的三维相对坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
[0009] 本发明的另一个方面提供一种爆破孔装药自动寻孔装置，包括：
[0010] 接收模块，用于接收预置在爆破孔周围的信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
[0011] 所述接收模块，还用于接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
[0012] 计算模块，用于根据所述孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
[0013] 解算调控模块，用于根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
[0014] 本发明的又一个方面提供一种爆破孔装药自动寻孔系统，包括：
[0015] 孔标发射器、臂端发射器、信号接收器、如上所述的爆破孔装药自动寻孔装置；
[0016] 所述爆破孔装药自动寻孔装置，用于接收预置在爆破孔周围的所述信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；
[0017] 所述爆破孔装药自动寻孔装置，还用于接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的所述臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
[0018] 所述爆破孔装药自动寻孔装置，还用于根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；
[0019] 所述爆破孔装药自动寻孔装置，还用于根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。
[0020] 本发明中，通过预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据所述相对三维坐标和相对位姿信息对机械臂各关节的伸缩长度和转动角度进行解算、调控，使得机械臂上的输药软管能够准确对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
附图说明
[0021] 图1为本发明提供的爆破孔装药自动寻孔方法一个实施例的流程图；
[0022] 图2为爆破孔口孔标发射器布置示意图；
[0023] 图3为爆破孔定位系统构成示意图；
[0024] 图4为机械臂的示意图；
[0025] 图5为本发明提供的爆破孔装药自动寻孔方法又一个实施例的流程图；
[0026] 图6为本发明提供的爆破孔装药自动寻孔装置一个实施例的结构示意图；
[0027] 图7为本发明提供的爆破孔装药自动寻孔系统一个实施例的结构示意图。
具体实施方式
[0028] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0029] 图1为本发明提供的爆破孔装药自动寻孔方法一个实施例的流程图，如图1所示，包括：
[0030] 101、控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数，第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，信号发送时间为孔标发射器向信号接收器发送超宽带UWB探测信号的时间；信号接收时间为信号接收器接收UWB探测信号的时间。
[0031] 本发明提供的爆破孔装药自动寻孔方法的执行主体为爆破孔装药自动寻孔装置，爆破孔装药自动寻孔装置具体可以为位于装药车上或者能够与装药车进行通信连接的其他设备上的控制平台。
[0032] 其中，爆破孔圆截面法向方向指的是，爆破孔的圆截面上与圆截面垂直的方向。孔标发射器的数量可以为至少两个，当孔标发射器的数量为两个时，其中一个孔标发射器位于爆破孔孔口处或孔口附近，另一个孔标发射器位于爆破孔外部，两个发射器可以组成爆破孔法线标杆，用于标识爆破孔所在的位置以及位姿。爆破孔口孔标发射器布置示意图可以如图2所示。
[0033] 信号接收器的数量为至少四个，信号接收器可以位于爆破孔周围。可选的，信号接收器具体可以位于各个爆破孔的圆截面法向汇聚方向的四周，信号接收器的位置信息可以作为对任意一个爆破孔定位时的参考坐标。另外，各个信号接收器之间还可以通过同步通讯电缆连接，由其中一个信号接收器将所有信号接收器接收到的参数发送给控制平台。
[0034] 孔标发射器和信号接收器具体为可以收发超宽带(Ultra-Wideband，UWB)信号的模块，孔标发射器能够向信号接收器发射UWB信号，UWB信号中携带孔标发射器发送UWB信号的时间。UWB信号具体可以为UWB脉冲。
[0035] 102、控制平台接收信号接收器所发送的第二组参数，第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间。
[0036] 此处臂端发射器的信号发送时间指的是，臂端发射器向信号接收器发送超宽带UWB探测信号的时间；对应的信号接收器的信号接收时间指的是，信号接收器接收到臂端发射器发送的UWB探测信号的时间。可选的，臂端发射器的数量可以为至少两个。
[0037] 103、控制平台根据爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息。
[0038] 具体地，步骤103具体可以包括：根据孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出爆破孔相对于接收器的三维坐标和位姿信息；根据机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于接收器的三维坐标和位姿信息；根据爆破孔相对于接收器的三维坐标和位姿信息，以及机械臂相对于接收器的三维坐标和位姿信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息。
[0039] 例如，在信号接收器的数量为四个，孔标发射器和臂端发射器的数量分别为两个的情况下，根据每个孔标发射器分别向四个信号接收器发送信号的信号发送时间以及对应的信号接收器的信号接收时间，以及四个信号接收器的位置信息，能够推算出每个孔标发射器的三维坐标；根据每个孔标发射器的三维坐标，进而能够推算出爆破孔所在位置的三维坐标以及位姿。本实施例中，爆破孔的位姿具体指的是爆破孔的圆截面法向方向。其中，爆破孔定位系统构成示意图可以如图3所示。类似的，根据上述方法还可以获得每个臂端发射器的三维坐标和位姿，进而推算出机械臂上输药软管的坐标和位姿。
[0040] 104、控制平台根据机械臂相对于爆破孔的相对三维坐标和相对位姿信息，对机械臂各关节的伸缩长度和转动角度进行解算、调控，以使装药车机械臂上的输药软管能够对准爆破孔。
[0041] 其中，机械臂的示意图如图4所示，机械臂具体可以包括多个关节。
[0042] 进一步地，为了减少能量损耗，步骤101之前，还可以包括：控制平台向孔标发射器、臂端发射器和和信号接收器发送启动信号，以使孔标发射器和臂端发射器向信号接收器发送探测信号。从而减少孔标发射器和臂端发射器向信号接收器发送探测信号的次数，减少发射器的能量损耗。
[0043] 本实施例中，通过预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据所述相对三维坐标和相对位姿信息对机械臂各关节的伸缩长度和转动角度进行解算、调控，使得机械臂上的输药软管能够准确对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
[0044] 图5为本发明提供的爆破孔装药自动寻孔方法又一个实施例的流程图，如图5所示，在图1所示实施例的基础上，步骤104具体可以包括：
[0045] 1041、控制平台获取机械臂中各个关节的伸缩长度、转动角度和运动范围信息。
[0046] 其中，机械臂可以看作是一系列由关节串联起来的连杆构成。关节的长度指的是，关节所连接的下一个连杆的长度；关节的转动角度指的是：关节所连接的下一个连杆绕关节转动的角度。关节的运动范围信息指的是：关节所连接的下一个连杆绕关节能够转动的最小角度与最大角度所确定的区间。
[0047] 1042、控制平台根据各个关节的伸缩长度、转动角度和运动范围信息，以及机械臂相对于爆破孔的相对三维坐标和相对位姿信息，推算机械臂轴线与爆破孔法线重合的情况下，各个关节的目标伸缩长度和目标转动角度。
[0048] 具体地，当将机械臂看作是一系列由关节串联起来的连杆构成时，控制平台可以为机械臂的每个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态，将描述一个连杆和下一个连杆间相对关系的齐次变换叫做A矩阵。一个A矩阵就是一个描述连杆坐标系间相对平移和旋转的齐次变换。一个连杆a和下一个连杆b坐标系间的相对平移可以用连杆b的长度来进行描述，连杆a和连杆b坐标系间的旋转可以用连杆a和连杆b之间的关节的转动角度来进行描述。表示任意两个连杆间相对关系的各个A矩阵的乘积，能够得到机械臂的第一个连杆与机械臂前端的输药软管的相对关系，从而能够结合机械臂相对于爆破孔的相对三维坐标和相对位姿信息，推算出机械臂轴线与爆破孔法线重合的情况下，各个关节的目标伸缩长度和目标转动角度。
[0049] 1043、控制平台根据各个关节的目标伸缩长度和目标转动角度，对各个关节进行控制，以便装药车机械臂上的输药软管能够对准爆破孔。
[0050] 其中，控制平台可以根据各个关节的目标转动角度，确定要发送给各个关节对应的控制阀的控制信号，使得控制阀能够控制液压马达的转动，从而使得各个关节的转动角度能够达到目标转动角度。此时，机械臂上的输药软管正对爆破孔，机械臂可以控制输药软管插入爆破孔，以便向爆破孔装药。
[0051] 本实施例中，通过预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据各个关节的伸缩长度、转动角度和运动范围信息，以及机械臂相对于爆破孔的相对三维坐标和相对位姿信息，推算机械臂轴线与爆破孔法线重合的情况下，各个关节的目标伸缩长度和目标转动角度，并根据各个关节的目标伸缩长度和目标转动角度对各个关节进行控制，使得机械臂上的输药软管能够对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
[0052] 本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
[0053] 图6为本发明提供的爆破孔装药自动寻孔装置一个实施例的结构示意图，如图6所示，包括：
[0054] 接收模块61，用于接收预置在爆破孔周围的信号接收器所发送的第一组参数，第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，信号发送时间为孔标发射器向信号接收器发送超宽带UWB探测信号的时间；信号接收时间为信号接收器接收UWB探测信号的时间。
[0055] 接收模块61，还用于接收信号接收器所发送的第二组参数，第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间。
[0056] 其中，孔标发射器的数量为至少两个；臂端发射器的数量为至少两个；信号接收器的数量为至少四个。当孔标发射器的数量为两个时，其中一个孔标发射器位于爆破孔孔口处或孔口附近，另一个孔标发射器位于爆破孔外部，两个孔标发射器可以组成爆破孔法线标杆，用于标识爆破孔所在的位置以及位姿。信号接收器可以位于爆破孔周围。可选的，爆破孔的数量可以为多个，信号接收器具体可以位于各个爆破孔的圆截面法向汇聚方向的四周，信号接收器的位置信息可以作为对任意一个爆破孔定位时的参考坐标。
[0057] 计算模块62，用于根据孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息。
[0058] 其中，计算模块62具体可以用于，根据爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出爆破孔相对于接收器的三维坐标和位姿信息；根据机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于接收器的三维坐标和位姿信息；控制平台根据爆破孔相对于接收器的三维坐标和位姿信息，以及机械臂相对于接收器的三维坐标和位姿信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息。
[0059] 解算调控模块63，用于根据机械臂相对于爆破孔的相对三维坐标和相对位姿信息，对机械臂各关节的伸缩长度和转动角度进行解算、调控，以使装药车机械臂上的输药软管能够对准爆破孔。
[0060] 其中，解算调控模块63具体用于，获取机械臂中各个关节的伸缩长度、转动角度和运动范围信息；根据各个关节的伸缩长度、转动角度和运动范围信息，以及机械臂相对于爆破孔的相对三维坐标和相对位姿信息，推算机械臂轴线与爆破孔法线重合的情况下，各个关节的目标伸缩长度和目标转动角度；根据各个关节的目标伸缩长度和目标转动角度，对各个关节进行控制，以便装药车机械臂上的输药软管能够对准爆破孔。
[0061] 需要进一步进行说明的是，爆破孔装药自动寻孔装置还可以包括：发送模块；发送模块用于，在接收模块61接收预置在爆破孔周围的信号接收器所发送的第一组参数之前，向孔标发射器、臂端发射器和信号接收器发送启动信号，以使孔标发射器和臂端发射器向信号接收器发送探测信号。
[0062] 本实施例中，通过预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据所述相对三维坐标和相对位姿信息对机械臂各关节的伸缩长度和转动角度进行解算、调控，使得机械臂上的输药软管能够准确对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
[0063] 图7为本发明提供的爆破孔装药自动寻孔系统一个实施例的结构示意图，如图7所示，包括：
[0064] 孔标发射器71、臂端发射器72、信号接收器73、如图6所示实施例中的爆破孔装药自动寻孔装置74；
[0065] 爆破孔装药自动寻孔装置74，用于接收预置在爆破孔周围的信号接收器所发送的第一组参数，第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，信号发送时间为孔标发射器向信号接收器发送超宽带UWB探测信号的时间；信号接收时间为信号接收器接收UWB探测信号的时间；
[0066] 爆破孔装药自动寻孔装置74，还用于接收信号接收器所发送的第二组参数，第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；
[0067] 爆破孔装药自动寻孔装置74，还用于根据爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息；
[0068] 爆破孔装药自动寻孔装置74，还用于根据机械臂相对于爆破孔的相对三维坐标和相对位姿信息，对机械臂各关节的伸缩长度和转动角度进行解算、调控，以使装药车机械臂上的输药软管能够对准爆破孔。
[0069] 其中，孔标发射器71的数量为至少两个；臂端发射器72的数量为至少两个；信号接收器73的数量为至少四个。当孔标发射器71的数量为两个时，其中一个孔标发射器71位于爆破孔孔口处或孔口附近，另一个孔标发射器71位于爆破孔外部，两个孔标发射器71可以组成爆破孔法线标杆，用于标识爆破孔所在的位置以及位姿。信号接收器73可以位于爆破孔周围。可选的，爆破孔的数量可以为多个，信号接收器73具体可以位于各个爆破孔的圆截面法向汇聚方向的四周，信号接收器的位置信息可以作为对任意一个爆破孔定位时的参考坐标。
[0070] 进一步地，爆破孔装药自动寻孔装置74根据爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息中，爆破孔装药自动寻孔装置74具体用于，[0071] 根据爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出爆破孔相对于接收器的三维坐标和位姿信息；根据机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于接收器的三维坐标和位姿信息；根据爆破孔相对于接收器的三维坐标和位姿信息，以及机械臂相对于接收器的三维坐标和位姿信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息。
[0072] 更进一步地，爆破孔装药自动寻孔装置74根据机械臂相对于爆破孔的相对三维坐标和相对位姿信息，对机械臂各关节的伸缩长度和转动角度进行解算、调控中，爆破孔装药自动寻孔装置74具体用于，
[0073] 获取机械臂中各个关节的伸缩长度、转动角度和运动范围信息；根据各个关节的伸缩长度、转动角度和运动范围信息，以及机械臂相对于爆破孔的相对三维坐标和相对位姿信息，推算机械臂轴线与爆破孔法线重合的情况下，各个关节的目标伸缩长度和目标转动角度；根据各个关节的目标伸缩长度和目标转动角度，对各个关节进行控制，以便装药车机械臂上的输药软管能够对准爆破孔。
[0074] 需要进行说明的是，爆破孔装药自动寻孔装置74在接收预置在爆破孔周围的信号接收器所发送的第一组参数之前，还可以向孔标发射器、臂端发射器和信号接收器发送启动信号，以使孔标发射器和臂端发射器向信号接收器发送探测信号。
[0075] 本实施例中，通过预置在爆破孔周围的信号接收器发送的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，预置在机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及信号接收器的位置信息，推算出机械臂相对于爆破孔的相对三维坐标和相对位姿信息，根据所述相对三维坐标和相对位姿信息对机械臂各关节的伸缩长度和转动角度进行解算、调控，使得机械臂上的输药软管能够准确对准爆破孔，从而实现爆破孔装药自动寻孔，提高地下工程爆破作业的机械化程度，减少作业人员的人工参与度，从而提高作业效率和作业安全性。
[0076] 最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
