一种实时获取凿岩台车双三角钻臂空间位置姿态的方法技术

本发明专利技术公开了一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，该方法通过建立双三角钻臂的三维模型，继而建立虚拟样机，对双三角钻臂进行运动学仿真。根据CFDH方法建立双三角钻臂的运动学方程，利用虚拟样机的仿真结果与运动学方程进行比较，以验证运动学方程的正确性，从而获取钻臂双三角钻臂的任意时刻的准确位置姿态。本发明专利技术为双三角钻臂的准确钻孔定位提供了可靠的保证。

A real-time method to obtain the spatial position and attitude of the double triangle drilling arm of the drilling jumbo

【技术实现步骤摘要】
一种实时获取凿岩台车双三角钻臂空间位置姿态的方法
本专利技术涉及电脑凿岩台车钻臂的定位技术，具体涉及一种实时获取凿岩台车双三角钻臂空间位置姿态的方法。
技术介绍
隧道及矿山开采过程中，普遍使用全液压凿岩台车钻孔，其钻孔位置的准确定位是整个凿岩钻孔施工的关键。通过机手在操作台操纵钻臂的运动，实现孔眼的定位。但凿岩钻机工作环境复杂，隧道内灰尘，水汽较多，且机手离断面较远，用这种方法孔眼定位不准确，效率低，偏差较大，从而严重影响了爆破效果。为了解决人工定位的不足，电脑凿岩台车以其能够实时获取钻臂三位空间坐标及姿态，实现精准的辅助定位、对孔，逐渐取代传统的全液压凿岩台车，迅速成为隧道施工的主流机械设备。采用双三角钻臂的结构形式，其优点是在空间中平行移动，动作迅速，结构紧凑，平稳性比较好，但是不容易控制，而定位精度是影响钻臂作业的重要因素，因此，如何使钻臂的运动轨迹实现精确控制，对于钻臂末端-纤杆能否精确定位钻孔将产生非常直接的影响。
技术实现思路
本专利技术的目的在于提供一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，它能够通过对双三角钻臂空间位置姿态进行准确获取，以辅助钎杆准确对准钻孔位置。为实现上述目的，本专利技术的一种实时获取前后双三角钻臂空间位置姿态的方法，该方法通过建立双三角钻臂的三维模型，继而建立虚拟样机，对双三角钻臂进行运动学仿真；根据CDFH方法建立双三角钻臂的运动学方程，利用虚拟样机的仿真结果与运动学方程进行比较，以验证运动学方程的正确性，从而获取钻臂双三角钻臂的任意时刻的准确位置姿态；包括如下步骤：步骤1，获得到验证点pi相对于车架坐标系{0}的三维空间测量坐标0pi_C；步骤2，基于改进CFDH法，建立双三角钻臂i＝1坐标系；步骤3，建立相邻关节坐标系的齐次坐标变换矩阵；步骤4，获取验证点pi的三维空间计算坐标。进一步地，在步骤1中，获取验证点pi相对于车架坐标系{0}三维空间测量坐标0pi_C的具体方法为：步骤1.1，建立三维实体模型，导入到软件中建立虚拟样机；步骤1.2，布置角度、长度传感器；双三角钻臂设有11个关节，其中8个为转动关节，3个为移动关节，分别在这11个关节上安装角度传感器和长度传感器；步骤1.3，在虚拟样机中获取点pi空间位置坐标；其步骤为：①在虚拟样机中设定，将传感器θ1，θ2，θ3，θ4，d5，θ6，θ7，θ8，θ9，d10，d11设定为任意一合理范围内的取值。将虚拟样机模型驱动到传感器设定值；②取点p1c，p2c，p3c，p4c，p5c，p6c，p7c，p8c，p9c，p10c，p11c分别为关节连杆1-11上任意一点，在虚拟样机中可以测得以上各点的三维空间测量坐标，得到pic＝{xi_c，yi_c，zi_c}。进一步地，在步骤2中，建立钻臂i＝1坐标系的具体方法为：步骤2.1，找出关节轴i和i+1之间的公垂线或关节轴i和i+1的交点，以关节轴i和i+1的交点作为钻臂坐标系{i}的原点Oi；步骤2.2，规定沿关节轴i的指向Zi；步骤2.3，规定从轴i到i+1公垂线，如果关节轴i和i+1相交，则规定Xi轴垂直于关节轴i和i+1所在的平面,确定Xi；步骤2.4，按照右手定则确定Yi；步骤2.5，重复步骤2.1-步骤2.4，建立坐标系{Xi，Yi，Zi，Oi}。进一步地，在步骤3中，建立相邻关节坐标系的齐次坐标变换矩阵的具体方法为：步骤3.1，定义相邻钻臂关节参数，分别为ai-1,αi-1,di,θi；ai-1：连杆长度，为沿Xi-1轴，从Zi-1移动到Zi的距离；αi-1：连杆转角，为绕Xi-1轴，从Zi-1旋转到Zi的角度；di：连杆偏距，为沿Zi轴，从Xi-1移动到Xi的距离；θi：关节角度，为绕Zi轴，从Xi-1旋转到Xi的角度；步骤3.2，定义相邻关节坐标系的齐次坐标变换矩阵；①将坐标系{i-1}绕Xi-1轴旋转αi-1角，使Zi-1轴与Zi轴平行，得到旋转矩阵RX(αi-1)。②将坐标系{i-1}沿当前Xi-1平移距离ai-1，使Zi-1轴与Zi轴重合，得到平移矩阵DX(αi-1)。将坐标系{i-1}绕当前Zi轴旋转θi角，使Xi-1轴与Xi轴平行，得到平移矩阵DZ(di)。③沿Zi轴平移距离di，使坐标系{i-1}与坐标系{i}完全重合。步骤3.3，建立相邻关节坐标系的齐次坐标变换矩阵；将步骤3.2中的四个矩阵以此相乘可以得到坐标系{i}相对于坐标系{i-1}的齐次变换矩阵：进一步地，在步骤4中，获取验证点pi的三维空间计算坐标的具体方法为：步骤4.1，坐标系{i}相对于坐标系{0}的齐次变换矩阵为步骤4.2，坐标系{i}中的点pi的局部坐标为ipi,则点pi相对于车架坐标系{0}的三维空间坐标为步骤4.3，验算点pi的三维空间计算坐标0pi_J与在虚拟样机中三维空间测量坐标0pi_C是否相等；若相等，则说明关节点i的参数ai-1,αi-1,di,θi选取，变换矩阵等正确,将i置为i+1，重复步骤2，步骤3，步骤4，获取关节点i+1的参数及变换矩阵；若不相等，则说明关节点i的参数ai-1,αi-1,di,θi选取有误，需要在虚拟样机中重新测量以上参数，直到0pi_J＝0pi_C。本专利技术的有益效果是：通过建立虚拟样机，在双三角钻臂上安装11个关节，并通过CFDH方法和齐次坐标，确定了钻臂上各关节的任意点和任意时刻的空间位置姿态，这种准确的位置姿态对纤杆的准确定位给予重要的支撑。附图说明图1是本专利技术的工作流程图；图2是本专利技术的虚拟样机；图3是本专利技术角度、长度传感器布置示意图；图4是本专利技术点pi空间位置的测量坐标示意图；图5是本专利技术基于CFDH法的钻臂空间坐标系示意图；图6是相邻两个关节坐标示意图；图中，1-翼式臂座I，2-翼式臂座II，3-后臂十字铰，4-钻臂伸缩缸筒，5-钻臂伸缩缸杆，6-前臂十字铰，7-旋转缸筒，8-旋转缸杆，9-推进器摆动缸，10-推进器补偿缸，11-钻机推进缸。具体实施方式下面结合附图及具体实施例对本专利技术作进一步详细说明。本专利技术所述的双三角钻臂类似于多关节机器人结构，因此，可以利用机器人研究中常用的理论对钻臂的运动学进行分析，其分析所采用的方法为CFDH(CoordinateFixedDenavit-Hartenberg)法，这种方法不仅提高了运动学分析的准确性和可操作性，而且节约了分析时间，为建立运动学方程提供了有效的手段。齐次矩阵变化因其直观的几何意义，适用于描述多个坐标之间的变换关系。不仅如此，齐次矩阵变化也可以用同一个矩阵表示旋转和位移的变换，这一特点使其在多关节机器人的运动学研究中得到广泛使用。本专利技术首先使用Pro/E软件对双三角钻臂上的零件进行三维建模，将建立好的各零件模型依本文档来自技高网...

【技术保护点】
1.一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，其特征在于：该方法通过建立双三角钻臂的三维模型，继而建立虚拟样机，对双三角钻臂进行运动学仿真；根据CFDH方法建立双三角钻臂的运动学方程，利用虚拟样机的仿真结果与运动学方程进行比较，以验证运动学方程的正确性，从而获取钻臂双三角钻臂的任意时刻的准确位置姿态；其步骤如下：/n步骤1，获取验证点p

【技术特征摘要】
1.一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，其特征在于：该方法通过建立双三角钻臂的三维模型，继而建立虚拟样机，对双三角钻臂进行运动学仿真；根据CFDH方法建立双三角钻臂的运动学方程，利用虚拟样机的仿真结果与运动学方程进行比较，以验证运动学方程的正确性，从而获取钻臂双三角钻臂的任意时刻的准确位置姿态；其步骤如下：
步骤1，获取验证点pi相对于车架坐标系{0}的三维空间测量坐标0pi_C；
步骤2，基于改进CFDH法，建立双三角钻臂i＝1坐标系；
步骤3，建立相邻关节坐标系的齐次坐标变换矩阵；
步骤4，获取验证点pi的三维空间计算坐标。


2.根据权利要求1中所述的一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，其特征在于：在步骤1中，获取验证点pi相对于车架坐标系{0}三维空间测量坐标0pi_C的具体方法为：
步骤1.1，建立双三角钻臂的三维实体模型，导入到软件中建立虚拟样机；
步骤1.2，布置角度、长度传感器：双三角钻臂设有11个关节，其中8个为转动关节，3个为移动关节，分别在这11个关节上安装角度传感器和长度传感器；
步骤1.3，在虚拟样机中获取点pi空间位置坐标；其步骤为：
①在虚拟样机中，将步骤1.2中传感器的角度、位移
θ1,θ2,θ3,θ4,d5,θ6,θ7,θ8,θ9,d10,d11设定为任意一合理范围内的取值，将虚拟样机模型驱动到传感器设定值；
②取点p1c,p2c,p3c,p4c,p5c,p6c,p7c,p8c,p9c,p10c,p11c分别为关节连杆1-11上任意一点，在虚拟样机中可以测得以上各点的三维空间测量坐标，得到pic＝{xi_c,yi_c,zi_c}。


3.根据权利要求1中所述的一种实时获取凿岩台车双三角钻臂空间位置姿态的方法，其特征在于：在步骤2中，建立钻臂i＝1坐标系的具体方法为：
步骤2.1，找出关节轴i和i+1之间的公垂线或关节轴i和i+1的交点，以关节轴i和i+1的交点作为钻臂坐标系{i}的原点Oi；
步骤2.2，规定沿关节轴i的指向Zi；
步骤2.3，规定从轴i到i+1公垂线，如果关节轴i和i+1相交，则规定Xi轴垂直于关节轴i和i+1所在的平面,确定Xi；
步骤2.4，按照右手定则确定Yi；
步骤2.5，重复步骤...

【专利技术属性】
技术研发人员：贾体锋，牛可，赵建东，王伟，周忠尚，牛勇，张小强，程猛，
申请(专利权)人：徐州徐工铁路装备有限公司，
类型：发明
国别省市：江苏;32