CN117664142B - 基于三维立体地图的飞行汽车路径规划方法 - Google Patents

Abstract

本申请公开了基于三维立体地图的飞行汽车路径规划方法，所述方法设置飞行汽车起点和终点，导入三维立体地图，获取地形及障碍物信息，考虑飞行汽车尺寸，在分析节点可飞行性的基础上，基于三维立体地图采用图搜索的方法进行路径搜索，生成飞行汽车规划路径，具有以下优点：确保飞行汽车经过各节点时与该位置的高程保持一定的安全距离，确保飞行汽车的安全飞行，更加贴近飞行汽车实际飞行环境，可应用于飞行汽车路径规划、航线导引等领域。

Description

基于三维立体地图的飞行汽车路径规划方法

技术领域

本发明属于飞行汽车路径规划领域，特别涉及一种基于三维立体地图的飞行汽车路径规划方法。

背景技术

随着智能化、电动化技术以及无人驾驶飞行器技术的不断发展及成熟，以飞行汽车为代表的空中交通工具应运而生。而随着地图制图技术的发展，三维立体地图也在向着高维高精度方向发展，面向飞行汽车的三维立体地图融合了城市空间定位信息、地面和高层建筑纹理等高维高精度数据。三维立体地图是飞行汽车低空飞行的重要保障和基础，为飞行汽车的通航、作业等提供基准底图支撑。

与传统的地面行驶车辆在平面上基于二维平面路径规划不同，飞行汽车需要基于三维立体地图，在空间三维立体空间进行路径规划，规划飞行汽车从城市任意一个起飞点飞行到任意一个降落点的飞行路径。由于城市三维立体空间复杂，加上飞行汽车自身较大的体型尺寸，基于三维立体地图的路径规划需要着重考虑飞行汽车与城市建筑障碍物、地形高度之间的安全距离，以保证飞行汽车在城市上空飞行的飞行安全。

发明内容

本发明要解决的技术问题是针对以上不足，提供基于三维立体地图的飞行汽车路径规划方法，设置飞行汽车起点和终点，导入三维立体地图，获取地形及障碍物信息，考虑飞行汽车尺寸，在分析节点可飞行性的基础上，基于三维立体地图采用图搜索的方法进行路径搜索，生成飞行汽车规划路径。在飞行汽车飞行路径规划时，考虑了飞行汽车尺寸，确保飞行汽车飞行路径上的各节点能够保证一定的安全距离，实现飞行汽车的安全飞行。

为解决以上技术问题，本发明采用以下技术方案：

基于三维立体地图的飞行汽车路径规划方法，包括以下步骤：

步骤一、设置飞行汽车起点和终点：设置飞行汽车起点为start，飞行汽车终点为goal，均为三维点坐标形式(x,y,z)，即start=(xs,ys,zs),goal=(xg,yg,zg)；

步骤二、导入三维立体地图：基于三维立体地图，建立待搜索的三维立体环境，为一系列网格化的三维坐标点（XX,YY,HH），表示城市建筑物高度和地形高度,XX与YY表征平面位置，HH为高程，表征城市建筑物高度和地形高度；

步骤三、基于地图路径搜索；

步骤四、生成飞行汽车规划路径：搜索完成后，在Close列表搜索，从终点开始，查找对应的父节点，直至起点，则为飞行汽车规划路径。

进一步的，所述步骤三中路径搜索包括以下步骤：

步骤1，初始化设置，设置open列表为空，Close列表为空，open列表形式为{x,y,z,a，g},为五列矩阵，其中前三列x,y,z为该点的三维位置坐标，第四列a为该点的估价值，即该点到终点的最佳路径的估计代价，第5列为路径值，即起点到该点的实际代价；Close列表形式为{xp,yp,zp,xc,yc,zc}，为六列矩阵，前三列为节点位置，后三列为该节点的子节点位置；

计算起点的估价值a,以及路径值g，将起点加入open列表，估价值为该点到终点的最佳路径的估计代价，可设置为起点与终点的距离；路径值为起点到该点的实际代价，为0，计算公式如下：

；

步骤2，对open列表第四列从小到大排序，寻找open列表中a值最小的节点，作为当前要处理的节点，称为当前节点；

步骤3，如果当前节点为终点，则结束搜索，否则搜索当前节点/>的子节点。

进一步的，所述步骤三中路径搜索还包括以下步骤：

步骤4，搜索当前节点的子节点，按搜索方向获取当前节点/>的子节点，设计空间三维立体搜索方向为前、后、左、右、上、下共6个方向，按搜索步长每个方向向前搜索一步获取相邻的节点，作为当前节点/>的子节点child，每个子节点计算公式如下：

；

其中，direc为搜索方向，如（0,0,1）表示向上搜索，step为搜索步长；

步骤5，判断子节点child的可飞行性，对于步骤4获取的每一子节点child，判断可飞行性，若子节点child与其所处地面位置的高程距离大于安全距离，则子节点child可飞行，即满足以下条件，否则不可飞行：

；

其中，为子节点child的高度，HH为子节点child位置的高程，为安全距离

飞行汽车尺寸较大覆盖多个节点所在地面位置，其子节点child对应高程HH的计算需要取飞行汽车覆盖地面位置的最大值，假设飞行汽车安全尺寸值为lc，则飞行汽车覆盖的多个节点地面位置坐标可表示为：

；

其中Ceil表示向上取整，对于每个i,j，计算其对应位置的高程距离为HH_k,步骤5中的HH取其中的最大值，即有：

。

步骤6，如果子节点child可飞行，则转到步骤7，否则，舍去子节点child。

进一步的，所述步骤三中路径搜索还包括以下步骤：

步骤7，根据子节点child的三维位置，在open列表前三列查找对比，判断子节点child是否在open列表，如果子节点child不在open列表，则将子节点child加入open列表，设置子节点child的路径值g为极大值，可设置为无穷大inf，即，否则转到步骤8；

步骤8，计算从当前节点到子节点child的路径值，计算公式如下：

其中，为当前节点的路径值，/>为当前节点到子节点child的距离。

进一步的，所述步骤三中路径搜索还包括以下步骤：

步骤9，判断从当前节点到子节点child的路径优劣，以路径值g判断，若从当前节点到子节点child路径值比原来保存的子节点child路径值小，原来保存的子节点child路径值可从open列表对应的子节点第五列获取，则从当前节点到子节点child的路径更优，即

；

其中，为原来保存的路径值。

进一步的，所述步骤三中路径搜索还包括以下步骤：

步骤10，若从当前节点到子节点child的路径更优，则更新子节点child的估价值a，路径值g,否则，舍去子节点child，计算公式如下：

；

其中，为子节点child到终点的曼哈顿距离，即x,y,z方向距离之和；

步骤11，保存当前节点为子节点child的父节点，与子节点child一起对应保存，保存至Close列表，前三列保存当前节点，后三列保存子节点。

本发明采用以上技术方案，与现有技术相比，具有如下技术效果：

基于三维立体地图，从飞行汽车起点开始生成子节点，进行图搜索，搜索时考虑了飞行汽车尺寸，确保飞行汽车经过各节点时与该位置的高程保持一定的安全距离，确保飞行汽车的安全飞行，更加贴近飞行汽车实际飞行环境，可应用于飞行汽车路径规划、航线导引等领域。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。

图1为本发明中飞行汽车路径规划方法流程图；

图2为本发明中基于地图进行路径搜索流程图。

具体实施方式

实施例，如图1所示，基于三维立体地图的飞行汽车路径规划方法，包括以下步骤：

步骤一、设置飞行汽车起点和终点：设置飞行汽车起点为start，飞行汽车终点为goal，均为三维点坐标形式(x,y,z)，即start=(xs,ys,zs),goal=(xg,yg,zg)。

步骤二、导入三维立体地图：基于三维立体地图，建立待搜索的三维立体环境，为一系列网格化的三维坐标点（XX,YY,HH），表示城市建筑物高度和地形高度,XX与YY表征平面位置，HH为高程，表征城市建筑物高度和地形高度。

步骤三、基于地图路径搜索。

如图2所示，基于三维立体地图采用图搜索的方法进行路径搜索，路径搜索步骤如下：

步骤1，初始化设置，设置open列表为空，Close列表为空，open列表形式为{x,y,z,a，g},为五列矩阵，其中前三列x,y,z为该点的三维位置坐标，第四列a为该点的估价值，即该点到终点的最佳路径的估计代价，第5列为路径值，即起点到该点的实际代价；Close列表形式为{xp,yp,zp,xc,yc,zc}，为六列矩阵，前三列为节点位置，后三列为该节点的子节点位置。

计算起点的估价值a,以及路径值g，将起点加入open列表，估价值为该点到终点的最佳路径的估计代价，可设置为起点与终点的距离；路径值为起点到该点的实际代价，为0，计算公式如下：

。

步骤2，对open列表第四列从小到大排序，寻找open列表中a值最小的节点，作为当前要处理的节点，称为当前节点。

步骤3，如果当前节点为终点，则结束搜索，否则转到步骤4。

步骤4，搜索当前节点的子节点，按搜索方向获取当前节点/>的子节点，为提高搜索效率，设计空间三维立体搜索方向为前、后、左、右、上、下共6个方向，按搜索步长每个方向向前搜索一步获取相邻的节点，作为当前节点/>的子节点child，每个子节点计算公式如下：

；

其中，direc为搜索方向，如（0,0,1）表示向上搜索，step为搜索步长。

步骤5，判断子节点child的可飞行性，对于步骤4获取的每一子节点child，判断可飞行性，若子节点child与其所处地面位置的高程距离大于安全距离，则子节点child可飞行，即满足以下条件，否则不可飞行：

；

其中，为子节点child的高度，HH为子节点child位置的高程，为安全距离。

考虑飞行汽车尺寸较大，可能覆盖多个节点所在地面位置，其子节点child对应高程HH的计算需要取飞行汽车覆盖地面位置的最大值，假设飞行汽车安全尺寸值为lc，则飞行汽车覆盖的多个节点地面位置坐标可表示为：

；

其中Ceil表示向上取整，对于每个i,j，计算其对应位置的高程距离为HH_k,步骤5中的HH取其中的最大值，即有：

。

步骤6，如果子节点child可飞行，则转到步骤7，否则，舍去子节点child。

步骤7，根据子节点child的三维位置，在open列表前三列查找对比，判断子节点child是否在open列表，如果子节点child不在open列表，则将子节点child加入open列表，设置子节点child的路径值g为极大值，可设置为无穷大inf，即，否则转到步骤8；

步骤8，计算从当前节点到子节点child的路径值，计算公式如下：

其中，为当前节点的路径值，/>为当前节点到子节点child的距离。

步骤9，判断从当前节点到子节点child的路径优劣，以路径值g判断，若从当前节点到子节点child路径值比原来保存的子节点child路径值小，原来保存的子节点child路径值可从open列表对应的子节点第五列获取，则从当前节点到子节点child的路径更优，即

；

其中，为原来保存的路径值。

步骤10，若从当前节点到子节点child的路径更优，则更新子节点child的估价值a，路径值g,否则，舍去子节点child，计算公式如下：

；

其中，为子节点child到终点的曼哈顿距离，即x,y,z方向距离之和。

步骤11，保存当前节点为子节点child的父节点，与子节点child一起对应保存，保存至Close列表，前三列保存当前节点，后三列保存子节点。

步骤12，返回步骤2进行循环。

步骤四、生成飞行汽车规划路径：搜索完成后，在Close列表搜索，从终点开始，查找对应的父节点，直至起点，则为飞行汽车规划路径。

本发明的描述是为了示例和描述起见而给出的，而并不是无遗漏的或者将本发明限于所公开的形式。很多修改和变化对于本领域的普通技术人员而言是显然的。选择和描述实施例是为了更好的说明本发明的原理和实际应用，并且使本领域的普通技术人员能够理解本发明从而设计适于特定用途的带有各种修改的各种实施例。

Claims (4)

1.基于三维立体地图的飞行汽车路径规划方法，其特征在于：包括以下步骤：

步骤一、设置飞行汽车起点和终点：设置飞行汽车起点为start，飞行汽车终点为goal，均为三维点坐标形式(x,y,z)，即start=(xs,ys,zs),goal=(xg,yg,zg)；

步骤二、导入三维立体地图：基于三维立体地图，建立待搜索的三维立体环境，为一系列网格化的三维坐标点（XX,YY,HH），表示城市建筑物高度和地形高度,XX与YY表征平面位置，HH为高程；

步骤三、基于三维立体地图进行路径搜索；

步骤四、生成飞行汽车规划路径：搜索完成后，在Close列表搜索，从终点开始，查找对应的父节点，直至起点，则为飞行汽车规划路径；

所述步骤三中路径搜索包括以下步骤：

步骤1，初始化设置，设置open列表为空，Close列表为空，open列表形式为{x,y,z,a，g},为五列矩阵，其中前三列x,y,z为该点的三维位置坐标，第四列a为该点的估价值，即该点到终点的最佳路径的估计代价，第5列为路径值，即起点到该点的实际代价；Close列表形式为{xp,yp,zp,xc,yc,zc}，为六列矩阵，前三列为节点位置，后三列为该节点的子节点位置；

计算起点的估价值a,以及路径值g，将起点加入open列表，估价值为该点到终点的最佳路径的估计代价，可设置为起点与终点的距离；路径值为起点到该点的实际代价，为0，计算公式如下：

；

步骤2，对open列表第四列从小到大排序，寻找open列表中a值最小的节点，作为当前要处理的节点，称为当前节点；

步骤3，如果当前节点为终点，则结束搜索，否则搜索当前节点/>的子节点；

所述步骤三中路径搜索还包括以下步骤：

步骤4，搜索当前节点的子节点，按搜索方向获取当前节点/>的子节点，设计空间三维立体搜索方向为前、后、左、右、上、下共6个方向，按搜索步长每个方向向前搜索一步获取相邻的节点，作为当前节点/>的子节点child，每个子节点计算公式如下：

；

其中，direc为搜索方向，step为搜索步长；

步骤5，判断子节点child的可飞行性，对于步骤4获取的每一子节点child，判断可飞行性，若子节点child与其所处地面位置高程的距离大于安全距离，则子节点child可飞行，即满足以下条件，否则不可飞行：

；

其中，为子节点child的高度，HH为子节点child位置的高程，h0为安全距离；

飞行汽车覆盖多个节点所在地面位置，其子节点child对应高程HH的计算需要取飞行汽车覆盖地面位置的最大值，假设飞行汽车安全尺寸值为lc，则飞行汽车覆盖的多个节点地面位置坐标可表示为：

；

其中Ceil表示向上取整，对于每个i,j，计算其对应位置的高程距离为HHk,即有：

；

步骤6，如果子节点child可飞行，则判断子节点child是否在open列表，否则，舍去子节点child。

2.如权利要求1所述的基于三维立体地图的飞行汽车路径规划方法，其特征在于：所述步骤三中路径搜索还包括以下步骤：

步骤7，根据子节点child的三维位置，在open列表前三列查找对比，判断子节点child是否在open列表，如果子节点child不在open列表，则将子节点child加入open列表，设置子节点child的路径值g为极大值，否则转到步骤8；

步骤8，计算从当前节点到子节点child的路径值，计算公式如下：

；

其中，为当前节点的路径值，/>为当前节点到子节点child的距离。

3.如权利要求1所述的基于三维立体地图的飞行汽车路径规划方法，其特征在于：所述步骤三中路径搜索还包括以下步骤：

步骤9，判断从当前节点到子节点child的路径优劣，以路径值g判断，若从当前节点到子节点child路径值比原来保存的子节点child路径值小，原来保存的子节点child路径值可从open列表对应的子节点第五列获取，则从当前节点到子节点child的路径更优，即

；

其中，为原来保存的路径值。

4.如权利要求3所述的基于三维立体地图的飞行汽车路径规划方法，其特征在于：所述步骤三中路径搜索还包括以下步骤：

步骤10，若从当前节点到子节点child的路径更优，则更新子节点child的估价值a，路径值g,否则，舍去子节点child，计算公式如下：

；

其中，为子节点child到终点的曼哈顿距离，即x,y,z方向距离之和；

步骤11，保存当前节点为子节点child的父节点，与子节点child一起对应保存，保存至Close列表，前三列保存当前节点，后三列保存子节点。