CN112828889A - 一种六轴协作机械臂路径规划方法及系统 - Google Patents

Abstract

本发明涉及机械臂路径规划技术领域，具体涉及一种六轴协作机械臂路径规划方法及系统，所述方法包括：选取包含障碍物的二维图像，将所述二维图像中的障碍物区域进行扩大，形成机械臂的状态空间；获取机械臂在状态空间中的初始节点和目标节点，并采用RRT算法构建所述初始节点到所述目标节点的随机树；在所述状态空间中有方向性的选择状态节点，对随机树的步长进行动态调整，采用改进RRT算法确定所述初始节点到目标节点的规划路径，本发明能够避免规划结果陷入局部极小值，能够快速得到较短的规划路径。

Description

一种六轴协作机械臂路径规划方法及系统

技术领域

本发明涉及机械臂路径规划技术领域，具体涉及一种六轴协作机械臂路径规划方法及系统。

背景技术

机器人是一种可编程、能自动执行工作的机器，这种机器具备一些与人或生物相似的智能能力，如感知能力、规划能力、动作能力和协同能力。尽管从第一台机器人出现至今只有60年，可是机器人的快速发展已经超越了人们的想象。机器人是集成了机械、电子、控制、人工智能等先进技术的自动化设备，在装备制造、新材料、生物医药、智慧新能源等高新产业领域得到了广泛应用，机器人己经成为促进科技进步和社会发展的强大推动力。机器人可以代替人类从事单调重复、环境差或者危险性高的工作，不仅解放了人类的双手，也改变了人们的生活方式。

机械臂的路径规划是机器人领域的基本问题，主要是解决机器人从初始点无碰撞安全的到达目标点的运动规划，随着研究的不断深入，国内外学者已经提出了很多路径规划算法，以进行路径规划；常用的路径规划算法有人工势场法、A*算法、蚁群算法、遗传算法、RRT(Rapidly-exploring-Random-Tree，快速搜索随机树)算法、RRT*算法等。人工势场法无法应用于高维空间；蚁群算法虽完备且最优，但收敛速度慢，未得到广泛应用；遗传算法的实时性较差；基于图搜索的A*算法采用启发式搜索算法，提高了整体效率，但难以满足高速性要求的多自由度机械臂的避障搜索问题；RRT算法是一种基于随机采样的查询步进式算法，适合解决高维空间和复杂约束下的路径规划问题，且规划速度快，但随机性大；RRT*算法在RRT基础上加入代价函数，使规划的路径具有渐进最优性，但延长了搜索时间。

但由于现有技术中各类算法的不完善性，容易导致规划结果陷入局部极小值，最终得到的路径不能满足实际需要。

发明内容

本发明提供一种六轴协作机械臂路径规划方法及系统，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。

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

一种六轴协作机械臂路径规划方法，所述方法包括以下步骤：

步骤S100、选取包含障碍物的二维图像，将所述二维图像中的障碍物区域进行扩大，形成机械臂的状态空间；

步骤S200、获取机械臂在状态空间中的初始节点和目标节点，并采用RRT算法构建所述初始节点到所述目标节点的随机树；

步骤S300、在所述状态空间中有方向性的选择状态节点，对随机树的步长进行动态调整，采用改进RRT算法确定所述初始节点到目标节点的规划路径。

进一步，步骤S300中，所述对随机树的步长进行动态调整，包括：

对RRT算法添加目标点的引力系数，使得随机树扩展受到随机节点与目标节点的引力系数共同作用。

进一步，所述步骤S300包括：

步骤S301、获取机械臂在状态空间中的初始节点和目标节点，将所述初始节点作为当前节点，将所述当前节点和所述目标节点连接形成一条直线，将所述直线作为测试路径并执行步骤S302；

步骤S302、判断所述测试路径与障碍物是否存在交点，若是，则将所述测试路径作为障碍路径，并执行步骤S303；若否，则将所述测试路径作为通行路径，并执行步骤S306；

步骤S303、确定至少一个所述障碍路径与障碍物的交点，将距离所述当前节点最近的交点作为第一交点；

步骤S304、采用RRT算法连接所述当前节点和第一交点，作为通行路径；

步骤S305、使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，将所述扩展路径作为测试路径并执行步骤S302；

步骤S306、连接所述初始节点和目标节点之间的至少一条通行路径，得到所述初始节点到目标节点的规划路径。

进一步，步骤S305中，所述使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，包括：

采用人工势场法确定目标节点的引力势场，根据所述引力势场确定所述随机树的扩展方向，在所述状态空间中按所述扩展方向随机选择节点，使用动态步长遍历所述随机树，直至找到目标节点，作为所述第一交点到所述目标节点的扩展路径。

进一步，所述在所述状态空间中按所述扩展方向随机选择节点的计算公式为：

其中，ρ₁为随机点方向的步长，ρ₂为目标点方向的步长，x_goal表示目标节点，x_rand表示随机节点，x_near表示交点，x_new表示随机选择的节点。

一种六轴协作机械臂路径规划系统，所述系统包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上述任一项所述的六轴协作机械臂路径规划方法。

本发明的有益效果是：本发明公开一种六轴协作机械臂路径规划方法及系统，本发明通过获取机械臂状态空间中的初始节点，并构建随机树，通过进行有方向性的在所述的状态空间中选择随机节点，遍历所述随机树得到规划路径。可避免规划结果陷入局部极小值，能够快速得到较短的规划路径。

附图说明

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

图1是本发明实施例中六轴协作机械臂路径规划方法的流程示意图；

图2是本发明实施例中随机树的节点选择原理图；

图3是本发明实施例中运动学正逆解关系的转换示意图；

图4是本发明实施例中运动学模型的示意图；

图5是本发明实施例中采用BRT算法进行路径规划的仿真示意图；

图6是本发明实施例中采用A*算法进行路径规划的仿真示意图；

图7是本发明实施例中采用模糊逻辑算法进行路径规划的仿真示意图；

图8是本发明实施例中采用遗传算法进行路径规划的仿真示意图；

图9是本发明实施例中采用人工势场算法进行路径规划的仿真示意图；

图10是本发明实施例中采用RRT*算法进行路径规划的仿真示意图。

具体实施方式

以下将结合实施例和附图对本申请的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本申请的目的、方案和效果。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

参考图1，如图1所示为本申请实施例提供的一种六轴协作机械臂路径规划方法，所述方法包括以下步骤：

步骤S100、选取包含障碍物的二维图像，将所述二维图像中的障碍物区域进行扩大，形成机械臂的状态空间；

如图1所示，在一个实施例中，所述二维图像为包含机械臂工作区域的图像，将机械臂工作区域内其他的设备作为障碍物；选择图片中带有略微大的黑色区域的图片，并将黑色区域作为障碍物区域,考虑到实际操作环境的限制，本实施例将转为二维图像，使用MATLAB在二维环境中进行仿真，为便于对障碍物的识别，本实施例中，将障碍物区域的面积扩大原始大小的1.5至2倍。

步骤S200、获取机械臂在状态空间中的初始节点和目标节点，并采用RRT算法构建所述初始节点到所述目标节点的随机树；

具体地，将关节在状态空间中初始位置所在的状态节点作为初始节点，将关节在状态空间中目标位置所在的状态节点作为目标节点；

步骤S300、在所述状态空间中有方向性的选择状态节点，对随机树的步长进行动态调整，采用改进RRT算法确定所述初始节点到目标节点的规划路径。

本实施例中，通过获取机械臂状态空间中的初始节点，并构建随机树，通过引进的引力思想，进行有方向性的在所述的状态空间中选择随机节点，遍历所述随机树，找到设定节点，并按照改进步长扩展出新的状态节点，并判断扩展过程中是否检测到碰撞，直至迭代搜索到达目标节点，以此将所述状态节点连接起来，得到规划路径。可避免规划结果陷入局部极小值，能够快速得到较短的规划路径，从而满足实际需要。

作为上述实施例的优选，步骤S300中，所述对随机树的步长进行动态调整，包括：

对RRT算法添加目标点的引力系数，使得随机树扩展受到随机节点与目标节点的引力系数共同作用。

本实施例中，通过调整引力系数获得不同长度的步长，基于大步长快速扩展，小步长确定收敛路径的思想，提高搜索效率。

参考图2，在一个优选的实施例中，所述步骤S300包括：

步骤S301、获取机械臂在状态空间中的初始节点和目标节点，将所述初始节点作为当前节点，将所述当前节点和所述目标节点连接形成一条直线，将所述直线作为测试路径并执行步骤S302；

步骤S302、判断所述测试路径与障碍物是否存在交点，若是，则将所述测试路径作为障碍路径，并执行步骤S303；若否，则将所述测试路径作为通行路径，并执行步骤S306；

步骤S303、确定至少一个所述障碍路径与障碍物的交点，将距离所述当前节点最近的交点作为第一交点；

步骤S304、采用RRT算法连接所述当前节点和第一交点，作为通行路径；

步骤S305、使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，将所述扩展路径作为测试路径并执行步骤S302；

步骤S306、连接所述初始节点和目标节点之间的至少一条通行路径，得到所述初始节点到目标节点的规划路径。

由基本RRT算法流程可知，它的扩展步长是固定的，并没有考虑采样点的分布区域。这难免会造成局部扩展速度不高的结果，本实施例中，采用贪婪策略使得随机树的步长变短，加快了随机树的扩展，从而适应更加复杂的环境，提高局部扩展速度。

具体地，利用目标节点的引力势场进行有方向性的采样，不断生成新的随机节点x_rand，并且不断的检测此随机点节点和目标节点之间是否存在障碍物，若不存在障碍物，直接连接该随机节点和目标节点，结束循环；若仍然存在障碍物，则连接下一个交点，直至到达目标节点，得到最佳路径。通过贪婪策略，引力思想，自适应步长产生新的节点以及不断尝试连接目标点，不仅提高了局部扩展速度，而且减少了许多冗余采样点的数量。

作为上述实施例的优选，步骤S305中，所述使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，包括：

采用人工势场法确定目标节点的引力势场，根据所述引力势场确定所述随机树的扩展方向，在所述状态空间中按所述扩展方向随机选择节点，使用动态步长遍历所述随机树，直至找到目标节点，作为所述第一交点到所述目标节点的扩展路径。

作为上述实施例的优选，所述在所述状态空间中按所述扩展方向随机选择节点的计算公式为：

其中，ρ₁为随机点方向的步长，ρ₂为目标点方向的步长，x_goal表示目标节点，x_rand表示随机节点，x_near表示交点，x_new表示随机选择的节点，x_init表示初始节点。

现有技术中的RRT算法随机性比较明显，规划出来的路径非常曲折，不适合实际使用中的路径规划，本实施例中，加入目标引力思想后的RRT算法能有效解决路径随机性强的问题。

在一个优选的实施例中，所述方法还包括：

建立机械臂的坐标系，运用D-H建模法创建运动学模型，通过正逆运动学算法确定与所述规划路径对应的机械臂各关节的旋转角度。

参考图3和图4，在一个优选的实施例中，所述建立机械臂的坐标系包括：

将机械臂中关节的运动轴正方向标定为Z轴，将垂直于Z轴并指向离开Z轴的方向标定为X轴；根据右手定则确定Y轴，从而建立所述机械臂的坐标系。

在一个优选的实施例中，所述运用D-H建模法创建运动学模型包括：

获取机械臂的连杆参数以及每个连杆的运行角度范围，根据所述连杆参数以及每个连杆的运行角度范围建立机械臂运动学模型；

所述连杆参数包括：连杆的长度、连杆的角度、连杆的偏距和关节角，所述运行角度范围即协作机器人的操作空间。

在一个实施例中，所述机械臂的连杆参数如下表所示。

连杆	连杆的长度	连杆的角度	连杆的偏距	关节角
					连杆1	10	Pi/2	40	Pi/2
连杆2	40	0	0	Pi/2
					连杆3	80	0	0	Pi/2
连杆4	0	Pi/2	0	Pi/2
					连杆5	0	Pi/2	80	0
连杆6	0	0	0	0

在一个实施例中，所述机械臂的正逆运动学分析包括：

正逆运动学分析：己知机器人各个关节的连杆长度和转动角度，计算机器人末端执行器相对于参考坐标系的位姿(位置和姿态)，双臂机器人的正向运动学表达式为：

即：

式中，n_x，n_y，n_z，o_x，o_y，o_z，a_x，a_y，a_z，p_x，p_y，p_z分别代表旋转角度和位置；

逆运动学分析：已知机器人各个连杆长度，指定末端执行器的目标位姿，来反求到达目标位姿时各关节的旋转角度。

机器人逆运动学分析主要存在两方面问题：解的存在性、和多重解问题操作臂的工作空间决定逆解是否存在，只有当期望的末端位姿在工作空间内部时，才存在运动学逆解。对于机械臂来说，机械臂可以从多个方位达到指定位姿，所以逆解可能会产生很多组，本实施例选择“最短行程”的解。

本发明提供的六轴协作机械臂路径规划方法，根据D-H参数数据，建立机械臂运动学模型，并在MATLAB中进行了二维空间的仿真，实验证明，所改进的算法最终所获得的目标路径与传统的RRT算法和其他算法相比，综合结果最优，在同一图像下，抽取一组图像对比和20组数据平均值对比如图5至图10所示。根据实验数据可知，本发明提供的实施例获得的路径为最短路径，且时间最短。

数据对比如下表所示。

RRT算法

A*算法

模糊逻辑

遗传算法

人工势场法

改进算法

Time

3.85e+01

2.5e+02

5.10e+01

4.05e+01

1.05e+01

2.25e+01

Length

8.25e+02

7.91e+01

8.95e+01

957

7.96e+02

7.50e+01

与图1的方法相对应，本发明实施例还提供一种六轴协作机械臂路径规划系统，所述系统包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上述任一实施例所述的六轴协作机械臂路径规划方法。

上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。

所述处理器可以是中央处理单元(Central-Processing-Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital-Signal-Processor，DSP)、专用集成电路(Application-Specific-Integrated-Circuit，ASIC)、现场可编程门阵列(Field-Programmable-Gate-Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述六轴协作机械臂路径规划系统的控制中心，利用各种接口和线路连接整个六轴协作机械臂路径规划系统可运行装置的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述六轴协作机械臂路径规划系统的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart-Media-Card，SMC)，安全数字(Secure-Digital，SD)卡，闪存卡(Flash-Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

尽管本申请的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求，考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本申请的预定范围。此外，上文以发明人可预见的实施例对本申请进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本申请的非实质性改动仍可代表本申请的等效改动。

Claims (6)

1.一种六轴协作机械臂路径规划方法，其特征在于，所述方法包括以下步骤：

步骤S100、选取包含障碍物的二维图像，将所述二维图像中的障碍物区域进行扩大，形成机械臂的状态空间；

步骤S200、获取机械臂在状态空间中的初始节点和目标节点，并采用RRT算法构建所述初始节点到所述目标节点的随机树；

步骤S300、在所述状态空间中有方向性的选择状态节点，对随机树的步长进行动态调整，采用改进RRT算法确定所述初始节点到目标节点的规划路径。

2.根据权利要求1所述的一种六轴协作机械臂路径规划方法，其特征在于，步骤S300中，所述对随机树的步长进行动态调整，包括：

对RRT算法添加目标点的引力系数，使得随机树扩展受到随机节点与目标节点的引力系数共同作用。

3.根据权利要求2所述的一种六轴协作机械臂路径规划方法，其特征在于，所述步骤S300包括：

步骤S301、获取机械臂在状态空间中的初始节点和目标节点，将所述初始节点作为当前节点，将所述当前节点和所述目标节点连接形成一条直线，将所述直线作为测试路径并执行步骤S302；

步骤S302、判断所述测试路径与障碍物是否存在交点，若是，则将所述测试路径作为障碍路径，并执行步骤S303；若否，则将所述测试路径作为通行路径，并执行步骤S306；

步骤S303、确定至少一个所述障碍路径与障碍物的交点，将距离所述当前节点最近的交点作为第一交点；

步骤S304、采用RRT算法连接所述当前节点和第一交点，作为通行路径；

步骤S305、使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，将所述扩展路径作为测试路径并执行步骤S302；

步骤S306、连接所述初始节点和目标节点之间的至少一条通行路径，得到所述初始节点到目标节点的规划路径。

4.根据权利要求3所述的一种六轴协作机械臂路径规划方法，其特征在于，步骤S305中，所述使用贪婪策略，以所述第一交点作为根节点向外扩展，得到所述第一交点到所述目标节点的扩展路径，包括：

采用人工势场法确定目标节点的引力势场，根据所述引力势场确定所述随机树的扩展方向，在所述状态空间中按所述扩展方向随机选择节点，使用动态步长遍历所述随机树，直至找到目标节点，作为所述第一交点到所述目标节点的扩展路径。

5.根据权利要求4所述的一种六轴协作机械臂路径规划方法，其特征在于，所述在所述状态空间中按所述扩展方向随机选择节点的计算公式为：

其中，ρ₁为随机点方向的步长，ρ₂为目标点方向的步长，x_goal表示目标节点，x_rand表示随机节点，x_near表示交点，x_new表示随机选择的节点。

6.一种六轴协作机械臂路径规划系统，其特征在于，所述系统包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如权利要求1至5任一项所述的六轴协作机械臂路径规划方法。

Images