CN110146085B - 基于建图和快速探索随机树的无人机实时规避重规划方法 - Google Patents
基于建图和快速探索随机树的无人机实时规避重规划方法 Download PDFInfo
- Publication number
- CN110146085B CN110146085B CN201910460860.6A CN201910460860A CN110146085B CN 110146085 B CN110146085 B CN 110146085B CN 201910460860 A CN201910460860 A CN 201910460860A CN 110146085 B CN110146085 B CN 110146085B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- point
- random tree
- node
- 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
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)
Abstract
本发明属于无人机***技术领域,公开了一种基于建图和快速探索随机树的无人机实时规避重规划方法。在无人机飞行过程中,通过利用机载深度相机传感器构建以无人机为中心的八叉树环境局部地图,局部地图随着无人机的运动实时更新。在局部地图的基础上,采用基于采样的快速探索随机树的路径规划方法,找出无人机避障路径关键位置点,利用均匀贝兹曲线将路径关键位置点线段组成的路径转换成平滑、动态的可飞行轨迹。本发明提出的方法简单,可用于计算资源有限的无人机机载***中实时避障。
Description
技术领域
本发明主要涉及到无人机***技术领域,特指一种基于建图和快速探索随机树的无人机实时规避重规划方法。本发明提出的方法简单,可用于计算资源有限的无人机机载***中实时避障。
背景技术
近年来,无人机的高敏捷性、高机动性、易控制等特点,使其在航拍、巡检、物流和救援等应用领域得到了广泛的应用。大多数情况下无人机的任务环境是未知的,或者存在不可预测的障碍。为了满足未知复杂环境下完全自主飞行的需要,实时局部重规划产生平滑、动态可行的轨迹具有重要意义。避免局部冲突的方法主要有两类:第一类是纯反应性方法,它不需要构建环境地图而是直接根据传感器的数据进行规划,这种方法计算速度快,但是不适用于杂乱的环境,而且会严重受到陷入局部极小值的影响;第二类是基于地图的局部避障方法,它利用各种技术,通过由传感器数据或已知的环境信息构建的局部地图,计算出可行的和局部最优的路径,局部地图构建的难点在于建图的快速性。本专利期望通过构建八叉树局部地图,进行实时局部轨迹重规划来生成无碰撞轨迹,使无人机具有在未知复杂环境中安全自主飞行的能力。
发明内容
本发明要解决的技术问题在于:针对无人机自主避障的需求,提出一种基于建图和快速探索随机树的无人机局部轨迹重规划方法。
针对现有技术存在的问题,本发明内容主要包括三个部分:
1、基于三维循环缓冲区的局部地图构建:通过深度距离传感器得到点云数据,利用体素滤波器对点云进行降采样,然后将点云转化为以八叉树形式存储的栅格地图,并将栅格地图存在以三维数组为存储格式的循环缓冲区。
(1)八叉树地图构建
通过深度距离传感器得到点云,利用体素滤波器对点云进行降采样。然后将点云转化为以八叉树形式存储的栅格地图。具体内容为:
用y∈R来表示八叉树地图中节点的占据情况。当不断感知到该节点被占据时,增加y的值;反之,则减小y的值。用x来表示该节点被占据的概率,概率越大则被占据的可能性越高。x和y之间的转换关系如下:
其中x=0.5表示未确定,通过概率对数转换,实现地图构建和实时更新,动态的对环境中的障碍物进行建模。
(2)基于三维循环缓冲区的局部地图构建
基于已构建的八叉树地图,通过一个以无人机为中心的三维循环缓冲区来构建局部地图。循环缓冲区是大小为N(N=(2p))的三维数组。具体内容为:
假设八叉树地图的分辨率为r,将三维空间中的点p(x,y,z)映射到缓冲区中的点p'(x',y',z')的映射过程为 :
假设缓冲区的中心点设为O(o1,o2,o3),可以通过以下的公式判断点p'是否在缓冲区内:
缓冲区与四旋翼一起移动。设四旋翼的坐标为Q(q1,q2,q3)。缓冲区中心的初始位置为四旋翼的起点。随着四旋翼飞行器的运动,当qi-oi>0时,oi=oi+d。否则,oi=oi-d,d为缓冲区中心的移动步长。
2、基于快速探索随机树的路径关键位置点生成:初始化随机树的第一个节点,然后在每一次循环过程中,在整个状态空间内产生一个任意的随机点,在产生随机点后,遍历随机树中的每一个节点,计算每一个节点与随机点之间的距离,找出距离此随机点最近的节点,在这两个节点的连线方向上扩展产生一个新节点,即生成一个路径关键位置点;新节点的父节点更新,是在新节点附近的规定范围内寻找相邻节点,作为替换新节点的父节点备选,并选择使得路径代价最小的点作为新节点的父节点。具体内容为:
(1)基于快速探索随机树算法的路径关键位置点生成
首先初始化树产生第一个节点xinit,在每一次循环过程中,产生一个随机点xrand。随机点的生成是任意的,即可以在整个状态空间内。在产生随机点后,遍历随机树中的每一个节点,计算每一个节点与随机点之间的距离,找出距离此随机点最近的节点,记为xnearest。在xnearest与xrand连线方向扩展产生新的节点xnew,新节点即为路径关键位置点。快速探索随机树算法在新节点xnew附近的规定范围r内寻找相邻节点,作为替换xnew父节点的备选。最终选择使得路径代价最小的点最为xnew的父节点。然后,快速探索随机树算法对树进行重新布线使所有节点的路径代价最小。
3、基于均匀贝兹曲线样条的轨迹平滑:基于均匀贝兹曲线样条的轨迹平滑的部分采用五次均匀贝兹曲线样条来来拟合快速探索随机树生成的路径关键位置点,得到平滑的轨迹。具体内容为:
使用均匀贝兹曲线样条来拟合快速探索随机树生成的路径关键位置点,得到平滑的轨迹。k-1次贝兹曲线样条可由如下公式表示:
pi∈R3是ti时刻贝兹曲线样条的控制点,Bi,k(t)是基函数。为了使轨迹能够被无人机跟随,轨迹必须连续到位置的四阶导数。因此使用五次均匀贝兹曲线样条来表示轨迹。均匀贝兹曲线样条控制点之间有固定的时间间隔Δt。对于5次贝兹曲线样条,在时间t∈[ti,ti+1),p(t)的值仅取决于6个连续的控制点[pi,pi+1,…,pi+5]。令:
则轨迹可以表示为:
其中Mk是一个大小为k×k的矩阵
令P=[pi,pi+1,…,pi+5]T,可以计算轨迹上任意时刻的速度和加速度。
由于平滑后轨迹会偏离初始路径,存在与障碍碰撞的可能。通过在路径中间增加中点的方法来优化轨迹。假设p1,p2,p3为快速探索随机树算法规划的路径关键位置点,m1为轨迹段p1p2的中点,m2为轨迹段p2p3的中点。m1和m2是无碰撞的,因为它位于快速探索随机树搜索算法返回的最优分段线性路径上。计算m1和m2的终点p'2,判断p1p'2和p'2p3是否与障碍发生碰撞。如果没有,则用p'2取代路径中的p2。通过增加路径关键位置点的方法来解决贝兹曲线样条轨迹与障碍发生碰撞的问题,必要时可进行多次插值来确保轨迹的安全。
本发明利用上述提出的方法,可以解决未知环境中无人机的自主避障与路径规划问题。通过构建的八叉树局部地图,可以在杂乱的环境中快速的对环境中的障碍进行建模,所需的时间为毫秒级。在局部地图中使用快速探索随机树实时生成路径关键位置点,并使用均匀贝兹曲线样条对位置点进行拟合,得到平滑且动力学可行的无碰撞轨迹,实现局部障碍的实时规避,使无人机具有在未知复杂环境中安全自主飞行的能力。
附图说明
图1为轨迹规划方法结果示意图。
图2为局部地图构建方法结果示意图。
图3为实时局部轨迹重规划方法结果示意图。
具体实施方式
下面结合附图和具体实施方式对本发明作进一步说明。
(1)基于三维循环缓冲区的局部地图构建
步骤1:从深度距离相机获取图像中各点的深度信息,转化为点云。
步骤2:用一个网格大小为0.2m×0.2m×0.2m体素滤波器对点云进行降采样。
步骤3:根据式(1),将降采样后的点云***分辨率为0.2m3的八叉树地图中。
步骤4:根据式(2),将八叉树地图中的点映射到一个由长度为64的三维数组组成的循环缓冲区中。
步骤5:将缓冲区的中心点设为无人机的起点,得到一个跟随无人机运动,且实时更新的局部地图,如图1所示。
(2)基于快速探索随机树的路径控制点生成与轨迹平滑
步骤1:首先确定无人机执行任务的起点S和目标点T,构建局部地图,局部地图随着无人机的飞行实时的更新。
步骤2:记录当前时间t,根据式(1),计算t时刻对应的全局轨迹上的位置点p作为此次局部规划的目标点。
步骤3:基于构建的局部地图,根据式(3),如果p点位于局部地图内且p点没有被障碍物占据,采用快速探索随机树算法得到从起点S到目标点p的无碰撞路径,即得到了安全的路径点。否则,等待Δt秒后,重新执行步骤(2)。
步骤4:根据式(6),将路径点作为控制点***到贝兹曲线样条中,拟合控制点得到平滑且动态可行的轨迹,如图2所示。
步骤5:采用增加路径点的方法对轨迹进行优化处理,使得平滑后的轨迹与初始路径更贴近,保证轨迹的安全。
步骤6:根据式(8),将得到的轨迹和计算出的速度和加速度发送给无人机,使无人机按照指定的速度跟随轨迹。
步骤7:以一个时间间隔Δt重复步骤(2)~步骤(6)的过程,直到无人机到达最终的目标点T,如图3所示。
以上仅是本发明的优选实施方式,本发明的保护范围并不仅局限于上述实施例,凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理前提下的若干改进和润饰,应视为本发明的保护范围。
Claims (3)
1.基于建图和快速探索随机树的无人机实时规避重规划方法,其特征在于,主要包括三个部分:一是局部八叉树地图构建;二是在所述局部八叉树地图的基础上基于快速探索随机树的路径关键位置点生成;三是基于关键位置点利用均匀贝兹曲线样条进行轨迹拟合平滑;所述局部八叉树地图存储在循环缓冲区中;所述循环缓冲区和无人机同时移动,所述循环缓冲区的缓冲区中心以预设步长移动;所述局部八叉树地图以无人机为中心;
在地图基础上基于快速探索随机树的路径关键位置点生成的步骤为:首先需要初始化随机树的第一个节点,节点代表八叉树地图状态空间内的位置点;设定随机树的搜索次数,在每一次搜索的过程中,首先在八叉树地图状态空间内产生一个任意的随机点,在产生随机点后,遍历随机树中的每一个节点,计算每一个节点与随机点之间的距离,找出距离此随机点最近的节点,然后在这两个节点的连线方向上扩展产生新的节点,即得到一个路径关键位置点;
在局部八叉树地图基础上基于快速探索随机树生成路径关键位置点之前,还包括:
记录当前时间t,计算t时刻对应的全局轨迹上的位置点p作为局部规划的目标点;
判断目标点是否位于局部八叉树地图内和是否被障碍物占据;
若是,则在局部八叉树地图上基于快速探索随机树生成路径关键位置点;
通过深度距离传感器得到点云数据,利用体素滤波器对点云进行降采样,然后将点云转化为以八叉树形式存储的栅格地图,并将栅格地图存储在以三维数组为存储格式的循环缓冲区。
2.根据权利要求1所述的基于建图和快速探索随机树的无人机实时规避重规划方法,其特征在于,
所述基于快速探索随机树的路径关键位置点生成包括,在得到一个新节点后,需要对所述快速探索随机树进行重新布线使得路径的代价最小;首先,需要在新节点附近的规定范围内寻找相邻节点,作为替换新节点的父节点备选,然后选择使得路径代价最小的点作为新节点的父节点。
3.根据权利要求1所述的基于建图和快速探索随机树的无人机实时规避重规划方法,其特征在于,
基于均匀贝兹曲线样条的轨迹平滑的部分采用五次均匀贝兹曲线样条来拟合快速探索随机树生成的路径控制点,得到平滑的轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910460860.6A CN110146085B (zh) | 2019-05-30 | 2019-05-30 | 基于建图和快速探索随机树的无人机实时规避重规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910460860.6A CN110146085B (zh) | 2019-05-30 | 2019-05-30 | 基于建图和快速探索随机树的无人机实时规避重规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110146085A CN110146085A (zh) | 2019-08-20 |
CN110146085B true CN110146085B (zh) | 2022-04-19 |
Family
ID=67592138
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910460860.6A Active CN110146085B (zh) | 2019-05-30 | 2019-05-30 | 基于建图和快速探索随机树的无人机实时规避重规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110146085B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113741490A (zh) * | 2020-05-29 | 2021-12-03 | 广州极飞科技股份有限公司 | 一种巡检方法、装置、飞行器及存储介质 |
CN111707269B (zh) * | 2020-06-23 | 2022-04-05 | 东南大学 | 一种三维环境下的无人机路径规划方法 |
CN111752306B (zh) * | 2020-08-14 | 2022-07-12 | 西北工业大学 | 一种基于快速扩展随机树的无人机航路规划方法 |
CN112379681A (zh) * | 2020-11-02 | 2021-02-19 | 中国兵器工业计算机应用技术研究所 | 无人机避障飞行方法、装置及无人机 |
CN112525195B (zh) * | 2020-11-20 | 2022-03-01 | 中国人民解放军国防科技大学 | 基于多目标遗传算法的飞行器航迹快速规划方法 |
CN112747736B (zh) * | 2020-12-22 | 2022-07-08 | 西北工业大学 | 一种基于视觉的室内无人机路径规划方法 |
CN114200933A (zh) * | 2021-12-06 | 2022-03-18 | 太原科技大学 | 一种桥式起重机三维路径规划方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104965518A (zh) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | 基于三维数字地图的电力巡检飞行机器人航线规划方法 |
CN106737671A (zh) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | 七自由度仿人机械臂的双层拟人运动规划方法 |
CN106949893A (zh) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | 一种三维避障的室内机器人导航方法和*** |
CN107631734A (zh) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | 一种基于D*_lite算法的动态平滑路径规划方法 |
CN108088445A (zh) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 基于八叉树表示的三维栅格地图路径规划***及方法 |
CN108776492A (zh) * | 2018-06-27 | 2018-11-09 | 电子科技大学 | 一种基于双目相机的四轴飞行器自主避障与导航方法 |
CN108876024A (zh) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | 路径规划、路径实时优化方法及装置、存储介质 |
KR20180135695A (ko) * | 2017-06-13 | 2018-12-21 | 성균관대학교산학협력단 | 장비 설치 공간, 설치 경로, 설치 순서 확인 방법 |
CN109582032A (zh) * | 2018-10-11 | 2019-04-05 | 天津大学 | 多旋翼无人机在复杂环境下的快速实时避障路径选择方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104867174B (zh) * | 2015-05-08 | 2018-02-23 | 腾讯科技(深圳)有限公司 | 一种三维地图渲染显示方法及*** |
-
2019
- 2019-05-30 CN CN201910460860.6A patent/CN110146085B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104965518A (zh) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | 基于三维数字地图的电力巡检飞行机器人航线规划方法 |
CN108088445A (zh) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 基于八叉树表示的三维栅格地图路径规划***及方法 |
CN106737671A (zh) * | 2016-12-21 | 2017-05-31 | 西安科技大学 | 七自由度仿人机械臂的双层拟人运动规划方法 |
CN106949893A (zh) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | 一种三维避障的室内机器人导航方法和*** |
KR20180135695A (ko) * | 2017-06-13 | 2018-12-21 | 성균관대학교산학협력단 | 장비 설치 공간, 설치 경로, 설치 순서 확인 방법 |
CN107631734A (zh) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | 一种基于D*_lite算法的动态平滑路径规划方法 |
CN108876024A (zh) * | 2018-06-04 | 2018-11-23 | 清华大学深圳研究生院 | 路径规划、路径实时优化方法及装置、存储介质 |
CN108776492A (zh) * | 2018-06-27 | 2018-11-09 | 电子科技大学 | 一种基于双目相机的四轴飞行器自主避障与导航方法 |
CN109582032A (zh) * | 2018-10-11 | 2019-04-05 | 天津大学 | 多旋翼无人机在复杂环境下的快速实时避障路径选择方法 |
Non-Patent Citations (2)
Title |
---|
刘阿敏.基于三维点云图的无人机路径规划.《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》.2019,(第02期),第11-14,21-23,45-46,50,52页. * |
基于三维点云图的无人机路径规划;刘阿敏;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20190215(第02期);第11-14,21-23,45-46,50,52页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110146085A (zh) | 2019-08-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110146085B (zh) | 基于建图和快速探索随机树的无人机实时规避重规划方法 | |
US11698638B2 (en) | System and method for predictive path planning in autonomous vehicles | |
CN107063280B (zh) | 一种基于控制采样的智能车辆路径规划***及方法 | |
CN112068588A (zh) | 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法 | |
WO2018190833A1 (en) | Swarm path planner system for vehicles | |
CN112432648A (zh) | 一种移动型机器人安全运动轨迹的实时规划方法 | |
Farmani et al. | Tracking multiple mobile targets using cooperative unmanned aerial vehicles | |
Yu et al. | A vision-based collision avoidance technique for micro air vehicles using local-level frame mapping and path planning | |
CN112650306A (zh) | 一种基于动力学rrt*的无人机运动规划方法 | |
WO2023147769A1 (en) | Systems, methods, and computer-readable media for spatio-temporal motion planning | |
CN112344938A (zh) | 基于指向和势场参数的空间环境路径生成及规划方法 | |
CN115525063A (zh) | 用于无人直升机的轨迹规划方法 | |
Fragoso et al. | Dynamically feasible motion planning for micro air vehicles using an egocylinder | |
Rocha et al. | A uav global planner to improve path planning in unstructured environments | |
KR20230083846A (ko) | 3차원 환경에서의 항공 차량 이동노선 계획 방법 | |
CN116048120B (zh) | 一种未知动态环境下小型四旋翼无人机自主导航***及方法 | |
CN117075612A (zh) | 一种基于改进可视图的快速路径规划与基于esdf地图的轨迹优化方法 | |
CN117073707A (zh) | 一种基于预测碰撞时间地图的动态环境路径规划方法 | |
Dubey et al. | Droan—disparity-space representation for obstacle avoidance | |
CN115016546B (zh) | 无人机三维路径规划方法、装置、电子设备和存储介质 | |
Marlow et al. | Local terrain mapping for obstacle avoidance using monocular vision | |
CN115981313A (zh) | 机器人路径规划方法、***、电子设备及存储介质 | |
CN116184993A (zh) | 一种基于Hybrid Astar与空间走廊的路径规划方法 | |
CN115268461A (zh) | 一种融合算法的移动机器人自主导航方法 | |
CN114815899A (zh) | 基于3d激光雷达传感器的无人机三维空间路径规划方法 |
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 | ||
CB03 | Change of inventor or designer information |
Inventor after: Niu Diefeng Inventor after: Ma Zhaowei Inventor after: Hu Jia Inventor after: Wu Lizhen Inventor before: Niu Diefeng Inventor before: Ma Zhaowei Inventor before: Hu Jia Inventor before: Wu Lizhen |
|
CB03 | Change of inventor or designer information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |