CN114593744B - 一种基于探路者算法的改进rrt路径规划方法 - Google Patents
一种基于探路者算法的改进rrt路径规划方法 Download PDFInfo
- Publication number
- CN114593744B CN114593744B CN202210206904.4A CN202210206904A CN114593744B CN 114593744 B CN114593744 B CN 114593744B CN 202210206904 A CN202210206904 A CN 202210206904A CN 114593744 B CN114593744 B CN 114593744B
- Authority
- CN
- China
- Prior art keywords
- new
- path
- node
- random
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 34
- 241000614201 Adenocaulon bicolor Species 0.000 title claims abstract description 11
- 238000005070 sampling Methods 0.000 claims abstract description 31
- 238000004364 calculation method Methods 0.000 claims description 20
- 238000001514 detection method Methods 0.000 claims description 16
- 230000006870 function Effects 0.000 claims description 8
- 238000012952 Resampling Methods 0.000 claims description 6
- 125000003275 alpha amino acid group Chemical group 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 230000003993 interaction Effects 0.000 claims description 2
- 230000008685 targeting Effects 0.000 abstract description 6
- 238000005457 optimization Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 10
- 230000004888 barrier function Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- IVDRCZNHVGQBHZ-UHFFFAOYSA-N 2-butoxyethyl 2-(3,5,6-trichloropyridin-2-yl)oxyacetate Chemical compound CCCCOCCOC(=O)COC1=NC(Cl)=C(Cl)C=C1Cl IVDRCZNHVGQBHZ-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于探路者算法的改进RRT路径规划方法。该方法在传统RRT路径规划算法的基础上引入引导路径，利用探路者优化算法生成最优引导路径，再利用最优引导路径指引RRT的新节点Xnew的生成，不仅利用了RRT算法的随机性，而且利用了引导路径使新节点朝向目标点方向扩展的特性，从而提高了路径规划的目标性，降低路径搜索的随机性和搜索时间，从而提高路径规划的效率、实时性和成功率，加强避障能力。本发明引入了路径点邻近区域采样的方法来确定随机点的生成，使每个路径点能够在特定区域内采样，不仅避免了采样点的随机性，提高RRT算法采样时的目标导向性，而且避免了与障碍物产生碰撞。
Description
技术领域
本发明涉及智能控制技术领域，具体是一种基于探路者算法的改进RRT路径规划方法。
背景技术
近年来，路径规划问题的研究在机器人领域兴起并成为研究的热点。路径规划问题对于大多数机器人而言是至关重要的，其目的是在工作空间中找到一条从初始点到目标点的无碰撞路径，并满足某些优化准则。路径规划又分为全局路径规划和局部路径规划，全局路径规划能够在已知的工作环境中生成一条从起点到终点的完整路径，局部路径规划能够利用传感器的信息实时避障。
路径规划算法主要包括基于采样的方法、人工势场法、可视图法、生物智能算法和基于深度学习的方法，其中基于采样的方法以概率路图法(Probabilistic Road Map，PRM)和快速搜索随机树(Rapidly-exploring Random Trees，RRT，中文简称随机树)为代表，生物智能算法的典型代表为遗传算法和蚁群算法。基于采样的路径规划方法具有强大的搜索能力，在探索高维空间方面具有突出的表现。
传统RRT算法虽然具有简单、快速的优点，但在生成随机树节点时，随机点Xrand是在整个状态空间中随机生成的，其节点扩展图如图3所示。为提高RRT算法采样时的目标导向性，文献《李金良,舒翰儒,刘德建,徐磊.基于改进RRT路径规划算法[J].组合机床与自动化加工技术,2021(02):22-24+29.》中提出一种改进的RRT路径规划算法，该算法结合目标偏向策略，使算法快速向目标节点收敛，该算法在采样判断时设定一个参数Pa，在每次扩展前随机得到一个(0-1)内的随机值P并进行判断，使随机树扩展更具目标性，但该随机值P的产生也具有随机性。文献《宋宇,王志明.改进RRT移动机器人路径规划算法[J].长春工业大学学报,2018,39(6):546-550.》中通过人工势场法和RRT算法相结合，以合力方向引导搜索树的扩张，可以减少采样点的个数和搜索路径长度，但搜索时间明显增加。
为了进一步增强RRT算法的快速性，RRT的衍生算法RRT-Connect被提出，该算法在起点和终点分别生成一棵随机树，直到两棵树相遇，但是RRT算法和RRT-Connect均存在局限性，由于他们的叶子都是随机生成的，缺乏目标性，生成的路径往往不是最优的，导致实时性差，效率低，而这些特性对于实际应用尤为重要。
发明内容
针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于探路者算法的改进RRT路径规划方法。
本发明解决所述技术问题的技术方案是，提供一种基于探路者算法的改进RRT路径规划方法，其特征在于，该方法步骤如下：
步骤1、创建机器人的工作空间C；工作空间C由障碍物空间Xobs和自由空间Xfree组成；
步骤2、给定机器人在工作空间C中的初始点Xt和目标点Xg的坐标；初始点Xt和目标点Xg位于自由空间Xfree；
步骤3、以步骤2给定的初始点Xt为起点，基于探路者算法生成一条最优引导路径；
步骤4、在步骤3生成的最优引导路径的基础上，通过RRT-Connect算法或RRT算法得到最终路径。
与现有技术相比，本发明有益效果在于：
(1)本发明在传统RRT路径规划算法的基础上引入引导路径，利用探路者优化算法生成最优引导路径，再利用最优引导路径指引RRT的新节点Xnew的生成，不仅利用了RRT算法的随机性，而且利用了引导路径使新节点朝向目标点方向扩展的特性，从而提高了路径规划的目标性，降低路径搜索的随机性和搜索时间，从而提高路径规划的效率、实时性和成功率，加强避障能力。
(2)本发明引入了路径点邻近区域采样的方法来确定随机点的生成，使每个路径点能够在特定区域内采样，不仅避免了采样点的随机性，提高RRT算法采样时的目标导向性，而且避免了与障碍物产生碰撞。
(3)本发明显著提高了路径搜索效率，在缩短路径长度方面取得了良好的效果，去掉了较多的冗余路径节点。
(4)本发明的路径采样节点大大减少。与传统RRT算法相比，本发明的PFA-RRT中路径采样节点减少了45％，PFA-RRT-Connect中路径采样节点减少了75％。
(5)本发明的路径采样节点的利用率提高。传统RRT算法的节点利用率为20％，本发明的PFA-RRT中路径采样节点利用率为44％，PFA-RRT-Connect中路径采样节点利用率为70％。
(6)本发明的路径规划时间减少。与传统RRT算法相比，本发明的PFA-RRT中路径规划时间减少了53％，PFA-RRT-Connect中路径规划时间减少了83％。
附图说明
图1为本发明的路径规划流程图；
图2为本发明的基于探路者算法生成引导路径的流程图；
图3为现有技术中的RRT算法节点扩展图；
图4为本发明的PFA-RRT-Connect的节点扩展图；
图5为本发明的PFA-RRT的节点扩展图；
图6为本发明实施例1的实验环境图；
图7为本发明实施例1的传统RRT算法路径规划结果图；
图8为本发明实施例1的PFA-RRT-Connect的路径规划结果图；
图9为本发明实施例1的PFA-RRT的路径规划结果图。
具体实施方式
下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。
本发明提供了一种基于探路者算法的改进RRT路径规划方法(简称方法)，其特征在于，该方法步骤如下：
步骤1、创建机器人的工作空间C：输入一张环境地图，利用matlab中的im2bw函数将环境地图转化为二值图像，得到机器人的工作空间C；工作空间C由障碍物空间Xobs和自由空间Xfree组成；
优选地，步骤1中，障碍物空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍物空间Xobs之外的空间；
步骤2、给定机器人在工作空间C中的初始点Xt和目标点Xg的坐标；初始点Xt和目标点Xg位于自由空间Xfree；
步骤3、以步骤2给定的初始点Xt为起点，基于PFA算法(Pathfinder Algorithm，探路者算法)生成一条最优引导路径；
优选地，步骤3中，基于PFA算法生成最优引导路径的具体步骤为：
(3.1)初始化PFA参数和种群中各个个体的中心点坐标，并定义各个个体的第一代的中心点坐标；
优选地，步骤(3.1)中，PFA参数包括最大迭代次数Kmax和种群中的个体数量i；
优选地，步骤(3.1)中，步骤(3.1)中，各个个体的第一代的中心点坐标采用unifrnd函数，如式(1)所示：
式(1)中，xmax、xmin分别为第i个个体的第一代的中心点的横坐标/>的上限和下限，ymax、ymin分别为第i个个体的第一代的中心点/>的纵坐标/>的上限和下限。
(3.2)根据步骤(3.1)定义的各个个体的第一代的中心点坐标和个体的计算公式，得到第一代的各个个体；再通过公式(3)计算第一代各个个体的适应度值Li；再比较第一代各个个体的适应度值大小，将适应度值最小的个体设为全局最优作为第一代探路者；第一代跟随者为种群中除了第一代探路者以外的所有个体；
优选地，步骤(3.2)中，种群由i个个体组成；个体是由各个路径点j组成的路径，受其中心点的影响，个体的计算公式如式(2)所示：
式(2)中，xi,j为第i个个体所有路径点j的横坐标集合，yi,j为第i个个体所有路径点j的纵坐标集合；unifrnd为路径点函数；a、b分别为路径点采样区间的上限和下限；表示第i个个体的第K代的中心点/>的横坐标，/>表示第i个个体的第K代的中心点/>的纵坐标；1为生成路径点数目的维度；n为路径点的数目，取值范围为10～15，包括初始点Xt、中心点和目标点Xg；
优选地，步骤(3.2)中，个体的适应度值Li为该个体中各个路径点的总长度，计算公式如式(3)所示：
(3.3)进行一次迭代来更新所有个体的中心点坐标，其中探路者的中心点坐标的更新公式为：
式(4)中，表示当代探路者的中心点坐标，/>表示上一代探路者的中心点坐标，/>表示更新后的当代探路者的中心点坐标；r1在[0,1]内服从均匀分布，为探路者移动的步长因子；A表示探路者移动的多向性和随机性，其更新公式为：
式(5)中，多向性由u1的取值决定，u1取[-1,1]范围内的随机数；步长因子大小的随机性由-2K/Kmax决定，与算法的迭代次数相关；
优选地，步骤(3.3)中，当进行第一次迭代时，根据第一代个体的定义，第一代探路者的中心点坐标与第二代探路者的中心点坐标/>相等。
(3.4)由式(4)得到后，再利用式(2)得到/>对应的个体，再根据式(3)计算该个体的适应度值/>然后采用保优策略，若/>小于上一代探路者的适应度值/>则将/>定义为当代探路者的中心点坐标/>否则，将上一代探路者的中心点坐标/>定义为当代探路者的中心点坐标/>
(3.5)根据步骤(3.4)定义的当代探路者的中心点坐标来更新当代跟随者的中心点坐标；跟随者的中心点坐标的更新公式为：
式(6)中，表示当代跟随者的中心点坐标，/>表示更新后的当代跟随者的中心点坐标；跟随者的移动不仅与步骤(3.4)定义的当代探路者的中心点坐标/>有关，而且受到其他跟随者/>的影响；参数R1和R2的更新公式如式(7)所示；ε表示跟随者移动的随机性，其更新公式如式(8)所示；
式(7)中，α和β分别表示跟随者之间的相互作用系数和探路者对跟随者的吸引系数，均在[1,2]服从均匀分布；r2和r3分别为与其他跟随者移动的步长因子和与其他探路者移动的步长因子，均为[0,1]范围内的随机数；
式(8)中，u2取[-1,1]范围内的随机数，决定随机运动的方向；Dij为当前跟随者与其他跟随者之间的距离，决定随机运动的步长；
(3.6)由式(6)得到后，再利用式(1)得到/>对应的个体，再根据式(3)计算对应个体的适应度值/>再比较/>和所有/>的大小，将适应度值最小的个体设为全局最优作为下一代探路者；
(3.7)根据式(1)得到下一代探路者的中心点坐标返回步骤(3.3)，直至根据最大迭代次数Kmax完成所有迭代；
(3.8)完成所有迭代后，根据最后一次迭代得到的探路者，将探路者中的路径点依次连接得到一条最优引导路径。
步骤4、在步骤3生成的最优引导路径的基础上，通过RRT-Connect算法或RRT算法得到最终路径。
优选地，步骤4中，RRT-Connect算法具体是：
S4.1、初始化两棵随机树T和T′及随机树参数；把初始点Xt加入随机树T中，把目标点Xg加入随机树T′中；
优选地，S4.1中，随机树参数包括目标点阈值h和扩展步长t；目标点阈值h通常设置为扩展步长t的1～1.5倍；
优选地，S4.1中，初始化后的随机树T和T′如下式所示：
{T.v(1).x T.v(1).y T.v(1).xPrev T.v(1).yPrev T.v(1).dist T.v(1).indPrev}
{T′.v′(1).x T′.v′(1).y T′.v′(1).xPrev T′.v′(1).yPrev T′.v′(1).distT′.v′(1).indPrev}
其中，T和T′为两棵随机树，v和v′分别是T和T′的随机树节点集合，xPrev为某一节点的父节点，dist为父节点到该节点的距离，indPrev为父节点的索引。
S4.2、在步骤3生成的最优引导路径上进行随机采样，生成两个路径采样点pathnode和pathnode′；再在路径采样点pathnode的邻近区域中通过式(9)产生一个随机树的随机点Xrand，同时在路径采样点pathnode′的邻近区域中通过式(10)产生一个随机树的随机点Xrand′(如图4所示)；
优选地，S4.2中，邻近区域是以路径采样点为中心的边长为2r的正方形，r表示一个单位邻近区域长度；
优选地，S4.2中，随机点Xrand的位置均通过式(9)生成，随机点Xrand′的位置均通过式(10)生成：
Xrand＝pathnode+r[2rand-1,2rand-1] (9)
Xrand′＝pathnode′+r[2rand-1,2rand-1] (10)
式(9)和(10)中，rand为[0,1]区间中的随机数。
S4.3、更新随机树T和T′的节点：
更新随机树T的节点：在随机树T的随机树节点集合v中找到与随机点Xrand之间的距离d最近的节点Xnear；再在节点Xnear和Xrand的直线上、从节点Xnear出发、向随机点Xrand方向、以扩展步长t截取节点Xnew，并计算节点Xnear和Xnew的连线与水平线的角度值θ；再判断从节点Xnear到Xnew之间是否存在障碍物；若存在障碍物，则舍弃该Xnew，返回S4.2重新进行采样；若不存在障碍物，则将该节点Xnew加入随机树节点集合v中；
更新随机树T′的节点：在随机树T′的随机树节点集合v′中找到与随机点Xrand′之间的距离d′最近的节点Xnear′；再在节点Xnear′和Xrand′的直线方向上，从节点Xnear′出发、以扩展步长t截取节点Xnew′，并计算节点Xnear′和Xnew′的连线与水平线的角度值θ′；再判断从Xnear′到Xnew′之间是否存在障碍物；若存在障碍物，则舍弃该Xnew′，返回S4.2重新进行采样；若不存在障碍物，则将Xnew′加入随机树节点集合v′中；
优选地，S4.3中，距离d的计算公式如式(11)所示，距离d′的计算公式如式(12)所示：
式(11)中，Xrand(1)为Xrand的横坐标，Xrand(2)为Xrand的纵坐标，T.v(i).x为随机树T节点的横坐标，T.v(i).y为随机树T节点的纵坐标；
式(12)中，Xrand(1)′为Xrand′的横坐标，Xrand(2)′为Xrand′的纵坐标，T′.v′(i).x为随机树T′节点的横坐标，T′.v′(i).y为随机树T′节点的纵坐标。
优选地，S4.3中，角度值θ的计算公式如式(13)所示，角度值θ′的计算公式如式(14)所示：
θ＝arctan(Xnew(2)-Xnear(2))/(Xnew(1)-Xnear(1)) (13)
θ′＝arctan(Xnew(2)′-Xnear(2)′)/(Xnew(1)′-Xnear(1)′) (14)
式(13)中，Xnew(1)为Xnew的横坐标，Xnew(2)为Xnew的纵坐标；Xnear(1)为Xnear的横坐标，Xnear(2)为Xnear的纵坐标；
式(14)中，Xnew(1)′为Xnew′的横坐标，Xnew(2)′为Xnew′的纵坐标；Xnear(1)′为Xnear′的横坐标，Xnear(2)′为Xnear′的纵坐标。
优选地，S4.3中，判断从Xnear到Xnew之间是否存在障碍物的方法是：以a为检测步长，从节点Xnear开始以增加检测步长a沿直线行至Xnew来检测两节点之间是否存在障碍；
节点Xnear直线距离增加检测步长a后的节点Xb如式(15)所示：
Xb＝Xnear+a[cos(θ)sin(θ)] (15)
优选地，S4.3中，判断从Xnear′到Xnew′之间是否存在障碍物的方法是：以a为检测步长，从节点Xnear′开始以增加检测步长a沿直线行至Xnew′来检测两节点之间是否存在障碍；
节点Xnear′直线距离增加检测步长a后的Xb′如式(16)所示：
Xb′＝Xnear′+a[cos(θ)sin(θ)] (16)
S4.4、计算节点Xnew与Xnew′之间的距离dg′来检测节点Xnew′是否到达Xnew附近；若dg′≥目标点阈值h，则节点Xnew′没有到达Xnew附近，返回S4.2并将节点Xnear′和Xnew′之间的路径连接；若dg′＜目标点阈值h，则节点Xnew′已到达Xnew附近，进入S4.5；
优选地，S4.4中，节点Xnew与Xnew′之间的距离dg′的计算公式如下：
S4.5、分别从初始点Xt和目标点Xg开始回溯，分别将随机树节点依次连接，最后将两棵随机树T和T′的各自的最后一个节点相连，得到最终路径。
优选地，步骤4中，RRT算法具体是：
A4.1、初始化一棵随机树T及随机树参数；把初始点Xt加入随机树T中；
优选地，A4.1中，随机树参数包括目标点阈值h和扩展步长t；目标点阈值h通常设置为扩展步长t的1～1.5倍；
优选地，A4.1中，初始化后的随机树T如下式所示：
{T.v(1).x T.v(1).y T.v(1).xPrev T.v(1).yPrev T.v(1).dist T.v(1).indPrev}
其中，T为随机树，v是T的随机树节点集合，xPrev为某一节点的父节点，dist为父节点到该节点的距离，indPrev为父节点的索引。
A4.2、在步骤3生成的最优引导路径上进行随机采样，生成一个路径采样点pathnode；再在该路径采样点pathnode的邻近区域中产生一个随机树的随机点Xrand(如图5所示)；
优选地，A4.2中，邻近区域是以该路径采样点pathnode为中心的边长为2r的正方形，r表示一个单位邻近区域长度；
优选地，A4.2中，随机点Xrand的位置通过式(9)生成。
A4.3、更新随机树T的节点：在随机树T的随机树节点集合v中找到与随机点Xrand之间的距离d最近的节点Xnear；再在节点Xnear和Xrand的直线上、从节点Xnear出发、向随机点Xrand方向、以扩展步长t截取节点Xnew，并计算节点Xnear和Xnew的连线与水平线的角度值θ；再判断从节点Xnear到Xnew之间是否存在障碍物；若存在障碍物，则舍弃该节点Xnew，返回A4.2重新进行采样；若不存在障碍物，则将该节点Xnew加入随机树节点集合v中；
优选地，A4.3中，距离d的计算公式如式(11)所示；角度值θ的计算公式如式(13)所示；
优选地，A4.3中，判断从Xnear到Xnew之间是否存在障碍物的方法是：以a为检测步长，从节点Xnear开始以增加检测步长a沿直线行至Xnew来检测两节点之间是否存在障碍；
节点Xnear直线距离增加检测步长a后的节点Xb如式(15)所示：
A4.4、计算节点Xnew与目标点Xg之间的距离dg来检测节点Xnew是否到达目标点Xg附近；若dg≥目标点阈值h，则节点Xnew没有到达目标点Xg附近，返回A4.2，并将节点Xnear和Xnew之间的路径连接；若dg＜目标点阈值h，则节点Xnew已到达目标点Xg附近，进入A4.5；
优选地，A4.4中，节点Xnew与目标点Xg的之间距离dg的计算公式如下：
式(13)中，Xnew(1)为Xnew的横坐标，Xnew(2)为Xnew的纵坐标；Xg(1)为Xg的横坐标，Xg(2)为Xg的纵坐标。
A4.5、沿目标点Xg回溯到初始点Xt，将随机树节点集合v中的节点依次连接，得到最终路径。
实施例1
步骤1中，二值图像为300*300的数字矩阵；数字矩阵中只包含0和1两种数字，0表示障碍物空间Xobs，1表示自由空间Xfree；
图6中，黑色部分表示静态障碍物，空白部分表示自由空间Xfree；初始点Xt由三角形表示，目标点Xg由正方形表示；静态障碍物随机分布，大小不一，障碍物的中心点坐标分别为[23.0,7.0]、[10.0,7.0]、[5.0,20.0]、[22.0,20.0]、[15.0,27.0]、[14.0,15.0]，障碍物的半径分别为5.0、5.0、4.0、4.5、2.0、1.5；
步骤2、给定机器人在工作空间C中的初始点Xt[0,0]和目标点Xg[30,30]；
步骤3、以步骤2给定的初始点Xt[0,0]为起点，基于PFA算法生成一条最优引导路径；其中最大迭代次数Kmax为10，个体数量为100；
步骤4、在步骤3生成的最优引导路径的基础上，通过RRT-Connect算法或RRT算法得到最终路径；其中目标点阈值h为3，扩展步长t为2；单位邻近区域长度r＝3；检测步长a＝0.5，若Xb在障碍物内，则坐标点为0；若Xb不在障碍物内，则坐标点为1；若Xb′在障碍物内，则坐标点为0；若Xb′不在障碍物内，则坐标点为1。
图7-9中，生成的新节点Xnew以“黑细线”相连接，最终反向查询形成一条以“黑粗线”表示的最终路径。
现有的RRT算法是从初始点Xt[0,0]开始朝着目标点Xg[30,30]扩展新节点。
表1引入引导路径前后的路径长度和仿真时耗等数据对比
传统RRT算法的路径规划结果图如图7所示，由图7可知，RRT算法具有快速、简单的优点，但也存在实时性差、效率低的问题，无法规划出一条最优路径。针对该问题，本发明引入了引导路径。图8和图9分别是本发明的PFA-RRT-Connect算法和PFA-RRT算法的路径规划结果图。由图8和图9可以看出，本发明算法使得随机树的扩展更具有目标性，提高了路径规划效率。由表1可知，平均路径由传统RRT的55.07m分别缩至44.37m和45.79m；随机树的节点个数也由136分别减至34和69。表1的数据显示路径规划在通过引导路径引入后所得路径长度明显优于传统RRT算法，路径分别减少了19.4％和16.8％，说明引导路径的作用显著。该改进的RRT算法明显提高了路径搜索效率，所获得路径长度更短。
本发明未述及之处适用于现有技术。
Claims (8)
1.一种基于探路者算法的改进RRT路径规划方法，其特征在于，该方法步骤如下：
步骤1、创建机器人的工作空间C；工作空间C由障碍物空间Xobs和自由空间Xfree组成；
步骤2、给定机器人在工作空间C中的初始点Xt和目标点Xg的坐标；初始点Xt和目标点Xg位于自由空间Xfree；
步骤3、以步骤2给定的初始点Xt为起点，基于探路者算法生成一条最优引导路径；
步骤4、在步骤3生成的最优引导路径的基础上，通过RRT-Connect算法或RRT算法得到最终路径；
所述RRT-Connect算法具体是：
S4.1、初始化两棵随机树T和T′及随机树参数；把初始点Xt加入随机树T中，把目标点Xg加入随机树T′中；
S4.2、在步骤3生成的最优引导路径上进行随机采样，生成两个路径采样点pathnode和pathnode′；再在路径采样点pathnode的邻近区域中产生一个随机树的随机点Xrand，同时在路径采样点pathnode′的邻近区域中产生一个随机树的随机点Xrand′；
S4.3、更新随机树T和T′的节点：
更新随机树T的节点：在随机树T的随机树节点集合v中找到与随机点Xrand之间的距离d最近的节点Xnear；再在节点Xnear和Xrand的直线上、从节点Xnear出发、向随机点Xrand方向、以扩展步长t截取节点Xnew，并计算节点Xnear和Xnew的连线与水平线的角度值θ；再判断从节点Xnear到Xnew之间是否存在障碍物；若存在障碍物，则舍弃该Xnew，返回S4.2重新进行采样；若不存在障碍物，则将该节点Xnew加入随机树节点集合v中；
更新随机树T′的节点：在随机树T′的随机树节点集合v′中找到与随机点Xrand′之间的距离d′最近的节点Xnear′；再在节点Xnear′和Xrand′的直线上、从节点Xnear′出发、向随机点Xrand′方向、以扩展步长t截取节点Xnew′，并计算节点Xnear′和Xnew′的连线与水平线的角度值θ′；再判断从Xnear′到Xnew′之间是否存在障碍物；若存在障碍物，则舍弃该Xnew′，返回S4.2重新进行采样；若不存在障碍物，则将Xnew′加入随机树节点集合v′中；
S4.4、计算节点Xnew与Xnew′之间的距离dg′来检测节点Xnew′是否到达Xnew附近；若dg′≥目标点阈值h，则节点Xnew′没有到达Xnew附近，返回S4.2并将节点Xnear′和Xnew′之间的路径连接；若dg′＜目标点阈值h，则节点Xnew′已到达Xnew附近，进入S4.5；
S4.5、分别从初始点Xt和目标点Xg开始回溯，分别将随机树节点依次连接，最后将两棵随机树T和T′的各自的最后一个节点相连，得到最终路径；
所述RRT算法具体是：
A4.1、初始化一棵随机树T及随机树参数；把初始点Xt加入随机树T中；
A4.2、在步骤3生成的最优引导路径上进行随机采样，生成一个路径采样点pathnode；再在该路径采样点pathnode的邻近区域中产生一个随机树的随机点Xrand；
A4.3、更新随机树T的节点：在随机树T的随机树节点集合v中找到与随机点Xrand之间的距离d最近的节点Xnear；再在节点Xnear和Xrand的直线上、从节点Xnear出发、向随机点Xrand方向、以扩展步长t截取节点Xnew，并计算节点Xnear和Xnew的连线与水平线的角度值θ；再判断从节点Xnear到Xnew之间是否存在障碍物；若存在障碍物，则舍弃该节点Xnew，返回A4.2重新进行采样；若不存在障碍物，则将该节点Xnew加入随机树节点集合v中；
A4.4、计算节点Xnew与目标点Xg之间的距离dg来检测节点Xnew是否到达目标点Xg附近；若dg≥目标点阈值h，则节点Xnew没有到达目标点Xg附近，返回A4.2，并将节点Xnear和Xnew之间的路径连接；若dg＜目标点阈值h，则节点Xnew已到达目标点Xg附近，进入A4.5；
A4.5、沿目标点Xg回溯到初始点Xt，将随机树节点集合v中的节点依次连接，得到最终路径。
2.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤1中，输入一张环境地图，利用matlab中的im2bw函数将环境地图转化为二值图像，得到机器人的工作空间C；障碍物空间Xobs由静态障碍物构成，自由空间Xfree表示除障碍物空间Xobs之外的空间。
3.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤3中具体是：
(3.1)初始化PFA参数和种群中各个个体的中心点坐标，并定义各个个体的第一代的中心点坐标；
(3.2)根据步骤(3.1)定义的各个个体的第一代的中心点坐标和个体的计算公式，得到第一代的各个个体；再计算第一代各个个体的适应度值Li；再比较第一代各个个体的适应度值大小，将适应度值最小的个体设为全局最优作为第一代探路者；第一代跟随者为种群中除了第一代探路者以外的所有个体；
(3.3)进行一次迭代来更新所有个体的中心点坐标，其中探路者的中心点坐标的更新公式为：
式(4)中，表示当代探路者的中心点坐标，/>表示上一代探路者的中心点坐标，/>表示更新后的当代探路者的中心点坐标；r1在[0,1]内服从均匀分布，为探路者移动的步长因子；A表示探路者移动的多向性和随机性，其更新公式为：
式(5)中，多向性由u1的取值决定，u1取[-1,1]范围内的随机数；步长因子大小的随机性由-2K/Kmax决定，与算法的迭代次数相关；Kmax表示最大迭代次数；
(3.4)得到后，再得到/>对应的个体，再计算该个体的适应度值/>然后，若/>小于上一代探路者的适应度值/>则将/>定义为当代探路者的中心点坐标/>否则，将上一代探路者的中心点坐标/>定义为当代探路者的中心点坐标/>
(3.5)根据步骤(3.4)定义的当代探路者的中心点坐标来更新当代跟随者的中心点坐标；跟随者的中心点坐标的更新公式为：
式(6)中，表示当代跟随者的中心点坐标，/>表示更新后的当代跟随者的中心点坐标；跟随者的移动不仅与步骤(3.4)定义的当代探路者的中心点坐标/>有关，而且受到其他跟随者/>的影响；参数R1和R2的更新公式如式(7)所示；ε表示跟随者移动的随机性，其更新公式如式(8)所示；
式(7)中，α和β分别表示跟随者之间的相互作用系数和探路者对跟随者的吸引系数，均在[1,2]服从均匀分布；r2和r3分别为与其他跟随者移动的步长因子和与其他探路者移动的步长因子，均为[0,1]范围内的随机数；
式(8)中，u2取[-1,1]范围内的随机数，决定随机运动的方向；Dij为当前跟随者与其他跟随者之间的距离，决定随机运动的步长；
(3.6)得到后，再计算得到/>对应的个体，再计算对应个体的适应度值/>再比较和所有/>的大小，将适应度值最小的个体设为全局最优作为下一代探路者；
(3.7)计算得到下一代探路者的中心点坐标返回步骤(3.3)，直至根据最大迭代次数Kmax完成所有迭代；
(3.8)完成所有迭代后，根据最后一次迭代得到的探路者，将探路者中的路径点依次连接得到一条最优引导路径。
4.根据权利要求3所述的基于探路者算法的改进RRT路径规划方法，其特征在于，步骤(3.1)中，各个个体的第一代的中心点坐标采用unifrnd函数，如式(1)所示：
式(1)中，xmax、xmin分别为第i个个体的第一代的中心点Ti 1的横坐标的上限和下限，ymax、ymin分别为第i个个体的第一代的中心点Ti 1的纵坐标/>的上限和下限；
步骤(3.2)中，个体是由各个路径点j组成的路径，受其中心点的影响，个体的计算公式如式(2)所示：
式(2)中，xi,j为第i个个体所有路径点j的横坐标集合，yi,j为第i个个体所有路径点j的纵坐标集合；unifrnd为路径点函数；a、b分别为路径点采样区间的上限和下限；表示第i个个体的第K代的中心点Ti K的横坐标，/>表示第i个个体的第K代的中心点Ti K的纵坐标；1为生成路径点数目的维度；n为路径点的数目，包括初始点Xt、中心点和目标点Xg；
步骤(3.2)中，个体的适应度值Li为该个体中各个路径点的总长度，计算公式如式(3)所示：
步骤(3.3)中，当进行第一次迭代时，根据第一代个体的定义，第一代探路者的中心点坐标与第二代探路者的中心点坐标/>相等。
5.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，S4.1中，随机树参数包括目标点阈值h和扩展步长t；目标点阈值h设置为扩展步长t的1～1.5倍；
S4.1中，初始化后的随机树T和T′如下式所示：
{T.v(1).x T.v(1).y T.v(1).xPrev T.v(1).yPrev T.v(1).dist T.v(1).indPrev}
{T′.v′(1).x T′.v′(1).y T′.v′(1).xPrev T′.v′(1).yPrev T′.v′(1).dist T′.v′(1).indPrev}
其中，T和T′为两棵随机树，v和v′分别是T和T′的随机树节点集合，xPrev为某一节点的父节点，dist为父节点到该节点的距离，indPrev为父节点的索引；
S4.2中，邻近区域是以路径采样点为中心的边长为2r的正方形，r表示一个单位邻近区域长度；
S4.2中，随机点Xrand的位置均通过式(9)生成，随机点Xrand′的位置均通过式(10)生成：
Xrand＝pathnode+r[2rand-1,2rand-1] (9)
Xrand′＝pathnode′+r[2rand-1,2rand-1] (10)
式(9)和(10)中，rand为[0,1]区间中的随机数。
6.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，S4.3中，距离d的计算公式如式(11)所示，距离d′的计算公式如式(12)所示：
式(11)中，Xrand(1)为Xrand的横坐标，Xrand(2)为Xrand的纵坐标，T.v(i).x为随机树T节点的横坐标，T.v(i).y为随机树T节点的纵坐标；
式(12)中，Xrand(1)′为Xrand′的横坐标，Xrand(2)′为Xrand′的纵坐标，T′.v′(i).x为随机树T′节点的横坐标，T′.v′(i).y为随机树T′节点的纵坐标；
S4.3中，角度值θ的计算公式如式(13)所示，角度值θ′的计算公式如式(14)所示：
θ＝arctan(Xnew(2)-Xnear(2))/(Xnew(1)-Xnear(1)) (13)
θ′＝arctan(Xnew(2)′-Xnear(2)′)/(Xnew(1)′-Xnear(1)′) (14)
式(13)中，Xnew(1)为Xnew的横坐标，Xnew(2)为Xnew的纵坐标；Xnear(1)为Xnear的横坐标，Xnear(2)为Xnear的纵坐标；
式(14)中，Xnew(1)′为Xnew′的横坐标，Xnew(2)′为Xnew′的纵坐标；Xnear(1)′为Xnear′的横坐标，Xnear(2)′为Xnear′的纵坐标；
S4.3中，判断从Xnear到Xnew之间是否存在障碍物的方法是：以a为检测步长，从节点Xnear开始以增加检测步长a沿直线行至Xnew来检测两节点之间是否存在障碍；
节点Xnear直线距离增加检测步长a后的节点Xb如式(15)所示：
Xb＝Xnear+a[cos(θ)sin(θ)] (15)
S4.3中，判断从Xnear′到Xnew′之间是否存在障碍物的方法是：以a为检测步长，从节点Xnear′开始以增加检测步长a沿直线行至Xnew′来检测两节点之间是否存在障碍；
节点Xnear′直线距离增加检测步长a后的Xb′如式(16)所示：
Xb′＝Xnear′+a[cos(θ)sin(θ)] (16)。
7.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，S4.4中，节点Xnew与Xnew′之间的距离dg′的计算公式如下：
式(17)中，Xnew(1)为Xnew的横坐标，Xnew(2)为Xnew的纵坐标；Xnew(1)′为Xnew′的横坐标，Xnew(2)′为Xnew′的纵坐标。
8.根据权利要求1所述的基于探路者算法的改进RRT路径规划方法，其特征在于，A4.4中，节点Xnew与目标点Xg的之间距离dg的计算公式如下：
式(13)中，Xnew(1)为Xnew的横坐标，Xnew(2)为Xnew的纵坐标；Xg(1)为Xg的横坐标，Xg(2)为Xg的纵坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210206904.4A CN114593744B (zh) | 2022-03-04 | 2022-03-04 | 一种基于探路者算法的改进rrt路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210206904.4A CN114593744B (zh) | 2022-03-04 | 2022-03-04 | 一种基于探路者算法的改进rrt路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114593744A CN114593744A (zh) | 2022-06-07 |
| CN114593744B true CN114593744B (zh) | 2024-04-26 |
Family
ID=81814819
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210206904.4A Active CN114593744B (zh) | 2022-03-04 | 2022-03-04 | 一种基于探路者算法的改进rrt路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114593744B (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (ko) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법 |
| CN110609547A (zh) * | 2019-08-21 | 2019-12-24 | 中山大学 | 一种基于可视图引导的移动机器人规划方法 |
| CN112179351A (zh) * | 2020-09-30 | 2021-01-05 | 上海电机学院 | 一种基于预规划路径优化rrt算法的三维避障航迹规划方法 |
| CN112306067A (zh) * | 2020-11-13 | 2021-02-02 | 湖北工业大学 | 一种全局路径规划方法及系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6606442B2 (ja) * | 2016-02-24 | 2019-11-13 | 本田技研工業株式会社 | 移動体の経路計画生成装置 |
-
2022
- 2022-03-04 CN CN202210206904.4A patent/CN114593744B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101339480B1 (ko) * | 2012-12-14 | 2013-12-10 | 고려대학교 산학협력단 | Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법 |
| CN110609547A (zh) * | 2019-08-21 | 2019-12-24 | 中山大学 | 一种基于可视图引导的移动机器人规划方法 |
| CN112179351A (zh) * | 2020-09-30 | 2021-01-05 | 上海电机学院 | 一种基于预规划路径优化rrt算法的三维避障航迹规划方法 |
| CN112306067A (zh) * | 2020-11-13 | 2021-02-02 | 湖北工业大学 | 一种全局路径规划方法及系统 |
Non-Patent Citations (4)
| Title |
|---|
| A new meta-heuristic optimizer: Pathfinder algorithm;Yapici, Hamza et al.;《APPLIED SOFT COMPUTING》;第78卷;545-568 * |
| 基于改进RRT算法的无人驾驶汽车轨迹规划;贺伊琳;高奇;赵丹;刘伟;;西北大学学报(自然科学版)(第05期);28-35 * |
| 复杂多障碍物环境下改进的RRT*路径规划算法;余艳碧;《现代计算机》;第27卷(第28期);8-13 * |
| 移动机器人路径规划的RRT*算法研究;王坤;《中国优秀硕士学位论文全文数据库 (信息科技辑)》;I140-310 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114593744A (zh) | 2022-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113219998B (zh) | 一种基于改进双向informed-RRT*的车辆路径规划方法 | |
| CN111610786B (zh) | 基于改进rrt算法的移动机器人路径规划方法 | |
| CN113467456B (zh) | 一种未知环境下用于特定目标搜索的路径规划方法 | |
| CN111457927A (zh) | 动态障碍物下的无人巡航船多目标路径规划方法 | |
| CN101604166B (zh) | 一种基于粒子群优化算法的移动机器人路径规划方法 | |
| CN110083165A (zh) | 一种机器人在复杂狭窄环境下路径规划方法 | |
| CN110110763B (zh) | 一种基于最大公共子图的栅格地图融合方法 | |
| CN113485375A (zh) | 一种基于启发式偏置采样的室内环境机器人探索方法 | |
| CN113703450B (zh) | 基于平滑因素改进蚁群算法的移动机器人路径规划方法 | |
| CN112987799A (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
| CN114167865B (zh) | 一种基于对抗生成网络与蚁群算法的机器人路径规划方法 | |
| CN113985888A (zh) | 一种基于改进蚁群算法的叉车路径规划方法及系统 | |
| Wang et al. | Efficient robot motion planning using bidirectional-unidirectional RRT extend function | |
| CN114593744B (zh) | 一种基于探路者算法的改进rrt路径规划方法 | |
| CN115903818A (zh) | 一种变电站巡检机器人路径规划方法 | |
| Liu et al. | A prior information heuristic based robot exploration method in indoor environment | |
| CN117109625B (zh) | 一种基于改进prm算法的无人驾驶路径规划方法 | |
| CN114967680A (zh) | 基于蚁群算法和卷积神经网络的移动机器人路径规划方法 | |
| CN116400737B (zh) | 一种基于蚁群算法的安全路径规划系统 | |
| CN116608855A (zh) | 一种基于改进哈里斯鹰算法的机器人路径规划方法 | |
| CN117315025A (zh) | 一种基于神经网络的机械臂6d位姿抓取方法 | |
| CN114509085B (zh) | 一种结合栅格和拓扑地图的快速路径搜索方法 | |
| CN107657148B (zh) | 基于长时间跨度全球中尺度涡旋的断点续追方法 | |
| CN115061467A (zh) | 基于改进高斯采样prm算法和狭窄通道识别的路径规划方法 | |
| CN111580563A (zh) | 一种基于种子搜索的无人机自主避障飞行方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |