CN108301766B - 一种钻孔自动定位凿岩台车的双钻臂车体定位方法 - Google Patents

Abstract

本发明涉及巷、隧道车体定位技术领域，提供了一种钻孔自动定位凿岩台车的双钻臂车体定位方法，利用巷道掘进定向激光，设置左右两束垂直于工作面的激光（垂足在工作面上位置已知）；凿岩台车行驶至工作面前，使钻钎轴线和两侧激光平行且距离固定，利用钻臂各关节随动传感器所测数据，反推计算车体与工作面的位姿关系；借助台车两条独立工作的钻臂，将求解车体位姿问题转化为求解两无穷集合的交集，对此进行理论与实验分析，确定求解车体位姿的方法，并按照左右双臂定位结果分别钻孔，实现了工作面不平整时的台车车体定位方法；该方法适用于在巷、隧道等不具备与外界通信条件下的车体定位，解决了单臂车体求解车体与巷道位置关系为无穷解的缺陷。

Description

一种钻孔自动定位凿岩台车的双钻臂车体定位方法

技术领域

本发明涉及巷、隧道车体定位技术领域，具体设计一种钻孔自动定位凿岩台车的双钻臂车体定位方法。

背景技术

自动控制凿岩台车受计算机控制系统控制工作，具有极高的钻孔精度，使用自动控制台车是今后井巷钻孔作业的趋势。车体定位是自动控制凿岩台车钻孔自动定位的先决工序，用以确定车体和巷道工作面的位置关系。可以将车体定位视为钻孔自动定位的一次手动操作示范，其完成质量直接影响钻孔自动定位的精度，影响钻爆法施工效果，具有重要地位。

在过去凿岩台车自动定位控制研究中，巷道与车体相对位置关系求解的一种重要方法是钻臂定位法，这一方法的原理是：建立单个钻臂和激光的平行关系，在各重要结点(车体、钻臂各轴、激光、工作面)设坐标系，通过各结点相互之间可测定的坐标系变换关系，最终得到车体和工作面两坐标系间的位姿关系。理想的“单臂定位方法”在钻臂末端存在无法锁定或消除的绕激光束横向旋转的随机不可控转角变量θ，使得车体位姿相对工作面不是唯一确定的，因此实际中无法使用。为了解决这一问题，在各钻臂前端安装一枚监测角度的传感器，虽然能够读取该变量θ值，配合单臂定位方法确定车体位姿，然而受安装位置的选择及安装平整度的影响，会引入传感器监测误差。且钻杆反复钻凿岩石的过程将造成传感器连接不稳定，不能准确测定数据。增加多个传感器还会提升计算难度，产生新的误差源。此外双臂凿岩台车工作时，整个工作面分为左右两个区域，爆后工作面凹凸不平，单臂定位方法设定的计算工作平面没有体现该特点。使用左臂完成车体定位后，误差通过左臂传导至右臂，右侧进行钻孔时，炮孔定位误差也会被放大，反之亦然。这些都是单臂定位方法存在的问题。

发明内容

本发明的目的就是克服现有技术的不足，提供了一种钻孔自动定位凿岩台车的双钻臂车体定位方法，适用于尺寸狭窄、条件复杂的作业环境下的双钻臂钻孔自动定位凿岩台车在巷、隧道等不具备与外界通信条件的情况下进行车体定位。

在工作面左右两侧设置垂直于工作面的激光，垂足在工作面上位置已知。钻孔自动定位凿岩台车行驶至工作面前，左右双臂同时使用激光靶穿孔定位，基于机器人运动学中“D-H法”分别建立双钻臂各自的坐标系变换矩阵。此时，左右两侧均存在独立的θ角，通过任何一侧钻臂单独定位均得到车体位姿的无穷解。然而同时满足两侧定位结果的真实解有且只有一种，引入集合判断标准首先找出车体基坐标系原点的位置。然后利用编程手段求解随机角变量θ，得到车体位姿矩阵。从矩阵中提取出车体实际停车高度、偏中线距离、俯仰角、翻转角、到工作面距离等信息。与现场利用全站仪得到的实验测试结果进行对比，确认通过双臂车体定位方法计算得到的车体位姿各项指标的正确性。

本发明的技术方案如下：

本发明一种钻孔自动定位凿岩台车的双钻臂车体定位方法，该方法建立双钻臂各自的坐标系变换矩阵，根据所述坐标系变换矩阵确定车体基坐标系原点的位置，计算出双钻臂末端各自的随机角变量θ，得到车体位姿矩阵；从所述车体位姿矩阵中提取出车体实际停车高度、偏中线距离、俯仰角、翻转角及到工作面距离。

进一步的，具体包括如下步骤：

步骤一、将凿岩台车的钻臂刚体简化为连杆，将转动轴、移动轴简化为关节，作钻臂简化模型图，根据凿岩台车的尺寸确定各关节的基准位置、相对角度及各关节的运动变化范围；

步骤二、人工手动操作车体钻臂进行定位：分别在两侧钻臂的推进梁安装前、后两个同样尺寸的激光靶，在激光靶相同位置开有小孔，保证前后两个小孔确定的轴线与钻钎平行，且具有固定可测的位置距离关系；在隧道左右两侧设置平行激光。车体停稳于工作面后，人工操作车体两侧钻臂，使激光先后穿过对应侧两个激光靶的小孔，并保持与钻钎平行；

步骤三、建立激光坐标系与车体基坐标系的关系；

步骤四、根据集合约束条件确定车体在工作面坐标系中的位姿矩阵。

进一步的，步骤三中，建立激光坐标系与车体基坐标系的关系的具体步骤为：

步骤3.1)根据D-H法在钻臂各关节建立坐标系，确定各坐标系的相对变换关系；建立左侧钻臂末端坐标系{O_6L}与左侧钻臂基坐标系{O_0L}之间的变换矩阵右侧钻臂末端坐标系{O_6R}与右侧钻臂基坐标系{O_0R}之间的变换矩阵因钻臂尺寸固定，且各关节转角、位移可通过传感器检测得到，故矩阵在任意时刻和状态下都是可测的，唯一确定的；

步骤3.2)将矩阵扩展为钻臂末端坐标系{O_6L}、{O_6R}与车体基坐标系{O_C}的变换矩阵

步骤3.3)当随机不可控转角变量θ_R＝0、θ_L＝0时，激光坐标系与钻臂末端坐标系仅存在平移关系；需要将矩阵扩充为激光坐标系{O_L}、{O_R}和车体基坐标系{O_C}的变换矩阵首先对矩阵求逆得到 再根据平移距离改写其中θ_R、θ_L分别为右侧钻臂末端、左侧钻臂末端的随机角变量。

进一步的，步骤四中，根据集合约束条件判断车体在工作面坐标系中的位姿矩阵的具体步骤为：

步骤4.1)已知左侧激光在工作面投影点O_L在工作面坐标系下的坐标为(U_L,V_L)，右侧激光在工作面投影点O_R在工作面坐标系下的坐标为(U_R,V_R)；根据矩阵可知：无论θ_R、θ_L取何值时，车体基坐标系{O_C}的原点O_C到左侧激光的距离均为L_L，到右侧激光的距离均为L_R；

步骤4.2)确定车体基坐标系原点的位置：在工作面上，至少有一个、至多有两个点到左侧激光投影点O_L的距离为L_L，同时到右侧激光投影点O_R的距离为L_R；当有两个点时，取位置较低点(U,V)为车体基坐标系原点O_C在工作面内的投影；

步骤4.3)根据确定的车体基坐标系原点O_C在工作面的投影位置，确定人工操作钻臂使激光通过小孔时，造成车体基坐标系{O_C}绕掘进方向旋转的夹角θ_R、θ_L。；所述绕掘进方向为激光指向方向；

步骤4.4)根据夹角θ_R、θ_L求得车体基坐标系{O_C}在工作面坐标系{O_G}下的坐标变换矩阵完成车体定位；从而由真实的位姿矩阵确定车体在断面内的停车高度、偏中线距离、俯仰角、翻转角、到工作面距离。

进一步的，对井下实际工作面不平整度的处理方法为：双臂车体定位后，车体到工作面的变换矩阵中隐含着车体到工作面的距离；因工作面不平整，左右两侧计算得到的距离不完全一致；钻孔时，因双钻臂凿岩台车分区域作业，距离较远的一侧，钻臂伸长量也会较大，钻孔深度应相对较小；距离较近的一侧，钻臂伸长量也会较小，钻孔深度应相对较大。

本发明的有益效果为：该方法依托现场实验，证明了实际可行性；不增加多余传感器、不存在两侧误差传递的问题，能体现工作面左右不平整情况；按照双臂车体定位结果得到的左右两个位姿矩阵分别安排钻孔，设置合理的钻钎钻进深度，有利于维持爆后工作面平整度；方法合理易行，适于推广应用。

附图说明

图1所示为钻孔自动定位凿岩台车机构示意图。

图2所示为单臂车体定位方法(不可靠)示意图。

图3所示为双臂车体定位方法示意图。

图4所示为集合约束条件确定唯一解示意图。

图5所示为凿岩台车机构图。

图6所示为本发明实施例中凿岩台车机构简图建立坐标系示意图。

图7所示为确定车体基坐标系原点的位置示意图。

图8所示为求随机不可控角变量示意图。

图9所示为全站仪测量车体位姿示意图。

具体实施方式

下文将结合具体附图详细描述本发明具体实施例。应当注意的是，下述实施例中描述的技术特征或者技术特征的组合不应当被认为是孤立的，它们可以被相互组合从而达到更好的技术效果。在下述实施例的附图中，各附图所出现的相同标号代表相同的特征或者部件，可应用于不同实施例中。

本实施例中，在实验厂房内对钻孔自动定位凿岩台车进行双臂车体定位方法的验证。搭建高3.6m，宽4.8m的钢结构模拟巷道。将车体停于模拟工作面前，车体基坐标系{O_C}原点O_C到车体履带底板高h＝1256mm，履带下方垫有约H＝420mm的高木方，稳定后不再移动。

车体定位是钻孔自动定位凿岩台车(如图1)在井下由电脑控制自动钻孔作业前必须进行的一项准备工作。单臂定位方法利用单侧钻臂进行操作，存在无法锁定或消除的随机不可控转角变量θ(左臂定位时表示为θ_L，右臂为θ_R)，钻臂末端坐标系的姿态与激光坐标系姿态不统一，导致车体基坐标系和工作面坐标系的变换关系无法求解(如图2)。因此提出一种双钻臂车体定位方法(如图3)，配合工作面集合约束条件(左臂定位有转角θ_L，车体位姿有一无穷解集A。同理右臂有转角θ_R和相应无穷解集B。双臂同时定位，能满足定位结果的真实解只有一个，且该解同时属于集合A和B，即A∩B。通过求两组无穷解集的交集得到车体定位的唯一解，如图4)，利用MATLAB编程求得车体坐标系原点位置(指步骤四①、②)，根据该位置计算出左臂转角θ_L和右臂转角θ_R(指步骤四③)，根据两转角值计算求得代表车体位姿的矩阵(指步骤四④)。主要实施步骤如下：

1)根据机构图，获得凿岩台车的基础尺寸信息。不同双臂凿岩台车的机构设计与尺寸(截面尺寸、长度)也不尽相同，但大体都能够简化为连杆和活动关节。将钻臂各刚体简化为连杆，将转动轴、移动轴简化为活动关节，作钻臂简化模型图，明确各关节的基准位置(如图5)及相对角度和各关节量的变化范围。

2)人工手动操作车体钻臂定位

分别在两侧钻臂推进梁合适位置安装前后两个同样尺寸的激光靶，在激光靶相同位置开有小孔，保证前后两个小孔确定的轴线与钻钎平行，且具有固定可测的位置距离关系。

在隧道左右两侧设置平行激光。车体停稳于工作面后，要求人工手动操作车体两侧钻臂进行一定量运动，使激光先后通过对应侧激光靶上两个小孔，使激光与钻钎平行。该操作过程中，车体位置不能发生移动，但左侧、右侧穿孔操作均以合适的方式进行，互相不发生干扰。

操作完成后，固连在钻臂各关节上的传感器可获得角度、移动距离数据。

3)建立激光坐标系与车体基坐标系的关系

①根据“D-H法”在钻臂各关节建立坐标系，明确各坐标系的相对变换关系：

以左侧钻臂为例，在各连杆端部都按照同一原则建立一个固连坐标系{O_i}，其中序号i＝1、2、……6(如图6)。按照台车模型确定相应的连杆尺寸参数a_i-1、d_i和连杆间角度参数α_i-1、θ_i列于表1，尺寸单位为mm，角度单位为°(度)。表中d_i、θ_i两列中θ₁-θ₅以及d₆为关节变量，其值由传感器实时确定，其余为常量。d_i、θ_i两列小括号内数据为图5各轴位于基准位置时变量取值。

表1 简化机构连杆参数表(左臂)

根据“D-H法”确定相邻两连杆坐标系的相对关系，坐标系{O_i}可以看成坐标系{O_i-1}按照一定顺序的四次变换(两个旋转Rot和两个平移Trans)得到。四次变换过程体现在坐标系{O_i}和{O_i-1}的变换矩阵A_i通式中：

根据左臂尺寸、角度信息(表1)，可依次确定左侧各变换矩阵如下(下标L表示左臂)：

式中角θ_i的正、余弦简化表示为：s_i、c_i。

由坐标系{O_i}和{O_i-1}的变换矩阵A_i，以及坐标系{O_i+1}和{O_i}的变换矩阵A_i+1，可确定坐标系{O_i-1}和{O_i+1}的变换矩阵最终建立左臂末端坐标系{O_6L}与左臂基坐标系{O_0L}之间的变换矩阵右臂末端坐标系{O_6R}与右臂基坐标系{O_0R}之间的变换矩阵

因钻臂尺寸固定，且各关节转角、位移都可以通过传感器检测得到，故矩阵在任意时刻和状态下都是可测的，唯一确定的。设：

其中：(n_xL,n_yL,n_Zl)、(o_xL,o_yL,o_zL)、(a_xL,a_yL,a_zL)、(p_xL,p_yL,p_zL)——{O_6L}系X_6L轴、Y_6L轴、Z_6L轴、原点O_6L在{O_0L}系下的坐标表达；(n_xR,n_yR,n_zR)、(o_xR,o_yR,o_zR)、(a_xR,a_yR,a_zR)、(p_xR,p_yR,p_zr)——{O_6R}系X_6R轴、Y_6R轴、Z_6R轴、原点O_6R在{O_0R}系下的坐标表达；

②将矩阵扩充为钻臂末端坐标系{O_6L}、{O_6R}与车体基坐标系{O_C}的变换矩阵

设车体基坐标系{O_C}位于左臂基坐标系{O_0L}和右臂基坐标系{O_0R}中间，三者仅有平移变换，而无旋转变换。设该平移距离为ε，ε表示与车体尺寸有关的常量，为正数。则钻臂末端坐标系{O_6L}、{O_6R}在车体基坐标系{O_C}下的位姿矩阵：

③随机不可控转角变量θ_R＝0、θ_L＝0时，激光坐标系与钻臂末端坐标系仅存在平移关系。因此，要将扩充为激光坐标系{O_L}、{O_R}和车体基坐标系{O_C}的变换矩阵首先对矩阵求逆得到

其中左侧：式中p_L＝(p_xL,p_yL+ε,p_zL)、n_R＝(n_xR,n_yR,n_zR)、o_R＝(o_xR,o_yR,o_zR)、a_R＝(a_xR,a_yR,a_zR)。右侧： 式中p_R＝(p_xR,p_yR–ε,p_zR)、n_R＝(n_xR,n_yR,n_zR)、o_R＝(o_xR,o_yR,o_zR)、a_R＝(a_xR,a_yR,a_zR)。

再根据平移距离改写考虑与两侧钻钎平行的激光，其在工作面的投影点与钻钎在工作面的投影点，在钻臂末端坐标系{O_6L}、{O_6R}的X、Y方向上存在固定距离εx、εy,设θ_R＝0、θ_L＝0时，车体基坐标系{O_C}在激光坐标系{O_L}、{O_R}在下的位姿矩阵：

具体到该台车，其两臂间距(即左右两钻臂基坐标系原点距离)2ε＝530mm；激光靶参数(即激光到钻杆末端坐标系原点的距离)εx＝60mm，εy＝120mm。

操作钻臂建立左右两侧钻臂和激光束的平行关系，完成后，读取左侧各传感器数据依次为：(-25.840°，-9.624°，0.309°，9.185°，28.330°，1183.680mm)，读取右侧各传感器数据依次为：(25.356°，-10.063°，0.369°，9.053°，-28.969°，1097.002mm)。

将以上数据带入到矩阵中计算，确定θ_R＝0、θ_L＝0时，{O_C}在{O_L}、{O_R}下的位姿变换矩阵：

根据矩阵对应元素关系有：p_xR ^-1+εx＝1633.8，p_yR ^-1-εy＝-129.6。

4)根据集合约束条件判断车体在工作面坐标系中的位姿矩阵

①已知左侧激光在工作面投影点O_L(左侧激光坐标系原点)在工作面坐标系下的坐标为(U_L,V_L)＝(1645,1875)，右侧激光在工作面投影点O_R(右侧激光坐标系原点)在工作面坐标系下的坐标为(U_R,V_R)＝(-1580,1840)。根据矩阵 可知：无论θ_R、θ_L取何值时，车体基坐标系{O_C}的原点O_C到左侧激光的距离均为L_L，到右侧激光的距离均为L_R。

②确定车体基坐标系原点的位置：设激光坐标系原点O_L、O_R在工作面内的坐标分别为(U_L,V_L)、(U_R,V_R)。在工作面上，到左侧激光投影点O_L距离L_L，同时到右侧激光投影点O_R距离L_R的点，至少有一个，至多有两个。有两个点时，取位置较低点(U,V)，为车体基坐标系原点O_C在工作面内的投影。

利用MATLAB软件编程求解，或以激光投影(1645,1875)、(-1580,1840)为圆心，以1609.7、1639.1为半径做圆，两圆相交点(如果为两点则舍去位置高的解)即为解(U,V)＝(49.4,1662.4)(如图7)。

③根据确定的车体基坐标系原点O_C的投影位置，确定人工操作钻臂使激光通过小孔时，造成车体基坐标系{O_C}绕掘进方向(激光指向)旋转的夹角θ_R、θ_L(如图8)。

以右侧为例，在工作面坐标系下，θ_R＝0时，车体基坐标系原点坐标到右侧激光投影点向量应为实际定位存在非零θ_R角时，该向量为(U-U_R,V-V_R)＝(1629.4,-177.6)。左侧同理。则：

④将θ_R＝0、θ_L＝0时，车体基坐标系{O_C}在激光坐标系{O_L}、{O_R}在下的标准位姿矩阵左乘旋转矩阵Rot(z,θ_L)、Rot(z,θ_R)得到新的位姿矩阵 相当于将θ_R＝0、θ_L＝0下的车体基坐标系绕右侧(左侧)激光坐标系Z轴各自旋转θ_R(θ_L)角度，得到存在非零定位转角θ_R、θ_L时车体基坐标系的坐标轴指向和相对激光坐标系的原点位置。

激光坐标系{O_L}、{O_R}和工作面坐标系{O_G}仅存在平移关系，且平移尺度已知。得到车体基坐标系{O_C}在工作面坐标系{O_G}下的两种表达形式(左右两侧得到的矩阵会有误差，为方便区别，增加角标为)，完成车体定位：

根据车体及坐标系在工作面坐标系下新的位姿矩阵可求得车体前后俯仰角、左右翻转角为(-0.76°，-0.43°)。由{O_C}原点的投影点坐标(U,V)＝(49.4,1662.4)，可得车体偏离中线距离E＝U＝49.4mm，并根据车身高度计算出车身下方石碴厚度H＝V-h＝1662.4-1256＝406.4mm,这与实验台车下方摆放的420mm木方高度较为一致。

再使用全站仪测量车体位姿，将测量结果与双臂定位计算结果进行对比验证。

在车体四周合适位置设置四控制点(如图9)，DG⊥DE且DG⊥GF。DE距巷道中线1645mm，与左侧激光指向重合，GF距巷道中线1580mm，与右侧激光指向重合。

在F点架设全站仪，设置FG方向为0角度方向。在车体左右两侧各找一点W_L、W_R，测量得到两点坐标为(1.481，2.157，0.969)、(1.490，1.093，0.978)，计算得到车体偏离模拟巷道中线位置E＝(2157+1093)/2-1580＝45mm，车体左右翻转角-0.48°。在车体右侧竖直面板上找前后两点W₁、W₂，测量得到两点坐标为(2.710，1.071，0.873)、(1.587，1.061，0.884)，计算得到前后俯仰角-0.56°。与双臂车体定位方法计算结果基本一致。

5)工作面不平整度问题

双臂车体定位后，车体到工作面的变换矩阵中隐含着车体到工作面的距离。注意到求出的车体基坐标系原点在两个矩阵中具有相同的X坐标 和相同的Y坐标但Z坐标不同说明上个循环工作结束后，工作面左侧平均掘进深度比右侧多出105.9mm。车体定位时，左臂伸出量相对较大。钻孔时，因双钻臂凿岩台车分区域作业，左侧钻孔深度应相对较小；右侧钻孔深度应相对较大。

以上所述仅是本发明针对特定机构双臂钻孔凿岩台车所提出的定位方式，应当指出，对于本技术领域的其他双臂台车来说，即使尺寸、机构发生一定变化，但仍以双侧机构通过集合约束条件进行车体定位分析的，这些变化也应该视为本发明的保护范围。

本文虽然已经给出了本发明的几个实施例，但是本领域的技术人员应当理解，在不脱离本发明精神的情况下，可以对本文的实施例进行改变。上述实施例只是示例性的，不应以本文的实施例作为本发明权利范围的限定。

Claims (4)

1.一种钻孔自动定位凿岩台车的双钻臂车体定位方法，其特征在于，该方法建立双钻臂各自的坐标系变换矩阵，根据所述坐标系变换矩阵确定车体基坐标系原点的位置，计算出双钻臂末端各自的随机角变量θ，得到车体位姿矩阵；从所述车体位姿矩阵中提取出车体实际停车高度、偏中线距离、俯仰角、翻转角及到工作面距离；

具体包括如下步骤：

步骤一、将凿岩台车的钻臂刚体简化为连杆，将转动轴、移动轴简化为关节，作钻臂简化模型图，根据凿岩台车的尺寸确定各关节的基准位置、相对角度及各关节的运动变化范围；

步骤二、人工手动操作车体钻臂进行定位：分别在两侧钻臂的推进梁安装前、后两个同样尺寸的激光靶，在激光靶相同位置开有小孔，保证前后两个小孔确定的轴线与钻钎平行，且具有固定可测的位置距离关系；在隧道左右两侧设置平行激光；车体停稳于工作面后，人工操作车体两侧钻臂，使激光先后穿过对应侧两个激光靶的小孔，并保持与钻钎平行；

步骤三、建立激光坐标系与车体基坐标系的关系；

步骤四、根据集合约束条件确定车体在工作面坐标系中的位姿矩阵。

2.如权利要求1所述的钻孔自动定位凿岩台车的双钻臂车体定位方法，其特征在于，步骤三中，建立激光坐标系与车体基坐标系的关系的具体步骤为：

步骤3.1)根据D-H法在钻臂各关节建立坐标系，确定各坐标系的相对变换关系；建立左侧钻臂末端坐标系{O_6L}与左侧钻臂基坐标系{O_0L}之间的变换矩阵右侧钻臂末端坐标系{O_6R}与右侧钻臂基坐标系{O_0R}之间的变换矩阵 因钻臂尺寸固定，且各关节转角、位移可通过传感器检测得到，故矩阵 在任意时刻和状态下都是可测的，唯一确定的；

步骤3.2)将矩阵扩展为钻臂末端坐标系{O_6L}、{O_6R}与车体基坐标系{O_C}的变换矩阵

步骤3.3)当随机不可控转角变量θ_R＝0、θ_L＝0时，激光坐标系与钻臂末端坐标系仅存在平移关系；需要将矩阵扩充为激光坐标系{O_L}、{O_R}和车体基坐标系{O_C}的变换矩阵首先对矩阵求逆得到 再根据平移距离改写为其中θ_R、θ_L分别为右侧钻臂末端、左侧钻臂末端的随机角变量。

3.如权利要求2所述的钻孔自动定位凿岩台车的双钻臂车体定位方法，其特征在于，步骤四中，根据集合约束条件判断车体在工作面坐标系中的位姿矩阵的具体步骤为：

步骤4.1)已知左侧激光在工作面投影点O_L在工作面坐标系下的坐标为(U_L,V_L)，右侧激光在工作面投影点O_R在工作面坐标系下的坐标为(U_R,V_R)；根据矩阵可知：无论θ_R、θ_L取何值时，车体基坐标系{O_C}的原点O_C到左侧激光的距离均为L_L，到右侧激光的距离均为L_R；

步骤4.2)确定车体基坐标系原点的位置：在工作面上，至少有一个、至多有两个点到左侧激光投影点O_L的距离为L_L，同时到右侧激光投影点O_R的距离为L_R；当有两个点时，取位置较低点(U,V)为车体基坐标系原点O_C在工作面内的投影；

步骤4.3)根据确定的车体基坐标系原点O_C在工作面的投影位置，确定人工操作钻臂使激光通过小孔时，造成车体基坐标系{O_C}绕掘进方向旋转的夹角θ_R、θ_L；所述绕掘进方向为激光指向方向；

步骤4.4)根据夹角θ_R、θ_L求得车体基坐标系{O_C}在工作面坐标系{O_G}下的坐标变换矩阵完成车体定位；从而由真实的位姿矩阵确定车体在断面内的停车高度、偏中线距离、俯仰角、翻转角、到工作面距离。

4.如权利要求3所述的钻孔自动定位凿岩台车的双钻臂车体定位方法，其特征在于，对井下实际工作面不平整度的处理方法为：双臂车体定位后，车体到工作面的变换矩阵中隐含着车体到工作面的距离；因工作面不平整，左右两侧计算得到的距离不一致；钻孔时，因双钻臂凿岩台车分区域作业，距离较远的一侧，钻臂伸长量也会较大，钻孔深度应相对较小；距离较近的一侧，钻臂伸长量也会较小，钻孔深度较大。