CN115617078B - 基于膨化障碍的无人机三维航迹快速规划方法 - Google Patents

基于膨化障碍的无人机三维航迹快速规划方法 Download PDF

Info

Publication number
CN115617078B
CN115617078B CN202211588708.4A CN202211588708A CN115617078B CN 115617078 B CN115617078 B CN 115617078B CN 202211588708 A CN202211588708 A CN 202211588708A CN 115617078 B CN115617078 B CN 115617078B
Authority
CN
China
Prior art keywords
node
task area
obstacle
map
height
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
CN202211588708.4A
Other languages
English (en)
Other versions
CN115617078A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202211588708.4A priority Critical patent/CN115617078B/zh
Publication of CN115617078A publication Critical patent/CN115617078A/zh
Application granted granted Critical
Publication of CN115617078B publication Critical patent/CN115617078B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于膨化障碍的无人机三维航迹快速规划方法，属于航迹自主规划技术领域。基于确定性规则建立根据任务区域内障碍物的膨化障碍，提出无需进行碰撞检测的改进3D‑A*算法，将碰撞检测环节转化进入启发式代价函数计算环节，在实现无人机安全避障的三维航迹规划的同时，进一步提高航迹规划效率，使本发明能够在线应用，本发明还具有生成航迹可靠和操作简单的优点。本发明能够在航迹规划工程应用中广泛应用于无人机的三维安全避障航迹规划。

Description

基于膨化障碍的无人机三维航迹快速规划方法
技术领域
本发明涉及一种基于膨化障碍的无人机三维航迹快速规划方法，属于航迹自主规划技术领域。
背景技术
随着任务环境的日趋复杂，无人机因成本低、作战灵活、功能多样、战场生存能力强等优点得到了比有人飞机更加广泛的应用，更加适合执行恶劣环境下的危险任务。无人机在诸多障碍环境中的安全飞行已经有广泛的应用需求，如无人机在城市楼宇密集的任务区域内执行侦察任务、无人机穿越防空火力密集的任务区域执行突防任务。在工程应用需求的牵引下，无人机在密集障碍内的安全飞行已逐渐成为当今各国无人机发展的必备能力。
航迹自主规划和任务分配是无人机任务规划系统的组成部分，而航迹自主规划是提升无人机自主执行任务能力的关键技术。任务环境日趋复杂，障碍物数量逐渐增多，恶劣的飞行环境使得各国对无人机快速反应能力愈发重视，这对规划算法的实时性、稳定性提出了更加苛刻的要求。提高航迹安全性的同时，保证算法规划效率对无人机具有重要意义，直接关系到无人机存活概率与任务成败。
在无人机航迹自主规划领域的实际应用中，无人机需要搭载任务规划板载计算机来规划可行的飞行航迹，但是往往存在以下几个问题：
1）传统的手动标定航迹方法无法满足实际作战需求；
2）在规划的过程中，无人机仍处在运动的状态，且板载计算机计算能力有限，难以通过复杂的轨迹规划方法快速得到可行航迹；导致航迹规划效率低，规划时间长，实时性较差；
3）航迹规划结果不满足无人机机动性能约束，使得在复杂环境下规划的航迹不可实现；
因此，提高航迹规划效率，提高规划结果安全性和可靠性，在变化环境中为无人机快速规划航迹或新航迹，对任务成败和无人机存活起到关键性作用。
根据无人机航迹规划算法架构的不同，目前针对无人机避障航迹规划方法主要分为两类：
第一类是基于确定规则型的搜索算法，如基于确定性方法最小化原理和状态空间搜索方法，例如以Dijkstra算法、A*算法、D*算法等为代表的图搜索算法，其基于启发式思想，引入节点的代价函数，选择代价最小的航迹节点逐步扩展，直至搜索至设定的航迹终点。
第二类是随机型的搜索算法，如基于采样的快速扩展随机树类算法，也称RRT类算法，和基于启发式优化算法的遗传算法、蚁群算法等。该类算法每次迭代时生成大量样本，并对样本进行筛选，但容易陷入局部最优，且收敛速度较慢。随机型搜索算法与确定型搜索算法相比，搜索空间不受限制，假设条件不受约束，对于优化函数不要求其特殊性，并 且可以并行性，但在搜索精度和规划时效性等方面不如基于确定规则型的搜索算法。
在避障方法方面，目前主要应用的方法为人工势场法，其把障碍物和威胁看成排斥力，目标点当作吸引力，通过合力控制物体的运动，有效规避行进过程中障碍物的阻碍。但是在障碍密集的环境中，易出现无人机因明显斥力而无法到达目标点的现象，和动态环境中航迹局部震荡，甚至与障碍物碰撞的危险现象。
近年来，A*算法被认为是最有效的寻路算法，在理论上是时间最优的。通过设计代价函数，使得对航迹点的搜索过程中总是选择代价函数值最小的节点作为下一个扩展节点，最终获得最短路径。其中，3D-A*算法是A*算法在三维场景中的应用拓展，其应用场景极丰富，算法可扩展性强，算法成熟，规划效率高。
发明内容
本发明公开的基于膨化障碍的无人机三维航迹快速规划方法要解决的技术问题是：基于确定性规则建立根据任务区域内障碍物的膨化障碍，提出无需进行碰撞检测的改进3D-A*算法，将碰撞检测环节转化进入启发式代价函数计算环节，在实现无人机安全避障的三维航迹规划的同时，进一步提高航迹规划效率，使本发明能够在线应用，本发明还具有生成航迹可靠和操作简单的优点。本发明能够在航迹规划工程应用中广泛应用于无人机的三维安全避障航迹规划。
本发明的目的是通过下述技术方案实现的。
本发明公开的基于膨化障碍的无人机三维航迹快速规划方法，首先根据无人机机动能力对任务区域进行离散化，制定无人机在离散任务区域地图内的机动规则；基于确定性规则建立任务区域内障碍物的膨化障碍，提升规划空间内可用航迹的安全性；基于提出的改进3D-A*算法生成无人机安全、可飞的三维航迹点序列。
本发明公开的基于膨化障碍的无人机三维航迹快速规划方法，包括以下步骤：
步骤1：获取无人机起点、终点的真实地理坐标；获取任务区域信息；根据无人机机动能力将任务区域离散为由正方体网格组成的地图，使无人机在离散任务区域地图节点间的移动满足运动学约束；无人机起点、终点需就近转化至离散任务区域地图节点上，以满足后续规划需要；
步骤1具体实现方法包括如下步骤：
步骤1.1：获取无人机的起点、终点的真实地理坐标；获取任务区域信息，所述任务区域信息为：任务区域范围，及任务区域内障碍物的范围和高度信息；所述任务区域为长方体。
步骤1.2：将三维空间的任务区域进行离散化；为使得无人机在离散任务区域地图节点间的移动满足无人机运动学约束，将任务区域离散为由正方体网格组成的地图，称为离散任务区域地图；所述正方体网格的边长△L为无人机最小转弯半径的2倍以上，离散任务区域地图原点为长方体底面4个顶点中的任意一个，离散任务区域地图原点高度与任务区域内高度最低点齐平。
步骤1.3：为满足后续规划的需要，需将实际无人机起点、终点就近转化至离散任务区域地图节点上，将如式（1）所示，
Figure 97943DEST_PATH_IMAGE001
（1）
Figure 218345DEST_PATH_IMAGE002
（2）
其中，
Figure 450612DEST_PATH_IMAGE003
为无人机起点的真实地理坐标；
Figure 580242DEST_PATH_IMAGE004
为无人机终 点的真实地理坐标；
Figure 129035DEST_PATH_IMAGE005
为任务区域原点的真实地理坐标；
Figure 685919DEST_PATH_IMAGE006
为起点对应的离散任务区域地图节点坐标；
Figure 687373DEST_PATH_IMAGE007
为终点对应的离散任务区域地图节点坐标；R为地球半径；round(·)为四舍五入运算符。
至此，依据无人机机动能力将任务区域离散化，并将无人机起点、终点位置转化至离散任务区域地图的节点上。
步骤2：依据步骤1获取的任务区域内障碍物的范围和高度信息，将障碍物在离散任务区域地图内进行离散化处理。
步骤2具体实现方法包括如下步骤：
步骤2.1：根据任务区域的范围，记离散任务区域地图的高度节点坐标下限为h min和高度节点坐标上限为h max。根据步骤1.2，
Figure 355114DEST_PATH_IMAGE008
（3）
其中，Z max为任务区域高度上限，ceil(·)为向上取整运算符。
步骤2.2：在离散任务区域地图高度为h min 的平面上，依据步骤1获取的任务区域内障碍物的范围和高度信息，获取该平面上每个离散节点处对应的障碍物高度，并将障碍物高度转化为离散任务区域地图内的障碍物节点坐标，如式（4）所示；
Figure 240637DEST_PATH_IMAGE009
（4）
其中，H(i,j)为离散节点(i,j)上对应的障碍物高度，h(i,j)为离散节点(i,j)上障碍物的高度节点坐标。
步骤2.3：如果h(i,j)≤h min，则将该节点对应的障碍物高度节点坐标设置为0；如果h(i,j)＞h max，则将该节点对应的障碍物高度节点坐标设置为h max；如果h(i,j)位于离散任务区域地图的4个铅垂边界面上，则将节点对应的障碍物高度节点坐标设置为h max，以保证无人机仅在任务区域内运动，不超出任务区域的范围。
至此，完成障碍物在离散任务区域地图内的离散化处理，实现离散任务区域地图内障碍物高程地图的生成，所述障碍物高程地图包络了任务区域内的所有障碍。
步骤3：根据步骤2获取的高程地图信息，基于确定性规则建立障碍物对应的膨化障碍，将障碍物合理的膨化，增加所有航迹可行解的安全性。
步骤3具体实现方法包括如下步骤：
步骤3.1：记离散任务区域地图高度为h min 的平面上每个节点的膨化障碍物高度为h e ；检查在离散任务区域地图高度为h min 的平面上的每一个离散节点(i,j)，如果该节点上对应的障碍物高度满足h(i,j)＝0，执行步骤3.2；如果该节点上对应的障碍物高度满足h(i,j)＞0，则h e (i,j)＝h(i,j)，执行步骤3.4。
步骤3.2：如果该节点不是起点或终点在离散任务区域地图高度为h min 的平面上的 投影，检查该节点周围8个节点的障碍物高度坐标，如果周围8个节点中存在障碍物高度坐 标大于0的节点，则将h(i,j)更改为周围8个节点的高度坐标的最大值
Figure 968422DEST_PATH_IMAGE010
，如式（5）所示，然后执行步骤3.4；
Figure 457172DEST_PATH_IMAGE011
（5）
如果该节点是起点或终点在离散任务区域地图高度为h min 的平面上的投影，执行步骤3.3。
步骤3.3：判断起点处膨化障碍高度h e,s ，如果h e,s ≥z s ，则令h e,s ＝z s ；如果h e,s ＜z s ，则令h e,s 保持不变；判断终点处膨化障碍高度h e,g ，如果h e,g ≥z g ，则令h e,g ＝z g ；如果h e,g ＜z g ，则令h e,g 保持不变。
步骤3.4：如果h e (i,j)＞0，且h(i,j)≤h max－1，则h e (i,j)＝h(i,j)＋1，即，将障碍物节点膨化至其上方的1个节点；如果h e (i,j)＞0，且h(i,j)＞h max－1，则h e (i,j)＝h(i,j)，即，障碍不再继续向上膨化；如果h e (i,j)＝0，则h e (i,j)同样保持不变。
步骤4：在离散任务区域地图内，根据步骤3获取的膨化障碍信息，将碰撞检测环节转化进入启发式代价函数计算环节，采用无需进行碰撞检测的改进3D-A*算法，进行无人机三维航迹规划，在实现无人机安全避障的三维航迹规划的同时，提高航迹规划效率。
步骤4具体实现方法包括如下步骤：
步骤4.1：在步骤1.2创建的离散任务区域地图中，规定无人机能够任意到达的运动方向；无人机能够沿着正方体网格的边、面对角线或体对角线运动，共26个方向，即以当前节点为中心，向其四周8个网格地图单元格节点扩展的26个方向。
步骤4.2：创建OPEN集与CLOSED集，其中OPEN集用来存放已经产生但是没有扩展的节点，CLOSED集用来存放已经被扩展的节点；初始化时，将离散任务区域地图内的无人机起点装入OPEN集，CLOSED集清空。
步骤4.3：若OPEN集为空，则规划失败，规划结束；若OPEN集不为空，则从OPEN集中 选取代价值最小的节点作为当前节点，将当前节点从OPEN集移入CLOSED集；并将代价值最 小的节点标记为
Figure 663026DEST_PATH_IMAGE012
。
步骤4.4：判断是否抵达网格化的终点节点，若抵达终点，则将终点放入CLOSED集，执行步骤4.6；若尚未抵达终点，则执行步骤4.5。
步骤4.5：将碰撞检测环节转化进入启发式代价函数计算环节；
以步骤4.3中节点
Figure 920832DEST_PATH_IMAGE013
为中心，向步骤4.1规定的26 个方向对应的离散任务区域地图节点创建待扩展空间，记待扩展空间内的待扩展节点为
Figure 85097DEST_PATH_IMAGE014
，k＝1,2,…,26，并直接计算每个待扩展节点的代价 值，选取代价最小的装入OPEN集，继续执行步骤4.3；所述每个待扩展节点的代价值F b 为
Figure 310411DEST_PATH_IMAGE015
（6）
其中，G b 为起点
Figure 54376DEST_PATH_IMAGE016
到待扩展节点
Figure 166688DEST_PATH_IMAGE017
的累计代价值，∑(·)为求和运算符，k G 为G b 的权重系数；
Figure 236275DEST_PATH_IMAGE018
为节点
Figure 699618DEST_PATH_IMAGE016
到节点
Figure 512853DEST_PATH_IMAGE019
的累计路径长度代价 值，
Figure 230404DEST_PATH_IMAGE020
为节点
Figure 736472DEST_PATH_IMAGE019
到待扩展节点
Figure 421531DEST_PATH_IMAGE017
的路径长度代价值；H b 为 由待扩展节点
Figure 38457DEST_PATH_IMAGE017
到终点
Figure 125362DEST_PATH_IMAGE021
的估计代价值，k H 为H b 的权重系数；L b 为代价函数中的节 点纵向扩展启发项，k L 为起点
Figure 536752DEST_PATH_IMAGE016
到终点
Figure 958375DEST_PATH_IMAGE021
的估计航程值，
Figure 113413DEST_PATH_IMAGE022
为待扩展节点
Figure 320403DEST_PATH_IMAGE017
处对应的膨化障碍高度坐标。
k L L b 的物理含义为，在判断所有带扩展节点的代价值时，优先选取待扩展节点处膨化障碍物高度为0的节点，其次是膨化障碍物高度较低、无需爬升即可抵达的节点，最后是需爬升高度以跨越膨化障碍节点高度的节点。
Figure 168273DEST_PATH_IMAGE023
为传统A*类算法的启发式代价函数，引导航迹快速向终点靠近， 但需要额外的碰撞检测计算辅助节点在三维扩展。将
Figure 827925DEST_PATH_IMAGE024
改进为F b ，将碰撞检测环节转化 进入启发式代价函数计算环节，只需索引待扩展节点处膨化障碍高度，无需碰撞检测导致 的额外计算量，提高计算效率。所述扩展节点的代价值即为节点距离。
步骤4.6：依次回溯每一步生成的节点，并根据式（1）和式（2）的逆运算将航迹点坐 标序列还原至航迹点的真实地理坐标序列，并将航迹起点和终点节点还原为无人机起点、 终点的真实地理坐标
Figure 521074DEST_PATH_IMAGE025
和
Figure 316992DEST_PATH_IMAGE026
；生成从起点到终点的三维安全飞 行航迹。
有益效果：
1、本发明公开的基于膨化障碍的无人机三维航迹快速规划方法，根据无人机机动能力将任务区域离散化，使无人机在离散任务区域节点间的移动均满足无人机运动学约束，简化了航迹规划的过程，提高了无人机航迹的规划效率，保证了所规划航迹的可行性。
2、本发明公开的基于膨化障碍的无人机三维航迹快速规划方法，依据确定性规则生成实际障碍物对应的膨化障碍物；无人机依据膨化障碍物进行三维航迹规划，无需对进行额外处理，航迹距离障碍物较远，航迹能够保证较高的飞行安全性。此外，基于安全区的三维飞行航迹生成规则简单，符合工程应用需求，具有较高的规划效率和规划实时性，易于工程实现，具有广泛的工程应用前景。
3、本发明公开的基于膨化障碍的无人机三维航迹快速规划方法，通过提出的改进3D-A*算法，将碰撞检测环节转化进入启发式代价函数计算环节，减少碰撞检测导致的额外计算量，提高计算效率；同时能够进一步提高无人机飞行的安全性，同时使得路径最短，提高了任务完成的效率。
附图说明
图1为本实施例基于膨化障碍的无人机三维航迹快速规划方法的流程图。
图2为离散任务区域地图中膨化障碍物的确定性生成规则示意图。
图3为无人机运动方向扩展示意图。扩展空间包括26个方向，即以当前节点为中心，向其四周8个网格地图单元格节点扩展的26个方向。
图4为具体实施方式中无人机在含膨化障碍物的任务区域内的飞行航迹。
图5为具体实施方式中无人机在含膨化障碍物的任务区域内的飞行航迹的俯视图。
图6为具体实施方式中无人机在实际任务区域内的飞行航迹。
图7为具体实施方式中无人机在实际任务区域内的飞行航迹的俯视图。
图8为具体实施方式中无人机的高度节点变化。横坐标为航迹长度，纵坐标为飞行高度。
具体实施方式
为了更好地说明本发明的目的和优点，下面通过无人机在含有密集障碍任务区域内飞行的航迹规划实施例，并结合附图与表格对本发明做出详细解释。
实施例：
本实施例中的无人机飞行速度为30m/s，最小转弯半径为r=150m；任务区域为平面24000mx24000m，高6000m的长方体。如图1所示，本实施例公开的基于膨化障碍的无人机三维航迹快速规划方法，包括如下步骤：
步骤1：获取无人机起点、终点位置信息，和任务区域信息。
步骤1.1：地球半径选取为R＝6371000m。地图网格选用正方体网格，网格宽度设置为2倍无人机转弯半径，即300m。离散化后的任务区域范围为(80,80,20)。为更好的验证本发明效果，障碍物范围和高度信息在仿真时由计算机随机生成。
步骤1.2：将无人机起点和终点转化至离散任务区域地图节点上，无人机起点为(5,7,12)，终点为(68,66,6)。
步骤2：依据任务区域的3D-A*网格化信息，对任务区域内的障碍物进行网格化。
步骤2.1：设置离散任务区域地图障碍的高度下限h min=0和高度上限h max=20，高度上限取自任务区域范围高度上限。
步骤2.2：在离散任务区域地图高度为h min的平面上，将该平面节点坐标上的每个离散节点处对应的障碍物高度转化为离散任务区域地图内的障碍物节点坐标。
步骤2.2：如果节点的高度坐标低于h min，则将该节点的高度坐标设置为0；如果节点的高度坐标高于h max，则将该节点的高度坐标设置为20；如果节点位于任务区域平面的铅垂边界上，即x=80或0，或y=80或0的所有地图节点，则将该节点高度坐标设置为20，以保证无人机仅在任务区域内运动，不超出任务区域的范围。
步骤3：根据步骤2获取的任务区域内障碍物的高程地图信息，基于确定性规则建立障碍物对应的膨化障碍，将障碍物合理的膨化，增加所有航迹可行解的安全性，如图2所示。
步骤3.1：记离散任务区域地图高度为h min 的平面上每个节点的膨化障碍物高度为h e ；检查在离散任务区域地图高度为h min 的平面上的每一个离散节点(i,j)，如果该节点上对应的障碍物高度满足h(i,j)＝0，执行步骤3.2；如果该节点上对应的障碍物高度满足h(i,j)＞0，则h e (i,j)＝h(i,j)，执行步骤3.4。
步骤3.2：如果该节点不是起点或终点在离散任务区域地图高度为h min 的平面上的 投影，检查该节点周围8个节点的障碍物高度坐标，如果周围8个节点中存在障碍物高度坐 标大于0的节点，则将h(i,j)更改为周围8个节点的高度坐标的最大值
Figure 101144DEST_PATH_IMAGE010
，然后执行步骤3.4。如果该节点是起点或终点在离散任务区域地图 高度为h min 的平面上的投影，执行步骤3.3。
步骤3.3：判断起点处膨化障碍高度h e,s ，如果h e,s ≥z s ，则令h e,s ＝z s ；如果h e,s ＜z s ，则令h e,s 保持不变；判断终点处膨化障碍高度h e,g ，如果h e,g ≥z g ，则令h e,g ＝z g ；如果h e,g ＜z g ，则令h e,g 保持不变。
步骤3.4：如果h e (i,j)＞0，且h(i,j)≤h max－1，则h e (i,j)＝h(i,j)＋1，即，将障碍物节点膨化至其上方的1个节点；如果h e (i,j)＞0，且h(i,j)＞h max－1，则h e (i,j)＝h(i,j)，即，障碍不再继续向上膨化；如果h e (i,j)＝0，则h e (i,j)同样保持不变。
规划开始时，以无人机航迹起点
Figure 248092DEST_PATH_IMAGE027
为例，执行步骤3.1时，起点处的障碍物高度为 0，则检查起点在离散任务区域地图高度为h min 的平面上的投影节点周围8个节点的障碍物 高度坐标，均为0，则起点处的膨化障碍高度h e,s ＝0。执行步骤3.2，转步骤3.3，由于h e,s ＜z s ＝12，因此h e,s ＝0不变。执行步骤3.4，由于h e,s ＝0，则h e,s ＝0保持不变，以此类推。
步骤4：在任务区域内，根据步骤3获取的膨化障碍信息，采用提出的无需进行碰撞检测的改进3D-A*算法，将碰撞检测环节转化进入启发式代价函数计算环节，在实现无人机安全避障的三维航迹规划的同时，提高航迹规划效率。
步骤4.1：规定无人机基于当前位置的26个可运动方向，即以当前节点为中心，向其四周8个网格地图单元格节点扩展的26个方向，如图3所示。
步骤4.2：创建OPEN集与CLOSED集。在规划初始，首先将无人机航迹起点(5,7,12)装入OPEN集，同时清空CLOSED集。即，OPEN集内仅包含起点(5,7,12)；CLOSED集为空，不包含任何节点信息。
步骤4.3：判断OPEN集是否为空，若为空，则航迹规划失败并结束；若不为空，则从OPEN集中选取代价值最小的节点作为当前的节点，将其从OPEN集移入CLOSED集。
规划开始时，首先以步骤4.2的结果为例，执行步骤4.3时，OPEN集不为空，取出OPEN表中代价值最小节点作为当前的节点，即节点(5,7,12)，并将此节点放入CLOSED集。步骤4.3结束时，OPEN集被置空，CLOSED集内存放了节点(5,7,12)，以此类推。
步骤4.4：判断是否抵达终点，若抵达终点，则将终点(68,66,6)放入CLOSED集，依次回溯每一步生成的节点，生成从起点到终点的三维安全飞行航迹；若尚未抵达目标节点，则执行步骤4.5。
步骤4.5：将碰撞检测环节转化进入启发式代价函数计算环节，减少碰撞检测导致 的额外计算量，提高计算效率。以步骤4.3中代价值最小节点
Figure 744932DEST_PATH_IMAGE028
为中心，向步骤4.1的26个方向对应的离散任务区域地图 节点创建待扩展空间，记待扩展空间内的待扩展节点为
Figure 660936DEST_PATH_IMAGE029
，k＝1,2,…,26。本实施例设置权重系数k G ＝1、k H ＝1.5和k L ＝87.8692，并直接计算每个待扩 展节点的代价值，选取代价最小的装入OPEN集，继续执行步骤4.3。
例如，当无人机航迹从(5,7,12)向终点(68,66,6)扩展时，创建以(5,7,12)为中心的待扩展空间，此待扩展空间共由26个待扩展节点构成，如表1所示。
表1 从(5,7,12)为中心形成的待扩展空间内包含的节点
Figure 585029DEST_PATH_IMAGE031
以此26个节点作为待扩展点，计算每个待扩展点的代价值。代价值为起始点到此待扩展节点的累计节点距离，与由该待扩展节点到终点的直线距离长度，和节点纵向扩展启发项的三者求和得到。得到从(5,7,12)到待扩展节点(6,8,12)的代价值最小，为F b =133.5359，其中，k G G b =1.7321，k H H b =131.8038，k L L b =0。将节点(6,8,12)装入OPEN集，继续执行步骤4.2。
基于任务区域内安全区的航迹规划结果如图4所示，其俯视图如图5所示；图4航迹规划结果反映在真实障碍分布的任务区域内的结果如图6所示，其俯视图如图7所示。无人机飞行高度随航迹长度的变化如图8所示。从本实施例的计算结果可以看出，无人机可从起点安全抵达终点，且航迹贴近安全区飞行，以缩短航迹长度，从而缩短飞行总时长，反映在真实障碍分布的任务区域内，无人机距离障碍均保持安全距离，在密集的障碍中机动，保证了自身的飞行安全性。
本实施例采用Matlab2018b软件进行仿真实现，仿真时长结果如表2所示。从本实施例的计算结果可以看出，提出的规划方法可以快速实现膨化障碍建立和安全航迹的规划。
表2 本实施例全流程仿真时长
3D-A*算法 本实施例所提出改进3D-A*算法
航迹规划时长/s 0.0650 0.0474
根据本实例公开的基于膨化障碍的无人机三维航迹快速规划方法，能够基于确定性规则快速生成膨化障碍，具有较高的地图处理效率；能够快速基于改进的3D-A*算法生成无人机从起点到终点的安全飞行航迹，将碰撞检测环节转化进入启发式代价函数计算环节，具有较高的规划效率。此外，基于膨化障碍和改进3D-A*算法的无人机三维航迹生成规则简单，符合工程应用需求，具有较高的规划效率和规划实时性，易于工程实现，具有广泛的工程应用前景。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例，用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.基于膨化障碍的无人机三维航迹快速规划方法，其特征在于：包括以下步骤：
步骤1：获取无人机起点、终点的真实地理坐标；获取任务区域信息；根据无人机机动能力将任务区域离散为由正方体网格组成的地图，使无人机在离散任务区域地图节点间的移动满足运动学约束；无人机起点、终点需就近转化至离散任务区域地图节点上，以满足后续规划需要；
步骤1具体实现方法包括如下步骤：
步骤1.1：获取无人机的起点、终点的真实地理坐标；获取任务区域信息，所述任务区域信息为：任务区域范围，及任务区域内障碍物的范围和高度信息；所述任务区域为长方体；
步骤1.2：将三维空间的任务区域进行离散化；为使得无人机在离散任务区域地图节点间的移动满足无人机运动学约束，将任务区域离散为由正方体网格组成的地图，称为离散任务区域地图；所述正方体网格的边长△L为无人机最小转弯半径的2倍以上，离散任务区域地图原点为长方体底面4个顶点中的任意一个，离散任务区域地图原点高度与任务区域内高度最低点齐平；
步骤1.3：为满足后续规划的需要，需将实际无人机起点、终点就近转化至离散任务区域地图节点上，如式（1）所示，
Figure QLYQS_1
（1）
Figure QLYQS_2
（2）
其中，
Figure QLYQS_3
为无人机起点的真实地理坐标；/>
Figure QLYQS_4
为无人机终点的真实地理坐标；/>
Figure QLYQS_5
为任务区域原点的真实地理坐标；/>
Figure QLYQS_6
为起点对应的离散任务区域地图节点坐标；/>
Figure QLYQS_7
为终点对应的离散任务区域地图节点坐标；R为地球半径；round(·)为四舍五入运算符；
至此，依据无人机机动能力将任务区域离散化，并将无人机起点、终点位置转化至离散任务区域地图的节点上；
步骤2：依据步骤1获取的任务区域内障碍物的范围和高度信息，将障碍物在离散任务区域地图内进行离散化处理；
步骤2具体实现方法包括如下步骤：
步骤2.1：根据任务区域的范围，记离散任务区域地图的高度节点坐标下限为h min和高度节点坐标上限为h max；根据步骤1.2得：
Figure QLYQS_8
（3）
其中，Z max为任务区域高度上限，ceil(·)为向上取整运算符；
步骤2.2：在离散任务区域地图高度为h min 的平面上，依据步骤1获取的任务区域内障碍物的范围和高度信息，获取该平面上每个离散节点处对应的障碍物高度，并将障碍物高度转化为离散任务区域地图内的障碍物节点坐标，如式（4）所示；
Figure QLYQS_9
（4）
其中，H(i,j)为离散节点(i,j)上对应的障碍物高度，h(i,j)为离散节点(i,j)上障碍物的高度节点坐标；
步骤2.3：如果h(i,j)≤h min，则将该节点对应的障碍物高度节点坐标设置为0；如果h(i,j)＞h max，则将该节点对应的障碍物高度节点坐标设置为h max；如果h(i,j)位于离散任务区域地图的4个铅垂边界面上，则将节点对应的障碍物高度节点坐标设置为h max；
至此，完成障碍物在离散任务区域地图内的离散化处理，实现离散任务区域地图内障碍物高程地图的生成，所述障碍物高程地图包络了任务区域内的所有障碍；
步骤3：根据步骤2获取的高程地图信息，基于确定性规则建立障碍物对应的膨化障碍；
步骤4：在离散任务区域地图内，根据步骤3获取的膨化障碍信息，将碰撞检测环节转化进入启发式代价函数计算环节，采用无需进行碰撞检测的改进3D-A*算法，进行无人机三维航迹规划；
步骤3具体实现方法包括如下步骤：
步骤3.1：记离散任务区域地图高度为h min 的平面上每个节点的膨化障碍物高度为h e ；检查在离散任务区域地图高度为h min 的平面上的每一个离散节点(i,j)，如果该节点上对应的障碍物高度满足h(i,j)＝0，执行步骤3.2；如果该节点上对应的障碍物高度满足h(i,j)＞0，则h e (i,j)＝h(i,j)，执行步骤3.4；
步骤3.2：如果该节点不是起点或终点在离散任务区域地图高度为h min 的平面上的投影，检查该节点周围8个节点的障碍物高度坐标，如果周围8个节点中存在障碍物高度坐标大于0的节点，则将h(i,j)更改为周围8个节点的高度坐标的最大值
Figure QLYQS_10
，如式（5）所示，然后执行步骤3.4；
Figure QLYQS_11
（5）
如果该节点是起点或终点在离散任务区域地图高度为h min 的平面上的投影，执行步骤3.3；
步骤3.3：判断起点处膨化障碍高度h e,s ，如果h e,s ≥z s ，则令h e,s ＝z s ；如果h e,s ＜z s ，则令h e,s 保持不变；判断终点处膨化障碍高度h e,g ，如果h e,g ≥z g ，则令h e,g ＝z g ；如果h e,g ＜z g ，则令h e,g 保持不变；
步骤3.4：如果h e (i,j)＞0，且h(i,j)≤h max－1，则h e (i,j)＝h(i,j)＋1，即，将障碍物节点膨化至其上方的1个节点；如果h e (i,j)＞0，且h(i,j)＞h max－1，则h e (i,j)＝h(i,j)，即，障碍不再继续向上膨化；如果h e (i,j)＝0，则h e (i,j)同样保持不变。
2.如权利要求1所述基于膨化障碍的无人机三维航迹快速规划方法，其特征在于：
步骤4具体实现方法包括如下步骤：
步骤4.1：在步骤1.2创建的离散任务区域地图中，规定无人机能够任意到达的运动方向；无人机能够沿着正方体网格的边、面对角线或体对角线运动，共26个方向，即以当前节点为中心，向其四周8个网格地图单元格节点扩展的26个方向；
步骤4.2：创建OPEN集与CLOSED集，其中OPEN集用来存放已经产生但是没有扩展的节点，CLOSED集用来存放已经被扩展的节点；初始化时，将离散任务区域地图内的无人机起点装入OPEN集，CLOSED集清空；
步骤4.3：若OPEN集为空，则规划失败，规划结束；若OPEN集不为空，则从OPEN集中选取代价值最小的节点作为当前节点，将当前节点从OPEN集移入CLOSED集；并将代价值最小的节点标记为
Figure QLYQS_12
；
步骤4.4：判断是否抵达网格化的终点节点，若抵达终点，则将终点放入CLOSED集，执行步骤4.6；若尚未抵达终点，则执行步骤4.5；
步骤4.5：将碰撞检测环节转化进入启发式代价函数计算环节；
以步骤4.3中节点
Figure QLYQS_13
为中心，向步骤4.1规定的26个方向对应的离散任务区域地图节点创建待扩展空间，记待扩展空间内的待扩展节点为
Figure QLYQS_14
，k＝1,2,…,26，并直接计算每个待扩展节点的代价值，选取代价最小的装入OPEN集，继续执行步骤4.3；所述每个待扩展节点的代价值F b 为
Figure QLYQS_15
（6）
其中，G b 为起点
Figure QLYQS_16
到待扩展节点/>
Figure QLYQS_22
的累计代价值，∑(·)为求和运算符，k G 为G b 的权重系数；/>
Figure QLYQS_25
为节点/>
Figure QLYQS_19
到节点/>
Figure QLYQS_20
的累计路径长度代价值，
Figure QLYQS_26
为节点/>
Figure QLYQS_29
到待扩展节点/>
Figure QLYQS_18
的路径长度代价值；H b 为由待扩展节点/>
Figure QLYQS_21
到终点/>
Figure QLYQS_27
的估计代价值，k H 为H b 的权重系数；L b 为代价函数中的节点纵向扩展启发项，k L 为起点/>
Figure QLYQS_28
到终点/>
Figure QLYQS_17
的估计航程值，/>
Figure QLYQS_23
为待扩展节点/>
Figure QLYQS_24
处对应的膨化障碍高度坐标；
步骤4.6：依次回溯每一步生成的节点，并根据式（1）和式（2）的逆运算将航迹点坐标序列还原至航迹点的真实地理坐标序列，并将航迹起点和终点节点还原为无人机起点、终点的真实地理坐标
Figure QLYQS_30
和/>
Figure QLYQS_31
；生成从起点到终点的三维安全飞行航迹。
CN202211588708.4A 2022-12-12 2022-12-12 基于膨化障碍的无人机三维航迹快速规划方法 Active CN115617078B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211588708.4A CN115617078B (zh) 2022-12-12 2022-12-12 基于膨化障碍的无人机三维航迹快速规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211588708.4A CN115617078B (zh) 2022-12-12 2022-12-12 基于膨化障碍的无人机三维航迹快速规划方法

Publications (2)

Publication Number Publication Date
CN115617078A CN115617078A (zh) 2023-01-17
CN115617078B true CN115617078B (zh) 2023-07-07

Family

ID=84880184

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211588708.4A Active CN115617078B (zh) 2022-12-12 2022-12-12 基于膨化障碍的无人机三维航迹快速规划方法

Country Status (1)

Country Link
CN (1) CN115617078B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104390640A (zh) * 2014-11-13 2015-03-04 沈阳航空航天大学 一种基于理想流体数值计算的无人机三维航路规划方法
WO2022057701A1 (zh) * 2020-09-18 2022-03-24 武汉理工大学 一种面向单艘无人测量船艇覆盖路径规划方法
CN114995415A (zh) * 2022-05-25 2022-09-02 武汉理工大学 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3754110B2 (ja) * 1995-07-14 2006-03-08 株式会社日立製作所 動的経路探索方法およびナビゲーション装置
FR2898699B1 (fr) * 2006-03-17 2014-05-09 Thales Sa Planificateur automatique de trajectoire
CN105405165B (zh) * 2015-10-21 2018-07-27 北京航空航天大学 一种通用型无人机飞行中地貌分析及迫降区域提取仿真系统
CN106843282B (zh) * 2017-03-17 2020-11-03 东南大学 基于m100开发平台的区域完全搜索与避障系统及方法
CN111811511B (zh) * 2020-06-23 2024-04-19 北京理工大学 一种基于降维解耦机制的无人机集群实时航迹生成方法
CN111924139B (zh) * 2020-08-03 2022-05-24 北京理工大学 基于膨胀预警区的小天体着陆避障常推力控制方法
CN113504788B (zh) * 2021-06-24 2024-02-20 北京农业智能装备技术研究中心 一种航空施药作业航线规划方法及系统
CN114326810B (zh) * 2021-12-29 2023-05-30 中山大学 一种无人机在复杂动态环境下的避障方法
CN114967744A (zh) * 2022-05-31 2022-08-30 哈尔滨工业大学 一种多无人机协同避障的规划方法
CN115328165B (zh) * 2022-09-16 2023-04-07 大连理工大学 一种基于安全调运走廊的航母舰载机甲板滑行轨迹规划方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104390640A (zh) * 2014-11-13 2015-03-04 沈阳航空航天大学 一种基于理想流体数值计算的无人机三维航路规划方法
WO2022057701A1 (zh) * 2020-09-18 2022-03-24 武汉理工大学 一种面向单艘无人测量船艇覆盖路径规划方法
CN114995415A (zh) * 2022-05-25 2022-09-02 武汉理工大学 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法

Also Published As

Publication number Publication date
CN115617078A (zh) 2023-01-17

Similar Documents

Publication Publication Date Title
Szczerba et al. Robust algorithm for real-time route planning
CN108563243B (zh) 一种基于改进rrt算法的无人机航迹规划方法
Hwangbo et al. Efficient two-phase 3D motion planning for small fixed-wing UAVs
CN106371445A (zh) 一种基于拓扑地图的无人车规划控制方法
CN115079705A (zh) 基于改进a星融合dwa优化算法的巡检机器人路径规划方法
CN110320930A (zh) 基于Voronoi图的多无人机编队队形可靠变换方法
CN112539750B (zh) 一种智能运输车路径规划方法
CN112985408A (zh) 一种路径规划优化方法及系统
Ma et al. A Fast path re-planning method for UAV based on improved A* algorithm
Hsu et al. Path planning for mobile robots based on improved ant colony optimization
CN116954239B (zh) 一种基于改进粒子群算法的无人机航迹规划方法及系统
CN112327927A (zh) 基于网格规划算法的编队无人机多角度打击航迹规划方法
CN115570566A (zh) 一种改进apf-rrt算法的机器人避障路径规划方法
CN114296474A (zh) 一种基于路径时间代价的无人机路径规划方法及系统
CN115060263A (zh) 一种考虑低空风和无人机能耗的航迹规划方法
Liu et al. A variable-step RRT* path planning algorithm for quadrotors in below-canopy
CN116009527A (zh) 基于动态场景结构膨胀感知的路径规划算法
CN116880561A (zh) 基于改进粒子群无人机路径规划安全增强的优化方法及系统
CN115617078B (zh) 基于膨化障碍的无人机三维航迹快速规划方法
CN113220008B (zh) 多火星飞行器的协同动态路径规划方法
CN115657725B (zh) 一种子母式无人机投放决策及路径规划一体化方法及系统
Zhao et al. Temporal and spatial routing for large scale safe and connected uas traffic management in urban areas
Sujit et al. Cooperative path planning for multiple UAVs exploring an unknown region
CN115326070A (zh) 城市低空环境下基于三维切线图法的无人机航迹规划方法
CN115220480A (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