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


背景技术：

2.锚杆支护机器人是煤矿巷道支护的重要装备，对巷道高效安全掘进具有巨大的意义。锚杆支护机器人的工作臂轨迹规划是自动化锚杆支护的技术基础。锚杆支护机器人的工作臂轨迹规划表示为位移、速度、加速度等位姿运动量关于时间的函数，一般在关节空间或笛卡尔空间中进行规划和表示。由于煤矿井下的工作环境复杂，因此对锚杆支护机器人作业有很高的要求。锚杆支护机器人的工作臂在运动过程中需要合理地规避外界环境中的障碍，同时还需要保证运动平稳且无冲击。
3.目前，锚杆支护机器人的工作臂轨迹规划一般采用点对点轨迹规划，但是点对点的轨迹规划无法同时满足上述要求，且工作臂在两锚孔间运动时容易与机身或外界环境发生碰撞，为井下安全支护带来了严重的安全隐患。


技术实现要素：

4.本发明的目的是提供一种锚杆支护机器人的工作臂轨迹规划方法及系统，保证锚杆支护机器人的工作臂运动轨迹平滑。
5.为实现上述目的，本发明提供了如下方案：
6.一种锚杆支护机器人的工作臂轨迹规划方法，包括：
7.根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；所述标记关节为工作臂的任意关节；
8.根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹；
9.对所述标记关节的无障碍行驶轨迹进行离散处理，确定所述标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间；
10.根据分段多项式算法、所述离散段数和每个所述离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；所述剩余关节为工作臂中除所述标记关节之外的任意关节。
11.可选地，所述离散时间的计算过程，具体包括：
12.根据公式计算所述标记关节在第n离散段对应的离散时间；
13.其中，v
max
为工作臂最大速度的速度约束，pn为所述标记关节的第n离散段的末尾路径点，p
n-1
为所述标记关节的第n离散段的起始路径点。
14.可选地，所述根据分段多项式算法、所述离散段数和每个所述离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹，具体包括：
15.根据公式
16.对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；
17.其中，si(t)为第i个剩余关节的行驶轨迹，s
in
(t)为第i个剩余关节在第n离散段的行驶轨迹，tn为标记关节在第n离散段对应的离散时间，n＝1,2,
…
,k，k为离散段数；a
i1m
、a
i2l
、
…
、a
i(k-1)l
、a
ikm
均为系数向量，m＝0,1,2,3,4，l＝0,1,2,3。
18.可选地，所述根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置，具体包括：
19.获取第一锚孔位置和第二锚孔位置；
20.基于逆运动学模型对所述第一锚孔位置进行计算，以确定标记关节的起始位置；所述逆运动学模型为通过工作臂的运动学模型确定的；所述工作臂的运动学模型为通过md-h参数法确定的六自由度工作臂运动模型；
21.基于所述逆运动学模型对所述第二锚孔位置进行计算，以确定标记关节的终止位置。
22.可选地，所述根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹，具体包括：
23.确定所述标记关节的速度信息；
24.根据所述标记关节的起终位置和速度信息，确定所述标记关节的无障碍行驶轨迹；
25.其中，所述速度信息包括：所述标记关节的起始位置的关节速度为0，所述标记关节的起始位置的关节加速度为0，所述标记关节的终止位置的关节速度为0，所述标记关节的终止位置的关节加速度为0。
26.可选地，所述系数向量的计算过程，具体包括：
27.根据公式计算系数向量；
28.其中，
其中n∈[2,k-1]，q为信息集合；所述信息集合包括所述标记关节的起终位置、所述标记关节的起始位置的关节速度、所述标记关节的起始位置的关节加速度、所述标记关节的终止位置的关节速度、所述标记关节的终止位置的关节加速度和所述标记关节在每个离散段的首尾位置。
[0029]
为达上述目的，本发明还提供了如下技术方案：
[0030]
一种锚杆支护机器人的工作臂轨迹规划系统，包括：
[0031]
起终位置确定模块，用于根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；所述标记关节为工作臂的任意关节；
[0032]
无障碍轨迹确定模块，用于根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹；
[0033]
离散处理模块，用于对所述标记关节的无障碍行驶轨迹进行离散处理，确定所述标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间；
[0034]
关节行驶轨迹确定模块，用于根据分段多项式算法、所述离散段数和每个所述离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；所述剩余关节为工作臂中除所述标记关节之外的关节。
[0035]
可选地，在确定所述离散时间方面，所述离散处理模块，还包括：
[0036]
根据公式计算所述标记关节在第n离散段对应的离散时间；
[0037]
其中，v
max
为工作臂最大速度的速度约束，pn为所述标记关节的第n离散段的末尾路径点，p
n-1
为所述标记关节的第n离散段的起始路径点。
[0038]
可选地，所述关节行驶轨迹确定模块，具体包括：
[0039]
行驶轨迹计算子模块，用于根据公式
[0040]
对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；
[0041]
其中，si(t)为第i个剩余关节的行驶轨迹，s
in
(t)为第i个剩余关节在第n离散段的行驶轨迹，tn为标记关节在第n离散段对应的离散时间，n＝1,2,
…
,k，k为离散段数；a
i1m
、a
i2l
、
…
、a
i(k-1)l
、a
ikm
均为系数向量，m＝0,1,2,3,4，l＝0,1,2,3。
[0042]
可选地，所述起终位置确定模块，具体包括：
[0043]
锚孔位置确定子模块，用于获取第一锚孔位置和第二锚孔位置；
[0044]
起始位置计算子模块，用于基于逆运动学模型对所述第一锚孔位置进行计算，以确定标记关节的起始位置；所述逆运动学模型为通过工作臂的运动学模型确定的；所述工作臂的运动学模型为通过md-h参数法确定的六自由度工作臂运动模型；
[0045]
终止位置计算子模块，用于基于所述逆运动学模型对所述第二锚孔位置进行计算，以确定标记关节的终止位置。
[0046]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0047]
首先确定标记关节的起终位置，根据标记关节的起终位置，确定标记关节的无障碍行驶轨迹；然后对标记关节的无障碍行驶轨迹进行离散处理，确定标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间；最后根据分段多项式算法、离散段数和每个离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定剩余关节的行驶轨迹。本发明实现了对锚杆支护机器人的工作臂轨迹的分段多项式插值，以保证锚杆支护机器人的工作臂运动轨迹平滑、运动平稳。同时由于采用了分段多项式插值的方法，解决了点对点轨迹规划中可能存在的碰撞问题，提高锚杆支护的安全性。
附图说明
[0048]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0049]
图1为本发明锚杆支护机器人的工作臂轨迹规划方法的流程示意图；
[0050]
图2为本发明锚杆支护机器人的工作臂轨迹规划系统的结构示意图；
[0051]
图3为本发明实施例三中关节1的关节运动轨迹图；
[0052]
图4为本发明实施例三中关节1的关节运动速度图；
[0053]
图5为本发明实施例三中关节1的关节运动加速度图；
[0054]
图6为本发明实施例三中工作臂的末端运动路径图。
具体实施方式
[0055]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0056]
本发明的目的是提供一种锚杆支护机器人的工作臂轨迹规划方法及系统，能够解决点对点轨迹规划存在的缺陷，可以经过所有预设的路径点，降低了碰撞发生的可能性，同时保证锚固支护机器人在满足约束条件的前提下轨迹、速度和加速度连续，运动平稳，无冲击。
[0057]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0058]
实施例一
[0059]
如图1所示，本实施例提供一种锚杆支护机器人的工作臂轨迹规划方法，所述工作臂轨迹规划方法包括：
[0060]
步骤100，根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；所述标记关节为工作臂的任意关节。具体地，步骤100，具体包括：
[0061]
步骤1001，获取第一锚孔位置和第二锚孔位置。具体地，通过锚杆支护机器人的视觉模块识别巷道顶板和侧帮需要安装锚杆的位置，进而确定以锚杆支护机器人为原点的世界坐标系下的第一锚孔位置和第二锚孔位置。
[0062]
步骤1002，基于逆运动学模型对所述第一锚孔位置进行计算，以确定标记关节的起始位置；所述逆运动学模型为通过工作臂的运动学模型确定的；所述工作臂的运动学模型为通过md-h参数法确定的六自由度工作臂运动模型。具体地，所述标记关节为在工作臂末端钻架上的关节。
[0063]
锚杆支护机器人工作臂的六自由度运动学模型为其中，为工作臂末端钻架的姿态矩阵，为工作臂末端钻架上的钻头的位置向量。然后根据工作臂的运动学模型，可以得到工作臂的逆运动学模型，换而言之，已知安装在工作臂末端钻架上的钻头位姿，即可得到对应的工作臂末端关节坐标。
[0064]
步骤1003，基于所述逆运动学模型对所述第二锚孔位置进行计算，以确定标记关节的终止位置。
[0065]
步骤200，根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹。具体地，所述步骤200，具体包括：
[0066]
步骤2001，确定所述标记关节的速度信息。
[0067]
步骤2002，根据所述标记关节的起终位置和速度信息，确定所述标记关节的无障碍行驶轨迹；其中，所述速度信息包括：所述标记关节的起始位置的关节速度为0，所述标记关节的起始位置的关节加速度为0，所述标记关节的终止位置的关节速度为0，所述标记关节的终止位置的关节加速度为0，以保证工作臂的运动平稳性，且在开始和停止的位置无冲击。
[0068]
具体地，第一锚孔位置与第二锚孔位置为巷道工作面上相邻的两个锚固位置。根据锚杆支护机器人工作臂的避障需求，锚杆支护机器人的规划模块以第一锚孔位置对应的标记关节的关节坐标为标记关节的起始位置，以第二锚孔位置对应的标记关节的关节坐标为标记关节的终止位置，规划出一条无障碍路径。
[0069]
步骤300，对所述标记关节的无障碍行驶轨迹进行离散处理，确定所述标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间。具体地，所述离散时间的计算过程，具体包括：
[0070]
根据公式计算所述标记关节在第n离散段对应的离散时间。
[0071]
其中，v
max
为工作臂最大速度的速度约束，pn为所述标记关节的第n离散段的末尾路径点，p
n-1
为所述标记关节的第n离散段的起始路径点。
[0072]
具体地，将标记关节的无障碍行驶轨迹离散为若干路径点[p0,p1,p2,p3,
…
,p
n-1
,pn]，其中，p0为标记关节的起始路径点，pn为标记关节的终止路径点，[p0,p1]为标记关节的第一离散段，[p
n-1,pn]为标记关节的第n离散段，并基于工作臂的逆运动学模型计算出各个路径点对应的标记关节的关节坐标。
[0073]
步骤400，根据分段多项式算法、所述离散段数和每个离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；所述剩余关节为工作臂中除所述标记关节之外的任意关节。具体地，所述步骤400，具体包括：根据公式
[0074]
对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；其中，si(t)为第i个剩余关节的行驶轨迹，s
in
(t)为第i个剩余关节在第n离散段的行驶轨迹，tn为标记关节在第n离散段对应的离散时间，n＝1,2,
…
,k，k为离散段数；a
i1m
、a
i2l
、
…
、a
i(k-1)l
、a
ikm
均为系数向量，m＝0,1,2,3,4，l＝0,1,2,3。
[0075]
进一步地，所述系数向量的计算过程，具体包括：
[0076]
根据公式计算系数向量；
[0077]
其中，其中，其中，其中，其中n∈[2,k-1]，q为信息集合；所述信息集合包括所述标记关节的起终位置、所述标记关节的起始位置的关节速度、所述标记关节的起始位置的关节加速度、所述标记关节的终止位置的关节速度、所述标记关节的终止位置的关节加速度和所述标记关节在每个离散段的首尾位置。具体地，所述标记关节的起始位置的关节速度、所述标记关节的起始位置的关节加速度、所述标记关节的终止位置的关节速度、所述标记关节的终止位置的关节加速度和所述标记关节在每个离散段的首尾位置均以向量的形式进行表示。
[0078]
最后将解算得到的系数向量代入公式
[0079]
中，即可得到锚杆支护机器人各关节的4-3
‑…‑
3-4分段多项式轨迹，进而求一阶导和二阶导可得到各关节的速度和加速度曲线。
[0080]
本实施例首先由锚孔的位置信息计算工作臂无障碍路径并离散成若干路径点；然后根据工作臂各关节的物理约束条件计算工作臂的运动时间；最后，通过工作臂在相邻路径点之间的运动时间计算得到工作臂分段多项式轨迹的系数，并得到工作臂分段多项式轨迹。本实施例提供的方法，相较于点对点轨迹规划，可以保证工作臂经过预设的路径点，并且能在满足工作臂的物理约束时保证工作臂在所有路径点处轨迹、速度和加速度连续，使得工作臂在无障碍路径下可以避免与外界环境碰撞，同时运动连续平稳。
[0081]
实施例二
[0082]
如图2所示，本实施例提供了一种锚杆支护机器人的工作臂轨迹规划系统，包括：
[0083]
起终位置确定模块101，用于根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；所述标记关节为工作臂的任意关节。
[0084]
所述起终位置确定模块，具体包括：
[0085]
锚孔位置确定子模块，用于获取第一锚孔位置和第二锚孔位置。
[0086]
起始位置计算子模块，用于基于逆运动学模型对所述第一锚孔位置进行计算，以确定标记关节的起始位置；所述逆运动学模型为通过工作臂的运动学模型确定的；所述工作臂的运动学模型为通过md-h参数法确定的六自由度工作臂运动模型。
[0087]
终止位置计算子模块，用于基于所述逆运动学模型对所述第二锚孔位置进行计算，以确定标记关节的终止位置。
[0088]
无障碍轨迹确定模块201，用于根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹。
[0089]
离散处理模块301，用于对所述标记关节的无障碍行驶轨迹进行离散处理，确定所述标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间。
[0090]
具体地，在确定所述离散时间方面，所述离散处理模块，还包括：
[0091]
根据公式计算所述标记关节在第n离散段对应的离散时间；
[0092]
其中，v
max
为工作臂最大速度的速度约束，pn为所述标记关节的第n离散段的末尾路径点，p
n-1
为所述标记关节的第n离散段的起始路径点。
[0093]
关节行驶轨迹确定模块401，用于根据分段多项式算法、所述离散段数和每个离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；所述剩余关节为工作臂中除所述标记关节之外的关节。
[0094]
具体地，所述关节行驶轨迹确定模块401，具体包括：
[0095]
行驶轨迹计算子模块，用于根据公式
[0096]
对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；其中，si(t)为第i个剩余关节的行驶轨迹，s
in
(t)为第i个剩余关节在第n离散段的行驶轨迹，tn为标记关节在第n离散段对应的离散时间，n＝1,2,
…
,k，k为离散段数；a
i1m
、a
i2l
、
…
、a
i(k-1)l
、a
ikm
均为系数向量，m＝0,1,2,3,4，l＝0,1,2,3。
[0097]
实施例三
[0098]
本实施例以锚杆支护机器人顶板上的两相邻锚孔间的路径为例，采用实施例一提供的一种锚杆支护机器人工作臂轨迹规划方法，其中，取k＝6，即规划出一条4-3-3-3-3-4分段多项式的工作臂轨迹。
[0099]
步骤1)，根据锚杆支护机器人的视觉模块识别锚孔，并计算得到相对锚杆支护机器人工作臂基坐标系的坐标，得到锚孔q0相对工作臂基坐标系的笛卡尔坐标为[4000mm，-776mm，2000mm]，关节坐标为[-12.4457
°
，14.2120
°
，443.1127mm，-14.2120
°
，12.4457
°
，0
°
]，锚孔q6相对工作臂基坐标系的笛卡尔坐标为[4000mm，-332mm，2000mm]，关节坐标为[-3.9344
°
，14.5603
°
，381.6752mm，-14.5603
°
，3.9344
°
，0
°
]；同时，工作臂在锚孔q0和锚孔q6处的速度v0、v6和工作臂在锚孔q0和锚孔q6处的加速度a0、a6均为零。
[0100]
步骤2)，根据锚杆支护机器人工作臂的避障需求，计算路径点[q1，q2，q3，q4，q5]的坐标。
[0101]
步骤3)，根据工作臂各关节的物理约束条件，求得各分段多项式的插值时间t均为2s。
[0102]
步骤4)，根据插值时间t和分段多项式公式，得到关节的运动轨迹(以关节1为例)，其中，关节1的关节运动轨迹图如附图3，关节1的关节运动速度图如附图4，关节1的关节运动加速度图如附图5，进而得到工作臂的末端运动路径图如附图6。
[0103]
相对于现有技术，本发明还具有以下优点：
[0104]
(1)本发明能够保证锚杆支护机器人工作臂运动平稳，在起始和终止位置无冲击，同时锚杆支护机器人在巷道工作面内能够完成定位找孔的锚杆支护任务，降低了碰撞发生的可能性。
[0105]
(2)本发明解决了点对点轨迹规划存在的缺陷，同时保证所规划的轨迹在各个时刻位移、速度和加速度连续，计算量减少，且轨迹平滑。
[0106]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
[0107]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据
本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。