CN108458717B - 一种迭代的快速扩展随机树irrt的无人机路径规划方法 - Google Patents

一种迭代的快速扩展随机树irrt的无人机路径规划方法 Download PDF

Info

Publication number
CN108458717B
CN108458717B CN201810224613.1A CN201810224613A CN108458717B CN 108458717 B CN108458717 B CN 108458717B CN 201810224613 A CN201810224613 A CN 201810224613A CN 108458717 B CN108458717 B CN 108458717B
Authority
CN
China
Prior art keywords
tree
path
new
node
random
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
CN201810224613.1A
Other languages
English (en)
Other versions
CN108458717A (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.)
Xidian University
Original Assignee
Xidian 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 Xidian University filed Critical Xidian University
Priority to CN201810224613.1A priority Critical patent/CN108458717B/zh
Publication of CN108458717A publication Critical patent/CN108458717A/zh
Application granted granted Critical
Publication of CN108458717B publication Critical patent/CN108458717B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/20—Instruments for performing navigational calculations

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)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种迭代的快速扩展随机树IRRT的无人机路径规划方法，本方法引入了启发信息，当随机采样增加新的叶节点Xnew时，引入随机概率P，让新的叶节点Xnew以一定的概率偏向目标点，削弱了新增的叶节点Xnew的随机性，同时也继承了原有算法的全部优点，加快了收敛的速度，在此基础上，通过迭代比较得到较优的路径，存储较优路径所在的上周期树，解决了在动态环境中的路径规划问题，也使得规划出的路径接近较优的路径，路径质量大大提高。

Description

一种迭代的快速扩展随机树IRRT的无人机路径规划方法
技术领域
本发明涉及无人机路径规划领域，特别是一种迭代的快速扩展随机树IRRT的无人机路径规划方法。
背景技术
无人驾驶飞机简称“无人机”，是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，或者由车载计算机完全地或间歇地自主地操作。目前，无人机应用在航拍、农业、植保、微型自拍、快递运输、灾难救援、观察野生动物、监控传染病、测绘、新闻报道、电力巡检、救灾、影视拍摄、制造浪漫等领域，有着广泛的用途。而要实现无人机的智能化,路径规划是无人机自主飞行的关键环节,路径规划的主要目的是根据任务需求规划出满足约束条件的飞行路径(从起点到终点),优秀的路径规划能够在很大程度上降低无人机耗油和减少碰到建筑物造成损坏的发生率，同时能很快的到达目标点。
无人机在三维环境中的飞行环境，和二维环境中的地面机器人相比更加复杂。传统的路径规划算法，像A*算法,Dijkstra算法,这些方法需要已知全部的环境信息，然后构建环境模型后才可以进行路径搜索，而且在高维环境中它们的计算量也呈指数形式递增，实时性很差。
针对无人机在陌生的三维环境中，无法事先建立好空间模型，同时考虑时间约束条件，规划维度上升到四维规划，因此在陌生高维环境下的路径规划，基于采样的路径规划方法取得很大的发展，例如概率地图(Probabilistic Roadmap Method,PRM)算法和快速随机搜索树方法(Rapidly-exploring Random Tree,RRT)。
基于采样的路径规划算法是在状态空间中进行采样然后进行碰撞检测，不需要对空间模型预处理，而且在高维环境中计算量也不会改变，所以很适合高维陌生的环境。和PRM算法相比,RRT算法通过随机采样构造快速搜索树节约了构造路线图所需要的内存空间,受到了研究者的普遍关注。
但是由于RRT基于随机采样法，尤其是在已经规划好的路径上动态出现障碍物时，需要重新规划全部路径，该算法会存在费时，收敛速度慢，路径规划可重复性差等方面的问题。
发明内容
本发明的目的是要提供一种迭代的快速扩展随机树IRRT的无人机路径规划方法，引入随机概率P并通过迭代比较存储较优的路径所在的上周期树来加快收敛速度，当无人机遇到动态障碍物时，进行局部重规划，不需要全局重新规划路径，来解决路径可重复性差的问题。
为达到上述目的，本发明是按照以下技术方案实施的：
一种迭代的快速扩展随机树IRRT的无人机路径规划方法，包括以下步骤：
S1、确定起始点Xstart和终点Xgoal，确定步长stepsize和最大迭代次数iter，以Xstart为快速扩展随机树RRT的根节点，以Xgoal为快速扩展随机树RRT的目标点，初始化快速扩展随机树RRT；
S2、以Xstart为起点对初始化后的快速扩展随机树RRT拓展，以步长stepsize生成随机点Xrand，和随机概率P偏向目标点Progoal，如果0<P<Progoal,则以一定的概率偏向Xgoal，沿Xgoal方向扩展；否则沿随机点Xrand方向扩展得到新的节点Xnew，在快速扩展随机树RRT中获取离Xrand最近的一个位形节点Xnearest，Xnearest向Xrand移动一个步长stepsize则在Xstart上增加新的节点Xnew，计算公式为：
Figure GDA0002361462530000031
Figure GDA0002361462530000032
其中，||Xtemptarget-Xnearest||是欧式距离；
S3、检测产生新的节点Xnew是否与周围的空间产生碰撞；如果产生碰撞，则返回步骤S2，重新选择新的节点Xnew，直到到达Xgoal所在区域；
S4、存储Xstart到Xgoal的路径，比较之后存储较优的路径，设为RRTreeoptimization，直到达到最大迭代次数，如果到达最大迭代次数时还没有到达目标点Xgoal则规划失败，得到较优的RRTree optimization后，存储历史树，定义为上周期树；
S5、当无人机按照步骤S4规划好的路径执行，遇到动态障碍物时，则沿着障碍物运动的反方向进行随机采样，按照步骤S4中存储的上周期树，利用原来较优的路径所在的RRTree optimization进行局部重规划，直到连接到较优的上周期树RRTree optimization中，则得到一条新的可行路径。
S6、将步骤S5得到的可行路径利用贝塞尔曲线实现进行平滑处理，即完成无人机路径规划，其中，得到的可行路径利用贝塞尔曲线实现进行平滑处理的计算公式为：
Figure GDA0002361462530000041
Figure GDA0002361462530000042
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
＝(P3-3P2+3P1-P0)t3+(3P2-6P1+3P0)t2+(3P1-3P0)t+P0
与现有技术相比，本发明针对随机性和路径的质量问题，本方法引入了启发信息，当随机采样增加新的叶节点Xnew时，引入随机概率P，让新的叶节点Xnew以一定的概率偏向目标点，削弱了新增的叶节点Xnew的随机性，同时也继承了原有算法的全部优点，确定了大致的目标方向，加快了收敛的速度，也使得规划出的路径接近较优的路径，路径质量大大提高。
而且解决了RRT算法在动态环境中不稳定问题，在存储历史路径之前进行比较找到一条较优的路径，将较优的路径作为历史路径(上周期树)存储，当遇到动态障碍物，在上周期树的基础上进行局部重规划，这里局部重规划中采样时在动态障碍物运动的反方向的周围采样，这样得到的路径没有偏离较优的路径，提高了达到最优路径的概率，即使没有遇到障碍物，路径也是相对较优的。在此基础上，同时采用偏向目标的RRT算法，也就是一定的概率偏向目标点，加快了算法的收敛速度。但是得到的一系列的采样点组成的路径，在实际的空间环境下飞行器不可能在直线飞行时突然拐向另一条飞行路线，因此要对这些离散的，不可飞行的采样点进行路径平滑，综合时间性能，最后采用贝塞尔曲线进行路径平滑。
附图说明
图1为本发明的流程图。
图2为基本RRT算法的扩展图。
图3为不同环境下的实验仿真图。
图4为路径长度对比图。
具体实施方式
下面结合具体实施例对本发明作进一步描述，在此发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。
如图1、图2所示，本实施例的一种迭代的快速扩展随机树IRRT的无人机路径规划方法，包括以下步骤：
S1、确定起始点Xstart和终点Xgoal，确定步长stepsize和最大迭代次数iter，以Xstart为快速扩展随机树RRT的根节点，以Xgoal为快速扩展随机树RRT的目标点，初始化快速扩展随机树RRT；
S2、以Xstart为起点对初始化后的快速扩展随机树RRT拓展，以步长stepsize生成随机点Xrand，和随机概率P偏向目标点Progoal，如果0<P<Progoal,则以一定的概率偏向Xgoal，沿Xgoal方向扩展；否则沿随机点Xrand方向扩展得到新的节点Xnew，在快速扩展随机树RRT中获取离Xrand最近的一个位形节点Xnearest，Xnearest向Xrand移动一个步长stepsize则在Xstart上增加新的节点Xnew，计算公式为：
Figure GDA0002361462530000051
Figure GDA0002361462530000052
其中，||Xtemptarget-Xnearest||是欧式距离；
S3、检测产生新的节点Xnew是否与周围的空间产生碰撞；如果产生碰撞，则返回步骤S2，重新选择新的节点Xnew，直到到达Xgoal所在区域；
S4、存储Xstart到Xgoal的路径，比较之后存储较优的路径，设为RRTreeoptimization，直到达到最大迭代次数，如果到达最大迭代次数时还没有到达目标点Xgoal则规划失败，得到较优的RRTree optimization后，存储历史树，定义为上周期树；
S5、当无人机按照步骤S4规划好的路径执行，则沿着障碍物运动的反方向进行随机采样，按照步骤S4中存储的上周期树，利用原来较优的路径所在的RRTree optimization进行局部重规划，直到连接到较优的上周期树RRTree optimization中，则得到一条新的可行路径。
S6、将步骤S5得到的可行路径利用贝塞尔曲线实现进行平滑处理，即完成无人机路径规划，其中，得到的可行路径利用贝塞尔曲线实现进行平滑处理的计算公式为：
Figure GDA0002361462530000061
Figure GDA0002361462530000062
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
＝(P3-3P2+3P1-P0)t3+(3P2-6P1+3P0)t2+(3P1-3P0)t+P0
其中，n＝3。
具体的本发明的迭代的快速扩展随机树IRRT算法的伪代码为：
Figure GDA0002361462530000071
对本发明的迭代的快速扩展随机树IRRT算法的伪代码进行说明：
第二行；初始化迭代次数iter；
第三行：调用基本的rrt算法；
第四行到第十行:通过iter次迭代比较得到较优路径所在的树；
第十二行到第十三行：如果目标点没有太大变化，执行局部重规划函数Localreplan；
第十四行到第十六行：如果目标点发生大的变化，则重新执行；
第十八行到结束：为局部重规划函数；从第一个节点开始搜索，直到最后一个节点；当节点出现碰撞，则删除节点和它的孩子节点，然后从删除节点的父节点开始重新扩展，直到连接到所存储的上周期树RRTree optimization中。
本发明的技术方案不限于上述具体实施例的限制，凡是根据本发明的技术方案做出的技术变形，均落入本发明的保护范围之内。

Claims (3)

1.一种迭代的快速扩展随机树IRRT的无人机路径规划方法，其特征在于，包括以下步骤：
S1、确定起始点Xstart和终点Xgoal，确定步长stepsize和最大迭代次数iter，以Xstart为快速扩展随机树RRT的根节点，以Xgoal为快速扩展随机树RRT的目标点，初始化快速扩展随机树RRT；
S2、以Xstart为起点对初始化后的快速扩展随机树RRT拓展，以步长stepsize生成随机点Xrand，和随机概率P偏向目标点Progoal，如果0<P<Progoal,则以一定的概率偏向Xgoal，沿Xgoal方向扩展；否则沿随机点Xrand方向扩展得到新的节点Xnew；
S3、检测产生新的节点Xnew是否与周围的空间产生碰撞；如果产生碰撞，则返回步骤S2，重新选择新的节点Xnew，直到到达Xgoal所在区域；
S4、存储Xstart到Xgoal的路径，比较之后存储较优的路径，设为RRTree optimization，直到达到最大迭代次数，如果到达最大迭代次数时还没有到达目标点Xgoal则规划失败，得到较优的RRTree optimization后，存储历史树，定义为上周期树；
S5、当无人机按照步骤S4规划好的路径执行，遇到动态障碍物时，则沿着障碍物运动的反方向进行随机采样，按照步骤S4中存储的上周期树，利用原来较优的路径所在的RRTreeoptimization进行局部重规划，直到连接到较优的上周期树RRTree optimization中，则得到一条新的可行路径；其中，从第一个节点开始搜索，直到最后一个节点；当节点出现碰撞，则删除节点和它的孩子节点，然后从删除节点的父节点开始重新扩展，直到连接到所存储的上周期树RRTree optimization中；
S6、将步骤S5得到的可行路径利用贝塞尔曲线实现进行平滑处理，即完成无人机路径规划。
2.根据权利要求1所述的迭代的快速扩展随机树IRRT的无人机路径规划方法，其特征在于：所述步骤S2中如果P＞Progoal时，在快速扩展随机树RRT中获取离Xrand最近的一个位形节点Xnearest，Xnearest向Xrand移动一个步长stepsize则在Xstart上增加新的节点Xnew，计算公式为：
Figure FDA0002361462520000021
Figure FDA0002361462520000022
其中，||Xtemptarget-Xnearest||是欧式距离。
3.根据权利要求1所述的迭代的快速扩展随机树IRRT的无人机路径规划方法，其特征在于：所述步骤S6中将步骤S5得到的可行路径利用贝塞尔曲线实现进行平滑处理的计算公式为：
Figure FDA0002361462520000023
Figure FDA0002361462520000024
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
＝(P3-3P2+3P1-P0)t3+(3P2-6P1+3P0)t2+(3P1-3P0)t+P0
其中n＝3。
CN201810224613.1A 2018-05-07 2018-05-07 一种迭代的快速扩展随机树irrt的无人机路径规划方法 Active CN108458717B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810224613.1A CN108458717B (zh) 2018-05-07 2018-05-07 一种迭代的快速扩展随机树irrt的无人机路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810224613.1A CN108458717B (zh) 2018-05-07 2018-05-07 一种迭代的快速扩展随机树irrt的无人机路径规划方法

Publications (2)

Publication Number Publication Date
CN108458717A CN108458717A (zh) 2018-08-28
CN108458717B true CN108458717B (zh) 2020-04-07

Family

ID=63236959

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810224613.1A Active CN108458717B (zh) 2018-05-07 2018-05-07 一种迭代的快速扩展随机树irrt的无人机路径规划方法

Country Status (1)

Country Link
CN (1) CN108458717B (zh)

Families Citing this family (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10878706B2 (en) * 2018-10-12 2020-12-29 Aurora Flight Sciences Corporation Trajectory planner for a vehicle
US10990099B2 (en) * 2018-11-16 2021-04-27 Great Wall Motor Company Limited Motion planning methods and systems for autonomous vehicle
US20200159233A1 (en) * 2018-11-16 2020-05-21 Great Wall Motor Company Limited Memory-Based Optimal Motion Planning With Dynamic Model For Automated Vehicle
CN109669459B (zh) * 2018-12-28 2022-05-10 西北工业大学 一种采用不变集的动态反馈路径规划方法
CN112045664A (zh) * 2019-06-06 2020-12-08 南京理工大学 一种基于ros系统的通用机械臂控制器
CN110285802B (zh) * 2019-06-11 2022-09-16 安徽理工大学 快速扩展随机树路径光滑方法
CN110531782A (zh) * 2019-08-23 2019-12-03 西南交通大学 用于社区配送的无人机航迹路径规划方法
CN110531770B (zh) * 2019-08-30 2023-06-02 的卢技术有限公司 一种基于改进的rrt路径规划方法和系统
CN111427341B (zh) * 2019-11-05 2023-11-14 杭州电子科技大学 一种基于概率地图的机器人最短预期时间目标搜索方法
CN111189455B (zh) * 2020-01-14 2022-02-01 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种无人机航路规划方法、系统及存储介质
US11592843B2 (en) 2020-04-08 2023-02-28 Lockheed Martin Corporation Autonomous aircraft local planning to avoid obstructions
CN111582556B (zh) * 2020-04-20 2023-09-22 西安工程大学 基于rrt算法的智能包裹分拣系统路径规划方法
CN111638526B (zh) * 2020-05-20 2022-08-26 电子科技大学 一种陌生环境下机器人自主建图的方法
CN111707264A (zh) * 2020-05-30 2020-09-25 同济大学 一种改进拓展式rrt路径规划方法、系统及装置
CN111723983B (zh) * 2020-06-12 2022-09-09 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种未知环境下无人机的时间参数化航路规划方法与系统
CN111879307A (zh) * 2020-06-22 2020-11-03 国网河北省电力有限公司信息通信分公司 一种基于车身参数及工程建设信息的车辆路径规划方法
CN111707269B (zh) * 2020-06-23 2022-04-05 东南大学 一种三维环境下的无人机路径规划方法
CN111752306B (zh) * 2020-08-14 2022-07-12 西北工业大学 一种基于快速扩展随机树的无人机航路规划方法
CN112902971B (zh) * 2021-03-22 2022-10-04 中国长江三峡集团有限公司 基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法、电子设备及存储介质
CN113515123B (zh) * 2021-06-25 2024-04-12 北京精密机电控制设备研究所 一种基于改进rrt算法的机器人实时路径规划方法
CN113359775B (zh) * 2021-07-08 2022-01-18 哈尔滨理工大学 一种动态变采样区域rrt无人车路径规划方法
CN113467500B (zh) * 2021-07-19 2022-10-11 天津大学 一种基于双目视觉的无人机非合作目标追踪系统
CN113741484A (zh) * 2021-09-30 2021-12-03 暨南大学 一种基于概率模型的路径规划方法、系统及介质
CN116578121B (zh) * 2023-07-10 2023-11-03 广东电网有限责任公司云浮供电局 基于约束采样的扩展随机树的生成方法及轨迹规划方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104155974B (zh) * 2013-07-29 2017-05-17 深圳信息职业技术学院 一种用于机器人快速避碰的路径规划方法及设备
CN104359473A (zh) * 2014-10-24 2015-02-18 南京航空航天大学 一种动态环境下无人机编队飞行的协同航迹智能规划方法
CN106406338B (zh) * 2016-04-14 2023-08-18 中山大学 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法
CN106444740B (zh) * 2016-07-15 2019-03-12 浙江工业大学 基于mb-rrt的无人机二维航迹规划方法

Also Published As

Publication number Publication date
CN108458717A (zh) 2018-08-28

Similar Documents

Publication Publication Date Title
CN108458717B (zh) 一种迭代的快速扩展随机树irrt的无人机路径规划方法
CN108681787B (zh) 基于改进双向快速扩展随机树算法的无人机路径优化方法
CN109839110B (zh) 一种基于快速随机搜索树的多目标点路径规划方法
US7447593B2 (en) System and method for adaptive path planning
Jun et al. Path planning for unmanned aerial vehicles in uncertain and adversarial environments
Kim et al. Real-time path planning with limited information for autonomous unmanned air vehicles
CN112987799B (zh) 一种基于改进rrt算法的无人机路径规划方法
Yu et al. Probabilistic path planning for cooperative target tracking using aerial and ground vehicles
Tabib et al. Computationally efficient information-theoretic exploration of pits and caves
CN109542117B (zh) 基于改进rrt的水下航行器滚动规划算法
CN110687923A (zh) 无人机长距离循迹飞行方法、装置、设备及存储介质
CN110456825B (zh) 一种基于改进快速随机搜索树的无人机在线运动规划方法
CN110726408A (zh) 一种基于改进蚁群算法的移动机器人路径规划方法
CN113359746A (zh) 基于改进双向RRT和Dijkstra融合算法的路径规划方法和装置
Xu et al. Dpmpc-planner: A real-time uav trajectory planning framework for complex static environments with dynamic obstacles
CN112947591A (zh) 基于改进蚁群算法的路径规划方法、装置、介质及无人机
Tanzmeister et al. Path planning on grid maps with unknown goal poses
CN114167865A (zh) 一种基于对抗生成网络与蚁群算法的机器人路径规划方法
Liu et al. Evasion of a team of dubins vehicles from a hidden pursuer
CN115870990A (zh) 一种机械臂避障路径规划方法
Li et al. Path planning of mobile robot based on improved td3 algorithm
CN113063419A (zh) 一种无人机路径规划方法及系统
Debnath et al. Energy efficient elliptical concave visibility graph algorithm for unmanned aerial vehicle in an obstacle-rich environment
Kuhlman et al. Physics-aware informative coverage planning for autonomous vehicles
CN114237282B (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