CN105156126A - 掘进钻车自动定位方法 - Google Patents

Abstract

本发明公开了一种掘进钻车自动定位方法,适用矿山掘进钻车在打眼时钎杆的定位。根据钻车在钻臂上设置6个自由度和8个关键点，并建立巷道的三维空间全局坐标系以及以钻臂与机身的轴套的铰接点为局部坐标原点的三维空间局部坐标系；使钻车位于全局坐标系的Z轴方向，并确定巷道掘进工作面上的打炮眼位置，然后用迭代搜索方法和逐步归位的方法求解钻臂6个自由度的参数值，计算钻臂上的8个关键点的空间坐标值，确定钻臂的最终位姿，使钎杆对正打眼位置进行钻孔。该方法计算快速、精确，完全适用于钻车自动定位打眼需要。

Description

掘进钻车自动定位方法

技术领域

本发明涉及一种多关节多自由度设备的定位方法，具体是指掘进钻车钻孔时自动定位方法，该方法适用矿山掘进钻车在打眼时钎杆的定位。

背景技术

在矿山开采过程中一般是用掘进钻车进行打孔，然后填埋炸药进行爆破。目前的掘进钻车工作部分是在能够行走的机身上铰接钻臂，钻臂分为左钻臂和右钻臂，左、右钻臂均通过轴套连接在钻车的操作台架上，钻臂主要部件包括轴套、钻臂架、支管、支架、定向滑道、滑架和钎杆，钻臂的前端铰接有滑架，滑架上安装有钎杆，钻臂上连接有使钻臂水平方向转动的油缸和使钻臂实现仰俯的油缸，同时钻臂自身能够伸缩；滑架上同样连接有使滑架仰俯和使滑架相对钻臂水平转动的油缸；另外钎杆在滑架上仅能够前进和后退，单个钻臂的具体结构见附图3a、附图3b等。在定位过程钎杆相对于滑架不动，因此将钎杆和滑架看做一个整体，这样掘进钻车工作部分的钻臂、滑架、钎杆就至少有六个自由度，因此要想使钎杆的钎头对准钻眼位置需要反复调整钻臂、滑架、钎杆。目前掘进钻车的操作主要还是人工控制，操作人员开动掘进钻车到达指定位置，然后操作各油缸，调整钻臂、滑架、钎杆的位置，使钎杆的钎头对准需钻孔位置，然后起到钎杆使钎杆前进，打出爆破孔。这样人工操作打出的爆破孔一般误差都很大，这样是实施爆破后会导致巷道成型质量差。同时操作者的劳动强度非常大。如果将目前的掘进钻车改造成能够自动找孔定位的自动控制方式，目前的多关节多自由度设备的定位方法程序复杂，程序运算时间长，对计算机运算速度要求高，导致无法应用于矿上巷道工程上，由于掘进钻车本身也是移动的，目前的定位方法的运算结果误差也较大。

发明内容

本发明要解决的技术问题是提供一种运算速度快且准确的掘进钻车自动定位方法，能够在给定所打炮眼和机身位姿后便可快速准确计算出钻车钻臂和滑架位置参数值，以使钎杆能够准确对准钻孔位置。

为解决上述技术问题，本发明所采取的技术方案是：

掘进钻车自动定位方法，将钻车机架上的轴套绕自身轴心线转动时带动钻臂的左右摆动设定为第一动作，将铰接在轴套上的钻臂架的上下摆动而带动钻臂的上下摆动设定为第二动作，将钻臂架前端套接的支管绕自身轴线相对于钻臂架的旋转设定为第三动作，将钻臂的支架带动定向滑道、滑架、钎杆绕支架与支管的横向铰接轴的上下摆动设定为第四动作，将定向滑道带动滑架、钎杆绕支架与定向滑道的竖向铰接轴做水平转动设定为第五动作，将滑架带动钎杆在定向滑道上前后伸缩移动设定为第六动作；

在巷道三维空间内建立全局坐标系，在全局坐标系内建立钻臂的局部坐标系；全局坐标系以掘进工作面中心垂线与巷道底板中心线的交点为坐标原点，自坐标原点沿掘进工作面竖直向上设定为Y轴正向，自坐标原点沿巷道底板向后设定为Z轴正向，面向掘进工作面时右手方向为X轴正向；局部坐标系是以第一动作绕轴中心线和第二动作绕轴中心的交点为原点，局部坐标系的各坐标轴正向与全局坐标系一致；

在全局坐标系中选取包括局部坐标原点在内的8个点作为关键点并将这8个关键点固联在钻臂的钎杆、滑架、定向滑道、支架、支管、钻臂架、轴套的相应位置或者虚拟位置上；

所述8个关键点包括：第1关键点是钎杆的钎尖所在的点；第2关键点是钎杆中心线上距离钎尖一定距离的点；第3关键点是位于与滑架的上平面平行的钎杆中心线所在平面内、距离钎尖特定距离的点，第1关键点和第3关键点连线垂直于钎杆轴线，面向掘进工作面时第3关键点位于钎杆(1)的右侧；第4关键点是由钻臂的实际结构尺寸确定的位于钎杆中心线上是距离第1关键点固定距离的点，第4关键点到第1关键点的距离大于第2关键点到第1关键点的距离，当钻臂上所有动作部件均处于零位时，第4关键点是第五动作的竖轴中心线与钎杆中心线的交点；第5关键点与第4关键点的连线垂直于与滑架的上平面平行的钎杆中心线所在平面，第5关键点到第4关键点的距离是由钻臂各部件的实际机械结构尺寸决定的固定数值，当钻臂上所有动作部件均是初始零位置时，第5关键点是第五动作的转动竖轴中心线与第三动作的旋转绕轴中心线的交点；第6关键点与第5关键点的连线平行于钎杆中心线，第6关键点到第5关键点的距离是由钻臂各部件的实际机械结构所决定的固定数值，当钻臂各部件均处于初始零位置时，第6关键点位于第四动作旋转绕轴的中心线上；第7关键点位于支管的端部，是第三动作的旋转绕轴的中心线和第四动作旋转绕轴中心线的交点，在钻臂的各部件处于初始零位置时，第6关键点与第7关键点重合；第8关键点是第一动作旋转绕轴的中心线和第二动作旋转绕轴中心线的交点；

使钻车位于巷道中、全局坐标系的Z轴方向，并确定巷道掘进工作面上需打的炮眼位置，选定打眼的钻臂并确定钎杆的最终位姿，确定钻杆的最终位姿时是先计算第1关键点、第2关键点、第3关键点在全局坐标系中的坐标值，然后将这三个关键点的全局坐标系的坐标值转换为所选定的打眼钻臂的局部坐标系的坐标值，然后计算出第4关键点、第5关键点、第6关键点在局部坐标系中的坐标值，得知第1关键点～第6关键点的局部坐标系的坐标值后，通过逆向求解计算出钻臂第一动作～第六动作的参数值，然后按照计算出的第一动作～第六6个动作参数值驱动钻臂各部件动作确定钻臂的最终位姿，使钎杆对正打眼位置进行钻孔。

本发明的技术方案的进一步限定在于：

在逆向求解第一动作～第六动作的参数值时，首先是运用迭代搜索方法求解第六动作和第五动作的参数值，之后通过逐步归位的方法依次求解第一动作、第二动作、第三动作和第四动作的参数值，在用迭代搜索方法和逐步归位的方法求解的过程中是调用包括刚体任意运动后刚体上一点坐标变换的计算方法、空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面之间夹角的计算方法、空间点绕空间轴线旋转一定角度后坐标变换的计算方法；

刚体任意动后刚体上一点坐标变换的计算方法是用于计算处于最终位置的钎杆上第四关键点、第五关键点、第六关键点坐标，该计算方法是输入钎杆任意动前第1关键点、第2关键点、第3关键点的坐标值以及动后第1关键点、第2关键点、第3关键点的坐标值，然后输入所需求解的第4关键点、第5关键点、第6关键点的动前的坐标值即可求出相应关键点动后的坐标值；

空间点沿空间直线延伸一定长度后坐标变换的计算方法是用于计算钎杆在最终位置状态下钎杆沿着钎杆轴线移动一段距离后钎杆上第1关键点～第6关键点的坐标值，运用该计算方法时，输入第1关键点坐标值和第2关键点坐标值以及所求的关键点移动之前的坐标值和所求关键点钎杆沿轴线移动的向量即可求出延伸后的该点坐标值；

空间点绕空间轴线旋转一定角度后坐标变换的计算方法是用于计算第一动作至第五动作的每一旋转动作归位后各相关关键点坐标值，运用该计算方法时输入空间点绕空间轴线旋转的角度向量和关键点旋转之前坐标值即可求出旋转后该点坐标值。

本发明的技术方案的进一步限定在于：

确定钻臂的最终位姿的计算流程为：

步骤1、确定钻车的左钻臂局部坐标系原点、右钻臂局部坐标系原点在全局坐标系中的位置坐标值，并确定所要打的炮眼在全局坐标系中坐标值，选定打炮眼的钻臂是左钻臂还是右钻臂；

步骤2、计算出第1关键点～第6关键点在全局坐标系中的全局坐标值并将各个关键点的全局坐标值转换为所选定的钻臂局部坐标系内的局部坐标值；

步骤3、运用迭代搜索方法计算出第六动作和第五动作的参数值；

步骤4、计算出第六动作和第五动作的参数值之后，将第六动作和第五动作分别归位到各自在局部坐标系内的初始零位置，求出归位后的第1关键点～第6关键点局部坐标系坐标值，然后调整第一动作，使整个钻臂绕该钻臂的局部坐标系的y轴回转，使第6关键点的x局部坐标值变为0，并根据第一动作回转之前的第6关键点的x局部坐标值和z局部坐标值计算出第一动作所回转的角度值，该角度值即为第一动作的参数值；

步骤5、计算出第一动作参数值后，使第一动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第二动作，使整个钻臂绕该钻臂的局部坐标系的x轴摆动，使第6关键点的y局部坐标值变为0，并根据第二动作回转之前的第6关键点的y局部坐标值和z局部坐标值计算出第二动作所摆动的角度值，该角度值即为第二动作的参数值；

步骤6、计算出第二动作参数值后，使第二动作归位到其在局部坐标系内的初始0位位置，计算出此时各关键点的局部坐标值；然后调整第三动作，将钻臂绕该钻臂的局部坐标系z轴回转，使第4关键点的x局部坐标值变为0，并根据第三动作回转之前的第4关键点的x局部坐标值和y局部坐标值计算出所回转的角度值，该角度值即为第三动作的参数值；

步骤7、计算出第三动作参数值后，使第三动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第四动作，使第1关键点和第4关键点组成的直线回转到水平状态，该回转角度的负值即为第四动作的参数值。

本发明的技术方案的进一步限定在于：运用迭代搜索方法计算出第六动作和第五动作的参数值时，按如下具体方法进行：

将第4关键点和第5关键点和第8关键点组成空间平面Ⅰ，将第4关键点、第5关键点和第6关键点组成空间平面Ⅱ，选取第六动作前后移动的伸缩下限值、伸缩上限值和伸缩中位值作为三次移动的移动值，并分别计算每次伸缩移动后的第五动作参数值和第六动作参数值，计算第五动作参数值是计算空间平面Ⅰ和空间平面Ⅱ之间夹角，每计算出一个第五动作参数值后按照此参数值使第五动作归位到局部坐标系内的初始零位置，然后计算出第五动作归位后的第6关键点的局部坐标值并计算第6关键点到第8关键点的距离，将计算得出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离进行比较，从三次比较结果中选取两个与第7关键点到第8关键点的距离最接近的第6关键点到第8关键点的距离的移动值作为下一次迭代运算的伸缩上限值和伸缩下限值，并根据此伸缩上限值和伸缩下限值继续求出伸缩中位值，之后继续按照此伸缩上限值、伸缩下限值和伸缩中位值使第六动作移动，并继续计算第五动作的参数值，计算按照新计算出的第五动作的参数值归位后的第6关键点到第8关键点的距离，并将计算得出的新的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离进行比较，直到某一次计算出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离的差值小于事先设定的值，便终止迭代，此时计算出的第六动作和第五动作参数值即为精确的第六动作和第五动作的参数值。

本发明的技术方案的进一步限定在于：

第一次对第六动作和第五动作进行迭代搜索计算时是将钎杆的最小可伸缩量、最大可伸缩量以及最小可伸缩量和最大可伸缩量的均值设定为伸缩下限值、伸缩上限值和伸缩中位值。

本发明的技术方案的进一步限定在于：

逐步归位的方法是每计算出一个自由度便将该自由度所对应动作归位并计算出该次归位后各关键点坐标值用于求解下一自由度。

由于采用上述技术方案，本发明所产生的有益效果在于：

本发明的方法在确定掘进钻车的钻眼位置时，运算速度快且准确，能够使掘进钻车自动快速定位，能够在给定所打炮眼和机身位姿后便可快速准确计算出钻车钻臂和滑架位置参数值，以使钎杆能够准确对准钻孔位置。采用本发明的方法对钻车上安装的计算机运算速度要求低，适合于实际矿山工程中使用。

附图说明

图1是掘进钻车在掘进巷道内所处位置；

图2是巷道全局坐标系和左右钻臂各自局部坐标系；其中O-X-Y-Z为全局坐标系，O₀-X₀-Y₀-Z₀为钻臂的局部坐标系；其余小坐标系是各运动部件各自的坐标系，本发明中没有涉及；

图3a是单个钻臂的正面结构示意图，图3b是图3a的俯视结构示意图，其中：1是钎杆，2是滑架，3是定向滑道，4是支架，5是支管，6是钻臂架，7是轴套，8是操作台架；

图4a～图4f是钻臂6个动作各自初始0位位置以及正负向运动状态示意图；其中：第一动作(第一自由度)是钻臂绕轴套7左右摆动(图4a)；第二动作(第二自由度)是钻臂绕铰接在轴套7上的旋转绕轴相对轴套上下摆动(图4b)；第三动作(第三自由度)是支管相对于钻臂架顺向逆向旋转(图4c)；第四动作(第四自由度)是支架4带动定向滑道3、滑架2、钎杆1绕支架4与支管5的铰接轴相对于支管5上下摆动(图4d)；第五动作(第五自由度)是定向滑道3、滑架2、钎杆1绕支架4上的竖轴水平旋转(图4e)，是滑架相对于滑架支架左右摆动；第六动作(第六自由度)是滑架2和钎杆1在定向滑道3上前后移动(图4f)。

图5是左右钻臂各自打某一炮眼时、定位完成后各关键点所处位置；图5a是部分关键点的放大图；

图6是单个钻臂各部件均处于初始0位位置时各关键点的位置图；其中第六关键点与第7关键点(支管关键点)位置是重合的。

具体实施方式

本发明需要解决的技术问题是为掘进钻车实现自动定位打眼提供一种精确、快速、适用于工程实际的运算算法。

掘进钻车自动定位系统运算方法基于掘进钻车结构，钻车主要工作机构为左右钻臂，左右钻臂均由轴套7、钻臂架6、支管5、支架4、定向滑道3、滑架2和钎杆1组成，且左右钻臂均通过轴套7铰接在钻车的机架8上，轴套7竖向设置在机架8上，轴套7绕其轴心线转动时，整个钻臂相对于机架8左右摆动，如图4a所示，本发明将此动作设定为第一动作(即第一自由度)。轴套7的上端直径方向也铰接有钻臂架6，钻臂架6在油缸的推拉作用下能够上下摆动，并带动整个钻臂上下摆动，如图4b所示，本发明将此动作设定为第二动作(即第二自由度)。钻臂架6的前端为伸缩套管，此伸缩套管内套接支管5，支管5绕自身和伸缩套管的共同中心线转动时，带动钻臂的支管5、支架4、定向滑道3、滑架2和钎杆1一起转动，如图4c所示，本发明将此动作设定为第三动作(即第三自由度)。在支管5的前端铰接有支架4，在支管5下面和支架4下面铰接一油缸，在油缸的推动下，支架4能够带动定向滑道3、滑架2和钎杆1相对于支管5上下摆动，如附图4d所示，本发明将此动作设定为第四动作(即第四自由度)。支架4的上面设置有定向滑道3，定向滑道3上面设置滑架2和钎杆1，定向滑道3与支架4之间通过一竖向的铰接轴实现铰接，使定向滑道3能够带动滑架2、钎杆1绕支架4与定向滑道3的竖向铰接轴做水平转动，如图4e所示，本发明将此动作设定为第五动作(即第五自由度)。定向滑道3上面设置滑架2，滑架2上面设置钎杆1，滑架2能够沿定向滑道3的长度方向前后滑动，从而带动钎杆1前后伸缩移动，本发明将此动作设定为第六动作(即第六自由度)；在自动定位过程中钎杆和滑架视为固联于一体的整体，在定位过程钎杆相对于滑架不动。上述描述的动作均是以钻臂处于初始零位置时为基准进行的描述。第一动作至第五动作用角度值量化计量，单位为°，第六个动作用长度值量化计量，单位mm。

本发明的钻臂动作的各自由度表述如下：

将钻车机架上的轴套7绕自身轴心线转动时带动钻臂的左右摆动设定为第一动作(第一自由度)，将铰接在轴套7上的钻臂架6的上下摆动而带动钻臂的上下摆动设定为第二动作(第二自由度)，将钻臂架6前端套接的支管5绕自身轴线相对于钻臂架6的旋转设定为第三动作(第三自由度)，将钻臂的支架4带动定向滑道3、滑架2、钎杆1绕支架4与支管5的横向铰接轴的上下摆动设定为第四动作(第四自由度)，第四动作是支架4相对于支管5上下摆动；将定向滑道3带动滑架2、钎杆1绕支架4与定向滑道3的竖向铰接轴做水平转动设定为第五动作(第五自由度)，第五动作是滑架相对于滑架支架左右摆动；将滑架2带动钎杆1在定向滑道3上前后伸缩移动设定为第六动作(第六自由度)。

实现本发明的方法还需要在按照如下方法建立巷道三维空间内的全局坐标系、钻车的两个钻臂的各运动部件的局部坐标系。

在巷道三维空间内建立全局坐标系，在全局坐标系内建立钻臂的局部坐标系；全局坐标系以掘进工作面中心垂线与巷道底板中心线的交点为坐标原点，自坐标原点沿掘进工作面竖直向上设定为Y轴正向，自坐标原点沿巷道底板向后设定为Z轴正向，面向掘进工作面时右手方向为X轴正向；局部坐标系是以第一动作绕轴中心线和第二动作绕轴中心的交点为原点，局部坐标系的各坐标轴正向与全局坐标系一致，全局坐标系和局部坐标系如附图2所示。

本发明还需在全局坐标系中选取包括局部坐标原点在内的8个点作为关键点，并将这8个关键点固联在钻臂的相应位置或者虚拟位置上。相应位置是指钻臂(包括钎杆1、滑架2、定向滑道3、支架4、支管5、钻臂架6、轴套7)的实际机械结构部件(刚体部件)，虚拟位置是指位于实际机械结构部件以外的位置(虚拟刚体)，虚拟刚体与实际刚体固联。

所述8个关键点包括：第1关键点是钎杆的钎尖所在的点。第2关键点是钎杆1中心线上距离钎尖一定距离的点，第2关键点是从第1关键点开始沿着钎杆轴线方向向钎杆尾端延伸的点，第2关键点距第1关键点为固定距离，该距离是人为设定距离，本实施例设定为500mm或400mm，设定之后在以后的计算中便不再更改。第3关键点是在钎杆所在的虚拟刚体上选定的一点，第3关键点和第1关键点的连线垂直于钎杆轴线，第3关键点是位于与滑架的上平面平行的钎杆中心线所在平面内、距离钎尖特定距离的点，当人面向掘进工作面(从钎杆尾部向钎杆前面看)时，第3关键点位于钎杆1的右侧；第3关键点在与滑架的上平面平行的一平面内，此平面是钎杆的中心线所在的平面；第3关键点与钎杆尖(第1关键点)的连线垂直于钎杆中心线(第1关键点与第2关键点的连线)，且该连线平行于滑架上表面，该点距钎杆尖是人为设定固定距离，本实施例中将该距离特定为500或600，设定之后便不再更改。第4关键点是由钻臂的实际结构尺寸确定的位于钎杆中心线上是距离第1关键点固定距离的点，该固定距离由钻臂各机械部件的实际结构尺寸决定，是从钎尖向钎杆尾端延伸的一个点，但不同于第2关键点，第4关键点到第1关键点的距离大于第2关键点到第1关键点的距离，当钻臂上所有动作部件均处于零位时，第4关键点位于第五自由度旋转绕轴上，是第五动作的竖轴中心线与钎杆中心线的交点。第5关键点是在钎杆所在的虚拟刚体上选定的一点，第5关键点与第4关键点的连线垂直于与滑架的上平面平行的钎杆中心线所在平面(即第1、2、3关键点组成的平面)，第5关键点到第4关键点的距离是由钻臂各部件的实际机械结构尺寸决定的固定数值，当钻臂上所有动作部件均是初始零位置时，第5关键点位于第五自由度旋转绕轴(竖轴)上，第5关键点是第五动作的竖轴中心线与第三动作的旋转绕轴中心线的交点，图6中钎杆第4关键点、第5关键点所在的虚线绕轴与定向滑道3相对于支架4摆动绕轴重合；第5关键点和第4点是关联的，当滑架2处于水平位置或者初始零位置时，第5关键点在第4点的正下方，且是一个固定距离。第1关键点到第5关键点都与钎杆所在刚体固联，并随钎杆移动而变动。第6关键点是在钎杆所在的虚拟刚体上选定的一点，第6关键点与第5关键点的连线平行于钎杆中心线(第1关键点和第4关键点的连线)，当钻臂各部件均处于初始零位置时，第6关键点位于第四动作旋转绕轴的中心线上，第6关键点与支管关键点(第7关键点)重合(如附图6所示)，第6关键点到第5关键点的距离是由钻臂各部件的实际机械结构所决定的固定数值，该距离根据钻臂各部件的实际结构尺寸确定，本实施例中第6关键点位于第5点正后方130mm处。第7关键点位于支管5的端部，是第三动作的旋转绕轴的中心线和第四动作旋转绕轴中心线的交点(即是支管5和支架4的铰接点)，在钻臂的各部件处于初始零位置时，第6关键点与第7关键点重合；第8关键点是第一动作旋转绕轴的中心线和第二动作旋转绕轴中心线的交点，第8关键点是钻臂架与轴套的铰接点，也是单个钻臂的局部坐标原点；无论钻臂的各部件如何运动，第7关键点到第8关键点的距离为固定值。

本发明是使用过程中，使钻车位于巷道中、全局坐标系的Z轴方向，并确定巷道掘进工作面上需打的炮眼位置、炮眼类型，选定打眼的钻臂(在程序运用时会提醒用户选定打眼钻臂为左钻臂或右钻臂)，即可根据打眼工艺要求计算确定钻臂、钎杆的最终位姿，确定钻杆的最终位姿时是先计算第1关键点、第2关键点、第3关键点在全局坐标系中的坐标值，然后将这三个关键点的全局坐标系的坐标值转换为所选定的打眼钻臂的局部坐标系的坐标值，然后计算出第4关键点、第5关键点、第6关键点在局部坐标系中的坐标值(第7关键点此时不需计算，第8关键点是局部坐标系的坐标原点，此时也不需计算)，以下所述的所有反算流程均在所选定的局部坐标系内运算。在得知第1关键点～第6关键点的局部坐标系的坐标值后，通过逆向求解计算出钻臂第一动作～第六动作的参数值，然后按照计算出的第一动作～第六动作参数值驱动钻臂各部件动作确定钻臂的最终位姿，使钎杆对正打眼位置进行钻孔。

本发明在逆向求解6个动作(第一动作～第六动作)的参数值时，首先是运用迭代搜索方法求解第六动作和第五动作的参数值，之后通过逐步归位的方法依次求解第一动作、第二动作、第三动作和第四动作的参数值，在用迭代搜索方法和逐步归位的方法求解过程中是调用包括刚体任意运动后刚体上一点坐标变换的计算方法、空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面之间夹角的计算方法、空间点绕空间轴线旋转一定角度后坐标变换的计算方法。

刚体任意动后刚体上一点坐标变换的计算方法是用于计算处于最终位置的钎杆上第四关键点、第五关键点、第六关键点坐标，这里的最终位置是指打眼时的钻臂及钎杆的位置，是在要打某一孔位(炮眼)时钻臂各件按照计算出来的参数值动作完成后，钎杆所处的位置。该计算方法是输入钎杆任意动前第1关键点、第2关键点、第3关键点的坐标值以及动后第1关键点、第2关键点、第3关键点的坐标值，然后输入所需求解的第4关键点、第5关键点、第6关键点的动前的坐标值即可求出相应关键点动后的坐标值；

空间点沿空间直线延伸一定长度后坐标变换的计算方法是用于计算钎杆在最终位置状态下钎杆1沿着钎杆轴线移动一段距离后钎杆上各关键点(第1关键点～第6个关键点)的坐标值，运用该计算方法时，输入第1关键点坐标值和第2关键点坐标值以及所求的关键点移动之前的坐标值和所求关键点钎杆沿轴线移动的向量即可求出延伸后的该点坐标值；钎杆沿轴线移动距离可大可小，方向可前可后、可正可负，因此所求关键点钎杆沿轴线移动的量用向量表示(正负数值)。

空间点绕空间轴线旋转一定角度后坐标变换的计算方法是用于计算第一动作至第五动作的每一旋转动作归位后各相关关键点坐标值，运用该计算方法时输入空间点绕空间轴线旋转的角度向量和关键点旋转之前坐标值即可求出旋转后该点坐标值。

本发明确定钻臂及钎杆的最终位姿的计算流程为：

步骤1、确定钻车的左钻臂局部坐标系原点、右钻臂局部坐标系原点在全局坐标系中的位置坐标值，并确定所要打的炮眼在全局坐标系中坐标值，选定打炮眼的钻臂是左钻臂还是右钻臂。

步骤2、计算出钻臂及钎杆上各个关键点(第1关键点～第6个关键点)在全局坐标系中的全局坐标值并将各个关键点的全局坐标值转换为所选定的钻臂局部坐标系内的局部坐标值；以下所述的所有反算流程均在所选定的局部坐标系内运算。

步骤3、运用迭代搜索方法计算出第六动作和第五动作的参数值；计算时，需使用空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面夹角角度的计算；第六自由度和第五自由度参数值的计算采用迭代搜索方法计算，第五自由度参数值的计算是转换为两个空间平面之间角度计算。

步骤4、计算出第六动作和第五动作的参数值之后，将第六动作和第五动作分别归位到各自在局部坐标系内的初始零位置，求出归位后的各关键点(第1关键点～第8个关键点)局部坐标系坐标值，然后调整第一动作，使整个钻臂绕该钻臂的局部坐标系的y轴回转，使第6关键点的x局部坐标值变为0，并根据第一动作回转之前的第6关键点的x局部坐标值和z局部坐标值计算出第一动作所回转的角度值，该角度值即为第一动作的参数值；

步骤5、计算出第一动作参数值后，使第一动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第二动作，使整个钻臂绕该钻臂的局部坐标系的x轴摆动，使第6关键点的y局部坐标值变为0，并根据第二动作回转之前的第6关键点的y局部坐标值和z局部坐标值计算出第二动作所摆动的角度值，该角度值即为第二动作的参数值；

步骤6、计算出第二动作参数值后，使第二动作归位到其在局部坐标系内的初始0位位置，计算出此时各关键点的局部坐标值；然后调整第三动作，将钻臂绕该钻臂的局部坐标系z轴回转，使第4关键点的x局部坐标值变为0，并根据第三动作回转之前的第4关键点的x局部坐标值和y局部坐标值计算出所回转的角度值，该角度值即为第三动作的参数值；

步骤7、计算出第三动作参数值后，使第三动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第四动作，使第1关键点和第4关键点(钎杆中心线)组成的直线回转到水平状态，该回转角度的负值即为第四动作的参数值。

本发明运用迭代搜索方法求解第六动作和第五动作的参数值时，需用到空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面夹角角度的计算方法。运用迭代搜索方法计算出第六动作和第五动作的参数值时，按如下具体方法进行：

迭代搜索方法中，将第4关键点和第5关键点和第8关键点组成空间平面Ⅰ，将第4关键点、第5关键点和第6关键点组成空间平面Ⅱ，并设定钎杆的伸缩量，钎杆的伸缩量是选取第六动作前后移动的伸缩下限值、伸缩上限值和伸缩中位值作为三次移动的移动值，第一次对第六动作和第五动作进行迭代搜索计算时是将钎杆的最小可伸缩量(如0)、最大可伸缩量(如本实施例中是2000mm)以及最小可伸缩量和最大可伸缩量的均值(1000mm)设定为伸缩下限值、伸缩上限值和伸缩中位值。

然后分别计算每次伸缩移动后的第五动作参数值和第六动作参数值，计算第五动作参数值是计算空间平面Ⅰ和空间平面Ⅱ之间夹角，每计算出一个第五动作参数值后按照此参数值使第五动作归位到局部坐标系内的初始零位置(即使滑架2回转到初始零位置)，然后计算出第五动作归位后的第6关键点的局部坐标值并计算第6关键点到第8关键点的距离(在此假定为X)，将计算得出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离(在此假定为L)进行比较，从三次比较结果中选取两个与第7关键点到第8关键点的距离最接近的第6关键点到第8关键点的距离的移动值作为下一次迭代运算的伸缩上限值和伸缩下限值，即3个X中与L最接近的两个X，并根据此伸缩上限值和伸缩下限值继续求出伸缩中位值，即求出两个X平均值，将较小的X再设定为伸缩下限值，将较大的X再设定为伸缩上限值，将平均值设置为伸缩中位值，之后继续按照此伸缩上限值、伸缩下限值和伸缩中位值使第六动作移动(即是使钎杆移动)，并继续计算第五动作的参数值，计算按照新计算出的第五动作的参数值归位后的第6关键点到第8关键点的距离(如假定为X1，此时又得到三个X1)，并将计算得出的新的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离(L)进行比较(将三个X1与L进行比较)，直到某一次计算出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离L的差值小于事先设定的值(本实施例中事先设定的值为10^-8mm)，即Xn与L的差值小于10^-8mm，便终止迭代，此时计算出的第六动作和第五动作参数值即为精确的第六动作和第五动作的参数值，且此时的第6关键点与支管关键点坐标重合。

本发明所述逐步归位的方法是每计算出一个自由度便将该自由度所对应动作归位并计算出该次归位后各关键点坐标值用于求解下一自由度。

Claims (6)

1.掘进钻车自动定位方法，其特征在于：将钻车机架上的轴套(7)绕自身轴心线转动时带动钻臂的左右摆动设定为第一动作，将铰接在轴套(7)上的钻臂架(6)的上下摆动而带动钻臂的上下摆动设定为第二动作，将钻臂架(6)前端套接的支管(5)绕自身轴线相对于钻臂架(6)的旋转设定为第三动作，将钻臂的支架(4)带动定向滑道(3)、滑架(2)、钎杆(1)绕支架(4)与支管(5)的横向铰接轴的上下摆动设定为第四动作，将定向滑道(3)带动滑架(2)、钎杆(1)绕支架(4)与定向滑道(3)的竖向铰接轴做水平转动设定为第五动作，将滑架(2)带动钎杆(1)在定向滑道(3)上前后伸缩移动设定为第六动作；

在巷道三维空间内建立全局坐标系，在全局坐标系内建立钻臂的局部坐标系；全局坐标系以掘进工作面中心垂线与巷道底板中心线的交点为坐标原点，自坐标原点沿掘进工作面竖直向上设定为Y轴正向，自坐标原点沿巷道底板向后设定为Z轴正向，面向掘进工作面时右手方向为X轴正向；局部坐标系是以第一动作绕轴中心线和第二动作绕轴中心的交点为原点，局部坐标系的各坐标轴正向与全局坐标系一致；

在全局坐标系中选取包括局部坐标原点在内的8个点作为关键点并将这8个关键点固联在钻臂的钎杆(1)、滑架(2)、定向滑道(3)、支架(4)、支管(5)、钻臂架(6)、轴套(7)的相应位置或者虚拟位置上；

所述8个关键点包括：第1关键点是钎杆的钎尖所在的点；第2关键点是钎杆(1)中心线上距离钎尖一定距离的点；第3关键点是位于与滑架的上平面平行的钎杆中心线所在平面内、距离钎尖特定距离的点，第1关键点和第3关键点连线垂直于钎杆轴线，面向掘进工作面时第3关键点位于钎杆(1)的右侧；第4关键点是由钻臂的实际结构尺寸确定的位于钎杆中心线上是距离第1关键点固定距离的点，第4关键点到第1关键点的距离大于第2关键点到第1关键点的距离，当钻臂上所有动作部件均处于零位时，第4关键点是第五动作的竖轴中心线与钎杆中心线的交点；第5关键点与第4关键点的连线垂直于与滑架的上平面平行的钎杆中心线所在平面，第5关键点到第4关键点的距离是由钻臂各部件的实际机械结构尺寸决定的固定数值，当钻臂上所有动作部件均是初始零位置时，第5关键点是第五动作的竖轴中心线与第三动作的旋转绕轴中心线的交点；第6关键点与第5关键点的连线平行于钎杆中心线，第6关键点到第5关键点的距离是由钻臂各部件的实际机械结构所决定的固定数值，当钻臂各部件均处于初始零位置时，第6关键点位于第四动作旋转绕轴的中心线上；第7关键点位于支管(5)的端部，是第三动作的旋转绕轴的中心线和第四动作旋转绕轴中心线的交点，在钻臂的各部件处于初始零位置时，第6关键点与第7关键点重合；第8关键点是第一动作旋转绕轴的中心线和第二动作旋转绕轴中心线的交点；

使钻车位于巷道中、全局坐标系的Z轴方向，并确定巷道掘进工作面上需打的炮眼位置，选定打眼的钻臂并确定钎杆的最终位姿，确定钻杆的最终位姿时是先计算第1关键点、第2关键点、第3关键点在全局坐标系中的坐标值，然后将这三个关键点的全局坐标系的坐标值转换为所选定的打眼钻臂的局部坐标系的坐标值，然后计算出第4关键点、第5关键点、第6关键点在局部坐标系中的坐标值，得知第1关键点～第6关键点的局部坐标系的坐标值后，通过逆向求解计算出钻臂第一动作～第六动作的参数值，然后按照计算出的第一动作～第六动作参数值驱动钻臂各部件动作确定钻臂的最终位姿，使钎杆对正打眼位置进行钻孔。

2.根据权利要求1所述的掘进钻车自动定位方法，其特征在于：在逆向求解6个动作的参数值时，首先是运用迭代搜索方法求解第六动作和第五动作的参数值，之后通过逐步归位的方法依次求解第一动作、第二动作、第三动作和第四动作的参数值，在用迭代搜索方法和逐步归位的方法求解过程中是调用包括刚体任意运动后刚体上一点坐标变换的计算方法、空间点沿空间直线延伸一定长度后坐标变换的计算方法、两空间平面之间夹角的计算方法、空间点绕空间轴线旋转一定角度后坐标变换的计算方法；

刚体任意动后刚体上一点坐标变换的计算方法是用于计算处于最终位置的钎杆上第四关键点、第五关键点、第六关键点坐标，该计算方法是输入钎杆任意动前第1关键点、第2关键点、第3关键点的坐标值以及动后第1关键点、第2关键点、第3关键点的坐标值，然后输入所需求解的第4关键点、第5关键点、第6关键点的动前的坐标值即可求出相应关键点动后的坐标值；

空间点沿空间直线延伸一定长度后坐标变换的计算方法是用于计算钎杆在最终位置状态下钎杆(1)沿着钎杆轴线移动一段距离后钎杆上第1关键点～第6关键点的坐标值，运用该计算方法时，输入第1关键点坐标值和第2关键点坐标值以及所求的关键点移动之前的坐标值和所求关键点钎杆沿轴线移动的向量即可求出延伸后的该点坐标值；

空间点绕空间轴线旋转一定角度后坐标变换的计算方法是用于计算第一动作至第五动作的每一旋转动作归位后各相关关键点坐标值，运用该计算方法时输入空间点绕空间轴线旋转的角度向量和关键点旋转之前坐标值即可求出旋转后该点坐标值。

3.根据权利要求1或2任一项所述的掘进钻车自动定位方法，其特征在于：确定钻臂的最终位姿的计算流程为：

步骤1、确定钻车的左钻臂局部坐标系原点、右钻臂局部坐标系原点在全局坐标系中的位置坐标值，并确定所要打的炮眼在全局坐标系中坐标值，选定打炮眼的钻臂是左钻臂还是右钻臂；

步骤2、计算出第1关键点～第6关键点在全局坐标系中的全局坐标值并将各个关键点的全局坐标值转换为所选定的钻臂局部坐标系内的局部坐标值；

步骤3、运用迭代搜索方法计算出第六动作和第五动作的参数值；

步骤4、计算出第六动作和第五动作的参数值之后，将第六动作和第五动作分别归位到各自在局部坐标系内的初始零位置，求出归位后的第1关键点～第6关键点局部坐标系坐标值，然后调整第一动作，使整个钻臂绕该钻臂的局部坐标系的y轴回转，使第6关键点的x局部坐标值变为0，并根据第一动作回转之前的第6关键点的x局部坐标值和z局部坐标值计算出第一动作所回转的角度值，该角度值即为第一动作的参数值；

步骤5、计算出第一动作参数值后，使第一动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第二动作，使整个钻臂绕该钻臂的局部坐标系的x轴摆动，使第6关键点的y局部坐标值变为0，并根据第二动作回转之前的第6关键点的y局部坐标值和z局部坐标值计算出第二动作所摆动的角度值，该角度值即为第二动作的参数值；

步骤6、计算出第二动作参数值后，使第二动作归位到其在局部坐标系内的初始0位位置，计算出此时各关键点的局部坐标值；然后调整第三动作，将钻臂绕该钻臂的局部坐标系z轴回转，使第4关键点的x局部坐标值变为0，并根据第三动作回转之前的第4关键点的x局部坐标值和y局部坐标值计算出所回转的角度值，该角度值即为第三动作的参数值；

步骤7、计算出第三动作参数值后，使第三动作归位到其在局部坐标系内的初始零位置，计算出此时各关键点的局部坐标值；然后调整第四动作，使第1关键点和第4关键点组成的直线回转到水平状态，该回转角度的负值即为第四动作的参数值。

4.根据权利要求3所述的掘进钻车自动定位方法，其特征在于：运用迭代搜索方法计算出第六动作和第五动作的参数值时，按如下具体方法进行：

将第4关键点和第5关键点和第8关键点组成空间平面Ⅰ，将第4关键点、第5关键点和第6关键点组成空间平面Ⅱ，选取第六动作前后移动的伸缩下限值、伸缩上限值和伸缩中位值作为三次移动的移动值，并分别计算每次伸缩移动后的第五动作参数值和第六动作参数值，计算第五动作参数值是计算空间平面Ⅰ和空间平面Ⅱ之间夹角，每计算出一个第五动作参数值后按照此参数值使第五动作归位到局部坐标系内的初始零位置，然后计算出第五动作归位后的第6关键点的局部坐标值并计算第6关键点到第8关键点的距离，将计算得出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离进行比较，从三次比较结果中选取两个与第7关键点到第8关键点的距离最接近的第6关键点到第8关键点的距离的移动值作为下一次迭代运算的伸缩上限值和伸缩下限值，并根据此伸缩上限值和伸缩下限值继续求出伸缩中位值，之后继续按照此伸缩上限值、伸缩下限值和伸缩中位值使第六动作移动，并继续计算第五动作的参数值，计算按照新计算出的第五动作的参数值归位后的第6关键点到第8关键点的距离，并将计算得出的新的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离进行比较，直到某一次计算出的第6关键点到第8关键点的距离与第7关键点到第8关键点的距离的差值小于事先设定的值，便终止迭代，此时计算出的第六动作和第五动作参数值即为精确的第六动作和第五动作的参数值。

5.根据权利要求4所述的掘进钻车自动定位方法，其特征在于：第一次对第六动作和第五动作进行迭代搜索计算时是将钎杆的最小可伸缩量、最大可伸缩量以及最小可伸缩量和最大可伸缩量的均值设定为伸缩下限值、伸缩上限值和伸缩中位值。

6.根据权利要求3所述的掘进钻车自动定位方法，其特征在于：所述逐步归位的方法是每计算出一个自由度便将该自由度所对应动作归位并计算出该次归位后各关键点坐标值用于求解下一自由度。