一种动态导向RRT*的机械臂路径规划方法技术

本发明专利技术公开了一种动态导向RRT*的机械臂路径规划方法，根据环境信息和生长树当前生长状况，引入环境复杂系数，获得自适应概率p

【技术实现步骤摘要】
一种动态导向RRT*的机械臂路径规划方法


[0001]本专利技术属于机械臂路径规划领域，尤其涉及基于一种动态导向RRT*的机械臂路径规划方法。

技术介绍

[0002]智能机械臂在各行各业有着广泛应用，如装配机械手、农业果实采摘机器人、医疗机器人等。目前农业领域落叶类药采摘机器人研究较少，比如金银花采摘机器人，在非结构化环境中工作，作业目标复杂，机器人采摘过程中需要安全通过环境中的障碍物，到达目标点。避障路径规划是机械臂技术中的一个重要内容，其目的是要实现机械臂能避开环境中的障碍物，完成整个规划过程，具备自主路径规划决策功能。
[0003]迄今为止，机械臂的路径规划方法主要分为基于搜索的路径规划算法、基于采样的路径规划算法、智能仿生算法。基于搜索的路径规划算法主要有A*算法、Dijkstra算法、D*算法、人工势场法；基于采样的路径规划算法有PRM算法、RRT系列算法；智能仿生路径规划算法有神经网络算法、蚁群算法、遗传算法等。然而上述方法各有其优缺点，如A*算法耗费时间长，人工势场法易陷入局部极小值等。基于搜索的路径规划算法，由于计算规模大，不适合在高维空间中应用，而采摘机械臂通常拥有6个自由度，机械臂在运动过程中运动学关系极为复杂，此类算法无法运用。智能仿生算法如强化学习，需要依赖训练成果，机械臂实际运行中时容易发生局部抖动震荡，表现不稳定。RRT算法减少了计算量，能够生成可供机械臂运行的可行路径，但是由于其算法存在无效节点多，路径质量差，使得机械臂运行过程中不能精确避障，浪费时间，机械臂常常会在无效空间中运动。
[0004]综上所述，亟需一种机械臂的路径规划方法，能够有效解决机械臂运动过程无效运动过多、运行路径质量较差、运行时间较长等技术问题。

技术实现思路

[0005]为了解决现有技术中存在的不足，本申请提出了一种动态导向RRT*的机械臂路径规划方法，能够解决机械臂运动过程无效运动过多、运行路径质量较差、运行时间较长等技术问题。
[0006]本专利技术所采用的技术方案如下：
[0007]一种动态导向RRT*的机械臂路径规划方法，包括如下步骤：
[0008]步骤1，获取机械臂工作的三维环境信息，在三维环境信息中，获得起点X
int
、目标点X
goal
和障碍物三维坐标；
[0009]步骤2，基于步骤1中的环境信息实现动态导向RRT*的机械臂路径规划，过程如下：
[0010]步骤2.1，基于步骤1中的环境信息，进行随机树生长，初始化生长步长r、自适应范围基本距离d、自适应基本概率p；
[0011]步骤2.2，计算当前生长树的自适应概率p
v
；
[0012]步骤2.3，基于自适应概率p
v
和垂面采样，获得生长树的新节点X
new
；
[0013]步骤2.4，在自适应范围内对新节点X
new
进行重选父节点，对自适应范围内其他节点以节点X
new
为父节点重连接；对重选父节点和重连接更新的路径进行碰撞检测，生成从目标点向起点的无碰撞路径，完成机械臂路径规划。
[0014]进一步，采用图像采集装置获取农业采摘机械臂工作的三维环境信息，并在三维空间C中选取起点X
int
、目标点X
goal
和障碍物。
[0015]进一步，所述步骤2.2中计算当前生长树的自适应概率p
v
的方法为：
[0016]遍历生长树已生成节点集合，找出距离目标点最近的节点并计算该节点的环境相对复杂系数h
nearest
，
[0017]当前生长树的自适应概率p
v
表示为：
[0018][0019]其中，p为自适应基本概率。
[0020]进一步，距离目标点最近的节点的环境相对复杂系数h
nearest
表示为：
[0021][0022]其中，为距离目标点最近节点处到三维空间C中所有障碍物的中心直线距离的和，S
int
为三维空间C中起点X
int
到所有障碍物的中心直线距离的和，S
goal
为三维空间C中目标点X
goal
到所有障碍物的中心直线距离的和，S
mid
为三维空间C中起点到目标点连线中点X
mid
到所有障碍物的中心直线距离的和，S为S
int
、S
goal
、S
mid
三者平均数。
[0023]进一步，所述步骤2.3中获得生长树的新节点X
new
的方法为：
[0024]步骤2.3.1，随机生成百分数，若不大于当前生长树的自适应概率p
v
，则将随机节点X
rand
设置为目标点X
goal
并转到步骤2.3.4；若大于当前生长树的自适应概率p
v
，则转到步骤2.3.2；
[0025]步骤2.3.2，根据随机采样函数，在三维空间C中随机采样生成随机节点X
rand
；
[0026]步骤2.3.3，搜寻距离随机节点X
rand
最近的节点X
nearest
，基于X
nearest
和X
goal
的连线进行垂面采样，将有效的随机节点归入节点集合T中；
[0027]步骤2.3.4，遍历生长树已生成节点集合T，搜寻距离随机节点X
rand
最近的节点X
nearest
，计算得最近节点X
nearest
处的变生长步长r*；
[0028]步骤2.3.5，以节点X
nearest
处的生长步长r*，自节点X
nearest
向随机节点X
rand
方向生长该步长，生成新节点X
new
。
[0029]进一步，所述步骤2.3.3中垂面采样的方法为：
[0030]连接节点X
nearest
和目标点X
goal
，过X
nearest
作X
nearest
和X
goal
连线的垂线，获得过节点X
nearest
的垂面，此垂面将三维空间C划分为两个空间，若随机节点X
rand
与目标点X
goal
在同一空间中，该随机节点有效，将有效的随机节点归入节点集合T中；若随机节点X
rand
与目标点X
goal
不在同一空间中，则舍去该随机节点，并重新采样。
[0031]进一步，计算最近节点X
nearest
处的生长步长r*，表示为：
[0032][0033]本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种动态导向RRT*的机械臂路径规划方法，其特征在于，包括如下步骤：步骤1，获取机械臂工作的三维环境信息，在三维环境信息中，获得起点X
int
、目标点X
goal
和障碍物三维坐标；步骤2，基于步骤1中的环境信息实现动态导向RRT*的机械臂路径规划，过程如下：步骤2.1，基于步骤1中的环境信息，进行随机树生长，初始化生长步长r、自适应范围基本距离d、自适应基本概率p；步骤2.2，计算当前生长树的自适应概率p
v
；步骤2.3，基于自适应概率p
v
和垂面采样，获得生长树的新节点X
new
；步骤2.4，在自适应范围内对新节点X
new
进行重选父节点，对自适应范围内其他节点以节点X
new
为父节点重连接；对重选父节点和重连接更新的路径进行碰撞检测，生成从目标点向起点的无碰撞路径，完成机械臂路径规划。2.根据权利要求1所述的一种动态导向RRT*的机械臂路径规划方法，其特征在于，采用图像采集装置获取农业采摘机械臂工作的三维环境信息，并在三维空间C中选取起点X
int
、目标点X
goal
和障碍物。3.根据权利要求1所述的一种动态导向RRT*的机械臂路径规划方法，其特征在于，所述步骤2.2中计算当前生长树的自适应概率p
v
的方法为：遍历生长树已生成节点集合，找出距离目标点最近的节点并计算该节点的环境相对复杂系数h
nearest
，当前生长树的自适应概率p
v
表示为：其中，p为自适应基本概率。4.根据权利要求3所述的一种动态导向RRT*的机械臂路径规划方法，其特征在于，距离目标点最近的节点的环境相对复杂系数h
nearest
表示为：其中，为距离目标点最近节点处到三维空间C中所有障碍物的中心直线距离的和，S
int
为三维空间C中起点X
int
到所有障碍物的中心直线距离的和，S
goal
为三维空间C中目标点X
goal
到所有障碍物的中心直线距离的和，S
mid
为三维空间C中起点到目标点连线中点X
mid
到所有障碍物的中心直线距离的和，S为S
int
、S
goal
、S
mid
三者平均数。5.根据权利要求1所述的一种动态导向RRT*的机械臂路径规划方法，其特征在于，所述步骤2.3中获得生长树的新节点X
new
的方法为：步骤2.3.1，随机生成百分数，若不大于当前生长树的自适应概率p
v
，则将随机节点X
rand
设置为目标点X
goal
并转到步骤2.3.4；若大于当前生长树的自适应概率p
v
，则转到步骤2.3.2；步骤2.3.2，根据随机采样函数，在三维空间C中随机采样生成随机节点X
rand
；步骤2.3.3，搜寻距离随机节点X
rand
最近的节点X
nearest
，基于X
nearest
和X
goal
的连线进行垂
面采样，将有效的随机节点归入节点集合T中；步骤2.3....

【专利技术属性】
技术研发人员：顾寄南，卢宝勇，范天浩，王文波，季晨，侯征辉，左宇，
申请(专利权)人：江苏大学，
类型：发明
国别省市：