CN103606171B - 一种井下快速定位炮孔进行自动装药的方法 - Google Patents

Abstract

一种井下快速定位炮孔进行自动装药的方法，属于矿山爆破技术领域。本发明通过结构光视觉系统来控制装药机械臂的动作，能自动进行对孔，且对孔精度高，能够完成从一个炮孔到另一个炮孔的自动定位。本发明的方法步骤包括：确定作业现场坐标系；选定一个需要定位的炮孔P₁，完成炮孔P₁的对孔及装药作业；自动定位第二个炮孔P₂，完成炮孔P₂的对孔及装药作业；参照炮孔P₂的对孔及装药步骤，完成第三个炮孔P₃及第四个炮孔P₄的对孔及装药作业，通过结构光视觉系统可得到四个炮孔的位置矢量对应关系，再重新计算炮孔坐标系到机械臂基座坐标系的变换矩阵，进行变换矩阵的修正，将修正后的变换矩阵用于后续炮孔的定位中。

Description

一种井下快速定位炮孔进行自动装药的方法

技术领域

本发明属于矿山爆破技术领域，特别是涉及一种井下快速定位炮孔进行自动装药的方法。

背景技术

矿山爆破作业中，在炮孔内装填炸药是一个非常重要的环节，现阶段，在炮孔内装填炸药的方式主要有两种，一种是以纯人工的方式进行炸药的填装，工人通过炮棍将成品炸药塞入炮孔内，或是工人将输药管塞入炮孔进行散装炸药的填装，但是纯人工装药使工人的劳动强度非常大，且安全性差；另一种是通过人工手动控制装药机械臂，通过机械臂末端的送管装置将输药管送入炮孔进行散装炸药的填装，但是该种方式对操作人员的技术水平要求非常高，要想人工手动控制装药机械臂实现送管装置的对孔非常困难，且对孔时操作过程繁琐，对孔时间普遍很长。

发明内容

针对现有技术存在的问题，本发明提供一种井下快速定位炮孔进行自动装药的方法，该方法通过在装药机械臂上加装结构光视觉系统，通过结构光视觉系统来控制装药机械臂的动作，并能实现自动控制机械臂末端送管装置进行对孔，且对孔精度高，面对井下的恶劣条件，能够完成从一个炮孔到另一个炮孔的自动定位，不但解决了传统方式中纯人工装药的工人劳动强度非常大及安全性差的问题，还解决了人工手动控制装药机械臂的对孔困难、操作过程繁琐及对孔时间长的问题。

为了实现上述目的，本发明采用如下技术方案：一种井下快速定位炮孔进行自动装药的方法，包括以下步骤：

步骤一：确定作业现场坐标系

所述作业现场坐标系包括炮孔坐标系O_P-x_py_pz_p、机械臂基座坐标系O₀-x₀y₀z₀、送管装置坐标系O_T-x_Ty_Tz_T及摄像机坐标系O_C-x_Cy_Cz_C，其中O_P为炮孔坐标系原点，x_p、y_p、z_p为炮孔坐标系坐标，O₀为机械臂基座坐标系原点，x₀、y₀、z₀为机械臂基座坐标系坐标，O_T为送管装置坐标系原点，x_T、y_T、z_T为送管装置坐标系坐标，O_C为摄像机坐标系原点，x_C、y_C、z_C为摄像机坐标系坐标；

步骤二：选定一个需要定位的炮孔P₁，完成炮孔P₁的对孔及装药作业

炮孔P₁为第一个填装炸药的炮孔，对孔前，首先手动控制机械臂移动，使炮孔P₁进入结构光视觉系统的摄像机视野中，此时结构光视觉系统会通过摄像机自动扫描炮孔P₁，并记录下炮孔P₁的空间位置坐标数据，根据摄像机坐标系到机械臂基座坐标系的变换矩阵驱动机械臂，使机械臂移向炮孔P₁，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₁，其中和均为事先对摄像机进行标定时获得的已知量，已经存储在结构光视觉系统内；然后结构光视觉系统会控制送管装置将输药管送入炮孔P₁内，完成炮孔P₁的装药工作；

步骤三：自动定位第二个炮孔P₂，完成炮孔P₂的对孔及装药作业

确定炮孔P₁在炮孔坐标系的位置矢量为^PP₁，而^PP₁=P（u,v,w）=[u₁v₁w₁1]^T，其中u₁,v₁,w₁为炮孔P₁的坐标值；

确定炮孔P₁在机械臂基座坐标系的位置矢量为⁰P₁；

经结构光视觉系统测得⁰P₁=[x₁y₂z₃1]^T，其中x₁,y₂,z₃为炮孔P₁的坐标值；

其中

$P_1^0=T_T^0\left(P_1^T\right)---\left(1\right)$

式中，为送管装置坐标系到机械臂基座坐标系的变换矩阵，^TP₁为炮孔P₁在送管装置坐标系的位置矢量；

由坐标齐次变换可知

$P_1^0=T_p^0\left(P_1^p\right)---\left(2\right)$

式中，为炮孔坐标系到机械臂基座坐标系的变换矩阵，^PP₁为炮孔P₁在炮孔坐标系的位置矢量，其中

$T_p^0=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

式中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、P_x、P_y及P_z均为变量值；

调整机械臂的定位位置，使机械臂基座坐标系的x₀、y₀、z₀坐标轴与炮孔坐标系的x_p、y_p、z_p坐标轴平行，则可简化为

$T_p^0=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(4\right)$

根据式（2）和式（4）可得

$P_1^0=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]P_1^p=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ w_1\\ 1\end{array}\right]=\left[\begin{array}{c}{u}_1+p_x\\ v_1+p_y\\ w_1+p_z\\ 1\end{array}\right]---\left(5\right)$

由⁰P₁=[x₁y₂z₃1]^T及^PP₁=[u₁v₁w₁1]^T可知，并根据

$\left\{\begin{array}{c}{x}_1=u_1+p_x\\ y_1=v_1+p_y\\ z_1=w_1+p_z\end{array}\right.---\left(6\right)$

可得P_x、P_y及P_z的值，即可得的值；

将代入式（2），并通过第二个炮孔P₂在炮孔坐标系的位置矢量^PP₂可得，炮孔P₂在机械臂基座坐标系的位置矢量⁰P₂，即同时得到炮孔P₂在送管装置坐标系的位置矢量，从而得到送管装置的位姿矩阵，通过该位姿矩阵驱动机械臂靠近炮孔P₂，然后通过结构光视觉系统进行位置修正，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₂，送管装置将输药管送入炮孔P₂内，完成炮孔P₂的装药工作；

步骤四：参照炮孔P₂的对孔及装药步骤，完成第三个炮孔P₃及第四个炮孔P₄的对孔及装药作业，此时通过结构光视觉系统可得到炮孔P₁、P₂、P₃及P₄的四个炮孔的位置矢量对应关系，再根据式（3）重新计算炮孔坐标系到机械臂基座坐标系的变换矩阵进行的修正，将修正后的用于后续炮孔的定位中。

本发明的有益效果：

本发明与现有技术相比，该方法通过在装药机械臂上加装结构光视觉系统，通过结构光视觉系统来控制装药机械臂的动作，并能实现自动控制机械臂末端送管装置进行对孔，且对孔精度高，面对井下的恶劣条件，能够完成从一个炮孔到另一个炮孔的自动定位，不但解决了传统方式中纯人工装药的工人劳动强度非常大及安全性差的问题，还解决了人工手动控制装药机械臂的对孔困难、操作过程繁琐及对孔时间长的问题。

附图说明

图1为作业现场坐标系示意图；

图中，O_P—炮孔坐标系原点，x_p、y_p、z_p—炮孔坐标系坐标，O₀—机械臂基座坐标系原点，x₀、y₀、z₀—机械臂基座坐标系坐标，O_T—送管装置坐标系原点，x_T、y_T、z_T—送管装置坐标系坐标，O_C—摄像机坐标系原点，x_C、y_C、z_C—摄像机坐标系坐标，P（u,v,w）—任一炮孔在炮孔坐标系的位置矢量

具体实施方式

下面结合附图和具体实施例对本发明做进一步的详细说明。

对孔前，炮孔的分布位置已经确定，即炮孔布置形式是爆破设计已知的，本实施例中的炮孔呈向上扇形，且结构光视觉系统已经与机械臂调试安装完毕。

采用所述的井下快速定位炮孔进行自动装药的方法，包括以下步骤：

步骤一：确定作业现场坐标系

所述作业现场坐标系（如图1所示）包括炮孔坐标系O_P-x_py_pz_p、机械臂基座坐标系O₀-x₀y₀z₀、送管装置坐标系O_T-x_Ty_Tz_T及摄像机坐标系O_C-x_Cy_Cz_C，其中O_P为炮孔坐标系原点，x_p、y_p、z_p为炮孔坐标系坐标，O₀为机械臂基座坐标系原点，x₀、y₀、z₀为机械臂基座坐标系坐标，O_T为送管装置坐标系原点，x_T、y_T、z_T为送管装置坐标系坐标，O_C为摄像机坐标系原点，x_C、y_C、z_C为摄像机坐标系坐标；本实施例中的炮孔均位于炮孔坐标系的y_pz_p平面内；

步骤二：选定一个需要定位的炮孔P₁，完成炮孔P₁的对孔及装药作业

炮孔P₁为第一个填装炸药的炮孔，对孔前，首先手动控制机械臂移动，使炮孔P₁进入结构光视觉系统的摄像机视野中，此时结构光视觉系统会通过摄像机自动扫描炮孔P₁，并记录下炮孔P₁的空间位置坐标数据，根据摄像机坐标系到机械臂基座坐标系的变换矩阵驱动机械臂，使机械臂移向炮孔P₁，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₁，其中和均为事先对摄像机进行标定时获得的已知量，已经存储在结构光视觉系统内；然后结构光视觉系统会控制送管装置将输药管送入炮孔P₁内，完成炮孔P₁的装药工作；

步骤三：自动定位第二个炮孔P₂，完成炮孔P₂的对孔及装药作业

确定炮孔P₁在炮孔坐标系的位置矢量为^PP₁，而^PP₁=P（u,v,w）=[u₁v₁w₁1]^T，其中u₁,v₁,w₁为炮孔P₁的坐标值；

本实施例中u₁=10，v₁=10，w₁=0，则^PP₁=[101001]^T；

确定炮孔P₁在机械臂基座坐标系的位置矢量为⁰P₁；

本实施例中经结构光视觉系统测得⁰P₁=[x₁y₂z₃1]^T=[202001]^T，其中x₁,y₂,z₃为炮孔P₁的坐标值；

其中

$P_1^0=T_T^0\left(P_1^T\right)---\left(1\right)$

式中，为送管装置坐标系到机械臂基座坐标系的变换矩阵，^TP₁为炮孔P₁在送管装置坐标系的位置矢量；

由坐标齐次变换可知

$P_1^0=T_p^0\left(P_1^p\right)---\left(2\right)$

式中，为炮孔坐标系到机械臂基座坐标系的变换矩阵，^PP₁为炮孔P₁在炮孔坐标系的位置矢量，其中

$T_p^0=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

式中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、P_x、P_y及P_z均为变量值；

调整机械臂的定位位置（通过调整机械臂的载体装药车的位置，即可实现机械臂位置的调整），使机械臂基座坐标系的x₀、y₀、z₀坐标轴与炮孔坐标系的x_p、y_p、z_p坐标轴平行，则可简化为

$T_p^0=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(4\right)$

根据式（2）和式（4）可得

$P_1^0=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]P_1^p=\left[\begin{array}{cccc}1& 0& 0& p_x\\ 0& 1& 0& p_y\\ 0& 0& 1& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ w_1\\ 1\end{array}\right]=\left[\begin{array}{c}{u}_1+p_x\\ v_1+p_y\\ w_1+p_z\\ 1\end{array}\right]---\left(5\right)$

由⁰P₁=[x₁y₂z₃1]^T=[202001]^T及^PP₁=[u₁v₁w₁1]^T=[101001]^T可知，并根据

$\left\{\begin{array}{c}{x}_1=u_1+p_x\\ y_1=v_1+p_y\\ z_1=w_1+p_z\end{array}\right.---\left(6\right)$

可得P_x=10,P_y=10,P_z=0,即

$T_p^0=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

将代入式（2），并通过第二个炮孔P₂在炮孔坐标系的位置矢量^PP₂可得，炮孔P₂在机械臂基座坐标系的位置矢量⁰P₂，即同时得到炮孔P₂在送管装置坐标系的位置矢量，从而得到送管装置的位姿矩阵，通过该位姿矩阵驱动机械臂靠近炮孔P₂，然后通过结构光视觉系统进行位置修正，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₂，送管装置将输药管送入炮孔P₂内，完成炮孔P₂的装药工作；

步骤四：参照炮孔P₂的对孔及装药步骤，完成第三个炮孔P₃及第四个炮孔P₄的对孔及装药作业，此时通过结构光视觉系统可得到炮孔P₁、P₂、P₃及P₄的四个炮孔的位置矢量对应关系，再根据式（3）重新计算炮孔坐标系到机械臂基座坐标系的变换矩阵进行的修正，将修正后的用于后续炮孔的定位中。

通过上述方法，实现了炮孔的全自动定位，定位过程中，不再需要人为控制机械臂使炮孔进入结构光视觉系统的摄像机视野中，机械臂能够自动移动，进而带动结构光视觉系统的摄像机移动，使需要定位的炮孔进入结构光视觉系统的摄像机视野中，并自动完成对孔及装药作业，并最终实现了机械臂完成对全部炮孔进行全自动装药的目的。

Claims (1)

1.一种井下快速定位炮孔进行自动装药的方法，其特征在于：包括以下步骤：

步骤一：确定作业现场坐标系

所述作业现场坐标系包括炮孔坐标系O_P-x_py_pz_p、机械臂基座坐标系O₀-x₀y₀z₀、送管装置坐标系O_T-x_Ty_Tz_T及摄像机坐标系O_C-x_Cy_Cz_C，其中O_P为炮孔坐标系原点，x_p、y_p、z_p为炮孔坐标系坐标，O₀为机械臂基座坐标系原点，x₀、y₀、z₀为机械臂基座坐标系坐标，O_T为送管装置坐标系原点，x_T、y_T、z_T为送管装置坐标系坐标，O_C为摄像机坐标系原点，x_C、y_C、z_C为摄像机坐标系坐标；

步骤二：选定一个需要定位的炮孔P₁，完成炮孔P₁的对孔及装药作业

炮孔P₁为第一个填装炸药的炮孔，对孔前，首先手动控制机械臂移动，使炮孔P₁进入结构光视觉系统的摄像机视野中，此时结构光视觉系统会通过摄像机自动扫描炮孔P₁，并记录下炮孔P₁在摄像机坐标系下的空间位置坐标数据，根据摄像机坐标系到机械臂基座坐标系的变换矩阵驱动机械臂，使机械臂移向炮孔P₁，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₁，其中和均为事先对摄像机进行标定时获得的已知量，已经存储在结构光视觉系统内；然后结构光视觉系统会控制送管装置将输药管送入炮孔P₁内，完成炮孔P₁的装药工作；

步骤三：自动定位第二个炮孔P₂，完成炮孔P₂的对孔及装药作业

确定炮孔P₁在炮孔坐标系的位置矢量为^PP₁，而^PP₁＝P(u,v,w)＝[u₁v₁w₁1]^T，其中u₁,v₁,w₁为炮孔P₁的坐标值；

确定炮孔P₁在机械臂基座坐标系的位置矢量为⁰P₁；

经结构光视觉系统测得⁰P₁＝[x₁y₂z₃1]^T，其中x₁,y₂,z₃为炮孔P₁的坐标值；

其中

式中，为送管装置坐标系到机械臂基座坐标系的变换矩阵，^TP₁为炮孔P₁在送管装置坐标系的位置矢量；

由坐标齐次变换可知

式中，为炮孔坐标系到机械臂基座坐标系的变换矩阵，^PP₁为炮孔P₁在炮孔坐标系的位置矢量，其中

式中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、P_x、P_y及P_z均为变量值；

调整机械臂的定位位置，使机械臂基座坐标系的x₀、y₀、z₀坐标轴与炮孔坐标系的x_p、y_p、z_p坐标轴平行，则可简化为

根据式(2)和式(4)可得

由⁰P₁＝[x₁y₂z₃1]^T及^PP₁＝[u₁v₁w₁1]^T，并根据

可得P_x、P_y及P_z的值，即可得的值；

将代入式(2)，并通过第二个炮孔P₂在炮孔坐标系的位置矢量^PP₂可得，炮孔P₂在机械臂基座坐标系的位置矢量⁰P₂，即同时得到炮孔P₂在送管装置坐标系的位置矢量，从而得到送管装置的位姿矩阵，通过该位姿矩阵驱动机械臂靠近炮孔P₂，然后通过结构光视觉系统进行位置修正，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₂，送管装置将输药管送入炮孔P₂内，完成炮孔P₂的装药工作；

步骤四：参照炮孔P₂的对孔及装药步骤，完成第三个炮孔P₃及第四个炮孔P₄的对孔及装药作业，此时通过结构光视觉系统可得到炮孔P₁、P₂、P₃及P₄的四个炮孔的位置矢量对应关系，再根据式(3)重新计算炮孔坐标系到机械臂基座坐标系的变换矩阵进行的修正，将修正后的用于后续炮孔的定位中。