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的斥力,所述斥力
其中β为斥力增益系数,dis=ρ(q_nearest,obstacle)为树节点q_nearest到障碍物obstacle轮廓的最近欧氏距离,ρ0为预设的障碍物斥力作用范围距离;
计算q_nearest受到的合力:
根据合力
计算扩展树的扩展步长εnew:
其中k为调节因子,控制步长的调整幅度,0<k<1;ρsafe为预设的树节点与障碍物间的安全距离;
s4、扩展树以最近点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ε。