CN110705803B - 基于三角形内心引导rrt算法的路径规划方法 - Google Patents

基于三角形内心引导rrt算法的路径规划方法 Download PDF

Info

Publication number
CN110705803B
CN110705803B CN201910964030.7A CN201910964030A CN110705803B CN 110705803 B CN110705803 B CN 110705803B CN 201910964030 A CN201910964030 A CN 201910964030A CN 110705803 B CN110705803 B CN 110705803B
Authority
CN
China
Prior art keywords
node
point
random
coordinate
value
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
Application number
CN201910964030.7A
Other languages
English (en)
Other versions
CN110705803A (zh
Inventor
张卫波
肖继亮
陈泉泉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fuzhou University
Original Assignee
Fuzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Fuzhou University filed Critical Fuzhou University
Priority to CN201910964030.7A priority Critical patent/CN110705803B/zh
Publication of CN110705803A publication Critical patent/CN110705803A/zh
Application granted granted Critical
Publication of CN110705803B publication Critical patent/CN110705803B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem

Abstract

本发明涉及一种基于三角形内心引导RRT算法的路径规划方法。本方法为了克服RRT算法存在的缺点及将目标点以一定概率出现在随机点中会导致陷入局部最小的危险,提出利用三角形内心来引导随机树的方法。通过将随机函数生成的随机点、随机树中与该随机点距离最近的点及目标点三点构成三角形的三个顶点，再计算该三角形的内心坐标，用该内心坐标作为随机树的生长方向；另外通过在一定循环次数下，记录使用内心引导的次数及随机树的生长情况来调整采样方式。这样不仅对随机树的生长进行了引导，还有效的避免了陷入局部最小风险。本发明的方法提高了规划的效率，规划路径所需时间更少，迭代次数更少，路径更短。

Description

基于三角形内心引导RRT算法的路径规划方法
技术领域
本发明涉及路径规划领域，具体涉及一种基于三角形内心引导RRT算法的路径规划方法。
背景技术
路径规划问题一直以来都是移动机器人领域的研究热点。路径规划主要任务就是在空间中找到一条从初始状态点到目标状态点的连续无碰撞路径，同时满足相应的条件约束。
常用的轨迹规划算法有图搜索算法、仿生智能算法、人工势场法等。传统的路径规划算法，如人工势场法，该方法基于矢量合成，通过障碍物对移动机器人的排斥力和目标位置对移动机器人的吸引力的合力作用下规划机器人的运动路径，但当吸引力与斥力的合力为零时，机器人就陷入了局部最小的情况。
RRT(快速搜索随机树)算法作为一种基于采样的增量式快速搜索算法，可以直接用于非完整性规划和动力学规划中。但由于基本RRT算法采用均匀采样的策略，缺乏引导信息，由此得出的路径迂回曲折，在较复杂的障碍物分布下，规划时间长；另外，对于将目标点以一定概率作为随机点出现在采样点中，这样虽然可以提高规划效率，但也可能导致陷入局部最小，最终在规定时间或迭代次数下找不到路径。因此，对于以上问题，急需提出一种既能够引导RRT算法快速找到路径，又应当避免陷入局部最小风险的算法。
发明内容
有鉴于此，本发明的目的在于提供一种基于三角形内心引导RRT算法的路径规划方法，克服当前RRT算法进行路径规划存在的缺点，在引入三角形内心降低搜索的随机性，提高收敛速率，另外通过对利用三角形内心进行采样点的数量和随机采样点的数量控制，可以有效避免陷入地图陷阱中，使得算法能够快速稳定的规划出相应路径。
为实现上述目的，本发明采用如下技术方案：
一种基于三角形内心引导RRT算法的路径规划方法，提供规划路径的起始点qstart、目标点qgoal、障碍物位置和规划地图，包括以下步骤：
步骤1：定义步长eps和最大迭代次数n，以qstart为随机树T的根节点，并将其作为下一次扩展随机树的父节点，以qgoal为随机树T的目标节点，三角形内心采样点数Ndc初始值为0，随机点采样数Rdc初始值为0，初始化随机树；
步骤2：通过随机函数生成随机点qrand的坐标值；
步骤3：遍历随机树T中的每个节点，通过计算T中节点与随机生成的节点qrand之间的距离，找出离节点qrand距离最小的节点设为qnear；
步骤4：在Ndc数值小于100的情况下，以随机节点qrand、离节点qrand距离最小的节点qnear及目标节点qgoal三点的坐标值作为三角形三个顶点的坐标值，计算以这三点组成的三角形的内心坐标节点qinh重新赋值给qrand；
步骤5：从节点qnear向节点qrand方向延伸步长eps，得到新节点qnew；
步骤6：判断新节点qnew与节点qnear之间是都存在障碍物，若不存在，则将生成的新节点qnew加入的随机树T中，并判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，若在目标点qgoal坐标值步长eps范围内，表明已经找到路径，跳出循环，否者进行下一步；若存在障碍物，则舍弃节点qnew；
步骤7：更新三角形内心采样点数Ndc及随机点采样数Rdc；
步骤8：重复步骤2-7，直到节点到达目标点的eps范围内或达到最大迭代次数；
步骤9：将位于目标节点eps范围内节点作为目标节点的父节点，从目标节点qgoal开始，根据父节点回溯到起点qstart，得到最终路径。
进一步的，步骤3、4、7中提到的距离计算时通过欧几里得公式计算得出两点之间的距离。具体公式如下：
Figure BDA0002229901040000031
上式中的x1、x2、y1、y2分别表示两点的x坐标值和y坐标值。
其中，在步骤4中的三角形内心坐标计算公式如下：
Figure BDA0002229901040000032
Figure BDA0002229901040000041
上式中的x1、x2、y1、y2分别表示两点的x坐标值和y坐标值。
进一步的，所述步骤5中新节点qnew坐标的计算过程为：
计算节点qnear与qrand之间的距离为val；若val小于eps，则qnew节点的坐标即为qrand点的坐标，否则qnew节点的坐标由以下公式计算得出：
Figure BDA0002229901040000042
上式中的qnear.x、qrand.x、qenar.y、qrand.y分别表示节点qnear和节点qrand两点的x坐标值和y坐标值。
进一步的，在步骤6中，判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，是通过判断新节点qnew是否在以qgoal为圆心，以eps为半径的圆域内。
进一步的，步骤7中的更新三角形内心采样点数Ndc及随机点采样数Rdc具体操作为：若Ndc数值小于100且新节点qnew到目标点的距离小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，则将新节点qnew到目标点的距离赋值给qmin，Ndc的值归零；若Ndc数值小于100且新节点qnew到目标点的距离不小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，Ndc数值加一；若Ndc数值小于100，Rdc数值归零；若Ndc数值不小于100，Rdc数值加一。若Rdc数值大于50，Ndc数值归零。
本发明与现有技术相比具有以下有益效果：
本发明提出一种基于三角形内心引导RRT算法的路径规划方法，在随机采样点的基础上通过利用随机点qrand，目标点qgoal和离随机点最近的节点qnear三点作为三角形的三个顶点，计算出三角形的内心作为新的随机点，在不失采样点随机性的同时达到随机树扩展趋向目标点的目的，大大提高了寻路效率。
附图说明
图1为本发明的方法流程图。
图2为本发明一实施例中采用算法的伪代码。
图3为本发明一实施例中RRT算法在有障碍物情况下某次的路径规划图。
图4为本发明一实施例中本发明方法在有障碍物情况下某次的路径规划图。
图5为本发明一实施例中RRT算法与本发明方法在找到路径时的迭代次数对比图。
图6为本发明一实施例中RRT算法与本本发明方法在找到路径时的最终路径长度对比图。
图7为本发明一实施例中RRT算法与本发明方法在找到路径时所需时间对比图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
在本实施例中，准备工作：规划环境的范围为1000X1000，设路径规划的起始点为qstart＝(50,50)，目标点为qgoal＝(950,950)，图中含有4个矩形障碍物，其位置如图3所示。
请参照图1，本发明提供基于三角形内心引导RRT算法的路径规划方法，提供规划路径的起始点qstart、目标点qgoal、障碍物位置和规划地图，包括以下步骤：
步骤1：定义步长eps＝15和最大迭代次数5000，以qstart为随机树T的根节点，并将其作为下一次扩展随机树的父节点，以qgoal为随机树T的目标节点，三角形内心采样点数Ndc初始值为0，随机点采样数Rdc初始值为0，初始化随机树。
步骤2：通过随机函数生成随机点qrand的坐标值；
步骤3：遍历随机树T中的每个节点，通过计算T中节点与随机生成的节点qrand之间的距离，找出离节点qrand距离最小的节点设为qnear；
步骤4：在Ndc数值小于100的情况下，以随机节点qrand、离节点qrand距离最小的节点qnear及目标节点qgoal三点的坐标值作为三角形三个顶点的坐标值，计算以这三点组成的三角形的内心坐标节点qinh重新赋值给qrand。
具体计算公式如下：
Figure BDA0002229901040000071
Figure BDA0002229901040000072
上式中的x1、x2、y1、y2分别表示两点的x坐标值和y坐标值。
步骤5：从节点qnear向节点qrand方向延伸步长eps，得到新节点qnew；新节点qnew坐标的计算过程为：先计算节点qnear与qrand之间的距离为val；若val小于eps，则qnew节点的坐标即为qrand点的坐标，否则qnew节点的坐标由以下公式计算得出：
Figure BDA0002229901040000073
上式中的qnear.x、qrand.x、qenar.y、qrand.y分别表示节点qnear和节点qrand两点的x坐标值和y坐标值。
步骤6：判断新节点qnew与节点qnear之间是否存在障碍物，若不存在，则将生成的新节点qnew加入的随机树T中,并判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，若在目标点qgoal坐标值步长eps范围内，表明已经找到路径，跳出循环，否者进行下一步；若存在障碍物，则舍弃节点qnew。
步骤7：更新三角形内心采样点数Ndc及随机点采样数Rdc。更新三角形内心采样点数Ndc及随机点采样数Rdc具体操作为：若Ndc数值小于100且新节点qnew到目标点的距离小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，则将新节点qnew到目标点的距离赋值给qmin，Ndc的值归零；若Ndc数值小于100且新节点qnew到目标点的距离不小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，Ndc数值加一；若Ndc数值小于100，Rdc数值归零；若Ndc数值不小于100，Rdc数值加一。若Rdc数值大于50，Ndc数值归零。
步骤8：重复步骤2-7，直到节点到达目标点的eps范围内或达到最大迭代次数。
步骤9：将位于目标节点eps范围内节点作为目标节点的父节点，从目标节点qgoal开始，根据父节点回溯到起点qstart，得到最终路径。如图3和图4所示。
为验证改进算法的效率和稳定性，通过分别运行50次RRT算法与50次本发明改进算法进行路径规划，记录试验数据。得到RRT算法与本发明改进RRT算法在找到路径时的迭代次数、路径长度及时间对比图如图5、图6和图7所示。
由图5、图6及图7可知，本发明方法与RRT算法相比较，在同样的环境中找到路径所需的迭代次数、路径长度及时间均有明显的减少。
试验结果数据的对比分析如下表所示：
Figure BDA0002229901040000081
Figure BDA0002229901040000091
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (3)

1.一种基于三角形内心引导RRT算法的路径规划方法，提供规划路径的起始点qstart、目标点qgoal、障碍物位置和规划地图，其特征在于，包括以下步骤：
步骤1：定义步长eps和最大迭代次数n，以qstart为随机树T的根节点，并将其作为下一次扩展随机树的父节点，以qgoal为随机树T的目标点，三角形内心采样点数Ndc初始值为0，随机点采样数Rdc初始值为0，初始化随机树；
步骤2：通过随机函数生成随机点qrand的坐标值；
步骤3：遍历随机树T中的每个节点，通过计算随机树T中节点与随机点qrand之间的距离，找出离随机点qrand距离最小的节点设为qnear；
步骤4：在Ndc数值小于100的情况下，以随机点qrand、离随机点qrand距离最小的随机点qnear及目标节点qgoal三点的坐标值作为三角形三个顶点的坐标值，计算三个顶点组成的三角形的内心坐标节点qinh重新赋值给随机点qrand；
步骤5：从节点qnear向随机点qrand方向延伸步长eps，得到新节点qnew；
步骤6：判断新节点qnew与节点qnear之间是否存在障碍物，若不存在，则将生成的新节点qnew加入的随机树T中，并判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，若在目标点qgoal坐标值步长eps范围内，表明已经找到路径，跳出循环，否则进行下一步；若存在障碍物，则舍弃新节点qnew；
步骤7：更新三角形内心采样点数Ndc及随机点采样数Rdc；
步骤8：重复步骤2-7，直到到达目标点的eps范围内或达到最大迭代次数；
步骤9：将位于目标点qgoal的步长eps范围内的节点作为目标节点的父节点，从目标点qgoal开始，根据父节点回溯到起始点qstart，得到最终路径；
所述步骤4中的三角形内心坐标计算公式如下：
Figure FDA0003566380050000021
Figure FDA0003566380050000022
上式中的x1表示随机点qrand的x坐标值，x2表示节点qnear的x坐标，x3表示目标点qgoal的x坐标值，y1表示随机点qrand的y坐标值，y2表示节点qnear的y坐标，y3表示目标点qgoal的y坐标值；
所述步骤5中新节点qnew坐标的计算过程为：
计算节点qnear与随机点qrand之间的距离为val；若val小于eps，则新节点qnew的坐标即为随机点qrand的坐标，否则新节点qnew的坐标由以下公式计算得出：
Figure FDA0003566380050000031
上式中的qnear.x表示节点qnear的x坐标、qrand.x表示随机点qrand的x坐标值、qenar.y表示节点qnear的y坐标、qrand.y表示随机点qrand的y坐标值,dist表示节点qnear和随机点qrand的距离。
2.根据权利要求1所述的基于三角形内心引导RRT算法的路径规划方法，其特征在于：所述步骤6中，判断新节点qnew的坐标值是否在目标点qgoal坐标值步长eps范围内，是通过判断新节点qnew是否在以qgoal为圆心，以eps为半径的圆域内。
3.根据权利要求1所述的基于三角形内心引导RRT算法的路径规划方法，其特征在于：所述步骤7中的更新三角形内心采样点数Ndc及随机点采样数Rdc具体操作为：若Ndc数值小于100且新节点qnew到目标点的距离小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，则将新节点qnew到目标点的距离赋值给qmin，Ndc的值归零；若Ndc数值小于100且新节点qnew到目标点的距离不小于随机树T中除新节点qnew以外的任一点到目标点的距离qmin，Ndc数值加一；若Ndc数值小于100，Rdc数值归零；若Ndc数值不小于100，Rdc数值加一；若Rdc数值大于50，Ndc数值归零。
CN201910964030.7A 2019-10-11 2019-10-11 基于三角形内心引导rrt算法的路径规划方法 Active CN110705803B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910964030.7A CN110705803B (zh) 2019-10-11 2019-10-11 基于三角形内心引导rrt算法的路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910964030.7A CN110705803B (zh) 2019-10-11 2019-10-11 基于三角形内心引导rrt算法的路径规划方法

Publications (2)

Publication Number Publication Date
CN110705803A CN110705803A (zh) 2020-01-17
CN110705803B true CN110705803B (zh) 2022-06-21

Family

ID=69199304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910964030.7A Active CN110705803B (zh) 2019-10-11 2019-10-11 基于三角形内心引导rrt算法的路径规划方法

Country Status (1)

Country Link
CN (1) CN110705803B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112488359B (zh) * 2020-11-02 2023-11-17 杭州电子科技大学 基于rrt与ospa距离的多智能体静止多目标围捕方法
CN113064426B (zh) * 2021-03-17 2022-03-15 安徽工程大学 一种改进双向快速搜索随机树算法的智能车路径规划方法
CN114047770B (zh) * 2022-01-13 2022-03-29 中国人民解放军陆军装甲兵学院 一种多内心搜寻改进灰狼算法的移动机器人路径规划方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107085437A (zh) * 2017-03-20 2017-08-22 浙江工业大学 一种基于eb‑rrt的无人机航迹规划方法
CN108444489A (zh) * 2018-03-07 2018-08-24 北京工业大学 一种改进rrt算法的路径规划方法
CN109542117A (zh) * 2018-10-19 2019-03-29 哈尔滨工业大学（威海） 基于改进rrt的水下航行器滚动规划算法
CN110228069A (zh) * 2019-07-17 2019-09-13 东北大学 一种机械臂在线避障运动规划方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6606442B2 (ja) * 2016-02-24 2019-11-13 本田技研工業株式会社 移動体の経路計画生成装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107085437A (zh) * 2017-03-20 2017-08-22 浙江工业大学 一种基于eb‑rrt的无人机航迹规划方法
CN108444489A (zh) * 2018-03-07 2018-08-24 北京工业大学 一种改进rrt算法的路径规划方法
CN109542117A (zh) * 2018-10-19 2019-03-29 哈尔滨工业大学（威海） 基于改进rrt的水下航行器滚动规划算法
CN110228069A (zh) * 2019-07-17 2019-09-13 东北大学 一种机械臂在线避障运动规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Triangular Geometry based Optimal Motion Planning using RRT*-Motion Planner;A H Qureshi et al.;《AMC2014-Yokohama》;20140316;全文 *

Also Published As

Publication number Publication date
CN110705803A (zh) 2020-01-17

Similar Documents

Publication Publication Date Title
CN113219998B (zh) 一种基于改进双向informed-RRT*的车辆路径规划方法
CN110705803B (zh) 基于三角形内心引导rrt算法的路径规划方法
US10365110B2 (en) Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles
CN111024092B (zh) 一种多约束条件下智能飞行器航迹快速规划方法
CN111562785B (zh) 一种群集机器人协同覆盖的路径规划方法及系统
JP6711949B2 (ja) １個以上の障害物を回避して始状態から終状態集合まで移動する物体の経路を決定する方法およびシステム
CN111610786B (zh) 基于改进rrt算法的移动机器人路径规划方法
CN112539750B (zh) 一种智能运输车路径规划方法
CN113867368A (zh) 一种基于改进海鸥算法的机器人路径规划方法
CN112327876B (zh) 一种基于终距指数的机器人路径规划方法
US20220203534A1 (en) Path planning method and biped robot using the same
CN112923944A (zh) 一种自动驾驶路径规划方法、系统及计算机可读存储介质
CN110954124A (zh) 一种基于a*-pso算法的自适应路径规划方法及系统
CN111427341B (zh) 一种基于概率地图的机器人最短预期时间目标搜索方法
CN110530373B (zh) 一种机器人路径规划方法、控制器及系统
Seder et al. Hierarchical path planning of mobile robots in complex indoor environments
CN115454067A (zh) 一种基于融合算法的路径规划方法
CN114237302B (zh) 一种基于滚动时域的三维实时rrt*航路规划方法
CN114115271A (zh) 一种改进rrt的机器人路径规划方法和系统
CN116009527A (zh) 基于动态场景结构膨胀感知的路径规划算法
Abiyev et al. Improved path-finding algorithm for robot soccers
CN111912411B (zh) 一种机器人导航定位方法、系统及存储介质
de Carvalho Santos et al. An exploratory path planning method based on genetic algorithm for autonomous mobile robots
Kneebone et al. Navigation planning in probabilistic roadmaps with uncertainty
CN117848372A (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