CN114227695B - 一种锚杆支护机器人工作臂轨迹规划方法及*** - Google Patents

Abstract

本发明涉及一种锚杆支护机器人工作臂轨迹规划方法及***，根据第一锚孔和第二锚孔的位置确定锚杆支护机器人工作臂的起止位置；在起止位置间进行工作臂末端关节的无障碍路径规划，确定工作臂末端关节的无障碍路径轨迹；将所述轨迹离散为若干个路径点，计算各路径点相对工作臂的关节坐标；基于工作臂摆动范围和各路径点相对工作臂的关节坐标，计算工作臂在相邻路径点间的运动时间，进而确定工作臂剩余关节的三次样条轨迹。采用三次样条插值法进行路径规划，保证工作臂可以经过预设路径点，满足锚杆支护机器人工作时的避障要求；且能够保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使锚杆支护机器人工作臂运动平稳，无冲击。

Description

一种锚杆支护机器人工作臂轨迹规划方法及***

技术领域

本发明涉及锚杆支护机器人自动锚杆支护技术领域，特别是涉及一种锚杆支护机器人工作臂轨迹规划方法及***。

背景技术

锚杆支护机器人是煤矿巷道支护的重要装备，对巷道高效安全掘进具有巨大的意义。锚杆支护机器人工作臂的轨迹规划是自动化锚杆支护的技术基础。锚杆支护机器人工作臂轨迹规划表示为位移、速度或加速度等位姿运动量关于时间的函数，一般在关节空间或笛卡尔空间中进行。煤矿井下复杂的工作环境，对锚杆支护机器人作业有很高的要求。锚杆支护机器人工作臂在运动过程中需要保证运动平稳且无冲击，同时还需要合理的规避外界环境中的障碍。

目前锚杆支护机器人工作臂轨迹规划一般采用点对点轨迹规划，例如：多项式轨迹规划、梯形轨迹规划等方法，无法同时满足上述要求，工作臂在相邻两锚孔运动时容易与机身和外界环境发生碰撞，为井下安全支护带来了严重的安全隐患。

发明内容

本发明的目的是提供一种锚杆支护机器人工作臂轨迹规划方法及***，能够保证锚杆支护机器人工作臂可以经过预设的路径点，满足锚杆支护机器人工作时的避障要求；同时保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使得锚杆支护机器人工作臂能够运动平稳，无冲击。

为实现上述目的，本发明提供了如下方案：

本发明提供了一种锚杆支护机器人工作臂轨迹规划方法，所述方法包括：

根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔；

在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹；

将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标；

基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间；

根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节。

可选地，在所述“基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间”之前，还包括：

在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。

可选地，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，......，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。

可选地，计算工作臂剩余关节的三次样条轨迹S_ik(t)的具体公式为：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度。

可选地，工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点；

计算工作臂到达剩余路径点时关节i的加速度集合a_i的具体公式为：

a_i＝A_i ^-1b_i；

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

为实现上述目的，本发明还提供了如下方案：

一种锚杆支护机器人工作臂轨迹规划***，所述***包括：

起止位置确定模块，用于根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔；

无障碍路径轨迹确定模块，用于在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹；

离散处理模块，用于将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标；

运动时间确定模块，用于基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间；

关节轨迹确定模块，用于根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节。

可选地，所述***还包括：

自由路径点确定模块，用于在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。

可选地，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，......，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。

可选地，计算工作臂剩余关节的三次样条轨迹S_ik(t)的具体公式为：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度。

可选地，工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点；

计算工作臂到达剩余路径点时关节i的加速度集合a_i的具体公式为：

a_i＝A_i ^-1b_i；

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供了一种锚杆支护机器人工作臂轨迹规划方法及***，该方法根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；在起始位置和终止位置之间进行工作臂的末端关节的无障碍路径规划，确定工作臂的末端关节的无障碍路径轨迹；将无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于工作臂的关节坐标；基于工作臂的摆动范围和各个路径点相对于工作臂的关节坐标，计算工作臂在相邻路径点之间的运动时间；根据工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹。本发明采用三次样条插值法进行路径规划，使得工作臂关节角速度、角加速度曲线光滑连续，且能够保证工作臂可以经过预设路径点，满足锚杆支护机器人工作时的避障要求；同时能够保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使得锚杆支护机器人工作臂能够运动平稳，无冲击。

附图说明

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

图1为本发明一种锚杆支护机器人工作臂轨迹规划方法的流程图；

图2为本发明一种锚杆支护机器人工作臂轨迹规划***的模块结构示意图；

图3为本发明具体实施例轨迹规划结果的位移曲线图；

图4为本发明具体实施例轨迹规划结果的速度曲线图；

图5为本发明具体实施例轨迹规划结果的加速度曲线图；

图6为本发明具体实施例工作臂末端的运动路径。

符号说明：

起止位置确定模块-1，无障碍路径轨迹确定模块-2，离散处理模块-3，运动时间确定模块-4，关节轨迹确定模块-5。

具体实施方式

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

本发明的目的是提供一种锚杆支护机器人工作臂轨迹规划方法及***，能够保证锚杆支护机器人工作臂可以经过预设的路径点，满足锚杆支护机器人工作时的避障要求；同时能够保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使得锚杆支护机器人工作臂能够运动平稳、无冲击。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明一种锚杆支护机器人工作臂轨迹规划方法，包括以下步骤：

S1：根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔。所述第一锚孔和所述第二锚孔为巷道顶板和侧帮需安装锚杆的位置。

进一步地，S1包括：通过MD-H参数法得到锚杆支护机器人工作臂的运动学模型(以六自由度的锚杆支护机器人工作臂为例)：

式中：为工作臂末端钻架的姿态矩阵，/>为工作臂末端钻架上的钻头的位置向量。

根据工作臂的运动学模型，可以得到工作臂的逆运动学模型：已知安装在工作臂末端钻架上的钻头位姿，即可得到对应的工作臂关节坐标。

通过安装于锚杆支护机器人钻架上的深度相机识别巷道顶板和侧帮需要安装锚杆的位置(即锚孔的位置)，然后计算锚孔位置相对锚杆支护机器人的坐标，以相邻两个锚孔位置作为工作臂的起始位置和终止位置，并根据工作臂的逆运动学模型求得相应的关节坐标。为了满足锚孔作业要求，锚杆支护机器人工作臂在起始位置和终止位置的速度和加速度均要求为0。

S2：在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹。

S3：将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标。

S4：基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间。

具体地，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，......，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。

S5：根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节。

进一步地，在步骤S4之前，还包括：

在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。具体地，设置自由路径点是为了保证工作臂在在起始位置和终止位置的速度和加速度均为0，各关节轨迹二阶连续，第一自由路径点和第二自由路径点不一定在无障碍路径上且自由路径点的关节坐标并非人为直接给定，而是根据公式计算得出。具体地，确定所述第一自由路径点相对于所述工作臂的关节坐标采用公式(3)计算：

其中，所述工作臂在第一路径点与第一自由路径点之间的运动时间Δt_i0还可由公式(4)计算：

由于工作臂在起始位置处的时刻t_i0为0，且Δt_i0可根据公式(2)计算得出，因此，根据公式(4)可以得到工作臂到达第二路径点时对应的时刻t_i2，进一步地，根据公式(2)求得工作臂从第二路径点到达第三路径点的时间，以此类推，进而可以求出工作臂关节i到达路径点k时对应的时刻t_ik。

进一步地，确定所述第二自由路径点相对于所述工作臂的关节坐标采用公式：

所述工作臂在第二自由路径点与第二路径点之间的运动时间为：

其中，q_i(K-1)为第二自由路径点K-1相对于工作臂关节i的关节坐标，Δt_i(K-1)为关节i在第二自由路径点K-1和第二路径点K之间的运动时间，为工作臂末端到达第二路径点K时关节i的速度，/>为工作臂末端到达第二路径点K时关节i的速度，/>为工作臂末端到达第二自由路径点K-1时关节i的加速度，同时，/>

进一步地，计算工作臂剩余关节的三次样条轨迹S_ik(t)的采用公式(7)：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度。

进一步地，由于关节i的轨迹在每个路径点处均连续且二阶可导，得到连续性条件：

s_ik(t_i(k+1))＝s_ik+1(t_i(k+1)) (8)

其中，S_ik(t_i(k+1))表示轨迹S_ik(t)在t_i(k+1)时刻的位移，为轨迹S_ik(t)在t_i(k+1)时刻的速度，/>为轨迹S_ik(t)在t_i(k+1)时刻的加速度。

进一步地，工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点。

由公式(8)、(9)和(10)可以得到计算工作臂到达剩余路径点时关节i的加速度集合a_i的公式为：

a_i＝A_i ^-1b_i (11)

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

进一步地，将求得的加速度a_i代入关节i的三次样条轨迹表达式(7)中即可求得关节i的三次样条轨迹。对该轨迹求一阶导和二阶导即可得到关节i的速度和加速度，所求轨迹、速度和加速度将作为工作臂各关节控制器的输入信号，使工作臂可以按照指定路径平稳运动。

为实现上述目的，本发明还提供了如下方案：

如图2所示，本发明一种锚杆支护机器人工作臂轨迹规划***包括：起止位置确定模块1、无障碍路径轨迹确定模块2、离散处理模块3、运动时间确定模块4和关节轨迹确定模块5。

起止位置确定模块1，用于根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔。

无障碍路径轨迹确定模块2，用于在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹。

离散处理模块3，用于将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标。

运动时间确定模块4，用于基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间。

关节轨迹确定模块5，用于根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节。

进一步地，所述***还包括：

自由路径点确定模块，用于在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。

进一步地，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，......，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。

进一步地，计算工作臂剩余关节的三次样条轨迹S_ik(t)的具体公式为：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度。

进一步地，工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点；

计算工作臂到达剩余路径点时关节i的加速度集合a_i的具体公式为：

a_i＝A_i ^-1b_i (11)

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

实施例

以锚杆支护机器人顶板间相邻两个安装锚杆位置(锚孔)间的路径为例，采用本发明锚杆支护机器人工作臂轨迹规划方法求解六自由度锚杆支护机器人工作臂的轨迹，取K＝7。

首先，根据锚杆支护机器人的视觉模块，识别锚孔位置，并计算得到其相对锚杆支护机器人工作臂基坐标系的坐标，得到起始位置S₀相对工作臂基坐标系的笛卡尔坐标为[4000mm,700mm,2000mm]，关节坐标为[102.4mm，138mm，487.3mm，-80.0mm，-67.8mm，0°]，终止位置S₇相对工作臂基坐标系的笛卡尔坐标为[4000mm,1300mm,2000mm]，关节坐标为[166.3mm，127.6mm，697.0mm，-73.7mm，-115.4mm，0°]；同时，工作臂在S₀和S₇处的速度v₀、v₇和加速度q₀、q₇均为零。

然后，根据锚杆支护机器人工作臂的避障需求，计算路径点[S₂，S₃，S₄，S₅]的坐标。进而，根据工作臂各关节的物理约束条件，求得各分段多项式的插值时间t分别为[0.208s，3.587s，3.726s，3.651s，4.838s，3.362s，0.197s]。

最后，根据插值时间t和三次样条轨迹模型，得到各关节的运动轨迹(安装顶板锚杆时关节6没有运动，因此附图3、附图4、附图5和附图6中对关节6不展示)，其位移如附图3，速度如附图4，加速度如附图5，进而得到工作臂末端的运动路径，如附图6。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的***而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

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

Claims (6)

1.一种锚杆支护机器人工作臂轨迹规划方法，其特征在于，所述方法包括：

根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔；

在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹；

将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标；

基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间；

根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节；

计算工作臂剩余关节的三次样条轨迹S_ik(t)的具体公式为：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度；

工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点；

计算工作臂到达剩余路径点时关节i的加速度集合a_i的具体公式为：

a_i＝A_i ^-1b_i；

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

2.根据权利要求1所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于，在所述“基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间”之前，还包括：

在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。

3.根据权利要求2所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，......，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。

4.一种锚杆支护机器人工作臂轨迹规划***，其特征在于，所述***包括：

起止位置确定模块，用于根据第一锚孔的位置确定锚杆支护机器人工作臂的起始位置，根据第二锚孔的位置确定锚杆支护机器人工作臂的终止位置；所述第一锚孔和所述第二锚孔为相邻的两个锚孔；

无障碍路径轨迹确定模块，用于在所述起始位置和所述终止位置之间进行工作臂的末端关节的无障碍路径规划，确定所述工作臂的末端关节的无障碍路径轨迹；

离散处理模块，用于将所述无障碍路径轨迹离散为若干个路径点，并分别计算各个路径点相对于所述工作臂的关节坐标；

运动时间确定模块，用于基于工作臂的摆动范围和各个路径点相对于所述工作臂的关节坐标，计算所述工作臂在相邻路径点之间的运动时间；

关节轨迹确定模块，用于根据所述工作臂在相邻路径点之间的运动时间，确定工作臂的剩余关节的三次样条轨迹；所述剩余关节为所述工作臂中除末端关节以外的关节；

计算工作臂剩余关节的三次样条轨迹S_ik(t)的具体公式为：

其中，i表示工作臂的关节序号，t为关节i的运动时间，t_ik为关节i到达路径点k时对应的时刻，t_i(k+1)为关节i到达路径点k+1时对应的时刻，Δt_ik为关节i在路径点k和k+1之间的运动时间，q_ik为路径点k对应的关节i的坐标，为工作臂末端关节到达路径点k时关节i的加速度，/>为工作臂末端关节到达路径点k+1时关节i的加速度；

工作臂末端关节到达剩余路径点时关节i的加速度的计算过程具体包括：

将工作臂到达剩余路径点时关节i的加速度记为集合a_i，令所述剩余路径点为除第一路径点和第K+1路径点以外的路径点；

计算工作臂到达剩余路径点时关节i的加速度集合a_i的具体公式为：

a_i＝A_i ^-1b_i；

其中，a_i为工作臂到达剩余路径点时关节i的加速度集合，A_i为方程的系数项，b_i为方程的常数项，为工作臂末端到达第K+1路径点K时关节i的速度，/>为工作臂末端到达第K+1路径点K时关节i的加速度。

5.根据权利要求4所述的锚杆支护机器人工作臂轨迹规划***，其特征在于，所述***还包括：

自由路径点确定模块，用于在第一路径点与第二路径点间设置第一自由路径点，在第K+1路径点与第K路径点间设置第二自由路径点，并确定所述第一自由路径点和所述第二自由路径点相对于所述工作臂的关节坐标，以满足起止位置的条件约束；所述条件约束为：所述末端关节在起始位置的速度为0，所述末端关节在起始位置的加速度为0，所述末端关节在终止位置的速度为0，所述末端关节在终止位置的加速度为0；所述第一路径点为K+1个路径点中的首个路径点；所述第二路径点为K+1个路径点中的第二个路径点，所述第K+1路径点为末个路径点，K+1为路径点总个数。

6.根据权利要求5所述的锚杆支护机器人工作臂轨迹规划***，其特征在于，计算所述工作臂在相邻路径点之间的运动时间Δt_ik采用公式：

其中，k为路径点序号；k∈[0，1，……，K-1]，S_k为工作臂到达第k个路径点的轨迹，v_max为工作臂的速度约束，Δt_ik为关节i在路径点k和k+1之间的运动时间。