CN115922716A

CN115922716A - 一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划

Info

Publication number: CN115922716A
Application number: CN202211605825.7A
Authority: CN
Inventors: 刘银华; 杨玉芳; 赵文政; 李晓甜; 许郭樑; 王雪琪
Original assignee: SAIC General Motors Corp Ltd; University of Shanghai for Science and Technology
Current assignee: SAIC General Motors Corp Ltd; University of Shanghai for Science and Technology
Priority date: 2022-12-14
Filing date: 2022-12-14
Publication date: 2023-04-07

Abstract

本发明提出一种融合工艺知识的双向RRT‑connect算法实现了工业机器人快速路径规划，在改进RRT算法的基础上，融合工艺知识，对工业机器人进行了运动规划。具体的该运动规划是在融合工艺知识的前提下，引入专家机制改善工业机器人的任务规划质量，同时结合局部扩展机制渐进最优思想对目标节点快速扩展随机树进行优化，缩短搜索时间和路径代价，使得搜索路径朝渐进最优解收敛。以工业机器人末端的位姿作为作业空间中的一个节点，并结合逆运动学与相关工艺知识求解关节姿态，之后，利用求解的角度进行碰撞检测。改进后的算法与工艺知识相融合，保证规划的稳定性的同时实现在短的搜索时间下渐优的避障路径，为工业机器人在不同场景下的应用提供技术支持。

Description

一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划

技术领域

本发明涉及工艺机器人的运动规划领域，尤其涉及一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划。

背景技术

工业机器人广泛应用于工业生产、军事等各个领域，在这个领域里，路径规划发挥着重大的作用。目前常见的路径规划算法主要有三种：以A*算法为主的在地图上直接进行搜索的直接搜索算法；以蚁群算法，遗传算法为主的启发式算法；以RRT、PRM为主的基于采样策略的算法等。前两种策略虽然可以找到全局最优解，但是不能满足复杂环境和高维空间的路径规划要求，1988年，La Valle提出快速扩展随机树算法(rapidly exploringrandom tree,RRT)，此种方法适用于高维空间下的机器人运动规划问题，具有概率完整性，快速的探索速度，但RRT算法的搜索具有盲目性，收敛速度慢，随机采样生成的路径会产生许多不必要节点，从而导致路径不平滑且不连续。此外，由于工艺约束导致现有的路径搜索算法的求解结果难以直接应用于实际制造生产中。随着2035年制造业将普及数字化，因此，提出一种融合工艺知识的基于改进快速扩展树(RRT)算法的工业机器人运动规划方法。

发明内容

本发明的目的在于提出一种融合工艺知识的改进RRT-connect算法的工业机器人运动规划方法。该方法适用于各种工业现场，同时，针对现有技术的不足，从质量和效率两个方向对RRT进行了改进，改进后的算法在规划时间以及各种路径代价上都优于传统RRT算法，同时提高了机械臂的平稳性。

为达到上述目的，本发明提出一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，包括以下步骤：

S101：基于工艺知识，确定机械臂初始位姿与目标姿态可行解集、障碍物信息以及工业机械臂的参数信息；

S102：基于动态碰撞检测算法对机械臂始末点位姿集合内两两姿态进行碰撞检测，若存在无冲突的轨迹则选取时间最短的姿态作为最终的始末姿态，若不存在无冲突的轨迹则转到S103；

S103：首先以传统RRT-connect算法作为基础，设置搜索步长作为每一步扩展的根据，设置终止阈值作为起点和终点两棵随机树的最终连接范围；然后基于专家机制快速筛选出搜索空间中的新节点；

S104：工业机器人按照上述策略分别从初始位置和终点位置出发，生成两棵随机树，之后依据终止阈值判断这两棵随机树是否处于可直接连接的范围，若低于终止阈值，停止搜索，将其连接，否则机械臂仍按照上述策略继续执行，直至低于终止阈值；

S105：按照次序从初始位置开始输出各路径点的位置和姿态作为一条无碰路径。

进一步的，所述S101是基于机器人末端所搭载的不同工具对不同类型的工艺规范，确定不同工业特征的最佳角度(特征量在Z轴的矢量)和第二矢量(特征量在Y轴的矢量)；对于规划的起始位置，分别基于逆运动学和碰撞检测算法求解无碰的可行位姿W和相应的旋转矩阵T；

为了保证传感器能够采集相应数据，计算T的第3列与工艺特征第二矢量(Y轴)之间的夹角θ，若θ在满足的工艺角度范围内，则保存此姿态，若不满足，舍弃此姿态；最终确定满足特定工艺要求的起始位姿的集合start、goal。

进一步的，在所述S102中，将满足工艺要求的机械臂始末位姿集合，进行两两组合，进一步通过五次多项式拟合得到机器人运动轨迹并离散成多组构型，最终基于碰撞检测算法确定两姿态间是否发生冲突；若存在无冲突的轨迹则选取时间最短的始末姿态作为无冲突的轨迹姿态，反之，则转到S103。

进一步的，在所述S103中，直接在笛卡尔坐标系下对采样点(x,y,z,i,j,k)进行搜索；为了降低搜索空间的搜索量，将采样点分解为两个搜索空间—坐标空间(x,y,z)和姿态空间(i,j,k)；并采用随机采样的方式对坐标空间值进行搜索；为了保障采样点的有效性，将随机采样和专家机制采样相融合，实现对于姿态空间的选取；专家机制的采样值可设置为：(1,0,0),(-1，0，0)，(0，1，0)，(0，-1，0),(0，0，1)，(0，0，-1)，((i₁+i₂)/2,(j₁+j₂)/2,(k₁+k₂)/2)，并根据概率选取相应的采样点。

进一步的，在所述S104中，将确定的start和goal位姿作为RRT-connect算法运动规划的起始姿态，最后，利用双向RRT进行规划，包括以下步骤：

步骤1：在初始化的基础上，分别从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2；

步骤2：在搜索空间上选择点q_rand1和q_rand2，如果q_rand1和q_rand2落在无障碍区间，那么遍历tree1和tree2找到离随机点最近的节点q_nearest1和q_nearest2；

步骤3：判断q_rand1和q_nearest1，q_rand2与q_nearest2连线上是否会发生碰撞:；如果连线之间不会发生碰撞，那么，判断q_rand1和q_nearest1，q_rand2与q_nearest2之间的距离是否小于扩展步长，如果q_nearest和q_rand之间的欧式距离小于扩展步长，则将随机点q_rand添加到扩展树上；反之，在最近节点和随机节点的方向上取扩展步长长度的点q_new添加到扩展树上，之后将q_rand添加到扩展树上；如果q_rand1和q_nearest1，q_rand2与q_nearest2连线上发生碰撞，则执行步骤2；同时，为了获取低成本的初始路径，计算路径与其成本C：

C＝q_nearest1·cost+q_nearest2·cost+Cost(q_rand1，q_nearest1)+Cost(q_rand1，q_nearest2)

当C>Cmax时，q_rand被拒绝，重新进行采样，执行步骤2；

步骤4：判断tree1和tree2之间的距离阈值是否小于终止阈值：如果两个树之间的距离D_tree1tree2大于终止阈值，重复执行步骤2和步骤3，如果D_tree1tree2小于阈值，则从相遇的tree1和tree2的节点上，分别反向追踪，获取到起始点和目标点的节点集，并保存路径。

进一步的，在所述步骤4中，对两随机树tree1和tree2的末端终点进行碰撞检测，若无冲突则连接两节点，完成规划任务；若产生冲突，则继续搜索新节点进行。

与现有技术相比，本发明的优势之处在于：本发明在改进RRT算法的基础上，融合工艺知识，对工业机器人进行了运动规划。具体的在基于工业机器人所处工业环境，应用相应的工艺知识，确定机器人初始位姿集合。根据初始位姿集合作为空间节点进行两两动态碰撞，同时，在双向RRT采样的过程中，引入专家机制，使工业机器人在工作空间中找到无碰路径的同时提高搜索效率并保证路径质量。

附图说明

图1融合工艺知识的改进RRT算法的工业机器人运动规划流程图；

图2改进RRT算法中局部扩展机制原理示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案作进一步地说明。

本发明提出一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，如图1所示，包括以下步骤：

S101：基于机器人末端所搭载的不同工具对不同类型的工艺规范，确定不同工业特征的最佳角度(特征量在Z轴的矢量)和第二矢量(特征量在Y轴的矢量)。对于规划的起始位置，分别基于逆运动学和碰撞检测算法求解无碰的可行位姿W和相应的旋转矩阵T。为了保证传感器能够采集相应数据，计算T的第3列与工艺特征第二矢量(Y轴)之间的夹角θ，若θ在满足的工艺角度范围内，则保存此姿态，若不满足，舍弃此姿态。最终确定满足特定工艺要求的起始位姿的集合start、goal。

S102：基于动态碰撞检测算法对机械臂始末点位姿集合内两两姿态进行碰撞检测，若存在无冲突的轨迹则选取时间最短的姿态作为最终的始末姿态，若存在冲突的轨迹则转到S103。

S103：首先以传统RRT-connect算法作为基础，设置合适的搜索步长作为每一步扩展的根据，设置合适的终止阈值作为起点和终点两棵随机树的最终连接范围。然后基于专家机制快速筛选出搜索空间中的新节点。

进一步的，S103中直接在笛卡尔坐标系下对采样点(x,y,z,i,j,k)进行搜索。为了降低搜索空间的搜索量，本文将采样点分解为两个搜索空间—坐标空间(x,y,z)和姿态空间(i,j,k)。并采用随机采样的方式对坐标空间值进行搜索；为了保障采样点的有效性，本文将随机采样和专家机制采样相融合，实现对于姿态空间的选取。专家机制的采样值可设置为：(1,0,0),(-1，0，0)，(0，1，0)，(0，-1，0),(0，0，1)，(0，0，-1)，((i₁+i₂)/2,(j₁+j₂)/2,(k₁+k₂)/2)，并根据概率选取相应的采样点。

S104：将所确定的start和goal位姿作为RRT-connect算法运动规划的起始姿态。最后，利用双向RRT进行规划，步骤如下：

1)如图2所示，分别从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2；

2)根据S101的采样方式，在搜索空间上选择点q_rand1和q_rand2，如果q_rand1和q_rand2落在无障碍区间，那么遍历tree1和tree2找到离随机点最近的节点q_nearest1和q_nearest2；

3)判断q_rand1和q_nearest1，q_rand2与q_nearest2连线上是否会发生碰撞。如果连线之间不会发生碰撞，那么，判断q_rand1和q_nearest1，q_rand2与q_nearest2之间的距离是否小于扩展步长，如果q_nearest和q_rand之间的欧式距离小于扩展步长，则将随机点q_rand添加到扩展树上。反之，在最近节点和随机节点的方向上取扩展步长长度的点q_new添加到扩展树上，之后将q_rand添加到扩展树上；如果q_rand1和q_nearest1，q_rand2与q_nearest2连线上发生碰撞，则执行(2)；同时，为了获取低成本的初始路径，计算路径与其成本C：

当C>Cmax时，q_rand被拒绝，重新进行采样，执行步骤2)。

4)判断tree1和tree2之间的距离阈值是否小于终止阈值：如果两个树之间的距离D_tree1tree2大于终止阈值，重复执行步骤2)和步骤3)。如果D_tree1tree2小于阈值，则从相遇的tree1和tree2的节点上，分别反向追踪，获取到起始点和目标点的节点集，并保存路径。

上述仅为本发明的优选实施例而已，并不对本发明起到任何限制作用。任何所属技术领域的技术人员，在不脱离本发明的技术方案的范围内，对本发明揭露的技术方案和技术内容做任何形式的等同替换或修改等变动，均属未脱离本发明的技术方案的内容，仍属于本发明的保护范围之内。

Claims

1.一种融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，包括以下步骤：

2.根据权利要求1所述的融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，所述S101是基于机器人末端所搭载的不同工具对不同类型的工艺规范，确定不同工业特征的最佳角度(特征量在Z轴的矢量)和第二矢量(特征量在Y轴的矢量)；对于规划的起始位置，分别基于逆运动学和碰撞检测算法求解无碰的可行位姿W和相应的旋转矩阵T；

3.根据权利要求2所述的融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，在所述S102中，将满足工艺要求的机械臂始末位姿集合，进行两两组合，进一步通过五次多项式拟合得到机器人运动轨迹并离散成多组构型，最终基于碰撞检测算法确定两姿态间是否发生冲突；若存在无冲突的轨迹则选取时间最短的始末姿态作为无冲突的轨迹姿态，反之，则转到S103。

4.根据权利要求3所述的融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，在所述S103中，直接在笛卡尔坐标系下对采样点(x,y,z,i,j,k)进行搜索；为了降低搜索空间的搜索量，将采样点分解为两个搜索空间—坐标空间(x,y,z)和姿态空间(i,j,k)；并采用随机采样的方式对坐标空间值进行搜索；为了保障采样点的有效性，将随机采样和专家机制采样相融合，实现对于姿态空间的选取；专家机制的采样值可设置为：(1,0,0),(-1，0，0)，(0，1，0)，(0，-1，0),(0，0，1)，(0，0，-1)，((i₁+i₂)/2,(j₁+j₂)/2,(k₁+k₂)/2)，

并根据概率选取相应的采样点。

5.根据权利要求4所述的融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，在所述S104中，将确定的start和goal位姿作为RRT-connect算法运动规划的起始姿态，最后，利用双向RRT进行规划，包括以下步骤：

步骤3：判断q_rand1和q_nearest1，q_rand2与q_nearest2连线上是否会发生碰撞:；如果连线之间不会发生碰撞，那么，判断q_rand1和q_nearest1，q_rand2与q_nearest2之间的距离是否小于扩展步长，如果q_nearest和q_rand之间的欧式距离小于扩展步长，则将随机点q_rand添加到扩展树上；反之，在最近节点和随机节点

的方向上取扩展步长长度的点q_new添加到扩展树上，之后将q_rand添加到扩展树上；如果q_rand1和q_nearest1，q_rand2与q_nearest2连线上发生碰撞，则执行步骤2；同时，为了获取低成本的初始路径，计算路径与其成本C：

C＝q_nearest1.cost+q_nearest2.cost+Cost(q_rand1,q_nearest1)+Cost(q_rand1,q_nearest2)

当C>Cmax时，q_rand被拒绝，重新进行采样，执行步骤2；

6.根据权利要求5所述的融合工艺知识的双向RRT-connect算法实现了工业机器人快速路径规划，其特征在于，在所述步骤4中，对两随机树tree1和tree2的末端终点进行碰撞检测，若无冲突则连接两节点，完成规划任务；若产生冲突，则继续搜索新节点进行。