CN113858202B

CN113858202B - 一种锚杆台车钻臂的逆解分析方法、装置、设备及介质

Info

Publication number: CN113858202B
Application number: CN202111153641.7A
Authority: CN
Inventors: 牛孔肖; 贾连辉; 荆留杰; 陈帅; 李鹏宇; 陈强; 王占辉
Original assignee: China Railway Engineering Equipment Group Co Ltd CREG
Current assignee: China Railway Engineering Equipment Group Co Ltd CREG
Priority date: 2021-09-29
Filing date: 2021-09-29
Publication date: 2023-04-11
Anticipated expiration: 2041-09-29
Also published as: CN113858202A

Abstract

本文提供了一种锚杆台车钻臂的逆解分析方法、装置、设备及介质，包括以下步骤：获取钻臂的锚杆梁的目标姿态和目标位置；对锚杆台车的补偿组赋初值；根据姿态求解锚杆台车的姿态组的转角；根据末端位置和约束条件求解锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角；判断姿态组和位置组是否有解，若无解，则调整补偿组，并继续求解姿态组和位置组至有解；根据所述姿态组和所述位置组的解调整所述钻臂，实现了根据钻臂的末端位置和姿态，逆向推导钻臂的各个关节的转角或者伸缩尺度，且在逆向推导过程中，加入了针对钻臂设计意图作为约束条件，且推导过程位姿有序，这样令各个关节的解唯一，提升了钻臂定位的精度。

Description

一种锚杆台车钻臂的逆解分析方法、装置、设备及介质

技术领域

本发明涉及机械臂控制技术领域，应用于锚杆台车领域，尤其是一种锚杆台车钻臂的逆解分析方法、装置及介质。

背景技术

为实现锚杆台车8自由度钻臂在作业过程中的自动定位,就必须对钻臂逆运动学进行分析，目前国内外针对隧道施工专用设备臂架运动学的研究集中在凿岩台车，而对锚杆台车研究较少。

主流的机器人逆运动学分析方法均没有考虑臂架的设计意图，分析结果与臂架结构设计的初衷不符，所以亟需一种充分考虑结构设计意图的锚杆台车钻臂逆运动学分析方法，来实现锚杆台车钻臂在作业过程中的自动定位提供算法基础。

发明内容

针对现有技术的上述问题，本文的目的在于，提供一种锚杆台车钻臂的逆解分析方法、装置、设备及介质，以解决现有技术中逆运动学分析结果与钻臂的臂架结构设计初衷不符的问题。

为了解决上述技术问题，本文的具体技术方案如下：一方面，本文提供一种锚杆台车钻臂的逆解分析方法，包括以下步骤：

获取钻臂的锚杆梁的目标姿态和目标位置；

对所述锚杆台车的补偿组赋初值；

根据所述目标姿态求解所述锚杆台车的姿态组的转角；

根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角；

判断所述姿态组和所述位置组是否有解，若无解，则调整所述补偿组，并继续求解所述姿态组和所述位置组至有解；

根据所述姿态组和所述位置组的解调整所述钻臂。

作为本文的一种实施例，在获取钻臂的锚杆梁的目标姿态和目标位置之前，包括：

根据所述钻臂的D-H参数获取所述钻臂的相邻关节的坐标系的齐次变换矩阵；

根据所述相邻关节的齐次变换矩阵，得到所述锚杆梁相对于所述钻臂的基座的所述目标姿态和所述目标位置。

作为本文的一种实施例，所述根据所述相邻关节的齐次变换矩阵，得到所述锚杆梁相对于所述钻臂的基座的所述目标姿态和所述目标位置，进一步包括：

获取所述钻臂的所有相邻关节的所述齐次变换矩阵；

根据所述补偿组到所述位置组再到姿态组的

顺序进行齐次变换矩阵相乘，得到所述钻臂的所述锚杆梁相对于所述基座的齐次变换矩阵；

提取所述钻臂的所述锚杆梁相对于所述基座的齐次变换矩阵的旋转矩阵和位置矩阵；

其中，所述旋转矩阵对应所述目标姿态，所述位置矩阵对应所述目标位置。

作为本文的一种实施例，所述根据所述目标姿态求解所述锚杆台车的姿态组的转角，进一步包括：

所述姿态组包括第一姿态关节和第二姿态关节；

所述第一姿态关节对应第一姿态方程，所述第二姿态关节对应第二姿态方程；

根据所述第二姿态方程和所述旋转矩阵，求解所述第二姿态关节的转角；

根据所述第二姿态的转角、所述第一姿态方程和所述旋转矩阵求解所述第一姿态关节的转角。

作为本文的一种实施例，在根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角之前，包括：

根据所述钻臂的双三角油缸的设计意图建立约束条件；

其中，所述双三角油缸用于关联所述位置组的第一位置关节和第五位置关节，还用于连接所述位置组的第二位置关节和第四位置关节。

作为本文的一种实施例，所述根据所述钻臂的双三角油缸的设计意图建立约束条件，进一步包括：

将所述第一位置关节和所述第五位置关节的转角进行关联，建立第一关联方程；

将所述第二位置关节和所述第四位置关节的转角进行关联，建立第二关联方程。

作为本文的一种实施例，所述根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角，进一步包括：

计算所述位置组的第五位置关节在所述基座的坐标系下的位置坐标；

所述第三位置关节对应第三位置方程，所述第二位置关节对应第二位置方程，所述第一位置关节对应第一位置方程；

根据所述第三位置方程、所述方向坐标和所述D-H参数，求解所述第三位置关节的位移；

根据所述第二位置方程、所述第三位置关节的位移、所述方向坐标和所述D-H参数，求解所述位置组的第二位置关节的转角；

根据所述第一位置方程，所述第三位置关节的位移、所述第二位置关节的转角、所述方向坐标和所述D-H参数，求解所述位置组的第一位置关节的转角；

根据所述第一关联方程求解所述第五位置关节的转角；

根据所述第二关联方程求解所述第四位置关节的转角。

作为本文的一种实施例，所述计算所述位置组的第五位置关节在所述基座的坐标系下的位置坐标，进一步包括：

令所述钻臂的所述锚杆梁到所述基座的齐次变换矩阵与除所述位置组的逆矩阵相乘，得到所述第五位置关节相对于所述基座的齐次变换矩阵；

根据所述位置组相对于所述基座的齐次变换矩阵得到所述位置坐标。

作为本文的一种实施例，所述判断所述姿态组和所述位置组是否有解，进一步包括：

判断所述姿态组的各个姿态关节，所述位置组的各个位置关节是否均满足所述钻臂的D-H参数的限定范围；

若均满足，则判断所述姿态组和所述位置组有解；

若不满足，则判断所述姿态组和所述位置组无解。

作为本文的一种实施例，所述则调整所述补偿组，进一步包括：

根据基准步长增大或减小所述补偿组的伸出长度。

另一方面，本文还提供一种锚杆台车钻臂的逆解分析装置，包括：

获取单元，用于获取钻臂的锚杆梁的目标姿态和目标位置；

赋值单元，用于对所述锚杆台车的补偿组赋初值；

姿态组求解单元，用于根据所述目标姿态求解所述锚杆台车的姿态组的转角；

位置组求解单元，用于根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角；

判断单元，用于判断所述姿态组和所述位置组是否有解，若无解，则调整所述补偿组，并继续求解姿态组和所述位置组至有解；

调整单元，用于根据所述姿态组和所述位置组的解调整所述钻臂。

另一方面，本文还提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如任意一项所述的锚杆台车钻臂的逆解分析方法。

另一方面，本文还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现任意一项所述的锚杆台车钻臂的逆解分析方法。

采用上述技术方案，实现了根据钻臂的末端位置，即锚杆梁的位置，逆向推导钻臂的各个关节的转角或者伸缩尺度，且在逆向推导过程中，加入了针对钻臂设计意图作为约束条件，且推导过程位姿有序，这样令各个关节的解唯一，提升了逆向推导的精确程度。

为让本文的上述和其他目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附图式，作详细说明如下。

附图说明

为了更清楚地说明本文实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本文的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了本文实施例一种锚杆台车钻臂的逆解分析装置的网络结构示意图；

图2示出了本文实施例一种锚杆台车钻臂的逆解分析方法的示意图；

图3示出了本文实施例一种锚杆台车钻臂的逆解分析方法的姿态组求解方法示意图；

图4示出了本文实施例一种锚杆台车钻臂的逆解分析方法的位置组求解方法示意图；

图5示出了本文实施例一种锚杆台车钻臂的逆解分析装置示意图；

图6示出了本文实施例一种锚杆台车钻臂的逆解分析装置的矩阵计算单元示意图；

图7示出了本文实施例一种锚杆台车钻臂的逆解分析装置的姿态组求解单元示意图；

图8示出了本文实施例一种锚杆台车钻臂的逆解分析装置的关联单元示意图；

图9示出了本文实施例一种锚杆台车钻臂的逆解分析装置的位置组求解单元示意图；

图10示出了本文实施例一种锚杆台车钻臂的逆解分析装置的数据流程图；

图11示出了本文实施例一种计算机设备示意图。

附图符号说明：

101、控制终端；

102、钻臂；

103、计算终端；

501、获取单元；

502、赋值单元；

503、姿态组求解单元；

5031、第二姿态关节求解模块；

5032、第一姿态关节求解模块；

504、位置组求解单元；

5041、第一位置关节求解模块；

5042、第二位置关节求解模块；

5043、第三位置关节求解模块；

5044、第四位置关节求解模块；

5045、第五位置关节求解模块；

505、判断单元；

506、调整单元；

507、关联单元；

5071、第一关联方程建立模块；

5072、第二关联方程建立模块；

508、矩阵计算单元；

5081、相邻关节矩阵计算模块；

5082、矩阵相乘计算模块；

5083、矩阵提取模块；

1102、计算机设备；

1104、处理器；

1106、存储器；

1108、驱动机构；

1110、输入/输出模块；

1112、输入设备；

1114、输出设备；

1116、呈现设备；

1118、图形用户接口；

1120、网络接口；

1122、通信链路；

1124、通信总线。

具体实施方式

下面将结合本文实施例中的附图，对本文实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本文一部分实施例，而不是全部的实施例。基于本文中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本文保护的范围。

需要说明的是，本文的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本文的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、装置、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

需要说明的是，本文的锚杆台车钻臂的逆解分析方法、装置、设备及介质可用于机械臂控制技术领域，还可以用于隧道掘进技术领域，也可以用于除机械臂控制技术领域和隧道掘进技术领域之外的任意领域，本文的锚杆台车钻臂的逆解分析方法、装置、设备及介质的应用领域不做限定。

如图1所示为本文实施例一种锚杆台车钻臂的逆解分析装置的网络结构示意图，在本图中描述了通过控制终端101，钻臂102和计算终端103相结合的锚杆台车钻臂的逆解分析的交互方法，其中控制终端101可以分别与钻臂102或者计算终端103进行有线或者无线的互连，且控制终端101可以调整钻臂102的各个关节的转角和伸缩长度，且控制终端101可以获取钻臂102的D-H参数，根据D-H参数控制终端可以解析钻臂102的限定范围，计算终端103可以对钻臂进行逆解的运算，并将最终计算的结果输入到控制终端101。

在本说明书一些实施例中，控制终端101可以为具有网络交互功能的电子设备，也可以为运行于该电子设备中，为钻臂102提供业务逻辑的软体。其中，控制终端101在正常运行过程中可以获取用户的指令，并将指令发送至计算终端103。

在本说明书一些实施例中，钻臂102可以为锚杆台车的钻臂，也可以为其他类型的钻孔车的钻臂，或者为钻孔车与打锚杆车的钻臂。

在本说明书一些实施例中，所述控制终端101和计算终端103可以是台式电脑、平板电脑、笔记本电脑、智能手机或者服务器等。

如图2所示为本文实施例一种锚杆台车钻臂的逆解分析方法的示意图，在本实施例中描述了根据功能性，可以将钻臂分解为补偿组、姿态组和位置组进行依次求解，在本文实施例中，钻臂可以是2R-P-4R-P型钻臂，R为旋转关节，需要求解其转角，P为伸缩关节，需要求解其伸缩长度，需要说明的是本文的方法为位姿有序类型的解法，其中，钻臂由基座到位置组，再到姿态组，再到补偿组的顺序组合构成，补偿组为伸缩关节，位置组为五个关节构成，分别为第一位置关节、第二位置关节、第三位置关节、第四位置关节和第五位置关节，其中第三位置关节为伸缩关节，第一位置关节、第二位置关节、第四位置关节和第五位置关节为旋转关节，姿态组由两个关节构成，分别为第一姿态关节和第二姿态关节，第一姿态关节和第二姿态关节均为旋转关节，在本文中，为了方便说明，第一姿态关节和第五位置关节相邻，第一位置关节和第五位置关节，第二位置关节和第四位置关节均通过双三角油缸关联，根据双三角油缸的实际工作时所体现的设计意图，即双三角油缸可以同时驱动第一姿态关节和第五姿态关节，第二姿态关节和第四姿态关节运动，将其的设计意图作为约束条件加入到逆解分析中，且钻臂的锚杆梁，即姿态组对应的位置，其姿态会影响钻臂的位置，所以在给定补偿组的位移长度后，优先根据锚杆梁末端位置求解钻臂的姿态组，再求解位置组，需要说明的，本文钻臂在手动控制模式下双三角油缸通过液压***需要同时驱动了上述4个位置关节，控制误差较大；而在自动控制模式下，液压***的耦合控制关闭，钻臂的液压***分别独立控制上述4个位置关节，利用逆解从软件层实现4个关节的同时控制，达到操作手手动控制的效果，误差小，即本文的逆解算法服务于钻臂的自动控制模式，使分别控制的4个位置关节达到了手动模式下双三角油缸想要达到的意图，提高了控制精度，该方法具体包括以下步骤：

步骤201、获取钻臂的锚杆梁的目标姿态和目标位置。

步骤202、对所述锚杆台车的补偿组赋初值。

步骤203、根据所述目标姿态求解所述锚杆台车的姿态组的转角。

步骤204、根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角。

步骤205、判断所述姿态组和所述位置组是否有解，若无解，则调整所述补偿组，并继续求解所述姿态组和所述位置组至有解。

步骤206、根据所述姿态组和所述位置组的解调整所述钻臂。

通过上述步骤，可以实现按照位姿有序的方法对8自由度的钻臂进行逆解分析，且逆解唯一，通过该解可以快速的调整钻臂，指导钻臂进行各个关节的调整。

作为本文的一个实施例，在步骤201之前还包括：

根据所述钻臂的D-H参数获取所述钻臂的相邻关节的坐标系的齐次变换矩阵。

根据所述相邻关节的齐次变换矩阵，得到所述锚杆梁相对于所述钻臂的基座的所述姿态和所述末端位置。

在本步骤中，D-H参数为Craig于1986年提出的改进型D-H参数，D-H参数的核心是4个参数是扭转角、连杆长度、关节角和连杆偏移，这4个参数唯一确定了关节相对位姿关系，在确定完成关节的相对位姿关系后，可以采用齐次变换矩阵的方式将位姿表达，本文中所指的位姿即为位置和姿态。

因为钻臂的活动范围是其物理特性所确定的，所以钻臂的活动范围是固定的，所以通过各个关节的活动范围可以推导钻臂的D-H参数表，如表1所示锚杆台车钻臂的D-H参数表。

表1

在本表中，α_i-1为连杆扭转角，a_i-1为连杆长度，θ_i为关节角，d_i为连杆偏移，在表1中，钻臂的各个构件，关节和连杆的标号为由基座到锚杆梁末端依次增大进行编号，基座为0，关节i位于构件i-1和i之间，锚杆梁末端为t，锚杆梁末端还设有虚拟关节t。

关节的坐标系符合以下约定，z_i轴在i关节的轴线上，x_i-1轴在z_i-1轴和z_i轴的公垂线上。

a_i-1是沿着x_i-1轴从z_i-1轴到z_i轴的距离，例如第一位置关节和第二位置关节之间的公垂线的长度。

α_i-1是绕着x_i-1轴从z_i-1轴旋转到z_i轴的转角，例如将第一位置关节的坐标的Z轴绕自身X轴旋转到与第二位置关节的Z轴重合时，需要旋转的角度。

d_i是沿着z_i轴从x_i-1轴到x_i轴的距离，例如相邻两条公垂线的距离。

θ_i是绕着z_i轴从x_i-1轴旋转到x_i轴的转角，例如将第一位置关节的坐标的X轴绕第二位置关节的Z轴旋转到与第二位置关节的X轴重合时，需要旋转的角度。

初始值受到钻臂关节传感器校准的影响，其对应钻臂的实际结构，与本文计算无关，所以不再赘述。

根据本文钻臂的D-H参数表得到相邻关节的齐次变换矩阵，具体推导公式作为本领域较为常规的技术手段，所以在此不再赘述，得到钻臂的相邻关节的齐次变换矩阵为

其中

为关节i相对于关节i-1的旋转矩阵，P为位置矩阵，O为透视矩阵，1为比例，S为正弦函数，C为余弦函数。

将所有的相邻关节的齐次变换矩阵进行矩阵相乘，可以得到钻臂末端相对于钻臂基座的齐次变换矩阵。

作为本文的一个实施例，所述根据所述相邻关节的齐次变换矩阵，得到所述锚杆梁相对于所述钻臂的基座的所述目标姿态和所述目标位置，进一步包括：

获取所述钻臂的所有相邻关节的所述齐次变换矩阵；

根据所述补偿组到所述位置组再到姿态组的顺序进行齐次变换矩阵相乘，得到所述钻臂的所述锚杆梁相对于所述基座的齐次变换矩阵；

在本步骤中，将钻臂的所有相邻关节的齐次变换矩阵进行矩阵相乘，包括虚拟关节，在本文中，位置组对应的齐次变换矩阵包括

姿态组对应的齐次变换矩阵包括

补偿组对应的齐次变换矩阵为

虚拟关节为

可以将按照位置组到姿态组到补偿组再到虚拟关节的顺序将对应的齐次变换矩阵进行矩阵相乘，例如下式

得到钻臂末端相对于钻臂基座的齐次变换矩阵，其中R表示钻臂的锚杆梁末端相对于基座的姿态，旋转矩阵R由矩阵[n_x n_y n_z]、[o_x o_yo_z]和[a_x a_y a_z]构成，P表示钻臂的锚杆梁末端相对于基座的位置，其中P为位置矩阵由[p_xp_y p_z]构成，因为三角油缸的设计意图是完成钻臂的大臂的摆臂和起落，使这两个动作不引起锚杆梁的姿态的变化，因此锚杆梁的姿态固定后，需要调整的仅有锚杆梁的位置，所以通过R可以求解出姿态组的第一姿态关节和第二姿态关节的转角，根据P可以解出位置组的第三位置关节的位移和其余的位置关节的转角。

作为本文的一个实施例，如图3所示一种锚杆台车钻臂的逆解分析方法的姿态组求解方法示意图，步骤203进一步包括：

步骤301、所述姿态组包括第一姿态关节和第二姿态关节；所述第一姿态关节对应第一姿态方程，所述第二姿态关节对应第二姿态方程。

步骤302、根据所述第二姿态方程和所述旋转矩阵，求解所述第二姿态关节的转角。

步骤303、根据所述第二姿态的转角、所述第一姿态方程和所述旋转矩阵求解所述第一姿态关节的转角。

在本步骤中，根据本文的位姿有序方法，优先求解最末端的第二姿态关节的转角，根据第二姿态方程q₇＝-arccos(n_x)，在本公式中只需要把旋转矩阵的n_x带入即可得到第二姿态关节的转角。

当得到第二姿态关节的转角后，根据第一姿态方程

将第二姿态关节的转角和旋转矩阵的n_y和n_z带入到第一姿态方程即可得到第一姿态关节的转角，其中S为正弦函数，arctan2为双精度浮点数，相当于C语言中的double类型参数，输入逗号前后的参数，返回的是该位置对应的以弧度表示的反正切值。

作为本文的一个实施例，在步骤204之前包括：

根据所述钻臂的双三角油缸的设计意图建立约束条件。

其中，所述双三角油缸用于连接所述位置组的第一位置关节和第五位置关节，还用于连接所述位置组的第二位置关节和第四位置关节。

在本步骤中，第一姿态关节和第五姿态关节，第二姿态关节和第四姿态关节均通过双三角油缸关联，第一姿态关节动，第五姿态关节由于双三角油缸的牵引也会动，反之亦反，第二姿态关节动，第四姿态关节模仿双三角油缸的联动效果也会动，反之亦然，所以根据双三角油缸的特性，将其作为约束条件，降低逆解分析的运算难度和阶数。

作为本文的一个实施例，所述根据所述钻臂的双三角油缸的设计意图建立约束条件，进一步包括：

将所述第一位置关节和所述第五位置关节的转角进行关联，建立第一关联方程。

在本步骤中，第一关联方程为q₅＝-q₁，该第一关联方程揭示了第一位置关节和第五位置关节的关节活动大小相同，活动方向不同，例如第一位置关节在Z轴的正方向转了30°的转角，那么根据第一关联方程可知，第五位置关节在Z轴的负方向转了30°的转角。

同样的，第二关联方程为q₄＝-q₂，该第二关联方程也揭示了第二位置关节和第四位置关节的关节活动大小相同，活动方向不同，例如第而位置关节在Z轴的正方向转了60°的转角，那么根据第二关联方程可知，第四位置关节在Z轴的负方向转了60°的转角。

作为本文的一个实施例，如图4所示一种锚杆台车钻臂的逆解分析方法的位置组求解方法示意图，步骤204进一步包括：

步骤401、计算所述位置组的第五位置关节在所述基座的坐标系下的位置坐标；所述第三位置关节对应第三位置方程，所述第二位置关节对应第二位置方程，所述第一位置关节对应第一位置方程。

步骤402、根据所述第三位置方程、所述位置坐标和所述D-H参数，求解所述第三位置关节的位移。

步骤403、根据所述第二位置方程、所述第三位置关节的位移、所述位置坐标和所述D-H参数，求解所述位置组的第二位置关节的转角。

步骤404、根据所述第一位置方程，所述第三位置关节的位移、所述第二位置关节的转角、所述位置坐标和所述D-H参数，求解所述位置组的第一位置关节的转角。

步骤405、根据所述第一关联方程求解所述第五位置关节的转角。

步骤406、根据所述第二关联方程求解所述第四位置关节的转角。

在本步骤中，可以通过各种数学变换获取位置组在钻臂的基座的坐标系下的位置坐标t_x、t_y和t_z，例如在等式两遍同乘逆矩阵，或者根据D-H参数直接进行推导后在次进行矩阵转化，将矩阵转化为旋转矩阵、位置矩阵、透视矩阵和比例的形式，本文对获取方式不做限定。

本文的第三位置方程为

其中a₁为所述第一位置关节到所述第二位置关节的距离，a₄为所述第四位置关节到所述第五位置关节的距离，d₅为所述第五位置关节对应运动构件长度，l₃为所述第三位置关节的初始伸缩长度，上述所有的参数皆为已知，所以可以求解出第三位置关节的位移。本文的第二位置方程为

其中将第三位置关节的位移和已知的参数带入第二位置方程，可以得到第二位置关节的转角。

本文的第一位置方程为

其中，将第三位置关节的位移、第二位置关节的转角和已知参数均带入到第一位置方程，即可得到第一位置关节的转角。

再通过第一关联方程q₅＝-q₁和第二关联方程q₄＝-q₂，即可求解出第四位置关节和第五位置关节的转角，通过上述方程，可以求解出位置组所有的位置关节的转角或者第三位置关节的位移。

作为本文的一个实施例，所述计算所述位置组的第五位置关节在所述基座的坐标系下的位置坐标，进一步包括：

令所述钻臂的所述锚杆梁到所述基座的齐次变换矩阵与除所述位置组的逆矩阵相乘，得到所述第五位置关节相对于所述基座的齐次变换矩阵。

在本步骤中，根据公式

以等号两侧同乘逆矩阵的形式，例如

因

为单位矩阵，所以等号右边变为

使用这样的方法将等号右边的

去掉，最终变为

其中，0、0和0为透视矩阵O，1为比例，L为单位矩阵，t_x、t_y和t_z为位置组在钻臂的基座的坐标系下的位置坐标。

作为本文的一个实施例，步骤205进一步包括：

判断所述姿态组的各个姿态关节，所述位置组的各个位置关节是否均满足所述钻臂的D-H参数的限定范围。

若均满足，则判断所述姿态组和所述位置组有解。

若不满足，则判断所述姿态组和所述位置组无解。

在本步骤中，因为根据上述方程解出的解未必存在实数解，或者解的范围不满足钻臂的实际活动范围，所以需要判断解是否满足钻臂D-H参数表所规定的最大活动范围，若所有的关节都满足D-H参数表，那么该逆解过程成功，可以根据这些解指导钻臂的各个关节进行调整，若存在关节不满足D-H参数表，那么该逆解过程失败，需要再次调整补偿组进行求解。

作为本文的一个实施例，步骤205进一步还包括：

根据基准步长增大或减小所述补偿组的伸出长度。

在本步骤中，需要说明的是，用户可以根据钻臂的实际构造在为补偿组赋初值时选择补偿组的位移的最大值或者最小值进行调整，例如用户选择补偿组的初始值为位移的最大值，那么需要调整补偿组时，需要不断地减小补偿组的伸出长度，例如用户选择补偿组的初始值为位移的最小值，那么需要调整补偿组时，需要不断地增大补偿组的伸出长度，具体是增大或者缩小补偿组，或者先增大再减小，或者先减小再增大的方式作为本领域人员容易想到的，理应属于本文的保护范围，在此不再赘述。

通过上述本文实施例的方法，将三角油缸对于位置关节的作用作为约束条件，将8自由度的逆解降低为6自由度进行求解，且采用位姿有序的方法，先求姿态组的姿态关节转角，再求位置组的第三位置关节的位移，最后使用约束条件和方程求解剩余的四个位置关节的转角，并判断解出的解是否满足钻臂的实际活动范围，所以该逆解方法求解速度快，不存在多解的情况，将钻臂的设计意图加入到逆解过程中，并满足了钻臂的实际调整需求。

如图5所示为本文实施例一种锚杆台车钻臂的逆解分析装置示意图，在本图中描述了锚杆台车钻臂的逆解分析装置的基本结构，其中的功能单元、模块可以采用软件方式实现，也可以采用通用芯片或者特定芯片实现，所述的功能单元、模块一部分或者全部可以在计算终端103上，或者其中的一部分也可以在控制终端101上，通过与计算终端103的配合来调整钻臂102，该***具体包括：

获取单元501，用于获取钻臂的锚杆梁的目标姿态和目标位置。

赋值单元502，用于对所述锚杆台车的补偿组赋初值。

姿态组求解单元503，用于根据所述目标姿态求解所述锚杆台车的姿态组的转角。

位置组求解单元504，用于根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角。

判断单元505，用于判断所述姿态组和所述位置组是否有解，若无解，则调整所述补偿组，并继续求解姿态组和所述位置组至有解。

调整单元506，用于根据所述解调整所述钻臂。

关联单元507，用于根据所述钻臂的双三角油缸的设计意图建立约束条件。

矩阵计算单元508，用于根据钻臂D-H参数计算齐次变换矩阵。

作为本文的一个实施例，如图6所示为本文实施例一种锚杆台车钻臂的逆解分析装置的矩阵计算单元示意图，矩阵计算单元508包括：

相邻关节矩阵计算模块5081，用于根据所述钻臂的D-H参数获取所述钻臂的相邻关节的坐标系的齐次变换矩阵。

矩阵相乘计算模块5082，用于根据所述姿态组到所述位置组再到补偿组的顺序进行齐次变换矩阵相乘。

矩阵提取模块5083，用于提取所述钻臂的所述锚杆梁相对于所述基座的齐次变换矩阵的旋转矩阵和位置矩阵。

作为本文的一个实施例，如图7所示为本文实施例一种锚杆台车钻臂的逆解分析装置的姿态组求解单元示意图，姿态组求解单元503包括：

第二姿态关节求解模块5031，用于根据所述第二姿态方程和所述旋转矩阵，求解所述第二姿态关节的转角。

第一姿态关节求解模块5032，用于根据所述第二姿态的转角、所述第一姿态方程和所述旋转矩阵求解所述第一姿态关节的转角。

作为本文的一个实施例，如图8所示为本文实施例一种锚杆台车钻臂的逆解分析装置的关联单元示意图，关联单元507包括：

第一关联方程建立模块5071，用于将所述第一位置关节和所述第五位置关节的转角进行关联，建立第一关联方程。

第二关联方程建立模块5072，用于将所述第二位置关节和所述第四位置关节的转角进行关联，建立第二关联方程。

作为本文的一个实施例，获取单元501，还用于获取所述第五位置关节的坐标系在所述基座的坐标系下的位置坐标。

作为本文的一个实施例，如图9所示为本文实施例一种锚杆台车钻臂的逆解分析装置的位置组求解单元示意图，位置组求解单元504包括：

第三位置关节求解模块5043，用于根据所述第三位置方程、所述位置坐标和所述D-H参数，求解所述第三位置关节的位移。

第二位置关节求解模块5042，用于根据所述第二位置方程、所述第三位置关节的位移、所述方向坐标和所述D-H参数，求解所述位置组的第二位置关节的转角。

第一位置关节求解模块5041，用于根据所述第一位置方程，所述第三位置关节的位移、所述第二位置关节的转角、所述位置坐标和所述D-H参数，求解所述位置组的第一位置关节的转角。

第五位置关节求解模块5045，用于根据所述第一关联方程求解所述第五位置关节的转角。

第四位置关节求解模块5044，用于根据所述第二关联方程求解所述第四位置关节的转角。

作为本文的一个实施例，矩阵相乘计算模块5082，还用于令所述钻臂的所述锚杆梁到所述基座的齐次变换矩阵与姿态组以及补偿组的逆矩阵相乘，得到所述第五位置关节相对于所述基座的齐次变换矩阵。

矩阵提取模块5083，还用于根据所述第五位置关节相对于所述基座的齐次变换矩阵得到所述位置坐标。

作为本文的一个实施例，调整单元506，还用于根据基准步长增大或减小所述补偿组的伸出长度。

通过上述本文实施例的装置，实现了根据钻臂的D-H参数提取钻臂的各个相邻关节的齐次变换矩阵，并根据各个关节的齐次变换矩阵得到钻臂的末端到基座的齐次变换矩阵，将齐次变换矩阵提取出旋转矩阵和位置矩阵，根据若干方程依次计算姿态组的转角和位置组的位移和转角，实现了对8自由度钻臂的逆解。

如图10所示为一种锚杆台车钻臂的逆解分析装置的数据流程图，本图中描述了控制终端101如何与钻臂102和计算终端103进行数据交互，并描述了计算终端103如何进行逆解运算的过程，其中，具体包括：

步骤1001、控制终端101获取钻臂102的D-H参数。

在本步骤中，可以获取锚杆台车的钻臂的关节参数，该关节参数为锚杆台车自带的，根据关节参数所指明的各个关节的活动范围，建立D-H参数的表格，例如表1形式，因为表格创建为本领域技术人员常用技术手段，所以在此不再赘述。

步骤1002、计算终端103根据D-H参数计算相邻关节的齐次变换矩阵。

在本步骤中，计算终端103根据D-H参数，计算终端103由基座到钻臂末端，将钻臂分为位置组、姿态组、补偿组和虚拟关节，将各个关节进行分类后，计算相邻关节的齐次变换矩阵

步骤1003、计算终端103根据相邻关节的齐次变换矩阵得到钻臂末端到钻臂基座的齐次变换矩阵。

在本步骤中，计算终端103由位置组、姿态组、补偿组和虚拟关节的顺序将相邻关节的齐次变换矩阵相乘，

其中

为钻臂末端到钻臂基座的齐次变换矩阵。

步骤1004、计算终端103根据钻臂末端到钻臂基座的齐次变换矩阵，提取旋转矩阵和位置矩阵。

在本步骤中，根据公式，将钻臂末端到钻臂基座的齐次变换矩阵化简为旋转矩阵、透视矩阵、位置矩阵和比例的形式，

得到钻臂末端姿态R和位置P。

步骤1005，计算终端103对补偿组赋初值。

在本步骤中，计算终端103选择补偿组对应的最小的位移对补偿组赋初值。

步骤1006，计算终端103根据第二姿态方程和第一姿态方程求解姿态组的转角。

在本步骤中，计算终端103根据第二姿态方程q₇＝-arccos(n_x)，将旋转矩阵的n_x带入到第二姿态方程，求解第二姿态关节的转角。

计算终端103根据第一姿态方程

将第二姿态关节的转角和旋转矩阵的n_y和n_z，求解第一姿态关节的转角。

步骤1007，控制终端101获取钻臂的三角油缸作为约束条件。

在本步骤中，控制终端101获取钻臂的实际关节构成，例如发现了三角油缸存在比例条件或者对称条件，那么就把这种条件作为约束条件。

步骤1008，计算终端103提取控制终端101的约束条件，将约束条件方程化。

在本步骤中，计算终端将代码化，或者在力学上机械学可以描述的公式，用数学公式或者方程进行表征，便于后续计算，例如将位置组的第一位置关节和第五位置关节的关系进行方程化为q₅＝-q₁，将第二位置关节和第四位置关节的关系进行方程化为q₄＝-q₂。

步骤1009，计算终端103计算位置组相对于钻臂基座的齐次变换矩阵，得到位置组的位置坐标。

在本步骤中，计算终端103，根据公式

以等号两侧同乘逆矩阵的形式，将公式化为

其中t_x、t_y和t_z为位置组在钻臂的基座的坐标系下的位置坐标。

步骤1010，计算终端103根据第三位置方程、第二位置方程、第一位置方程和约束条件求解位置组的第三位置关节位移和其余位置关节的转角。

在本步骤中，计算终端103将方向坐标、D-H参数带入到第三位置方程

解得第三位置关节的位移。

计算终端103将方向坐标、第三位置关节的位移和D-H参数带入第二位置方程，

可以得到第二位置关节的转角。

计算终端103将方向坐标、第三位置关节的位移、第二位置关节的转角和D-H参数带入第一位置方程，

可以得到第一位置关节的转角。

计算终端103根据q⁵＝-q₁和q₄＝-q₂，可以得到第四位置关节的转角和第五位置关节的转角。

步骤1011，计算终端103将解发送至控制终端101。

步骤1012，控制终端101判断解是否满足D-H参数所规定的限定范围。

在本步骤中，若满足，则到步骤1013；

若不满足，则令计算终端103的补偿组加一定的步长后返回步骤1005。

步骤1013，控制终端101控制钻臂102调整。

如图11所示，为本文实施例提供的一种计算机设备，所述计算机设备1102可以包括一个或多个处理器1104，诸如一个或多个中央处理单元(CPU)，每个处理单元可以实现一个或多个硬件线程。计算机设备1102还可以包括任何存储器1106，其用于存储诸如代码、设置、数据等之类的任何种类的信息。非限制性的，比如，存储器1106可以包括以下任一项或多种组合：任何类型的RAM，任何类型的ROM，闪存设备，硬盘，光盘等。更一般地，任何存储器都可以使用任何技术来存储信息。进一步地，任何存储器可以提供信息的易失性或非易失性保留。进一步地，任何存储器可以表示计算机设备1102的固定或可移除部件。在一种情况下，当处理器1104执行被存储在任何存储器或存储器的组合中的相关联的指令时，计算机设备1102可以执行相关联指令的任一操作。计算机设备1102还包括用于与任何存储器交互的一个或多个驱动机构1108，诸如硬盘驱动机构、光盘驱动机构等。

计算机设备1102还可以包括输入/输出模块1110(I/O)，其用于接收各种输入(经由输入设备1112)和用于提供各种输出(经由输出设备1114))。一个具体输出机构可以包括呈现设备1116和相关联的图形用户接口(GUI)1118。在其他实施例中，还可以不包括输入/输出模块1110(I/O)、输入设备1112以及输出设备1114，仅作为网络中的一台计算机设备。计算机设备1102还可以包括一个或多个网络接口1120，其用于经由一个或多个通信链路1122与其他设备交换数据。一个或多个通信总线1124将上文所描述的部件耦合在一起。

通信链路1122可以以任何方式实现，例如，通过局域网、广域网(例如，因特网)、点对点连接等、或其任何组合。通信链路1122可以包括由任何协议或协议组合支配的硬连线链路、无线链路、路由器、网关功能、名称服务器等的任何组合。

对应于图2-图4和图10中的方法，本文实施例还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行上述方法的步骤。

本文实施例还提供一种计算机可读指令，其中当处理器执行所述指令时，其中的程序使得处理器执行如图2-图4和图10所示的方法。

应理解，在本文的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本文实施例的实施过程构成任何限定。

还应理解，在本文实施例中，术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系。例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本文的范围。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的***、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本文所提供的几个实施例中，应该理解到，所揭露的***、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另外，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，也可以是电的，机械的或其它的形式连接。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本文实施例方案的目的。

另外，在本文各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本文的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本文各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

需要说明的是，本申请所涉及的用户信息(包括但不限于用户设备信息、用户个人信息等)和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等)，均为经用户授权或者经过各方充分授权的信息和数据。

本文中应用了具体实施例对本文的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本文的方法及其核心思想；同时，对于本领域的一般技术人员，依据本文的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本文的限制。

Claims

1.一种锚杆台车钻臂的逆解分析方法，其特征在于，包括以下步骤：

获取钻臂的锚杆梁的目标姿态和目标位置；

对所述锚杆台车的补偿组赋初值；

根据所述目标姿态求解所述锚杆台车的姿态组的转角；

根据所述钻臂的双三角油缸的设计意图建立约束条件；

其中，所述双三角油缸用于关联位置组的第一位置关节和第五位置关节，还用于连接所述位置组的第二位置关节和第四位置关节；

根据所述姿态组和所述位置组的解调整所述钻臂。

2.根据权利要求1所述的锚杆台车钻臂的逆解分析方法，其特征在于，在获取钻臂的锚杆梁的目标姿态和目标位置之前，包括：

3.根据权利要求2所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述根据所述相邻关节的齐次变换矩阵，得到所述锚杆梁相对于所述钻臂的基座的所述目标姿态和所述目标位置，进一步包括：

获取所述钻臂的所有相邻关节的所述齐次变换矩阵；

根据所述位置组到所述姿态组再到补偿组的顺序进行齐次变换矩阵相乘，得到所述钻臂的所述锚杆梁相对于所述基座的齐次变换矩阵；

4.根据权利要求3所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述根据所述目标姿态求解所述锚杆台车的姿态组的转角，进一步包括：

所述姿态组包括第一姿态关节和第二姿态关节；

根据所述第二姿态关节的转角、所述第一姿态方程和所述旋转矩阵求解所述第一姿态关节的转角。

5.根据权利要求2所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述根据所述钻臂的双三角油缸的设计意图建立约束条件，进一步包括：

6.根据权利要求5所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角，进一步包括：

根据所述第三位置方程、所述位置坐标和所述D-H参数，求解所述第三位置关节的位移；

根据所述第二位置方程、所述第三位置关节的位移、所述位置坐标和所述D-H参数，求解所述位置组的第二位置关节的转角；

根据所述第一位置方程，所述第三位置关节的位移、所述第二位置关节的转角、所述位置坐标和所述D-H参数，求解所述位置组的第一位置关节的转角；

根据所述第一关联方程求解所述第五位置关节的转角；

根据所述第二关联方程求解所述第四位置关节的转角。

7.根据权利要求6所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述计算所述位置组的第五位置关节在所述基座的坐标系下的位置坐标，进一步包括：

8.根据权利要求2所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述判断所述姿态组和所述位置组是否有解，进一步包括：

若均满足，则判断所述姿态组和所述位置组有解；

若不满足，则判断所述姿态组和所述位置组无解。

9.根据权利要求1所述的锚杆台车钻臂的逆解分析方法，其特征在于，所述则调整所述补偿组，进一步包括：

根据基准步长增大或减小所述补偿组的伸出长度。

10.一种锚杆台车钻臂的逆解分析装置，其特征在于，包括：

获取单元，用于获取钻臂的锚杆梁的目标姿态和目标位置；

赋值单元，用于对所述锚杆台车的补偿组赋初值；

位置组求解单元，用于根据所述钻臂的双三角油缸的设计意图建立约束条件；

其中，所述双三角油缸用于关联所述位置组的第一位置关节和第五位置关节，还用于连接所述位置组的第二位置关节和第四位置关节；还用于根据所述目标位置和约束条件求解所述锚杆台车的位置组的第三位置关节的位移和其余位置关节的转角；

11.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-9任意一项所述的锚杆台车钻臂的逆解分析方法。

12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-9任意一项所述的锚杆台车钻臂的逆解分析方法。