CN116408800B - 一种基于孔位坐标的锚杆台车自动定位方法 - Google Patents

Abstract

本发明属于串联机械臂运动学算法领域，其公开了一种基于孔位坐标的锚杆台车自动定位方法，包括以下步骤：(1)建立总运动学变换矩阵；(2)计算两个末端位姿的对应的变换矩阵；(3)基于施作面坐标系和孔位坐标得到两个末端位姿在施作面坐标系和基座坐标系下的描述；(4)求模糊量，通过向量叉乘确定施作面坐标系相对基座坐标系的旋转矩阵；(5)求直线L₂与平面A的交点即为偏移矩阵，最终解得变换矩阵采用本发明提供的方法输入孔位坐标可实现基于施作面孔位坐标的锚杆台车高精度自动定位，提高了对孔精度、锚杆安装速度和整机自动化水平，适用于串联机械臂运动学算法领域。

Description

一种基于孔位坐标的锚杆台车自动定位方法

技术领域

本发明属于串联机械臂运动学算法领域，更具体地，涉及一种基于孔位坐标的锚杆台车自动定位方法。

背景技术

随着我国隧道工程数量与质量的飞速提升和隧道工程锚杆支护作业的普及，如今的隧道工程对掘进效率和作业环境，提出了更高的需求，以前进行锚杆作业时是分几道工序单独进行的，首先在洞室的顶拱或边壁要装锚杆的地方钻孔，这要用凿岩台车；钻孔结束后，要向孔中加注水泥砂浆，这要用注浆机；注浆结束后，再向孔中装入锚杆，这要用顶推设备。由于设备多，互相干扰，再加上重复定位，所以安装锚杆的质量和速度都很低，而且也很不安全。为了改变这种状况，科研和施工人员就研制出了一种能将钻孔、注浆和装锚杆这三道工序，在一台设备上依次完成的全机械化设备：锚杆台车。之后，国内各大隧道设备公司相继推出了多款全机械化锚杆台车，锚杆台车在爆破法钻孔与钻锚杆孔等作业中发挥着难以替代的作用。

锚杆台车的机械臂上装备凿岩机、注浆系统和换杆机构，可一次性完成钻孔、洗孔和推杆作业。但是，目前我国的锚杆台车的自动化水平有待提升，存在着锚杆台车在隧道中定位困难，对孔精度较低，安装锚杆速度较慢，工作效率低的问题。

发明内容

针对现有技术所存在的上述缺点，本发明的目的在于提供一种基于孔位坐标的锚杆台车自动定位方法，用于提升整车自动化水平，实时、准确的获得施工平面与锚杆台车之间的位置关系，实现了基于施作面孔位坐标的锚杆台车高精度定位，同时也提高了对孔精度和锚杆安装速度，提高了工作效率。

为实现上述目的，按照本发明的一个方面，提供了一种基于孔位坐标的锚杆台车自动定位方法，所述方法主要包括以下步骤：

步骤一，基于所述锚杆台车机械臂的结构特征，采用DH法，在基座、一号关节、二号关节、三号关节、四号关节、五号关节、六号关节、七号关节、T号关节处依次设置坐标系{0}～坐标系{T}，共T+1个坐标系，建立所述锚杆台车机械臂的总运动学变换矩阵：

步骤二，基于所述总运动学变换矩阵：根据所述锚杆台车机械臂末端执行器对钻孔点1和钻孔点2完成对孔作业时的两个末端位姿的关节变量，计算所述两个末端位姿从所述坐标系{T}变换到基座坐标系{0}的两个齐次变换矩阵：/>和/>

步骤三，基于所述施作面建立坐标系{S}，根据所述钻孔点1和钻孔点2的孔位坐标，计算所述两个末端位姿的Z_T轴对应的两个单位矢量在施作面坐标系{S}下的描述^Sp₁和^Sp₂，再将所述施作面坐标系{S}下的两个单位矢量^Sp₁和^Sp₂变换到基座坐标系{0}；

步骤四，基于步骤三求解所述施作面坐标系{S}到基座坐标系{0}的变换矩阵实际工程中，所述两个单位矢量不能保证相交，所以先对所述变换矩阵/>中的旋转矩阵求模糊量，再使用向量叉乘求解所述旋转矩阵。所述两个单位矢量^sp₁和^sp₂在基座坐标系{0}下的描述为⁰p₁和⁰p₂，对应的两条直线为L₁和L₂；

步骤五，基于所述两条直线L₁和L₂和变换矩阵中的旋转矩阵求解偏移矩阵。确定所述两条直线L₁和L₂的公垂线方向，根据所述直线L₁与公垂线确定平面A，求所述平面A与直线L₂的交点坐标，即为偏移矩阵，最终解得所述变换矩阵/>

进一步地，所述锚杆台车机械臂的总运动学变换矩阵为：

其中，为坐标系{1}到基础坐标系{0}的变换矩阵，/>为坐标系{2}到坐标系{1}的变换矩阵，/>为坐标系{3}到坐标系{2}的变换矩阵，/>为坐标系{4}到坐标系{3}的变换矩阵，/>为坐标系{5}到坐标系{4}的变换矩阵，/>为坐标系{6}到坐标系{5}的变换矩阵，/>为坐标系{7}到坐标系{6}的变换矩阵，/>为坐标系{T}到坐标系{7}的变换矩阵。

进一步地，基于所述总运动学变换矩阵计算所述两个齐次变换矩阵：/>和

其中，

进一步地，所述施作面坐标系{S}在从锚杆台车驾驶室向前看时，X轴指向右，Y轴指向前，Z轴指向上。

进一步地，基于所述钻孔点1和钻孔点2的孔位坐标计算两个位姿的Z_T轴与施作面坐标系{S}的Z轴的夹角和/>

其中，

进一步地，基于所述夹角和/>算得所述施作面坐标系{S}下的单位矢量^Sp₁和^Sp₂；

其中，

进一步的，根据所述和/>计算所述/>中的n_Sx、n_Sy、n_Sz和模糊量a_Sx'、a_Sy'、a_Sz'，通过向量叉乘，计算o_Sx、o_Sy、o_Sz，再次使用向量叉乘来进行误差补偿，重新求a_Sx、a_Sy、a_Sz；

其中，⁰p₁＝[a_x1 a_y1 a_z1 0]^T；

⁰p₂＝[a_x2 a_y2 a_z2 0]^T；

进一步的，所述公垂线与所述直线L₁确定的平面A的方程为：

Nx+Oy+Pz+Q＝0；

其中，N＝a_x1a_y1a_y2-a_y1 ²a_x2-a_z1 ²a_x2+a_x1a_z1a_z2；

O＝a_x1a_y1a_x2-a_x1 ²a_y2-a_z1 ²a_y2+a_y1a_z1a_z2；

P＝a_x1a_z1a_x2-a_x1 ²a_z2-a_y1 ²a_z2+a_y1a_z1a_y2；

Q＝-Np_x1-Op_y1-Pp_z1。

进一步的，所述平面A与L₂交点坐标为(p_Sx，p_Sy，p_Sz)，交点坐标为所述施作面坐标系{S}的原点在基座坐标系{0}下的位置坐标；

其中，p_Sx＝a_x2t+p_x2，p_Sy＝a_y2t+p_y2，p_Sz＝a_z2t+p_z2；

其中，

与现有技术相比，本发明提供的技术方案具有如下有益效果：

1.本发明通过对旋转矩阵求模糊量进行计算误差补偿，建立施作面坐标系{S}到基座坐标系{0}的变换矩阵能够方便快捷的实现锚杆台车在隧道中的准确定位，同时确定了锚杆台车与施作面孔位坐标之间的坐标关系，提高了位姿变换的准确性和对孔精度；

2.在锚杆台车作业前，孔位坐标即为已知参数，将所有孔位坐标输入系统，即可获得对应孔位在基座坐标系{0}下的描述，实现了锚杆台车在隧道中高精度自动定位及良好的对孔精度，安装锚杆速度快，提高了锚杆台车的自动化程度和作业效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。下面描述中的附图仅是本发明的一些实施例，本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：

图1为本发明基于孔位坐标的锚杆台车自动定位方法的算法流程图；

图2为本发明一实施例提供的锚杆台车机械臂坐标系示意图；

具体实施方式

为了使本发明的目的、技术方案及优点更清楚明白，后文结合附图和实施例，对本发明进行进一步纤细说明。应当理解此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明，此外下面描述的本发明实施例中所涉及到的技术特征只要彼此之间为构成冲突就可以相互结合。

如图1所示，为本发明的算法流程图；如图2所示，本发明一实施例提供的锚杆台车机械臂坐标系示意图。

一种基于孔位坐标的锚杆台车自动定位方法，所表述的方法至少包括如下步骤：

步骤一，基于锚杆台车机械臂的结构特征，采用DH法，在基座、一号关节、二号关节、三号关节、四号关节、五号关节、六号关节、七号关节、T号关节处依次设置坐标系{0}～坐标系{T}，共T+1个坐标系，建立锚杆台车机械臂的总运动学变换矩阵：

其中，锚杆台车机械臂的总运动学变换矩阵为：

其中，为坐标系{1}到基础坐标系{0}的变换矩阵，/>为坐标系{2}到坐标系{1}的变换矩阵，/>为坐标系{3}到坐标系{2}的变换矩阵，/>为坐标系{4}到坐标系{3}的变换矩阵，/>为坐标系{5}到坐标系{4}的变换矩阵，/>为坐标系{6}到坐标系{5}的变换矩阵，为坐标系{7}到坐标系{6}的变换矩阵，/>为坐标系{T}到坐标系{7}的变换矩阵；

步骤二，基于总运动学变换矩阵：根据锚杆台车机械臂末端执行器对钻孔点1和钻孔点2完成对孔作业时的两个末端位姿的关节变量，计算两个末端位姿从坐标系{T}到基座坐标系{0}的两个齐次变换矩阵：/>和/>

其中，

步骤三，基于施作面建立坐标系{S}，根据钻孔点1和钻孔点2的孔位坐标，计算两个末端位姿的Z_T轴对应的两个单位矢量在施作面坐标系{S}下的描述^Sp₁和^Sp₂，再将施作面坐标系{S}下的两个单位矢量^Sp₁和^Sp₂变换到基座坐标系{0}；

施作面坐标系{S}在从锚杆台车驾驶室向前看时，X轴指向右，Y轴指向前，Z轴指向上。根据钻孔点1和钻孔点2的孔位坐标计算所述两个位姿的Z_T轴与所述施作面坐标系{S}的Z轴的夹角和/>根据夹角/>和/>计算施作面坐标系{S}下的单位矢量^Sp₁和^Sp₂；

其中，

步骤四，基于步骤三求解施作面坐标系{S}到基座坐标系{0}的变换矩阵实际工程中，两个单位矢量不能保证相交，所以先对变换矩阵/>中的旋转矩阵求模糊量，再使用向量叉乘求解旋转矩阵。两个单位矢量^Sp₁和^Sp₂在基座坐标系{0}下的描述为⁰p₁和⁰p₂，对应的两条直线为L₁和L₂；

其中，⁰p₁＝[a_x1 a_y1 a_z1 0]^T；

⁰p₂＝[a_x2 a_y2 a_z2 0]T；

步骤五，基于两条直线L₁和L₂和变换矩阵中的旋转矩阵求解偏移矩阵。确定两条直线L₁和L₂的公垂线方向，根据直线L₁与公垂线确定平面A，求平面A与直线L₂的交点坐标，即为偏移矩阵，最终解得变换矩阵/>

申请号为申请号202110171742.0，名称为锚杆台车控制系统的中国发明专利，公开了一种锚杆台车控制系统，包括不设置在施工现场的远程控制端和设置在锚杆台车上的网络通讯模块、数据采集模块、控制器；所述远程控制端通过所述网络通讯模块向所述锚杆台车下发控制指令，以使用户远程实时操控所述锚杆台车；所述数据采集模块用于将采集的施工现场环境数据、目标物位置数据、所述锚杆台车的车身姿态数据和路径信息反馈至所述远程控制端和所述控制器；所述控制器用于根据所述锚杆台车的当前位置数据、作业面参数信息和目标位置数据控制臂架各关节从当前位置运动至目标位置；所述控制器进一步用于：根据所述锚杆台车的当前位置和所述作业面的轮廓数据计算所述锚杆台车和所述作业面的相对位置信息；根据所述臂架的车速姿态数据计算所述臂架与所述锚杆台车的相对位置信息；根据所述锚杆台车和所述作业面的相对位置信息、所述臂架与所述锚杆台车的相对位置信息计算得到所述臂架与所述作业面的相对位置信息；基于所述臂架与所述作业面的相对位置信息，根据所述臂架的各关节与所述目标位置之间的距离，对各关节动作进行分区域pid控制，以通过不同区域应用不同的pid控制各关节的阀门，实现所述臂架各关节从当前位置运动到所述目标位置。

通过本发明提供的基于孔位坐标的锚杆台车自动定位方法，能够确定锚杆台车、机械臂的姿态数据、施作面和孔位之间准确的相对位置信息，配合上述发明中的控制系统，为其数据采集模块提供所需的位置及姿态信息，能够通过远程控制，实现锚杆施工作业的自动化操作，保证作业质量的同时，提高工作效率。

根据和/>计算/>中的n_Sx、n_Sy、n_Sz和模糊量P_Sx'、P_Sy'、P_Sz'，通过向量叉乘，计算o_Sx、o_Sy、o_Sz，再次使用向量叉乘来进行误差补偿，重新求a_Sx、a_Sy、a_Sz，算得旋转矩阵。计算平面A后与L2交点坐标(p_Sx，p_Sy，P_Sz)；

其中，

公垂线与直线L₁确定的平面A为：

Nx+Oy+Pz+Q＝0；

其中，N＝a_x1a_y1a_y2-a_y1 ²a_x2-a_z1 ²a_x2+a_x1a_z1a_z2；

O＝a_x1a_y1a_x2-a_z1 ²a_y2-a_z1 ²a_y2+a_y1a_z1a_z2；

P＝a_x1a_z1a_x2-a_x1 ²a_z2-a_y1 ²a_z2+a_y1a_z1a_y2；

Q＝-Np_x1-Op_y1-Pp_z1；

平面A与所述L2交点坐标为(p_Sx，p_Sy，p_Sz)，交点坐标为施作面坐标系{S}的原点在基座坐标系{0}下的位置坐标；

其中，p_Sx＝a_x2t+p_x2，p_Sy＝a_y2t+p_y2，p_Sz＝a_z2t+p_z2；

其中，

传统技术中锚杆台车作业施工时，受锚杆台车定位精度的影响，其机械臂相对于施作面相对位置不能做到有效定位，且相对于孔位定位精度不能做到精确定位，锚杆台车的机械臂找作业面时经常出现臂架顶作业面不稳和顶作业面过猛的问题，而顶作业面不稳会导致锚杆台车钻孔时钻杆偏移，极易损坏钻杆，而顶作业面过猛会导致臂架损伤、台车滑移乃至倾斜等重大事故发生，本发明通过对旋转矩阵求模糊量进行计算误差补偿，建立施作面坐标系{S}到基座坐标系{0}的变换矩阵能够方便快捷的实现锚杆台车在隧道中的准确定位，同时确定了锚杆台车与施作面孔位坐标之间的坐标关系，提高了位姿变换的准确性和对孔精度；在锚杆台车作业前，孔位坐标即为已知参数，将所有孔位坐标输入系统，即可获得对应孔位在基座坐标系{0}下的描述，实现了锚杆台车在隧道中高精度自动定位及良好的对孔精度，安装锚杆速度快，能够避免锚杆台车的机械臂找作业面时经常出现臂架顶作业面不稳和顶作业面过猛的问题，同时提高了锚杆台车的自动化程度和作业效率。

上文结合具体实施例描述了本发明申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

还需要指出的是，在本申请的装置和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (6)

1.一种基于孔位坐标的锚杆台车自动定位方法，其特征在于，包括以下步骤：

步骤一，基于锚杆台车机械臂的结构特征，采用DH法，在基座、一号关节、二号关节、三号关节、四号关节、五号关节、六号关节、七号关节、T号关节处依次设置坐标系{0}～坐标系{T}，共T+1个坐标系，建立锚杆台车机械臂的总运动学变换矩阵：

步骤二，基于总运动学变换矩阵：根据锚杆台车机械臂末端执行器对钻孔点1和钻孔点2完成对孔作业时的两个末端位姿的关节变量，计算两个末端位姿从坐标系{T}变换到基座坐标系{0}的两个齐次变换矩阵：/>和/>

步骤三，基于施作面建立施作面坐标系{S}，根据钻孔点1和钻孔点2的孔位坐标，计算两个末端位姿的Z_T轴对应的两个单位矢量在施作面坐标系{S}下的描述^Sp₁和^Sp₂，再将施作面坐标系{S}下的两个单位矢量^sp₁和^sp₂变换到基座坐标系{0}；

基于所述钻孔点1和钻孔点2的孔位坐标求得两个末端位姿的Z_T轴与施作面坐标系{S}的Z轴的夹角和/>其中：

施作面坐标系{S}下的单位矢量^Sp₁和^Sp₂为：

步骤四，基于步骤三求解施作面坐标系{S}到基座坐标系{0}的变换矩阵实际工程中，两个单位矢量不能保证相交，所以先对变换矩阵/>中的旋转矩阵求模糊量，再使用向量叉乘求解旋转矩阵，两个单位矢量^sp₁和^sp₂在基座坐标系{0}下的描述为⁰p₁和⁰p₂，对应的两条直线为L₁和L₂；

步骤五，基于两条直线L₁和L₂和变换矩阵中的旋转矩阵求解偏移矩阵，确定两条直线L₁和L₂的公垂线方向，根据直线L₁与公垂线确定平面A，求平面A与直线L₂的交点坐标，即为偏移矩阵，最终解得所述变换矩阵/>

根据和/>计算/>中的n_Sx、n_Sy、n_Sz和模糊量a_Sx'、a_Sy'、a_Sz'，通过向量叉乘，计算o_Sx、o_Sy、o_Sz，再次使用向量叉乘来进行误差补偿，重新求a_Sx、a_Sy、a_Sz；其中：

⁰p₁＝[a_x1 a_y1 a_z1 0]^T；

⁰p₂＝[a_x2 a_y2 a_z2 0]^T；

2.根据权利要求1所述的一种基于孔位坐标的锚杆台车自动定位方法，其特征在于：所述锚杆台车机械臂的总运动学变换矩阵为：

其中，为坐标系{1}到基础坐标系{0}的变换矩阵，/>为坐标系{2}到坐标系{1}的变换矩阵，/>为坐标系{3}到坐标系{2}的变换矩阵，/>为坐标系{4}到坐标系{3}的变换矩阵，/>为坐标系{5}到坐标系{4}的变换矩阵，/>为坐标系{6}到坐标系{5}的变换矩阵，/>为坐标系{7}到坐标系{6}的变换矩阵，/>为坐标系{T}到坐标系{7}的变换矩阵。

3.根据权利要求2所述的一种基于孔位坐标的锚杆台车自动定位方法，其特征在于：所述两个齐次变换矩阵和/>为：

4.根据权利要求3所述的一种基于孔位坐标的锚杆台车自动定位方法，其特征在于：所述施作面坐标系{S}在从锚杆台车驾驶室向前看时，X轴指向右，Y轴指向前，Z轴指向上。

5.根据权利要求4所述的一种基于孔位坐标的锚杆台车自动定位方法，其特征在于：所述公垂线与所述直线L₁确定的平面A的方程为：

Nx+Oy+Pz+Q＝0

其中，N＝a_x1a_y1a_y2-a_y1 ²a_x2-a_z1 ²a_x2+a_x1a_z1a_z2；

O＝a_x1a_y1a_x2-a_x1 ²a_y2-a_z1 ²a_y2+a_y1a_z1a_z2；

P＝a_x1a_z1a_x2-a_x1 ²a_z2-a_y1 ²a_z2+a_y1a_z1a_y2；

Q＝-Np_x1-Op_y1-Pp_z1。

6.根据权利要求5所述的一种基于孔位坐标的锚杆台车自动定位方法，其特征在于：所述平面A与所述L₂交点坐标为(p_Sx，p_Sy，p_Sz)，交点坐标为所述施作面坐标系{S}的原点在基座坐标系{0}下的位置坐标；

其中，p_Sx＝a_x2t+p_x2；p_Sy＝a_y2t+p_y2；p_Sz＝a_z2t+p_z2；

其中，中间变量