一种改进人工势场法引导的双向RRT*路径规划方法技术

本发明专利技术涉及一种改进人工势场法引导的双向RRT*路径规划方法，首先根据目标偏置策略引导两棵随机树分别从起始点和终点交替采样，以减少冗余点个数和提高采样效率；其次提出一种自适应人工势场算法，通过重新设计势力函数解决引力或斥力过大以及出现局部陷阱等问题；然后将双向RRT*算法和改进人工势场算法融合，并对生成路径应用关键点平滑和二次采样优化策略，大大提升了算法采样效率。大大提升了算法采样效率。大大提升了算法采样效率。

【技术实现步骤摘要】
一种改进人工势场法引导的双向RRT*路径规划方法


[0001]本专利技术涉及移动机器人路径规划
，具体的说，是一种改进人工势场法引导的双向RRT*路径规划方法。

技术介绍

[0002]近年以来智能移动机器人在日常生活之中占比愈来愈重，而有效的路径规划是移动机器人完成各项工作指标和任务的前提。快速扩展随机树(Rapidly
‑
exploring Random Trees)因其概率完备性和规划路径的高效性等特点成为研究路径规划方向重要手段之一。该算法是一种基于采样和生长随机树的方法，其优点在于无需构建栅格地图对搜索空间进行几何划分，只需要对采样空间不断均匀采样和障碍物碰撞检验，即可找到由起始点到终点的可行路径。
[0003]然而，传统的快速随机树法亦存在一定的缺陷，例如，采样节点在搜索空间中的随机选取导致了随机树扩展方向上缺乏目的性，算法产生冗余点过多以致路径规划时间效率低下，生成路径过于曲折不够平滑，对移动机器人来说难以满足动力学约束等。

技术实现思路

[0004]本专利技术的目的是针对现有技术的不足，提供一种改进人工势场法引导的双向RRT*路径规划方法，首先根据目标偏置策略引导两棵随机树分别从起始点和终点交替采样，以减少冗余点个数和提高采样效率；其次提出一种自适应人工势场算法，通过重新设计势力函数解决引力或斥力过大以及出现局部陷阱等问题；然后将双向RRT*算法和改进人工势场算法融合，并对生成路径应用关键点平滑和二次采样优化策略，大大提升了算法采样效率。
[0005]为实现上述目的，本专利技术提供了如下技术方案：
[0006]一种改进人工势场法引导的双向RRT*路径规划方法，包括以下步骤：
[0007]步骤1、初始化起始点、目标点、障碍物环境，并创建两棵RRT树，一棵从起始点T1开始，另一棵从目标点T2开始，同时设置目标算法参数值；
[0008]步骤2、根据已设置的目标算法参数值在采样空间选取随机采样点，对于树T1，目标点建立自适应引力势场，障碍物处建立自适应斥力势场，并计算树T1中距离采样点最近节点所受目标点引力、采样点引力、障碍物斥力三者的矢量合力，确定扩展树生长方向；
[0009]步骤3、将矢量合力分解到X，Y轴，由分量计算在坐标轴上的延伸步长，以此确定新节点的坐标，同时连接新节点和最近节点并进行障碍物碰撞检验，若未通过，则返回步骤2重新选择采样点，若通过，则执行步骤4；
[0010]步骤4、通过最小代价路径判断，是否需要对新节点进行重新选择其父节点，是否需要在新节点邻域内对随机树进行重新布线，若是，则执行相应操作，对树中的节点进行优化，尝试剪枝和重新连接节点，改善路径的质量；否则，进入步骤5；
[0011]步骤5、将上述步骤生成的新节点作为树T2的随机采样点，对于树T2，重复步骤2至步骤4，直至两棵随机树分别生成的新节点距离小于所设置的距离阈值，连接两点，同时从
生成的两新节点通过父节点回溯至起始点和目标点，得到可行路径；
[0012]步骤6、对于步骤5所得可行路径，使用采样优化和关键点平滑策略，去除冗余的分支和节点，保留路径的关键点，减少路径的长度和复杂度，使路径更加平滑和自然。
[0013]具体的，步骤1中所述初始化起始点、目标点、障碍物环境，并创建两棵RRT树，过程如下：
[0014]步骤1.1、导入并读取地图数据，将获取的0～255色域的灰度图进行二值化操作，以便障碍物碰撞检验；
[0015]步骤1.2、设置起始点坐标Q
init
和终点坐标Q
goal
，并以此为根节点分别建立两棵随机树，同时定义两棵随机树连接的距离阈值和障碍物影响范围。
[0016]进一步地，步骤1中所述算法参数值包括目标偏置概率值、随机树扩展步长、引力常数和斥力常数。
[0017]具体的，步骤2所述自适应引力势场的建立和扩展树生长方向的确定过程如下：
[0018]步骤2.1、在搜索空间由目标概率偏置值确定随机采样点Q
rand
，若随机取值小于设置值，则在采样空间随机选取一点作为采样点，反之，则将目标终点设置为采样点；
[0019]步骤2.2、找到随机树分支中距离采样点最近的叶子节点Q
nearest
，然后分别以目标点和障碍物建立自适应引力和斥力势场函数；
[0020]步骤2.3、获得组合势场函数后，通过梯度下降法计算Q
rand
、障碍物、Q
goal
对叶子节点Q
nearest
三者的矢量合力，确定新节点Q
new
生长方向。
[0021]具体的，步骤3中所述连接新节点和最近节点并进行障碍物碰撞检验，包括：
[0022]步骤3.1、在计算得到Q
rand
、障碍物、Q
goal
对叶子节点Q
nearest
三者的矢量合力F
total
后，将其分解为两个坐标轴X，Y方向上的分力F
x
和F
y
，选择各轴上合力分量最大值的绝对值作为F
max
，那么比例系数k＝L/F
max
，其中L为算法扩展步长，在X轴上的延伸步长为L
x
＝kF
x
，Y轴上的延伸步长为L
y
＝kF
y
，将Q
nearest
坐标加上延伸步长，即为Q
new
坐标；
[0023]步骤3.2、连接上述Q
nearest
、Q
new
，进行障碍物碰撞检验，判断Q
nearest
、Q
new
的连接线是否通过障碍物，若未通过，则从步骤2开始重新选择采样点，反之，则将新节点Q
new
加入随机树中。
[0024]具体的，步骤4中所述对新节点进行重新选择其父节点及在新节点邻域内对随机树进行重新布线的过程如下：
[0025]步骤4.1、以Q
new
为圆心，半径R建立领域，在邻域内考虑所有包含的相邻节点，对于每个相邻节点，计算从起点到该相邻节点并通过Q
new
代价，如果该代价较小，则将该相邻节点设置为Q
new
的新父节点，重复以上过程直至检验完毕所有相邻节点。
[0026]步骤4.2、从目标位置开始，沿着新的父节点关系，依次向起点回溯，根据最小路径代价原理，进行优化操作，生成新的路径。
[0027]具体的，步骤5所述直至两棵随机树分别生成的新节点距离小于所设置的距离阈值，包括：
[0028]将生成的新父节点Q
new
作为随机树T2的随机采样点Q
rand
，以这个新的目标点作为第二棵随机树的扩展方向，按照步骤2、步骤3中方法生成新的节点Q...

【技术保护点】

【技术特征摘要】
1.一种改进人工势场法引导的双向RRT*路径规划方法，其特征在于，包括以下步骤：步骤1、初始化起始点、目标点、障碍物环境，并创建两棵RRT树，一棵从起始点T1开始，另一棵从目标点T2开始，同时设置目标算法参数值；步骤2、根据已设置的目标算法参数值在采样空间选取随机采样点，对于树T1，目标点建立自适应引力势场，障碍物处建立自适应斥力势场，并计算树T1中距离采样点最近节点所受目标点引力、采样点引力、障碍物斥力三者的矢量合力，确定扩展树生长方向；步骤3、将矢量合力分解到X，Y轴，由分量计算在坐标轴上的延伸步长，以此确定新节点的坐标，同时连接新节点和最近节点并进行障碍物碰撞检验，若未通过，则返回步骤2重新选择采样点，若通过，则执行步骤4；步骤4、通过最小代价路径判断，是否需要对新节点进行重新选择其父节点，是否需要在新节点邻域内对随机树进行重新布线，若是，则执行相应操作，对树中的节点进行优化，尝试剪枝和重新连接节点，改善路径的质量；否则，进入步骤5；步骤5、将上述步骤生成的新节点作为树T2的随机采样点，对于树T2，重复步骤2至步骤4，直至两颗随机树分别生成的新节点距离小于所设置的距离阈值，连接两点，同时从生成的两新节点通过父节点回溯至起始点和目标点，得到可行路径；步骤6、对于步骤5所得可行路径，使用采样优化和关键点平滑策略，去除冗余的分支和节点，保留路径的关键点，减少路径的长度和复杂度，使路径更加平滑和自然。2.根据权利要求1所述的一种改进人工势场法引导的双向RRT*路径规划方法，其特征在于，步骤1中所述初始化起始点、目标点、障碍物环境，并创建两棵RRT树，具体过程如下：步骤1.1、导入并读取地图数据，将获取的0～255色域的灰度图进行二值化操作，以便障碍物碰撞检验；步骤1.2、设置起始点坐标Q
init
和终点坐标Q
goal
，并以此为根节点分别建立两棵随机树，同时定义两棵随机树连接的距离阈值和障碍物影响范围。3.根据权利要求1所述的一种改进人工势场法引导的双向RRT*路径规划方法，其特征在于，步骤1中所述算法参数值包括目标偏置概率值、随机树扩展步长、引力常数和斥力常数。4.根据权利要求1所述的一种改进人工势场法引导的双向RRT*路径规划方法，其特征在于，步骤2所述自适应引力势场的建立和扩展树生长方向的确定过程如下：步骤2.1、在搜索空间由目标概率偏置值确定随机采样点Q
rand
，若随机取值小于设置值，则在采样空间随机选取一点作为采样点，反之，则将目标终点设置为采样点；步骤2.2、找到随机树分支中距离采样点最近的叶子节点Q
nearest
，然后分别以目标点和障碍物建立自适应引力和斥力势场函数；步骤2.3、获得组合势场函数后，通过梯度下降法计算Q
rand
、障碍物、Q
goal
对叶子节点Q
nearest
三者的矢量合力，确定新节点Q
new
生长方向。5.根据权利要求1所述的一种改进人工势场法引导的双向RRT*路径规划方法，其特征在于，步骤3中所述连接新节点和最近节点并进行障碍物碰撞检验，包括：步骤3.1、在计算得到Q
rand
、障碍物、Q
goal
对叶子节点Q
nearest
三者的矢量合力F
total
后，将其分解为两个坐标轴X，Y方向上的分力F
x
和F
y
，选择各轴上合力分量最大值的绝对值作为F
max
，那么比例系数k＝L/F
max
，其中L为算法扩展步长，在X轴上的延伸步长为L
x
＝kF
x
，Y轴上
的延伸步长为L...

【专利技术属性】
技术研发人员：熊剑，闵志豪，衷卫声，郭杭，廖玮，张强，权略，
申请(专利权)人：南昌大学，
类型：发明
国别省市：