一种结合动态步长RRT*算法和势场法的路径规划方法与流程

文档序号:23142008发布日期:2020-12-01 13:20阅读:来源:国知局

技术特征:

1.一种结合动态步长rrt*算法和势场法的路径规划方法,其特征在于,包括:

s1、确定机器人的起始点q_start、目标点q_goal的位置坐标,确定障碍物的位置和轮廓;确定rrt*基准步长ε;

如果从q_start到q_goal的直线路径没有碰触障碍物,则规划的路径为从q_start到q_goal的直线段;否则,将机器人的起始点q_start作为rrt*算法中扩展树的根节点,根据步骤s2-s8确定规划路径:

s2、在机器人运动区域内通过随机采样的方式获取随机状态点q_random,在扩展树上查找距随机状态点q_random距离最近的节点q_nearest;

s3、计算目标点对最近点q_nearest的引力,所述引力的大小为:

其中α表示引力增益系数,ρ(q_nearest,q_goal)为树节点q_nearest的到目标点q_goal的欧氏距离,d*为预设的树节点到目标点的距离阈值;

的方向为从最近点q_nearest指向目标点q_goal的向量;

计算障碍物对最近点q_nearest的斥力,所述斥力的大小为:

其中β为斥力增益系数,dis=ρ(q_nearest,obstacle)为树节点q_nearest到障碍物obstacle轮廓的最近欧氏距离,ρ0为预设的障碍物斥力作用范围距离;

的方向为从障碍物obstacle轮廓上的最近点指向树节点q_nearest的向量,所述障碍物obstacle轮廓上的最近点为障碍物obstacle轮廓上到树节点q_nearest欧氏距离最小的点;

计算q_nearest受到的合力:

根据合力方向的单位向量与树节点q_nearest至随机状态点q_random方向的单位向量的合向量方向作为扩展树新节点q_new的搜索扩展方向

计算扩展树的扩展步长εnew:

其中k为调节因子,控制步长的调整幅度,0<k<1;ρsafe为预设的树节点与障碍物间的安全距离;

s4、扩展树以最近点q_nearest为父节点沿搜索方向扩展长度εnew得到新节点q_new,新节点q_new的位置:

其中是节点q_nearest指向随机状态点q_random方向的单位向量,是合力f_total(q_nearest)的单位向量;

判断新节点与原扩展树最近点q_nearest之间的直线段是否会碰触障碍物,若会碰触障碍物则跳转到步骤s2重新获取随机状态点q_random,否则将新节点q_new添加到扩展树中形成新的扩展树;

s5、重新确定新节点q_new的父节点;

s6、对扩展树重新布线;

s7、如果新节点q_new到q_goal的路径没有碰触障碍物,且直线距离小于ε,将目标点q_goal添加到扩展树,其父节点为q_new,扩展树扩展结束;否则跳转至步骤s2进行下一次扩展;

s8、从目标点开始逆向遍历扩展树,层层迭代直至起始点为止,得到从起始点到目标点的机器人移动路径。

2.根据权利要求1所述的路径规划方法,其特征在于,所述步骤s5具体包括:

s51、以新节点q_new为圆心,预设长度p为半径,确定q_new的邻域圆,位于所述邻域圆内的扩展树节点构成近邻节点集合s_newneighbor;

s52、依次计算集合s_newneighbor中每个近邻节点q_newneighbor[i]到起始点q_start的路径代价costfromstart(i);i=1,2,...,n,n为集合s_newneighbor中元素的个数;

计算新节点到每个近邻节点的路径代价cost(new,i);

计算新节点到起始点的路径代价costfromstart(q_new);

s53、如果costfromstart(i)+cost(new,i)≤costfromstart(q_new),将新节点q_new的父节点更改为q_newneighbor[i];如果有多个近邻节点满足costfromstart(i)+cost(new,i)≤costfromstart(q_new),选择其中costfromstart(i)+cost(new,i)最小的近邻节点作为q_new的父节点。

3.根据权利要求2所述的路径规划方法,其特征在于,所述步骤s6具体包括:

s61、重新计算集合s_newneighbor中每个近邻节点q_newneighbor[i]到起始点q_start的路径代价costfromstart(i);

计算新节点到每个近邻节点的路径代价cost(new,i);

计算新节点到起始点的路径代价costfromstart(q_new);

s62、如果costfromstart(q_new)+cost(new,i)≤costfromstart(i),将集合s_newneighbor中第i个近邻节点q_newneighbor[i]的父节点更改为q-new。

4.根据权利要求2所述的路径规划方法,其特征在于,所述路径代价为路径长度。

5.根据权利要求2所述的路径规划方法,其特征在于,所述预设长度p为rrt*基准步长ε的2倍:p=2ε。


技术总结
本发明公开了一种结合动态步长RRT*算法和势场法的路径规划方法,该方法通过构建目标点吸力势场和障碍物斥力势场,引导扩展树向目标点方向扩展,减少了冗余通路,提高了路径规划效率;同时根据最近点与障碍物的最近欧氏距离计算扩展树的扩展步长,兼顾算法收敛速度和路径平滑的,提高了机器人避障的灵活性。

技术研发人员:臧强;张国林;王泽嘉;靳雨桐
受保护的技术使用者:南京信息工程大学
技术研发日:2020.08.18
技术公布日:2020.12.01
当前第2页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1