[发明专利]一种井下快速定位炮孔进行自动装药的方法

摘要

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

主权项

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₁的坐标值；其中P10=TT0(P1T)---(1)]]>式中，为送管装置坐标系到机械臂基座坐标系的变换矩阵，^TP₁为炮孔P₁在送管装置坐标系的位置矢量；由坐标齐次变换可知P10=Tp0(P1p)---(2)]]>式中，为炮孔坐标系到机械臂基座坐标系的变换矩阵，^PP₁为炮孔P₁在炮孔坐标系的位置矢量，其中Tp0=nxoxaxpxnyoyaypynzozazpz0001---(3)]]>式中，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坐标轴平行，则可简化为Tp0=100px010py001pz0001---(4)]]>根据式（2）和式（4）可得P10=100px010py001pz0001P1p=100px010py001pz0001u1v1w11=u1+pxv1+pyw1+pz1---(5)]]>由⁰P₁=[x₁ y₂ z₃ 1]^T及^PP₁=[u₁ v₁ w₁ 1]^T可知，并根据x1=u1+pxy1=v1+pyz1=w1+pz---(6)]]>可得P_x、P_y及P_z的值，即可得的值；将代入式（2），并通过第二个炮孔P₂在炮孔坐标系的位置矢量^PP₂可得，炮孔P₂在机械臂基座坐标系的位置矢量⁰P₂，即同时得到炮孔P₂在送管装置坐标系的位置矢量，从而得到送管装置的位姿矩阵，通过该位姿矩阵驱动机械臂靠近炮孔P₂，然后通过结构光视觉系统进行位置修正，再根据送管装置坐标系到机械臂基座坐标系的变换矩阵驱动送管装置对准炮孔P₂，送管装置将输药管送入炮孔P₂内，完成炮孔P₂的装药工作；步骤四：参照炮孔P₂的对孔及装药步骤，完成第三个炮孔P₃及第四个炮孔P₄的对孔及装药作业，此时通过结构光视觉系统可得到炮孔P₁、P₂、P₃及P₄的四个炮孔的位置矢量对应关系，再根据式（3）重新计算炮孔坐标系到机械臂基座坐标系的变换矩阵进行的修正，将修正后的用于后续炮孔的定位中。