[发明专利]一种爆破孔装药自动寻孔方法、装置和系统

权利要求书

1.一种爆破孔装药自动寻孔方法，其特征在于，包括：

控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数，所述第一组参数包括：预置在爆破孔圆截面法向方向上的爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述信号发送时间为所述孔标发射器向所述信号接收器发送超宽带UWB探测信号的时间；所述信号接收时间为所述信号接收器接收所述UWB探测信号的时间；

所述控制平台接收所述信号接收器所发送的第二组参数，所述第二组参数包括：预置在装药车机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间；

所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息；

所述控制平台根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，以使所述装药车机械臂上的输药软管能够对准所述爆破孔。

2.根据权利要求1所述的方法，其特征在于，所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，包括：

所述控制平台根据所述爆破孔口孔标发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述爆破孔相对于所述接收器的三维坐标和位姿信息；

所述控制平台根据所述机械臂前端的臂端发射器的信号发送时间以及对应的信号接收器的信号接收时间，以及所述信号接收器的位置信息，推算出所述机械臂相对于所述接收器的三维坐标和位姿信息；

所述控制平台根据所述爆破孔相对于所述接收器的三维坐标和位姿信息，以及所述机械臂相对于所述接收器的三维坐标和位姿信息，推算出所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息。

3.根据权利要求1或2所述的方法，其特征在于，所述爆破孔口孔标发射器的数量为至少两个；所述臂端发射器的数量为至少两个；所述信号接收器的数量为至少四个。

4.根据权利要求3所述的方法，其特征在于，所述爆破孔的数量为多个；

所述信号接收器位于各爆破孔圆截面法向汇聚方向的四周。

5.根据权利要求1所述的方法，其特征在于，所述控制平台根据所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，对所述机械臂各关节的伸缩长度和转动角度进行解算、调控，包括：

所述控制平台获取所述机械臂中各个关节的伸缩长度、转动角度和运动范围信息；

所述控制平台根据所述各个关节的伸缩长度、转动角度和运动范围信息，以及所述机械臂相对于所述爆破孔的相对三维坐标和相对位姿信息，推算所述机械臂轴线与所述爆破孔法线重合的情况下，所述各个关节的目标伸缩长度和目标转动角度；

所述控制平台根据所述各个关节的目标伸缩长度和目标转动角度，对所述各个关节进行控制，以便所述装药车机械臂上的输药软管能够对准所述爆破孔。

6.根据权利要求1所述的方法，其特征在于，所述控制平台接收预置在爆破孔周围的信号接收器所发送的第一组参数之前，还包括：

所述控制平台向所述孔标发射器、所述臂端发射器和所述信号接收器发送启动信号，以使所述孔标发射器和所述臂端发射器向所述信号接收器发送探测信号。