CN112013846A - 一种结合动态步长rrt*算法和势场法的路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于机器人自主导航技术领域，具体涉及一种能够避障的路径规划方法。

背景技术

随着机器人技术的不断发展，对机器人的研究不断深入以及其不断增长的市场应用，使得机器人学正成为国内外热门的研究学科。路径规划作为移动机器人自动导航的重要组成部分，向来是机器人学领域的重点与热点方向。有效的路径规划是机器人完成各项作业和任务的前提。按照算法特性的不同，路径规划可以分为基于传统算法的规划方法和基于智能算法的规划方法。传统算法大多是基于计算机图论基础或构建相关几何模型。智能算法则是近年来新兴的仿生优化算法，模仿自然界生物群体的经验或特性具有优秀的非线性逼近能力，RRT(Rapidly exploring Random Tree)快速搜索随机树算法便是其中之一。

RRT算法扩展流程类似树枝的生长过程，通过随机采样将扩展树的生长方向引导向空白区域空间，最终探索到目标点区域。其原理简单明确，但由于其采样的随机性和步长的固定性，规划出的路径常常不是最优路径，并且路径也不够平滑。另一方面扩展树没有关于目标点方向的导向，导致产生许多冗长路径，增大计算量。虽然RRT*算法在原算法基础上，通过重新选择父节点和重布线这两个过程，在一定程度上减少了路径代价和冗余通路，但固定步长导致的路径不平滑和目标点没有对扩展树扩展产生导向这两个问题，依旧可能会导致扩展树对目标点的收敛速度非常慢。

发明内容

发明目的：针对现有技术中存在的问题，本发明提供了一种合动态步长RRT*算法和势场法的路径规划方法，该方法解决了目标点对于扩展树扩展没有导向作用的问题，减少了冗余通路，提高了机器人路径规划的效率，且通过动态步长提高了避障灵活性。

技术方案：本发明采用如下技术方案：

一种结合动态步长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轮廓的最近欧氏距离，ρ₀为预设的障碍物斥力作用范围距离；

的方向为从障碍物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、从目标点开始逆向遍历扩展树，层层迭代直至起始点为止，得到从起始点到目标点的机器人移动路径。

所述步骤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的父节点。

所述步骤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。

所述路径代价为路径长度。

所述预设长度P为RRT*基准步长ε的2倍：P＝2ε。

有益效果：与现有技术相比，本发明公开的路径规划方法具有以下有益效果：

(1)本发明结合了智能算法中的RRT*算法和传统算法中的势场法，在RRT*算法的基础上在目标点处对扩展树产生一个引力吸引，在障碍物处对扩展树产生一个斥力排斥，使得搜索树向目标点方面进行搜索扩展，解决了目标点对于扩展树扩展没有导向作用的问题，减少了冗余通路，提高了机器人路径规划的效率；(2)本发明同时引入了动态步长，在随机树离障碍物较远时采用基准步长，加快算法收敛速度，当随机数生长到障碍物附近阈值范围内时，采用小步长，确保新生成轨迹的平滑度；使步长随着与障碍物之间的距离而变化，更好地解决实时性与平滑度之间的矛盾与原RRT*算法相比，提高了避障的灵活性。

附图说明

图1为本发明公开的路径规划方法的流程图；

图2为根据环境信息建立人工势场示意图；

图3为重新选择父节点和重新布线行为的示意图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

本发明公开了一种结合动态步长RRT*算法和势场法的路径规划方法，如图1所示，包括：

S1、确定机器人的起始点q_start、目标点q_Goal的位置坐标，确定障碍物的位置和轮廓；确定RRT*基准步长ε；本实施例中，设置ε＝5；

如果从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^*为预设的树节点到目标点的距离阈值，本实施例中，设定α＝1；d^*＝8；

的方向为从最近点q_nearest指向目标点q_Goal的向量；

在障碍物处构建一个斥力势场，使障碍物对树节点产生斥力，使扩展树远离障碍物方向。树节点所受斥力大小与其到障碍物的距离成反比，方向从障碍物指向树节点，障碍物对最近点q_nearest的斥力

的大小计算如下：

其中β为斥力增益系数，dis＝ρ(q_nearest，Obstacle)为树节点q_nearest到障碍物Obstacle轮廓的最近欧氏距离，ρ₀为预设的障碍物斥力作用范围距离；本实施例中，设定β＝0.5；ρ₀＝8；

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

计算q_nearest受到的合力：

根据合力

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

如图2所示，在障碍物和目标点的联合势场下，通过

和

的合力

作用引导扩展树的搜索方向，与原扩展方向相比，明显更偏向目标点。

本发明中扩展树采用动态扩展步长，在扩展树离障碍物较远时，没有到达安全阈值时采用基准步长ε，加快算法收敛。当扩展树扩展到障碍物附近阈值范围内时采用小步长，确保轨迹平滑度。扩展树的动态扩展步长ε_new为：

其中k为调节因子，控制步长的调整幅度，0＜k＜1；ρ_safe为预设的树节点与障碍物间的安全距离；本实施例中，k＝0.8；ρ_safe＝15；

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的父节点，具体包括：

S51、以新节点q_new为圆心，预设长度P为半径，确定q_new的邻域圆，位于所述邻域圆内的扩展树节点构成近邻节点集合S_newneighbor；本实施例中，预设长度P为RRT*基准步长ε的2倍：P＝2ε；

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_nearest；

如果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所示，图中q_new邻域圆内有2个近邻节点q_newneighbor[1]和q_newneighbor[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。

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

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

Claims (5)

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轮廓的最近欧氏距离，ρ₀为预设的障碍物斥力作用范围距离；

的方向为从障碍物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ε。

Images