一种基于探路者算法的改进RRT路径规划方法技术

本发明专利技术公开了一种基于探路者算法的改进RRT路径规划方法。该方法在传统RRT路径规划算法的基础上引入引导路径，利用探路者优化算法生成最优引导路径，再利用最优引导路径指引RRT的新节点X

【技术实现步骤摘要】
一种基于探路者算法的改进RRT路径规划方法


[0001]本专利技术涉及智能控制
，具体是一种基于探路者算法的改进RRT路径规划方法。

技术介绍

[0002]近年来，路径规划问题的研究在机器人领域兴起并成为研究的热点。路径规划问题对于大多数机器人而言是至关重要的，其目的是在工作空间中找到一条从初始点到目标点的无碰撞路径，并满足某些优化准则。路径规划又分为全局路径规划和局部路径规划，全局路径规划能够在已知的工作环境中生成一条从起点到终点的完整路径，局部路径规划能够利用传感器的信息实时避障。
[0003]路径规划算法主要包括基于采样的方法、人工势场法、可视图法、生物智能算法和基于深度学习的方法，其中基于采样的方法以概率路图法(Probabilistic Road Map，PRM)和快速搜索随机树(Rapidly
‑
exploring Random Trees，RRT，中文简称随机树)为代表，生物智能算法的典型代表为遗传算法和蚁群算法。基于采样的路径规划方法具有强大的搜索能力，在探索高维空间方面具有突出的表现。
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于探路者算法的改进RRT路径规划方法，其特征在于，该方法步骤如下：步骤1、创建机器人的工作空间C；工作空间C由障碍物空间X
obs
和自由空间X
free
组成；步骤2、给定机器人在工作空间C中的初始点X
t
和目标点X
g
的坐标；初始点X
t
和目标点X
g
位于自由空间X
free
；步骤3、以步骤2给定的初始点X
t
为起点，基于探路者算法生成一条最优引导路径；步骤4、在步骤3生成的最优引导路径的基础上，通过RRT
‑
Connect算法或RRT算法得到最终路径。2.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤1中，输入一张环境地图，利用matlab中的im2bw函数将环境地图转化为二值图像，得到机器人的工作空间C；障碍物空间X
obs
由静态障碍物构成，自由空间X
free
表示除障碍物空间X
obs
之外的空间。3.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤3中具体是：(3.1)初始化PFA参数和种群中各个个体的中心点坐标，并定义各个个体的第一代的中心点坐标；(3.2)根据步骤(3.1)定义的各个个体的第一代的中心点坐标和个体的计算公式，得到第一代的各个个体；再计算第一代各个个体的适应度值L
i
；再比较第一代各个个体的适应度值大小，将适应度值最小的个体设为全局最优作为第一代探路者；第一代跟随者为种群中除了第一代探路者以外的所有个体；(3.3)进行一次迭代来更新所有个体的中心点坐标，其中探路者的中心点坐标的更新公式为：式(4)中，表示当代探路者的中心点坐标，表示上一代探路者的中心点坐标，表示更新后的当代探路者的中心点坐标；r1在[0,1]内服从均匀分布，为探路者移动的步长因子；A表示探路者移动的多向性和随机性，其更新公式为：式(5)中，多向性由u1的取值决定，u1取[
‑
1,1]范围内的随机数；步长因子大小的随机性由
‑
2K/K
max
决定，与算法的迭代次数相关；K
max
表示最大迭代次数；(3.4)得到后，再得到对应的个体，再计算该个体的适应度值然后，若小于上一代探路者的适应度值则将定义为当代探路者的中心点坐标否则，将上一代探路者的中心点坐标定义为当代探路者的中心点坐标(3.5)根据步骤(3.4)定义的当代探路者的中心点坐标来更新当代跟随者的中心点坐标；跟随者的中心点坐标的更新公式为：式(6)中，表示当代跟随者的中心点坐标，表示更新后的当代跟随者的中心点坐
标；跟随者的移动不仅与步骤(3.4)定义的当代探路者的中心点坐标有关，而且受到其他跟随者的影响；参数R1和R2的更新公式如式(7)所示；ε表示跟随者移动的随机性，其更新公式如式(8)所示；式(7)中，α和β分别表示跟随者之间的相互作用系数和探路者对跟随者的吸引系数，均在[1,2]服从均匀分布；r2和r3分别为与其他跟随者移动的步长因子和与其他探路者移动的步长因子，均为[0,1]范围内的随机数；式(8)中，u2取[
‑
1,1]范围内的随机数，决定随机运动的方向；D
ij
为当前跟随者与其他跟随者之间的距离，决定随机运动的步长；(3.6)得到后，再计算得到对应的个体，再计算对应个体的适应度值再比较和所有的大小，将适应度值最小的个体设为全局最优作为下一代探路者；(3.7)计算得到下一代探路者的中心点坐标返回步骤(3.3)，直至根据最大迭代次数K
max
完成所有迭代；(3.8)完成所有迭代后，根据最后一次迭代得到的探路者，将探路者中的路径点依次连接得到一条最优引导路径。4.根据权利要求3所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤(3.1)中，各个个体的第一代的中心点坐标采用unifrnd函数，如式(1)所示：式(1)中，xmax、xmin分别为第i个个体的第一代的中心点T
i1
的横坐标的上限和下限，ymax、ymin分别为第i个个体的第一代的中心点T
i1
的纵坐标的上限和下限；步骤(3.2)中，个体是由各个路径点j组成的路径，受其中心点的影响，个体的计算公式如式(2)所示：式(2)中，x
i,j
为第i个个体所有路径点j的横坐标集合，y
i,j
为第i个个体所有路径点j的纵坐标集合；unifrnd为路径点函数；a、b分别为路径点采样区间的上限和下限；表示第i个个体的第K代的中心点T
iK
的横坐标，表示第i个个体的第K代的中心点T
iK
的纵坐标；1为生成路径点数目的维度；n为路径点的数目，包括初始点X
t
、中心点和目标点X
g
；步骤(3.2)中，个体的适应度值L
i
为该个体中各个路径点的总长度，计算公式如式(3)所示：
步骤(3.3)中，当进行第一次迭代时，根据第一代个体的定义，第一代探路者的中心点坐标与第二代探路者的中心点坐标相等。5.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤4中，RRT
‑
Connect算法具体是：S4.1、初始化两棵随机树T和T
′
及随机树参数；把初始点X
t
加入随机树T中，把目标点X
g
加入随机树T
′
中；S4.2、在步骤3生成的最优引导路径上进行随机采样，生成两个路径采样点path
node
和path
node
′
；再在路径采样点path
node
的邻近区域中产生一个随机树的随机点X
rand
，同时在路径采样点path
node
′
的邻近区域中产生一个随机树的随机点X
rand
′
；S4.3、更新随机树T和T
′
的节点：更新随机树T的节点：在随机树T的随机树节点集合v中找到与随机点X
rand
之间的距离d最近的节点X
near
；再在节点X
near
和X
rand
的直线上、从节点X
near
出发、向随机点X
rand
方向、以扩展步长t截取节点X
new
，并计算节点X
near
和X
new
的连线与水平线的角度值θ；再判断从节点X
near
到X
new
之间是否存在障碍物；若存在障碍物，则舍弃该X
new
，返回S4.2重新进行采样；若不存在障碍物，则将该节点X
new
加入随机树节点集合v中；更新随机树T
′
的节点：在随机树T
′
的随机树节点集合v
′
中找到与随机点X
rand
′
之间的距离d
′
最近的节点X
near
′
；再在节点X
near
′
和X
rand
′
的直线上、从节点X
near
′
出发、向随机点X
rand
′
方向、以扩展步长t截取节点X
new
′
，并计算节点X
near
′
和X
new
′
的连线与水平线的角度值θ
′
；再判断从X
near
′
到X
new
′
之间是否存在障碍物；若存在障碍物，则舍弃该X
new
′
，返回S4.2重新进行采样；若不存在障碍物，则将X
new
′
加入随机树节点集合v
′
中；S4.4、计算节点X
new
与X
new
′
之间的距离d
g
′
来检测节点X
new
′
是否到达X
new
附近；若d
g
′
≥目标点阈值h，则节点X
new
′
没有到达X
new
附近，返回S4.2并将节点X
near
′
和X
new
′
之间的路径连接；若d
g
′
＜目标点阈值h，则节点X
new
′
已到达X
new
附近，进入S4.5；S4.5、分别从初始点X
t
和目标点X
g
开始回溯，分别将随机树节点依次连接，最后将两棵随机树T和T
′
的各自的最后一个节点相连，得到最终路径。6.根据权利要求5所述的基于探路者算法的改进RRT路径规划方法，其特征在于，S4.1中，随...

【专利技术属性】
技术研发人员：张燕，李小觅，孙善乐，李璇，董美琪，何国涛，
申请(专利权)人：河北工业大学，
类型：发明
国别省市：