CN114179086B - 一种喷湿台车臂架末端的位姿规划方法 - Google Patents

Abstract

本发明涉及一种喷湿台车臂架末端的位姿规划方法，属于矿山类隧道湿喷台车技术领域。方法包括：获取待喷涂区域参数，包括喷枪末端的位置，喷枪360°关节的旋转角度，喷枪240°关节的旋转角度；根据第一约束条件确定臂架末端的第一姿态；根据第二约束条件计算出臂架末端的第一位置；根据臂架末端的第一位置判断臂架是否超限；当臂架超限时，将超限时臂架末端的位置作为临界位置，计算臂架末端的旋转角度；根据旋转角度计算得到臂架末端的第二姿态；进而计算出臂架末端的第二位置；第二姿态和第二位置作为超限后架末端所规划的位姿。本发明在臂架超限后，调整臂架末端为第二姿态和第二位置，使得在大臂关节的运动范围内，喷嘴位置到达指定的位置。

Description

一种喷湿台车臂架末端的位姿规划方法

技术领域

本发明涉及一种喷湿台车臂架末端的位姿规划方法，属于矿山类隧道湿喷台车技术领域。

背景技术

湿喷台车在进行自动喷涂过程中为减少回弹率，要求喷枪垂直待喷面。为减少喷枪刷动运动控制的复杂度，即只让喷枪360°关节转动，要求喷枪喷射方向垂直臂架末端。喷枪垂直待喷面，又垂直臂架末端，则要求臂架末端姿态平行于掌子面姿态，也即末端指向隧道掘进方向。

当对拱架基部进行喷涂时，湿喷臂需总体下俯，在保持臂架末端姿态和喷枪喷角度的情况下，喷嘴位置由于车身高度、大臂等关节下俯角度到达极限位置而无法下探至拱架基部。由此需要重新规划湿喷臂的姿态，使得在大臂关节运动范围内，喷嘴位置到达指定的近地面位置且喷射角度满足垂直要求。

为此，需要提出一种臂架超限时臂架末端位姿规划的技术方案。

发明内容

本申请的目的在于提供一种喷湿台车臂架末端的位姿规划方法，为臂架超限时臂架末端位姿规划提出一种行之有效的技术方案。

为实现上述目的，本申请提出了一种喷湿台车臂架末端的位姿规划方法的技术方案，位姿规划方法包括以下步骤：

1)获取待喷涂区域参数，待喷涂区域参数包括喷枪末端的位置，喷枪360°关节的旋转角度，喷枪240°关节的旋转角度；

2)根据臂架末端坐标系平行掌子面坐标系的第一约束条件确定臂架末端的第一姿态；

3)根据喷枪末端的位置固定不变的第二约束条件，结合喷枪360°关节的旋转角度，喷枪240°关节的旋转角度计算出臂架末端的第一位置；

4)根据臂架末端的第一位置判断臂架是否超限；

5)当臂架超限时，将超限时臂架末端的位置作为临界位置，根据临界位置计算臂架末端的旋转角度；

6)根据旋转角度计算得到臂架末端的第二姿态；同时根据旋转角度计算得到调整后的喷枪360°关节的旋转角度、以及调整后的喷枪240°关节的旋转角度；

7)根据所述第二约束条件，结合调整后的喷枪360°关节的旋转角度、以及调整后的喷枪240°关节的旋转角度计算出臂架末端的第二位置；

8)将臂架末端的第二姿态作为超限后臂架末端所规划的姿态，将臂架末端的第二位置作为超限后架末端所规划的位置。

本发明的喷湿台车臂架末端的位姿规划方法的技术方案的有益效果是：本发明基于待喷涂区域参数计算出臂架末端的第一姿态和第一位置，通过第一位置判断臂架是否超限，臂架超限后，通过对臂架末端进行旋转，进而调整臂架末端为第二姿态和第二位置，将第二姿态和第二位置作为臂架超限后的位姿进行规划，使得在大臂关节的运动范围内，喷嘴位置到达指定的近地面位置且喷射角度满足垂直要求，实现了臂架超限后的喷涂规划。

进一步的，为了准确的判断出臂架是否超限，判断臂架是否超限的步骤包括：确定臂架末端的第一位置与大臂根部位置的高度差，若高度差小于等于设定阈值，则臂架超限；若高度差大于设定阈值，则臂架未超限。

进一步的，当臂架未超限时，将臂架末端的第一姿态作为未超限臂架末端所规划的姿态，将臂架末端的第一位置作为未超限架末端所规划的位置。

进一步的，所述步骤5)中根据临界位置计算臂架末端的旋转角度的计算过程为：

其中，(x_临，y_临，z_临)为台车坐标系{0}下臂架末端临界位置的坐标；θ_x为沿臂架末端自身坐标系X轴旋转的角度；θ_y为沿臂架末端自身坐标系Y轴旋转的角度；θ_z为沿臂架末端自身坐标系Z轴旋转的角度。

进一步的，所述步骤6)中臂架末端的第二姿态的计算过程为：

其中，

为台车坐标系{0}下臂架末端的第二姿态；/>

台车坐标系{0}下临界位置对应的临界姿态。

进一步的，调整后的喷枪240°关节的旋转角度的计算过程为：

其中：θ_8aim为调整后的喷枪240°关节的旋转角度；Cx＝cos(θ_x)，Sx＝sin(θ_x)，Cy＝cos(θ_y)，Sy＝sin(θ_y)；C₇＝cos(θ₇)，S₇＝sin(θ₇)，θ₇为调整前的喷枪360°关节的旋转角度；C₈＝cos(θ₈)，S₈＝sin(θ₈)，θ₈为调整前的喷枪240°关节的旋转角度。

进一步的，调整后的喷枪360°关节的旋转角度的计算过程为：

其中，θ_7aim为调整后的喷枪360°关节的旋转角度；

为掌子面坐标系{f}下喷枪末端的第一位置的y轴大小；C_8aim＝cos(θ_8aim)，S_8aim＝sin(θ_8aim)。

进一步的，所述步骤7)中，臂架末端的第二位置的计算过程为：

其中，

为台车坐标系{0}下臂架末端的第二位置；/>

为喷枪360°关节坐标系{7}转换到臂架末端坐标系{6}的转换矩阵；/>

喷枪240°关节坐标系{8}转换到喷枪360°关节坐标系{7}的转换矩阵；/>

喷枪末端坐标系{9}转换到喷枪240°关节坐标系{8}的转换矩阵；/>

台车坐标系{0}下臂架末端的第二姿态。

进一步的，所述步骤2)中，臂架末端的第一姿态为：

其中，

为隧道坐标系{G}下臂架末端的第一姿态；n_f为掌子面在隧道坐标系{G}下的X轴，o_f为掌子面在隧道坐标系{G}下的Y轴，a_f为掌子面在隧道坐标系{G}下的Z轴。

进一步的，所述步骤3)中，臂架末端的第一位置为：

其中，

为隧道坐标系{G}下臂架末端的第一位置，/>

为喷枪360°关节坐标系{7}转换到臂架末端坐标系{6}的转换矩阵；/>

喷枪240°关节坐标系{8}转换到喷枪360°关节坐标系{7}的转换矩阵；/>

喷枪末端坐标系{9}转换到喷枪240°关节坐标系{8}的转换矩阵；

为隧道坐标系{G}下喷枪末端的位置。

附图说明

图1是本发明喷湿台车臂架和喷枪的结构图；

图2是本发明喷湿台车臂架末端的位姿规划方法坐标建立示意图；

图3是本发明臂架超限时，臂架末端旋转的俯视矢量示意图；

图4是本发明臂架超限时，臂架末端旋转的左侧视矢量示意图；

图中：1为大臂、2为小臂、3为臂架末端、4为360°关节、5为240°关节、6为喷枪末端。

具体实施方式

喷湿台车臂架末端的位姿规划方法实施例：

本发明的主要构思在于，当臂架下俯到达极限位置时，臂架末端的位置也会同样收到限制，导致喷枪中的喷嘴位置无法满足喷涂路径规划，因此本发明在臂架超限时，调整臂架末端的姿态，使得大臂和小臂尽量平行，解除超限的现象，同时调整喷枪240°关节和360°关节的旋转角度，使得喷枪依然垂直待喷面，在大臂俯仰关节的运动范围内，可将喷嘴送至指定位置。

喷湿台车由台车、以及如图1所示臂架和喷枪组成，臂架包括大臂1和小臂2，大臂1的首端设置在一个湿喷臂底座上，湿喷臂底座固定在台车上，大臂1的末端与小臂2的首端连接，小臂2的末端也即臂架末端3，臂架末端3与喷枪的首端连接，喷枪末端6设置有喷嘴。喷枪上设置有360°关节4和240°关节5，分别用于控制喷枪的回转和摆动。

喷湿台车臂架末端的位姿规划方法是基于如图2所示的坐标系，{0}表示湿喷臂底座姿态/台车坐标系(Spray Jumbo)；{6}表示臂架末端坐标系(Boom)；{9}表示喷枪末端坐标系(Nozzle)；{f}表示掌子面坐标系(face profile)，原点在掌子面底部中间的位置；{G}表示隧道坐标系(Tunnel)；{7}表示喷枪360°关节坐标系；{8}表示喷枪240°关节坐标系；{7}和{8}坐标系图中未给出。

喷湿台车臂架末端的位姿规划方法，包括以下步骤：

1)获取待喷涂区域参数，待喷涂区域参数包括喷枪末端在掌子面坐标系{f}下的位置

喷枪360°关节的旋转角度θ₇，喷枪240°关节的旋转角度θ₈，掌子面坐标系{f}转换到隧道坐标系{G}的转换矩阵/>

其中，θ₇为一个变量，θ₈为一个固定值，θ₈＝90°。

本步骤中，待喷涂区域参数的获取过程如下：

首先，对开挖轮廓做预处理，用参数描述多段圆弧或直线构成的轮廓；

其次，对单拱架单片段轮廓进行辐角规划，确定该片段的n个喷涂子区域喷射辐角，进而根据子区域喷射辐角确定喷涂子区域的参数。

2)根据待喷涂区域参数得到臂架末端的第一位姿，臂架末端的第一位姿包括臂架末端的第一姿态和第一位置。

本步骤中，在隧道坐标系{G}下进行求解，因此将喷枪末端在掌子面坐标系{f}下的位置

转化到隧道坐标系{G}下，喷枪末端在隧道坐标系{G}的位置/>

建立约束条件，在约束条件下得到臂架末端的第一位姿，然后基于D-H模型的逆运动学将臂架末端操作空间解算到关节空间(解算到关节空间为后续的本步骤，本申请不作具体介绍)，在隧道坐标系{G}下臂架末端的第一位姿

分解如下：

其中，

为隧道坐标系{G}下臂架末端的第一姿态；/>

为隧道坐标系{G}下臂架末端的第一位置。

约束条件包括：

第一约束条件：臂架末端坐标系{6}平行掌子面坐标系{f}；

第二约束条件：喷枪末端的位置为确定的

也即喷枪末端的位置已固定，不变化；

标定台车在隧道坐标系{G}下的位姿

掌子面在隧道坐标系{G}下的位姿

其中，n_f为掌子面在隧道坐标系{G}下的X轴，o_f为掌子面在隧道坐标系{G}下的Y轴，a_f为掌子面在隧道坐标系{G}下的Z轴。

根据第一约束条件得到隧道坐标系{G}下臂架末端的第一姿态

根据第二约束条件，在已知喷枪360°关节的旋转角度θ₇，θ₈时，喷枪末端的位置同时满足：

其中，

为在臂架末端坐标系{6}下喷枪末端的位置；/>

为在隧道坐标系{G}下臂架末端的第一姿态；/>

为在臂架末端坐标系{6}下隧道坐标原点的位置；/>

为在隧道坐标系{G}下喷枪末端的位置；/>

为喷枪360°关节坐标系{7}转换到臂架末端坐标系{6}的转换矩阵，与θ₇相关；/>

为喷枪240°关节坐标系{8}转换到喷枪360°关节坐标系{7}的转换矩阵，与θ₈相关；/>

为喷枪末端坐标系{9}转换到喷枪240°关节坐标系{8}的转换矩阵，为一个固定值；/>

为喷枪末端坐标系{9}原点(0,0,0)^T的齐次坐标。

由此可以得到：

那么隧道坐标系{G}下臂架末端的第一位置

此时，隧道坐标系{G}下臂架末端的第一位姿

如下：

将

转换到台车坐标系{0}，台车坐标系{0}下臂架末端的第一位姿/>

为：

其中，

为台车坐标系{0}转换到隧道坐标系{G}的转换矩阵，/>

为台车坐标系{0}下臂架末端的第一姿态，/>

为台车坐标系{0}下臂架末端的第一位置，/>

3)当臂架末端的第一位置与大臂根部位置的高度差大于设定阈值时，则判断臂架未超限，将臂架末端的第一姿态作为未超限臂架末端所规划的姿态，将臂架末端的第一位置作为未超限架末端所规划的位置；当臂架末端的第一位置与大臂根部位置的高度差小于等于设定阈值时，则判断臂架超限，将此时的臂架末端的位置作为临界位置，根据临界位置计算臂架末端的旋转角度，进而根据旋转角度计算出臂架末端的第二姿态、调整后的喷枪360°关节的旋转角度、以及调整后的喷枪240°关节的旋转角度。

本实施例中，根据臂架末端的第一位置与大臂根部位置的高度差判断臂架是否超限，当高度差小于等于2000mm时，判断为超限情况，具体判断条件如下：

其中，

的第3行，也即台车坐标系{0}下臂架末端的高度，由于台车与大臂根部高度相同，因此/>

也即臂架末端的位置与大臂根部位置的高度差；const＝2000mm(该值是一个经验值，由实验得出)。

当要对近地面的区域进行喷涂时，步骤2)中的隧道坐标系{G}下臂架末端的位姿

需要较大的大臂下俯角度才能达到，当下俯角度超出大臂的下俯角范围时，臂架超限，步骤2)中的隧道坐标系{G}下臂架末端的第一位姿/>

是不可用的，此时需要调整臂架末端的姿态，让臂架末端绕其X、Y轴转动一定角度，使得大臂和小臂能尽量平行，如图3、图4所示，臂架末端旋转前为(X₆、Y₆、Z₆)，旋转后为(X₆'、Y₆'、Z₆')，则可在大臂下俯角度范围内将喷枪末端送到规划的位置，并且臂架末端的姿态调整后，若喷枪360°关节的旋转角度、以及喷枪240°关节的旋转角度还是步骤2)中的旋转角度，则喷枪末端的喷射角度不再垂直待喷面，因此还需重新调整喷枪360°关节的旋转角度、以及喷枪240°关节的旋转角度。

具体的，臂架末端姿态的调整(也即臂架末端的第二姿态)过程如下：

将臂架末端超限时的位置作为临界位置，台车坐标系{0}下臂架末端的临界位置

根据临界位置计算臂架末端的旋转角度，旋转角度包括沿臂架末端自身坐标系X轴旋转的角度θ_x，Y轴旋转的角度θ_y，Z轴旋转的角度θ_z：

根据θ_x、θ_y、θ_z计算出台车坐标系{0}下臂架末端的第二姿态

其中，/>

台车坐标系{0}下调整前的臂架末端的姿态，也即临界位置对应的临界姿态；

其中：

喷枪360°关节的旋转角度、以及喷枪240°关节的旋转角度的调整过程如下：

对于喷枪240°关节有，

其中：Cx＝cos(θ_x)，Sx＝sin(θ_x)，Cy＝cos(θ_y)，Sy＝sin(θ_y)，C₇＝cos(θ₇)，S₇＝sin(θ₇)；C₈＝cos(θ₈)，S₈＝sin(θ₈)。

考虑泥浆管安装位置对喷枪的干涉，喷枪240°关节的摆动有2种状态，即调整后的喷枪240°关节的旋转角度θ_8aim为：

/>

对于360°关节有，

其中：C_8aim＝cos(θ_8aim)，S_8aim＝sin(θ_8aim)，考虑对隧道壁左右幅的喷射，喷枪360°关节的运动范围超过90°，因此调整后的喷枪360°关节的旋转角度θ_7aim为：

其中，

为掌子面坐标系{f}下喷枪末端的第一位置的y轴大小。

4)以臂架末端的第二姿态

调整后的喷枪360°关节的旋转角度θ_7aim、以及调整后的喷枪240°关节的旋转角度θ_8aim，结合第二约束条件规划臂架末端超限后的第二位置，将臂架末端的第二姿态作为超限后臂架末端所规划的姿态，将臂架末端的第二位置作为超限后架末端所规划的位置。

由此臂架末端超限后，台车坐标系{0}下臂架末端的第二位置

为：

臂架末端超限后，台车坐标系{0}下臂架末端的第二位姿

为：

上述实施例中，将大臂根部位置的位置作为参考，根据臂架末端的第一位置和大臂根部位置的高度差判断臂架是否超限，作为其他实施方式，也可以采用其他高度不变的参考物，例如台车等作为参考，本发明对此不做限制。

本发明通过在臂架超限时，对臂架姿态的调整，同时调整喷枪关节至新的旋转角度θ_7aim和θ_8aim，则可在大臂俯仰关节的运动范围内，将喷枪送至喷涂区域规划的位置。

Claims (8)

1.一种喷湿台车臂架末端的位姿规划方法，其特征在于，包括以下步骤：

步骤1)获取待喷涂区域参数，待喷涂区域参数包括喷枪末端的位置，喷枪360°关节的旋转角度，喷枪240°关节的旋转角度；

步骤2)根据臂架末端坐标系平行掌子面坐标系的第一约束条件确定臂架末端的第一姿态；

步骤3)根据喷枪末端的位置固定不变的第二约束条件，结合喷枪360°关节的旋转角度，喷枪240°关节的旋转角度计算出臂架末端的第一位置；

步骤4)根据臂架末端的第一位置判断臂架是否超限；判断臂架是否超限的步骤包括：确定臂架末端的第一位置与大臂根部位置的高度差，若高度差小于等于设定阈值，则臂架超限；若高度差大于设定阈值，则臂架未超限；

步骤5)当臂架超限时，将超限时臂架末端的位置作为临界位置，根据临界位置计算臂架末端的旋转角度；所述步骤5)中根据临界位置计算臂架末端的旋转角度的计算过程为：

θ_z＝0；

其中，(x_临，y_临，z_临)为台车坐标系{0}下臂架末端临界位置的坐标；θ_x为沿臂架末端自身坐标系X轴旋转的角度；θ_y为沿臂架末端自身坐标系Y轴旋转的角度；θ_z为沿臂架末端自身坐标系Z轴旋转的角度；

步骤6)根据旋转角度计算得到臂架末端的第二姿态；同时根据旋转角度计算得到调整后的喷枪360°关节的旋转角度、以及调整后的喷枪240°关节的旋转角度；

步骤7)根据所述第二约束条件，结合调整后的喷枪360°关节的旋转角度、以及调整后的喷枪240°关节的旋转角度计算出臂架末端的第二位置；

步骤8)将臂架末端的第二姿态作为超限后臂架末端所规划的姿态，将臂架末端的第二位置作为超限后臂架末端所规划的位置。

2.根据权利要求1所述的喷湿台车臂架末端的位姿规划方法，其特征在于，当臂架未超限时，将臂架末端的第一姿态作为未超限臂架末端所规划的姿态，将臂架末端的第一位置作为未超限臂架末端所规划的位置。

3.根据权利要求1所述的喷湿台车臂架末端的位姿规划方法，其特征在于，所述步骤6)中臂架末端的第二姿态的计算过程为：

/>

其中，

为台车坐标系{0}下臂架末端的第二姿态；/>

台车坐标系{0}下临界位置对应的临界姿态。

4.根据权利要求1所述的喷湿台车臂架末端的位姿规划方法，其特征在于，调整后的喷枪240°关节的旋转角度的计算过程为：

其中：θ_8aim为调整后的喷枪240°关节的旋转角度；Cx＝cos(θ_x)，Sx＝sin(θ_x)，Cy＝cos(θ_y)，Sy＝sin(θ_y)；C₇＝cos(θ₇)，S₇＝sin(θ₇)，θ₇为调整前的喷枪360°关节的旋转角度；C₈＝cos(θ₈)，S₈＝sin(θ₈)，θ₈为调整前的喷枪240°关节的旋转角度。

5.根据权利要求4所述的喷湿台车臂架末端的位姿规划方法，其特征在于，调整后的喷枪360°关节的旋转角度的计算过程为：

其中，θ_7aim为调整后的喷枪360°关节的旋转角度；

为掌子面坐标系{f}下喷枪末端的第一位置的y轴大小；C_8aim＝cos(θ_8aim)，S_8aim＝sin(θ_8aim)。

6.根据权利要求5所述的喷湿台车臂架末端的位姿规划方法，其特征在于，所述步骤7)中，臂架末端的第二位置的计算过程为：

其中，

为台车坐标系{0}下臂架末端的第二位置；/>

为喷枪360°关节坐标系{7}转换到臂架末端坐标系{6}的转换矩阵；/>

喷枪240°关节坐标系{8}转换到喷枪360°关节坐标系{7}的转换矩阵；/>

喷枪末端坐标系{9}转换到喷枪240°关节坐标系{8}的转换矩阵；/>

台车坐标系{0}下臂架末端的第二姿态。

7.根据权利要求1所述的喷湿台车臂架末端的位姿规划方法，其特征在于，所述步骤2)中，臂架末端的第一姿态为：

/>

其中，

为隧道坐标系{G}下臂架末端的第一姿态；n_f为掌子面在隧道坐标系{G}下的X轴，o_f为掌子面在隧道坐标系{G}下的Y轴，a_f为掌子面在隧道坐标系{G}下的Z轴。

8.根据权利要求7所述的喷湿台车臂架末端的位姿规划方法，其特征在于，所述步骤3)中，臂架末端的第一位置为：

其中，

为隧道坐标系{G}下臂架末端的第一位置，/>

为喷枪360°关节坐标系{7}转换到臂架末端坐标系{6}的转换矩阵；/>

喷枪240°关节坐标系{8}转换到喷枪360°关节坐标系{7}的转换矩阵；/>

喷枪末端坐标系{9}转换到喷枪240°关节坐标系{8}的转换矩阵；/>

为隧道坐标系{G}下喷枪末端的位置。/>

Images