掘进钻车自动定位方法
【技术领域】
[0001]本发明涉及一种多关节多自由度设备的定位方法，具体是指掘进钻车钻孔时自动定位方法，该方法适用矿山掘进钻车在打眼时钎杆的定位。
【背景技术】
[0002]在矿山开采过程中一般是用掘进钻车进行打孔，然后填埋炸药进行爆破。目前的掘进钻车工作部分是在能够行走的机身上铰接钻臂，钻臂分为左钻臂和右钻臂，左、右钻臂均通过轴套连接在钻车的操作台架上，钻臂主要部件包括轴套、钻臂架、支管、支架、定向滑道、滑架和钎杆，钻臂的前端铰接有滑架，滑架上安装有钎杆，钻臂上连接有使钻臂水平方向转动的油缸和使钻臂实现仰俯的油缸，同时钻臂自身能够伸缩；滑架上同样连接有使滑架仰俯和使滑架相对钻臂水平转动的油缸；另外钎杆在滑架上仅能够前进和后退，单个钻臂的具体结构见附图3a、附图3b等。在定位过程钎杆相对于滑架不动，因此将钎杆和滑架看做一个整体，这样掘进钻车工作部分的钻臂、滑架、钎杆就至少有六个自由度，因此要想使钎杆的钎头对准钻眼位置需要反复调整钻臂、滑架、钎杆。目前掘进钻车的操作主要还是人工控制，操作人员开动掘进钻车到达指定位置，然后操作各油缸，调整钻臂、滑架、钎杆的位置，使钎杆的钎头对准需钻孔位置，然后起到钎杆使钎杆前进，打出爆破孔。这样人工操作打出的爆破孔一般误差都很大，这样是实施爆破后会导致巷道成型质量差。同时操作者的劳动强度非常大。如果将目前的掘进钻车改造成能够自动找孔定位的自动控制方式，目前的多关节多自由度设备的定位方法程序复杂，程序运算时间长，对计算机运算速度要求高，导致无法应用于矿上巷道工程上，由于掘进钻车本身也是移动的，目前的定位方法的运算结果误差也较大。
【发明内容】
[0003]本发明要解决的技术问题是提供一种运算速度快且准确的掘进钻车自动定位方法，能够在给定所打炮眼和机身位姿后便可快速准确计算出钻车钻臂和滑架位置参数值，以使钎杆能够准确对准钻孔位置。
[0004]为解决上述技术问题，本发明所采取的技术方案是:
[0005]掘进钻车自动定位方法，将钻车机架上的轴套绕自身轴心线转动时带动钻臂的左右摆动设定为第一动作，将铰接在轴套上的钻臂架的上下摆动而带动钻臂的上下摆动设定为第二动作，将钻臂架前端套接的支管绕自身轴线相对于钻臂架的旋转设定为第三动作，将钻臂的支架带动定向滑道、滑架、钎杆绕支架与支管的横向铰接轴的上下摆动设定为第四动作，将定向滑道带动滑架、钎杆绕支架与定向滑道的竖向铰接轴做水平转动设定为第五动作，将滑架带动钎杆在定向滑道上前后伸缩移动设定为第六动作；
[0006]在巷道三维空间内建立全局坐标系，在全局坐标系内建立钻臂的局部坐标系；全局坐标系以掘进工作面中心垂线与巷道底板中心线的交点为坐标原点，自坐标原点沿掘进工作面竖直向上设定为Y轴正向，自坐标原点沿巷道底板向后设定为Z轴正向，面向掘进工作面时右手方向为X轴正向；局部坐标系是以第一动作绕轴中心线和第二动作绕轴中心的交点为原点，局部坐标系的各坐标轴正向与全局坐标系一致；
[0007]在全局坐标系中选取包括局部坐标原点在内的8个点作为关键点并将这8个关键点固联在钻臂的钎杆、滑架、定向滑道、支架、支管、钻臂架、轴套的相应位置或者虚拟位置上;
[0008]所述8个关键点包括:第I关键点是钎杆的钎尖所在的点；第2关键点是钎杆中心线上距离钎尖一定距离的点；第3关键点是位于与滑架的上平面平行的钎杆中心线所在平面内、距离钎尖特定距离的点，第I关键点和第3关键点连线垂直于钎杆轴线，面向掘进工作面时第3关键点位于钎杆(I)的右侧；第4关键点是由钻臂的实际结构尺寸确定的位于钎杆中心线上是距离第I关键点固定距离的点，第4关键点到第I关键点的距离大于第2关键点到第I关键点的距离，当钻臂上所有动作部件均处于零位时，第4关键点是第五动作的竖轴中心线与钎杆中心线的交点；第5关键点与第4关键点的连线垂直于与滑架的上平面平行的钎杆中心线所在平面，第5关键点到第4关键点的距离是由钻臂各部件的实际机械结构尺寸决定的固定数值，当钻臂上所有动作部件均是初始零位置时，第5关键点是第五动作的转动竖轴中心线与第三动作的旋转绕轴中心线的交点；第6关键点与第5关键点的连线平行于钎杆中心线，第6关键点到第5关键点的距离是由钻臂各部件的实际机械结构所决定的固定数值，当钻臂各部件均处于初始零位置时，第6关键点位于第四动作旋转绕轴的中心线上；第7关键点位于支管的端部，是第三动作的旋转绕轴的中心线和第四动作旋转绕轴中心线的交点，在钻臂的各部件处于初始零位置时，第6关键点与第7关键点重合；第8关键点是第一动作旋转绕轴的中心线和第二动作旋转绕轴中心线的交点；
[0009]使钻车位于巷道中、全局坐标系的Z轴方向，并确定巷道掘进工作面上需打的炮眼位置，选定打眼的钻臂并确定钎杆的最终位姿，确定钻杆的最终位姿时是先计算第I关键点、第2关键点、第3关键点在全局坐标系中的坐标值，然后将这三个关键点的全局坐标系的坐标值转换为所选定的打眼钻臂的局部坐标系的坐标值，然后计算出第4关键点、第5关键点、第6关键点在局部坐标系中的坐标值，得知第I关键点?第6关键点的局部坐标系的坐标值后，通过逆向求解计算出钻臂第一动作?第六动作的参数值，然后按照计算出的第一动作?第六6个动作参数值驱动钻臂各部件动作确定钻臂的最终位姿，使钎杆对正打眼位置进行钻孔。
[0010]本发明的技术方案的进一步限定在于:
[0011]在逆向求解第一动作?第六动作的参数值时，首先是运用迭代搜索方法求解第六动作和第五动作的参数值，之后通过逐步归位的方法依次求解第一动作、第二动作、第三动作和第四动作的参数值，在用迭代搜索方法和逐步归位的方法求解的过程中是调用包括刚体任意运动后刚体上一点坐标变换的计算方法、空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面之间夹角的计算方法、空间点绕空间轴线旋转一定角度后坐标变换的计算方法；
[0012]刚体任意动后刚体上一点坐标变换的计算方法是用于计算处于最终位置的钎杆上第四关键点、第五关键点、第六关键点坐标，该计算方法是输入钎杆任意动前第I关键点、第2关键点、第3关键点的坐标值以及动后第I关键点、第2关键点、第3关键点的坐标值，然后输入所需求解的第4关键点、第5关键点、第6关键点的动前的坐标值即可求出相应关键点动后的坐标值；
[0013]空间点沿空间直线延伸一定长度后坐标变换的计算方法是用于计算钎杆在最终位置状态下钎杆沿着钎杆轴线移动一段距离后钎杆上第I关键点?第6关键点的坐标值，运用该计算方法时，输入第I关键点坐标值和第2关键点坐标值以及所求的关键点移动之前的坐标值和所求关键点钎杆沿轴线移动的向量即可求出延伸后的该点坐标值；
[0014]空间点绕空间轴线旋转一定角度后坐标变换的计算方法是用于计算第一动作至第五动作的每一旋转动作归位后各相关关键点坐标值，运用该计算方法时输入空间点绕空间轴线旋转的角度向量和关键点旋转之前坐标值即可求出旋转后该点坐标值。
[0015]本发明的技术方案的进一步限定在于:
[0016]确定钻臂的最终位姿的计算流程为:
[0017]步骤1、确定钻车的左钻臂局部坐标系原点、右钻臂局部坐标系原点在全局坐标系中的位置坐标值，并确定所要打的炮眼在全局坐标系中坐标值，选定打炮眼的钻臂是左钻臂还是右钻臂；
[0018]步骤2、计算出第I关键点?第6关键点在全局坐标系中的全局坐标值并将各个关键点的全局坐标值转换为所选定的钻臂局部坐标系内的局部坐标值；
[0019]步骤3、运用迭代搜索方法计算出第六动作和第五动作的参数值；
[0020]步骤4、计算出第六动作和第