CN107885916A - 一种基于cfdh法的凿岩台车钻臂运动学分析方法 - Google Patents

Abstract

本发明涉及一种基于CFDH(Coordinate Fixed Denavit‑Hartenberg)法的凿岩台车钻臂运动学分析方法。该方法首先确定连杆参数，包括连杆的尺寸参数有连杆长度和连杆扭角，相邻连杆的关系参数有连杆距离和关节转角；再将钻臂简化为多关节机器人的结构，通过设置钻臂各杆件坐标系，确定相邻杆件的坐标变换矩阵，进而运用CFDH法建立凿岩台车钻臂运动学方程；最后利用在MATLAB中根据建立的运动学方程绘制出凿岩台车钻臂的有效工作空间。该方法解决了钻臂研究中坐标系确定困难的问题，并且坐标系组的建立直观准确，为凿岩台车钻臂的运动学分析提供了有效的手段。

Description

一种基于CFDH法的凿岩台车钻臂运动学分析方法

技术领域

本发明涉及工程机械运动学分析领域，具体涉及一种基于CFDH法的凿岩台车钻臂运动学分析方法。

背景技术

全液压凿岩台车是集机械、电气及液压于一体的大型凿岩设备，是矿山、隧道及地下工程采用钻爆法施工的一种重要机具，它主要由钻臂、凿岩机、推进器以及底盘等组成。全液压凿岩台车不仅可以极大地减轻工人的体力劳动，改善施工作业条件，提高凿孔作业的效率，而且在实际施工过程中更加易于实现高效化和自动化。我国目前用到的液压凿岩设备大部分依靠国外进口，而且大多都价值不菲。如果自行设计液压凿岩台车，其成本将比直接进口大幅降低，同时后期的维护费用、零件更换等也将得到极大的降低和便利。正因为如此，开展全液压凿岩台车的研制对我国的制造装备现代化具有十分重要的意义。

全液压凿岩台车钻臂就如同凿岩台车的“臂膀”，是凿岩台车最为核心的部件，钻臂是凿岩台车工作的定位机构，凿岩台车只有通过钻臂各关节的运动，才能完成定位、凿岩等动作。因此为了提高隧道施工的效率，稳定性，安全性能，对全液压凿岩台车钻臂的研究势在必行。

发明内容

本申请提供一种基于CFDH法的凿岩台车钻臂运动学分析方法，解决现有D-H分析法不能针对所有关节连杆进行建模、建立钻臂模型与实体不一致，以及无法进行局部关节位置分析等问题。

为达到上述目的，本发明的技术方案是这样实现的：

一种基于CFDH法的凿岩台车钻臂运动学分析方法，该方法通过以下步骤实现：

步骤1：确定连杆参数，包括连杆的尺寸参数：连杆长度和连杆扭角；相邻连杆的关系参数：连杆距离和关节转角；

步骤2：根据CFDH法建立连杆坐标系，确定相邻连杆坐标变换矩阵

步骤3：将凿岩台车钻臂简化为多关节机器人的结构，通过设置杆件坐标系，确定相邻杆件的坐标变换矩阵。将各杆件的坐标变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵即凿岩台车钻臂运动学方程；

步骤4：将凿岩台车钻臂的运动学方程输入到MATLAB中，绘制出凿岩台车钻臂的有效工作空间。

进一步的，所述步骤1中确定连杆参数，其具体步骤为：

步骤1.1：确定连杆的尺寸参数

连杆长度a_i：两个关节轴线i和i-1沿公垂线的距离；

连杆扭角α_i：两个关节轴线i和i-1的夹角；

步骤1.2：确定相邻连杆的关系参数

连杆距离d_i：沿关节轴线i方向，两个公垂线之间的距离；

关节转角θ_i：垂直于关节轴线的平面内，两个公垂线之间的夹角。

进一步的，所述步骤2中建立连杆坐标系，确定相邻连杆坐标变换矩阵，其具体步骤为：

步骤2.1：规定坐标轴X_i；坐标轴Z_i；坐标轴Y_i；坐标原点O_i；

步骤2.2：根据CFDH法建立连杆坐标系。相邻连杆坐标变换矩阵是由坐标系{i-1}转换到坐标系{i}的四个变换得到的，这四个变换分别是：绕Z_i-1轴转θ_i；绕Z_i-1轴移动d_i；沿X_i轴移动a_i；沿X_i轴转α_i。

进一步的，所述步骤3中建立凿岩台车钻臂运动学方程，其具体步骤为：

步骤3.1：将凿岩台车钻臂简化为多关节机器人的结构，建立钻臂坐标系组，此坐标系组中将基坐标系{0}设在钻臂底板，端部坐标系{7}则设在钎杆的顶端；步骤3.2：根据实际测量数据写出各相邻连杆的变换矩阵： 将变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵即凿岩台车钻臂运动学方程

步骤3.3：取各关节初始值代入运动学方程，验证运动学方程是否正确。

进一步的，所述的步骤4绘制凿岩台车钻臂的有效工作空间具体为：

步骤4.1：利用MATLAB软件编程，使各关节变量随机选取取值范围内的值，将其代入运动学方程中，由此得到钻臂末端点的三维坐标值；

步骤4.2：利用plot函数将三维坐标点输出成图形，便得到了工作空间图形化的三维结果。

与现有技术相比，本发明的有益效果如下：

本发明应用于凿岩台车钻臂运动学分析中，使用了一种坐标系固定在实体上的D-H表示法―CFDH法，解决了钻臂研究中坐标系确定困难的问题，并且坐标系组的建立更直观准确。本发明不仅提高了运动学分析的精确性和可操作性，而且节约了分析时间，为优化改进凿岩台车提供了有效的分析手段。

附图说明

图1本发明分析方法的流程图；

图2为钻臂连杆坐标系示意图；

图3为某型号全液压凿岩台车实拍图；

图4为凿岩台车钻臂坐标系组示意图；

图5为凿岩台车钻臂工作空间三维示意图；

图6为工作空间在XY面上的投影示意图；

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本申请所说“连接”、“联接”，如无特别说明，均包括直接和间接连接(联接)。

本发明提供一种基于CFDH法的凿岩台车钻臂运动学分析方法，如图1所示，该方法通过以下步骤实现：

步骤1：确定连杆参数，包括连杆的尺寸参数：连杆长度和连杆扭角；相邻连杆的关系参数：连杆距离和关节转角；

步骤2：根据CFDH法建立连杆坐标系，相邻连杆坐标变换矩阵由坐标系{i-1}转换到坐标系{i}的四个变换得到；

步骤3：将凿岩台车钻臂简化为多关节机器人的结构，通过设置钻臂各杆件坐标系，确定相邻杆件的坐标变换矩阵。将各杆件的坐标变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵即凿岩台车钻臂运动学方程；

步骤4：将凿岩台车钻臂的运动学方程输入到MATLAB中，绘制出凿岩台车钻臂的有效工作空间。

下面以某型号凿岩台车为例，说明本发明一种基于CFDH法的凿岩台车钻臂运动学分析方法的具体实施步骤：

步骤1：确定连杆参数，包括连杆的尺寸参数：连杆长度和连杆扭角；相邻连杆的关系参数：连杆距离和关节转角；

可以将钻臂看作多个连杆通过关节连接的机械臂。CFDH法中，连杆参数的规定如下：

(1)连杆的尺寸参数

连杆长度a_i：两个关节轴线i和i-1沿公垂线的距离；

连杆扭角α_i：两个关节轴线i和i-1的夹角；

(2)相邻连杆的关系参数

连杆距离d_i：沿关节轴线i方向，两个公垂线之间的距离；

关节转角θ_i：垂直于关节轴线的平面内，两个公垂线之间的夹角。

步骤2：建立连杆坐标系及相邻连杆坐标变换矩阵；

(1)规定坐标轴及坐标原点

坐标轴X_i：沿连杆i-1两关节轴线的公垂线指向i关节；

坐标轴Z_i：与i关节的轴线重合；

坐标轴Y_i：按右手直角坐标系法则确定。

坐标原点O_i：当关节轴线i-1和关节轴线i相交时，取两轴线的交点；当关节轴线i-1和关节轴线i异面时，取两轴线的公垂线与关节轴线i的交点；当关节轴线i-1和关节汗轴线i平行时，取关节轴线i与关节轴线i-1的公垂线与关节轴线i的交点。

连杆坐标系的建立及参数的规定如图2所示。

(2)根据CFDH法建立连杆坐标系，相邻连杆坐标变换矩阵由坐标系{i-1}转换到坐标系{i}的四个变换得到：

绕Z_i-1轴转θ_i：Rot(Z_i-1,θ_i)

沿Z_i-1轴移动d_i：Trans(Z_i-1,d_i)

沿X_i轴移动a_i：Trans(X_i,a_i)

绕X_i轴转α_i：Rot(X_i,α_i)

转动连杆的CFDH参数为a_i、α_i、d_i、θ_i，其中关节转角θ_i是关节变量，连杆长度a_i、连杆扭角α_i、连杆距离d_i是固定不变的。这四个参数确定了连杆i相对于连杆i-1的位姿，即坐标变换矩阵为如式1所示。

步骤3：建立凿岩台车钻臂运动学方程

(1)图3是某型号凿岩台车实拍图，将凿岩台车钻臂简化为多关节机器人的结构，建立钻臂坐标系组，此坐标系组中将基坐标系{0}设在钻臂底板，端部坐标系{7}则设在钎杆的顶端。

(2)图4是某型号凿岩台车钻臂坐标系组，根据实际测量数据写出各相邻连杆的变换矩阵：将变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵及凿岩台车钻臂运动学方程

根据某公司提供的实际测量数据可知:

a₁＝270mm；a₂＝3100mm；a₄＝1240mm；a₆＝460mm；

d₃＝220mm；d₄＝1280mm；d₅＝950mm；d₆＝545mm。

结合下表1和公式1，可得钻臂各连杆之间的变换矩阵。

注：si＝sinθ_i,ci＝cosθ_i(i＝1,2,3,4,5,6)

表1

(3)将上面的矩阵依次相乘，得到钻臂末端对基座的关系矩阵及凿岩台车钻臂运动学方程。

要具体描述一个坐标系相对于另一个坐标系的关系，必须给出坐标系的原点位置和它的坐标轴的方向。式(9)中，p为钻臂末端的位置；n为X轴主矢量方向余弦；o为Y轴主矢量方向余弦；a为Z轴主矢量方向余弦。根据CFDH法的内容分别用n_x,n_y,n_z,o_x,o_y,o_z,a_x,a_y,a_z来表示机械臂末端执行器的位姿，用p_x,p_y,p_z来表示机械臂末端执行器的位置。

(4)根据某公司提供的实际测量数据，在钻臂初始状态时

p_x＝6300mm,p_y＝-750mm,p_z＝5200mm。

为验证结果是否正确，取各关节初始值θ₀＝θ₁＝θ₂＝θ₅＝θ₆＝0°；θ₃＝θ₄＝90°；d₇＝3000mm；l₁＝0mm。由CFDH法计算可得：p_x＝6330mm,p_y＝-739mm,p_z＝5230mm。

根据规定，实际测量和计算结果的误差在5％以内，说明运动学方程建立正确。由下表2可知，实际测量和计算结果的误差远小于规定的误差范围，可见钻臂的运动学方程建立正确。

表2

步骤4：绘制凿岩台车钻臂的有效工作空间

(1)利用MATLAB软件编程，使各关节变量随机选取取值范围内的值，将其代入运动学方程，由此得到钻臂末端点的三维坐标值；

(2)利用plot函数将三维坐标点输出成图形，便得到了工作空间图形化的三维结果。利用for循环随机选取这样15000个点，就可以形成直观性很强的钻臂工作空间图。随机关节变量由下式求得：

θ_i＝θ_i ^min+(θ_i ^max-θ_i ^min)×rand(1) (10)

上式中，θ_i ^min为关节i转动范围的最小值；θ_i ^max为关节i转动范围的最大值；rand(1)指随机选取0～1之间的数。

(3)钎杆末端运动所达空间位置集合即钻臂工作空间,图5为钻臂工作空间三维图。在凿岩过程中，最为关注的掌子面上钻孔的区域，即钻臂工作空间在XY面上的投影，由图6可以看出，XY面的投影点的分布成一扇形，数据分析可以看出是一个扇形半径为0.9×104mm的类似扇形，满足凿岩台车施工使用要求。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (5)

1.一种基于CFDH法的凿岩台车钻臂运动学分析方法，其特征在于，该方法通过以下步骤实现：

步骤1：确定连杆参数，所述连杆参数包括连杆的尺寸参数和相邻连杆的关系参数；连杆的尺寸参数包括连杆长度和连杆扭角；相邻连杆的关系参数包括连杆距离和关节转角；

步骤2：根据CFDH法建立连杆坐标系，确定相邻连杆坐标变换矩阵

步骤3：将凿岩台车钻臂简化为多关节机器人的结构，通过设置杆件坐标系，确定相邻杆件的坐标变换矩阵；将各杆件的坐标变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵即凿岩台车钻臂运动学方程；

步骤4：将凿岩台车钻臂的运动学方程输入到MATLAB中，绘制出凿岩台车钻臂的有效工作空间。

2.根据权利要求1所述的分析方法，其特征在于，所述步骤1中确定连杆参数，其具体步骤为：

步骤1.1：确定连杆的尺寸参数

连杆长度a_i：两个关节轴线i和i-1沿公垂线的距离；

连杆扭角α_i：两个关节轴线i和i-1的夹角；

步骤1.2：确定相邻连杆的关系参数

连杆距离d_i：沿关节轴线i方向，两个公垂线之间的距离；

关节转角θ_i：垂直于关节轴线的平面内，两个公垂线之间的夹角。

3.根据权利要求1或2所述的分析方法，其特征在于，所述步骤2中建立连杆坐标系，确定相邻连杆坐标变换矩阵，其具体步骤为：

步骤2.1：规定坐标轴X_i；坐标轴Z_i；坐标轴Y_i；坐标原点O_i；

步骤2.2：根据CFDH法建立连杆坐标系。相邻连杆坐标变换矩阵是由坐标系{i-1}转换到坐标系{i}的四个变换得到的，这四个变换分别是：绕Z_i-1轴转θ_i；绕Z_i-1轴移动d_i；沿X_i轴移动a_i；沿X_i轴转α_i。

4.根据权利要求3所述的分析方法，其特征在于，所述步骤3中建立凿岩台车钻臂运动学方程，其具体步骤为：

步骤3.1：将凿岩台车钻臂简化为多关节机器人的结构，建立钻臂坐标系组，此坐标系组中将基坐标系{0}设在钻臂底板，端部坐标系{7}则设在钎杆的顶端。

步骤3.2：根据实际测量数据写出各相邻连杆的变换矩阵： 将变换矩阵依次相乘，得到钻臂末端对基座的关系矩阵即凿岩台车钻臂运动学方程

步骤3.3：取各关节初始值代入运动学方程，验证运动学方程是否正确。

5.根据权利要求4所述的分析方法，其特征在于，所述的步骤4绘制凿岩台车钻臂的有效工作空间具体为：

步骤4.1：利用MATLAB软件编程，使各关节变量随机选取取值范围内的值，将其代入运动学方程中，由此得到钻臂末端点的三维坐标值；

步骤4.2：利用plot函数将三维坐标点输出成图形，便得到了工作空间图形化的三维结果。