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ε。