CN116276955A

CN116276955A - 一种钻锚机器人钻臂轨迹规划方法、系统及电子设备

Info

Publication number: CN116276955A
Application number: CN202211526865.2A
Authority: CN
Inventors: 张旭辉; 黄梦瑶; 杨文娟; 雷孟宇; 李彬; �田�浩
Original assignee: Weinan Shaanxi Coal Qichen Technology Co ltd; Xian University of Science and Technology
Current assignee: Weinan Shaanxi Coal Qichen Technology Co ltd; Xian University of Science and Technology
Priority date: 2022-11-30
Filing date: 2022-11-30
Publication date: 2023-06-23

Abstract

本发明公开一种钻锚机器人钻臂轨迹规划方法、系统及电子设备，涉及轨迹规划技术领域，方法包括：获取钻锚机器人的三维工作区域模型、工作起始点和钻孔点；采用快速扩展随机树算法确定钻锚机器人的初始钻臂路径；初始钻臂路径为所述钻锚机器人的钻臂，在所述三维工作区域模型中，从所述工作起始点至所述钻孔点的路径；采用连接法去除钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径；采用二阶贝塞尔曲线对第一优化路径进行拟合，以得到钻锚机器人的最终钻臂路径。本发明对钻锚机器人钻臂轨迹进行优化，以辅助实现快速准确地找孔和打孔。

Description

一种钻锚机器人钻臂轨迹规划方法、系统及电子设备

技术领域

本发明涉及轨迹规划技术领域，特别是涉及一种钻锚机器人钻臂轨迹规划方法、系统及电子设备。

背景技术

综掘工作面设备智能化程度高低是制约煤矿智能化发展的主要原因，而巷道锚固支护效率低是其中的关键影响因素，巷道锚固支护自动化控制是实现煤矿巷道掘进智能化、无人化的主要环节。

目前，煤矿井下钻锚机器人钻臂在进行锚固作业时首先需要在巷道顶端找孔并打孔，将整个巷道顶端全部均匀打上孔，然后才能装填锚固剂并将锚杆打入固定。在整个过程中，找孔打孔都是人工完成，需要耗费大量的时间，效率低。而综掘工作面支护与锚固效率不高导致了智能化程度低、采掘失衡问题，进而影响煤矿的掘进。

在确定多个钻孔的过程中，需要对钻锚机器人钻臂的运行轨迹进行规划，传统的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。基于快速扩展随机树(RRT，rapidlyexploringrandomtree)的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。该算法覆盖率较高，搜索范围广，但是RRT算法的缺点是得到的路径质量不是最优且不光滑，可能有棱角，不利于钻锚机器人钻臂在实际中的运行。

发明内容

本发明的目的是提供一种钻锚机器人钻臂轨迹规划方法、系统及电子设备，对钻锚机器人钻臂轨迹进行优化，以辅助实现快速准确地找孔和打孔。

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

本发明提供一种钻锚机器人钻臂轨迹规划方法，包括：

获取钻锚机器人的三维工作区域模型、工作起始点和钻孔点；

采用快速扩展随机树算法确定所述钻锚机器人的初始钻臂路径；所述初始钻臂路径为所述钻锚机器人的钻臂，在所述三维工作区域模型中，从所述工作起始点至所述钻孔点的路径；

采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径；

采用二阶贝塞尔曲线对所述第一优化路径进行拟合，以得到所述钻锚机器人的最终钻臂路径。

可选地，所述三维工作区域模型中包括障碍物区域和自由区域；

所述采用快速扩展随机树算法确定所述钻锚机器人的初始钻臂路径，具体包括：

以所述工作起始点为起始点，以所述钻孔点为目标点，并将所述工作起始点作为随机树初次扩展的根节点；

针对随机树的每次扩展，在所述自由区域内确定随机节点，根据所述随机节点确定所述随机树中的近距离节点；所述近距离节点为所述随机树在当前次扩展中与所述随机节点的距离最小的根节点；

在所述近距离节点与所述随机节点的连线上随机确定待用节点；所述待用节点与所述近距离节点之间的距离为一个预设步长；

判断所述近距离节点与所述随机节点的连线是否与所述障碍物区域相交，以得到第一结果；

若所述第一结果表示相交，则舍弃所述待用节点，并返回在所述自由区域内确定随机节点，根据所述随机节点确定所述随机树中的近距离节点的步骤；

若所述第一结果表示未相交，则将所述待用节点作为新增根节点加入所述随机树，并在所述新增根节点与所述目标点之间的距离大于或等于一个预设步长时进入下一次扩展，直至扩展的次数达到预设扩展次数或者所述新增根节点与所述目标点之间的距离小于一个预设步长时，停止扩展；

循环遍历所述随机树的所有根节点，与所述目标点连接以得到所述钻锚机器人的初始钻臂路径。

可选地，根据所述随机节点确定所述随机树中的近距离节点，具体包括：

基于NEAREST函数利用欧氏距离，选择所述随机树在当前次扩展中与所述随机节点的距离最小的根节点。

可选地，采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径，具体包括：

将所述初始钻臂路径划分为多个路段；每个路段包括路段起点和路段终点；

将所述工作起始点作为优化起始点，将所述优化起始点与第i个路段的路段终点连接；i≥1；

判断所述优化起始点与所述第i个路段的路段终点的连线之间是否存在障碍物，以得到第二结果；

若所述第二结果表示存在障碍物，则将所述优化起始点与第i个路段的路段起点的连线标记为优化路段，并将所述优化起始点更新为第i个路段的路段起点，返回将所述优化起始点与第i个路段的路段终点连接的步骤；

若所述第二结果表示不存在障碍物，判断所述第i个路段的路段终点是否为钻孔点，以得到第三结果；

若所述第三结果表示否，则将所述第i个路段更新为第i+1个路段，返回将所述工作起始点与第i个路段的路段终点连接的步骤；

若所述第三结果表示是，则将所述优化起始点与第i个路段的路段终点的连线标记为优化路段；

提取所有所述优化路段，以确定第一优化路径。

可选地，所述二阶贝塞尔曲线的计算公式为：

B(t)＝(1-t)²*P₀+2t(1-t)*P₁+t²*P₂ 0≤t≤1

其中，P₀表示拟合曲线起始点，P₁表示拟合曲线控制点，P₂表示拟合曲线结束点，t为系数，B(t)表示以点P₀为起始点、点P₂为结束点的拟合曲线。

可选地，所述钻锚机器人的三维工作区域模型采用MATLAB软件绘制。

为达上述目的，本发明还提供了如下技术方案：

一种钻锚机器人钻臂轨迹规划系统，包括：

模型构建模块，用于获取钻锚机器人的三维工作区域模型、工作起始点和钻孔点；

第一路径优化模块，用于采用快速扩展随机树算法确定所述钻锚机器人的初始钻臂路径；所述初始钻臂路径为所述钻锚机器人的钻臂，在所述三维工作区域模型中，从所述工作起始点至所述钻孔点的路径；

第二路径优化模块，用于采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径；

第三路径优化模块，用于采用二阶贝塞尔曲线对所述第一优化路径进行拟合，以得到所述钻锚机器人的最终钻臂路径。

一种电子设备，包括存储器和处理器；

所述存储器用于存储计算机程序，所述处理器用于运行所述计算机程序以执行钻锚机器人钻臂轨迹规划方法。

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

本发明提供了一种钻锚机器人钻臂轨迹规划方法、系统及电子设备，基于预设的工作起始点和钻孔点初步采用快速扩展随机树算法，快速确定钻锚机器人在煤矿井下复杂三维空间中的初始钻臂路径，然后依次采用连接法去除冗余点、采用二阶贝塞尔曲线对初始钻臂路径进行优化，以得到最优且光滑的钻臂路径，进而实现快速准确找孔、打孔，从而提高锚固效率，提高综掘工作面设备的智能化程度。

附图说明

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

图1为本发明钻锚机器人钻臂轨迹规划方法的流程示意图；

图2为本发明RRT算法原理示意图；

图3为本发明路径R示意图；

图4为本发明去除冗余点原理示意图；

图5为本发明路径R1示意图；

图6为本发明路径R2示意图；

图7为本发明钻锚机器人钻臂轨迹规划系统的结构示意图。

具体实施方式

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

煤矿井下钻锚机器人钻臂在进行锚固作业时首先需要在巷道顶端找孔并打孔，每两个孔之间距离相等，一般约为90cm，需要将整个巷道顶端全部均匀打上孔，然后才能装填锚固剂并将锚杆打入固定。钻锚机器人钻臂找孔时，顺利找到第一个点尤为重要，因此，本发明提出一种钻锚机器人钻臂轨迹规划方法、系统及电子设备，使用RRT算法得到路径规划图，然后使用连线法去除冗余点的方法对路径进行优化，再用Bezier曲线拟合对路径进行平滑处理。即结合RRT、连线法去冗余点和二阶Bezier曲线拟合各自的优点，自动快速地找到第一个钻孔点并优化轨迹，从而使轨迹规划的速度快的同时，质量更优、路径更加光滑。

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

实施例一

如图1所示，本实施例提供一种钻锚机器人钻臂轨迹规划方法，包括：

步骤100，获取钻锚机器人的三维工作区域模型、工作起始点和钻孔点。具体地，在MATLAB软件中绘制煤矿井下部分三维模型，并设定工作起始点以及巷道顶端某一钻孔点(第一个孔)。其中，三维工作区域模型中包括障碍物区域和自由区域。

步骤200，采用快速扩展随机树算法(RRT)确定所述钻锚机器人的初始钻臂路径；所述初始钻臂路径为所述钻锚机器人的钻臂在所述三维工作区域模型中，从所述工作起始点至所述钻孔点的路径。

步骤200，具体包括：

1)以所述工作起始点为起始点，以所述钻孔点为目标点，并将所述工作起始点作为随机树初次扩展的根节点。

2)针对随机树的每次扩展，在所述自由区域内确定随机节点，根据所述随机节点确定所述随机树中的近距离节点；所述近距离节点为所述随机树在当前次扩展中与所述随机节点的距离最小的根节点；具体地，基于NEAREST函数利用欧氏距离，选择所述随机树在当前次扩展中与所述随机节点的距离最小的根节点。

3)在所述近距离节点与所述随机节点的连线上随机确定待用节点；所述待用节点与所述近距离节点之间的距离为一个预设步长。

4)判断所述近距离节点与所述随机节点的连线是否与所述障碍物区域相交，以得到第一结果。若所述第一结果表示相交，则舍弃所述待用节点，并返回在所述自由区域内确定随机节点，根据所述随机节点确定所述随机树中的近距离节点的步骤；若所述第一结果表示未相交，则将所述待用节点作为新增根节点加入所述随机树，并在所述新增根节点与所述目标点之间的距离大于或等于一个预设步长时进入下一次扩展，直至扩展的次数达到预设扩展次数或者所述新增根节点与所述目标点之间的距离小于一个预设步长时，停止扩展；

5)循环遍历所述随机树的所有根节点，与所述目标点连接以得到所述钻锚机器人的初始钻臂路径。

RRT算法是通过逐步迭代的增量方式去生成随机树的，RRT算法节点扩展过程如图2所示。首先以Xstart作为起始点和随机树T的根节点，进行初始化，设置最大迭代次数(依照具体情况而定)，随后进入循环。下文示例的循环过程中，随机树T包括Xstart对应的根节点和Xnear对应的根节点，循环过程如下：

在三维工作区域模型内产生随机节点Xrand，随机节点Xrand属于集合RANDOM，NEAREST函数利用欧氏距离选择随机树T中距离Xrand最近的根节点，即为图中的Xnear节点。若随机树中只有点Xstart一个根节点，则Xnear节点即为Xstart节点。

然后将Xrand与Xnear连接起来，连接线的方向就是随机树生长的方向。设置一个步长u作为树一次生长的步长，在树生长的方向上生长一个步长，在生长的末端会产生一个新的结点Xnew。

判断Xrand与Xnear的连线上是否存在障碍物，如果存在障碍物，则放弃该新的结点Xnew；如果没有，则将Xnew结点加入到树中。

从三维工作区域模型中随机采样状态点，重复上述步骤，直到随机树新生成的结点到钻孔点的距离小于一个步长u或者达到最大迭代次数，则终止树的生长。直接将该新结点与钻孔点相连。

随机树确定后，标记钻孔点返回，循环遍历父节点，探索至随机树T的根节点。此时，从随机树中找到一条连接所有根节点和钻孔点且无碰撞障碍物的可行路径。

在一个具体应用中，在MATLAB软件里面编写RRT算法轨迹规划程序，运行程序，自动生成了一条由工作起始点O到钻孔点A的路径，记为路径R，如图3所示。后面所有钻孔点都是均匀相距90cm。因此，路径R的规划就显得尤为重要。由于RRT算法本身的原因，得到的路径R并不是最优(最近)路径并且很粗糙，会产生振动影响打孔精度。因此，需要对路径R进行优化。针对路径不是最优问题采用连线法去除冗余点，使路径缩小到最短，提高打孔效率，节约时间。

步骤300，采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径。

步骤300，具体包括：

1)将所述初始钻臂路径划分为多个路段；每个路段包括路段起点和路段终点。

2)将所述工作起始点作为优化起始点，将所述优化起始点与第i个路段的路段终点连接；i≥1。

3)判断所述优化起始点与所述第i个路段的路段终点的连线之间是否存在障碍物，以得到第二结果。

4)若所述第二结果表示存在障碍物，则将所述优化起始点与第i个路段的路段起点的连线标记为优化路段，并将所述优化起始点更新为第i个路段的路段起点，返回将所述优化起始点与第i个路段的路段终点连接的步骤。

5)若所述第二结果表示不存在障碍物，判断所述第i个路段的路段终点是否为钻孔点，以得到第三结果；若所述第三结果表示否，则将所述第i个路段更新为第i+1个路段，返回将所述工作起始点与第i个路段的路段终点连接的步骤；若所述第三结果表示是，则将所述优化起始点与第i个路段的路段终点的连线标记为优化路段。

6)提取所有所述优化路段，以确定第一优化路径。

RRT算法得到的路径中包含许多弯曲很小的地方，有些地方完全可以改为直线。下面以图4为例，对连接法去冗余点的原理进行示意：

图4中路径起点为Xstart，设置一个步长X，将路径按照步长X均匀划分为若干段，可得到X₁点、X₂点、X₃点、X₄点、X₅点等，图4中的黑体正方形表示障碍物。连接Xstart和X₁点，如果二者之间没有障碍物，那么就舍弃X₁点；再将Xstart与X₂连接起来，继续重复上述判断操作。如图4所示，可得Xstart到X₄之间可以优化为一段路径。如果二者之间有障碍物，则说明该小段路径已经为最优路径，接下来以X₄作为起点，连接X₄与X₅，继续进行优化步骤。依次循环，直至路径所划分的若干个点全部跑完，则此时得到的路径会比RRT算法得到的初始路径质量更优、更加优化。

在一个实际应用中，将路径R导入MATLAB中，编写去除冗余点程序，可看到仿真后的优化结果，得到路径R1实现缩短路径R的轨迹，如图5所示。

步骤400，采用二阶贝塞尔曲线对所述第一优化路径进行拟合，以得到所述钻锚机器人的最终钻臂路径，从而解决轨迹不平滑、可能有棱角的问题。基于二阶Bezier曲线拟合的方案，平滑后路径远离规划路径的路径点，产生的曲线更短。

一个连续函数，可将它写成若干个伯恩斯坦多项式相加的形式，并且，随着n→∞，这个多项式将一致收敛到原函数，这个就是伯恩斯坦斯的逼近性质。Bezier曲线的二阶表达式为：

B(t)＝(1-t)²*P₀+2t(1-t)*P₁+t²*P₂ 0≤t≤1

其中，P₀表示拟合曲线起始点，P₁表示拟合曲线控制点，P₂表示拟合曲线结束点，t为系数(也可以理解为百分比)，表示从0到1的变化过程；B(t)表示以点P₀为起始点、点P₂为结束点的拟合曲线。

在一个具体应用中，为了解决路径R1比较粗糙，会产生振动的问题，采用python软件编程实现对路径R1的拟合，可以得到最终的最优路径R2，如图6所示。

综上，针对RRT算法得到的路线质量不优问题，采用连线法去除冗余点的方法来优化路径。针对路径不光滑，可能有棱角的问题，使用Bezier曲线拟合法，使用MATLAB软件和python软件编程实现上述算法，可以快速实现钻锚机器人钻臂的轨迹规划，得到曲线光滑的钻臂路径。

实施例二

如图7所示，为了执行上述实施例一对应的方法，以实现相应的功能和技术效果，本实施例提供了一种钻锚机器人钻臂轨迹规划系统，包括：

模型构建模块101，用于获取钻锚机器人的三维工作区域模型、工作起始点和钻孔点。

第一路径优化模块201，用于采用快速扩展随机树算法确定所述钻锚机器人的初始钻臂路径；所述初始钻臂路径为所述钻锚机器人的钻臂，在所述三维工作区域模型中，从所述工作起始点至所述钻孔点的路径。

第二路径优化模块301，用于采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径。

第三路径优化模块401，用于采用二阶贝塞尔曲线对所述第一优化路径进行拟合，以得到所述钻锚机器人的最终钻臂路径。

实施例三

本实施例提供一种电子设备，所述电子设备包括存储器和处理器。

所述存储器用于存储计算机程序，所述处理器用于运行所述计算机程序以执行实施例一的钻锚机器人钻臂轨迹规划方法。

相对于现有技术，本发明还具有以下优点：

本发明基于RRT的路径规划算法，通过对状态空间中的采样点进行碰撞检测，能够有效地解决高维空间和复杂约束的路径规划问题。但是RRT算法的缺点是得到的路径质量不是最优且不光滑，可能有棱角。因此，针对得到的路线质量不优问题，采用连线法去除冗余点的方法来优化路径。针对路径不光滑，可能有棱角的问题，使用二阶Bezier曲线拟合法，将得到的路径规划曲线光滑处理。可以实现在煤矿井下复杂三维空间中快速准确的规划出一条最优光滑路径，实现快速准确找孔、打孔，从而提高提高锚固效率，提高综掘工作面设备的智能化程度。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

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

Claims

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

2.根据权利要求1所述的钻锚机器人钻臂轨迹规划方法，其特征在于，所述三维工作区域模型中包括障碍物区域和自由区域；

3.根据权利要求2所述的钻锚机器人钻臂轨迹规划方法，其特征在于，根据所述随机节点确定所述随机树中的近距离节点，具体包括：

4.根据权利要求1所述的钻锚机器人钻臂轨迹规划方法，其特征在于，采用连接法去除所述钻锚机器人的初始钻臂路径中的冗余点，以得到第一优化路径，具体包括：

提取所有所述优化路段，以确定第一优化路径。

5.根据权利要求1所述的钻锚机器人钻臂轨迹规划方法，其特征在于，所述二阶贝塞尔曲线的计算公式为：

B(t)＝(1-t)²*P₀+2t(1-t)*P₁+t²*P₂ 0≤t≤1

6.根据权利要求1所述的钻锚机器人钻臂轨迹规划方法，其特征在于，所述钻锚机器人的三维工作区域模型采用MATLAB软件绘制。

7.一种钻锚机器人钻臂轨迹规划系统，其特征在于，所述钻锚机器人钻臂轨迹规划系统包括：

8.一种电子设备，其特征在于，所述电子设备包括存储器和处理器；

所述存储器用于存储计算机程序，所述处理器用于运行所述计算机程序以执行权利要求1-6任一项所述的钻锚机器人钻臂轨迹规划方法。