CN108253987A - 一种基于a*算法的无人机轨迹规划方法、设备及存储设备 - Google Patents

Abstract

本发明提供了一种基于A*算法的无人机轨迹规划方法、设备及存储设备，方法包括步骤：S1：加载数据；S2：预处理；S3：载入OPEN表；S4：OPEN表为空否；S5：空，到达目标点；S6：否则，取代价最小点放入CLOSE表；S7：判断到达终点否；S8：是，完成轨迹规划；S9：否，扩展邻节点；S10：判断邻节点为空否；S11：空，取父节点并判断OPEN表为空否；S12：空，继续取父节点并判断OPEN表为空否；S13：否则，返回S6；S14：若扩展邻节点不为空则执行约束处理；S15：判断扩展邻节点在CLOSE表中否；S16：若在，该点已遍历，返回S7；S17：否则，判断该点存于OPEN表中否；S18：若是，计算评价函数并返回S4；S19：否则，返回S3。设备用来实现所述方法。本发明可以规划无人机的轨迹。

Description

一种基于A*算法的无人机轨迹规划方法、设备及存储设备

技术领域

本发明涉及智能控制领域，具体涉及一种基于A*算法的无人机轨迹规划方法、设备及存储设备。

背景技术

随着低空领域开放，航空领域将面临空中交通流量大、飞行密度高、空域结构复杂的状态，此时军用和民用无人机也将会大量涌现，对无人机的飞行安全和空中管制带来了新的挑战。如何保证战时各型无人机使用空域时互不影响，同时不危及有人机执行作战任务时的飞行安全；如何保证平时无人机的各种任务飞行、训练和科研试飞活动能够合理地使用空域，并不影响军民航有人飞机的正常飞行活动，这已成为当前和以后若干年内亟待解决的重要问题。

发明内容

本发明提供了一种基于A*算法的无人机轨迹规划方法、设备及存储设备，在A*算法中引入约束条件和预处理机制，可以有效规划无人机在飞行过程中的轨迹，从而有效解决上述问题。

本发明提供的技术方案是：一种基于A*算法的无人机轨迹规划方法，所述方法包括步骤：S1：加载数据；S2：对无人机轨迹规划中的参数进行预处理；S3：将任务轨迹点载入OPEN表中；S4：判断当前航迹点的OPEN表是否为空；S5：如果为空，则无人机到达目标点；S6：如果不为空，则取当前航迹点的OPEN表中代价最小点放入CLOSE表中并删除OPEN表中的代价最小点；S7：判断无人机是否到达终点；S8：如果是，则输出轨迹点完成无人机轨迹规划；S9：如果否，扩展当前航迹点的邻节点；S10：判断扩展后的邻节点是否为空；S11：如果为空，则取所述空白邻节点的父节点并判断父节点的OPEN表是否为空；S12：如果为空，则继续取父节点的父节点并判断新父节点的OPEN表是否为空；S13：如果不为空，则返回S6继续执行；S14：如果扩展后的邻节点不为空则执行约束处理；S15：判断扩展后的邻节点是否存在于CLOSE表中；S16：如果存在，则该邻节点已经遍历，返回S7继续执行；S17：如果不存在，则判断扩展后的邻节点是否存在于OPEN表中；S18：如果存在于OPEN表中，则计算到达邻节点的航迹的评价函数并返回S4继续执行；S19：如果不存在于OPEN表中，则计算到达邻节点的航迹的评价函数并返回S3继续执行。存储设备，所述存储设备存储指令及数据用于实现所述一种基于A*算法的无人机轨迹规划方法。一种基于A*算法的无人机轨迹规划设备，所述设备包括处理器及所述存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现所述的一种基于A*算法的无人机轨迹规划方法。

本发明的有益效果是：本发明提供了一种基于A*算法的无人机轨迹规划方法、设备及存储设备，通过在A*算法中引入约束条件和预处理机制，可以有效规划无人机在飞行过程中的轨迹，为无人机轨迹规划提供了一种新思路。

附图说明

图1是本发明实施例中基于A*算法的无人机轨迹规划方法的整体流程图；

图2是本发明实施例中球体模型威胁的判断步骤流程图；

图3是本发明实施例中柱体模型威胁的判断步骤流程图；

图4是本发明实施例中球体模型威胁的说明示意图；

图5是本发明实施例中柱体模型威胁判断二维平面投影示意图；

图6是本发明实施例的硬件设备工作示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述，下文中提到的具体技术细节，如：方法，设备等，仅为使读者更好的理解技术方案，并不代表本发明仅局限于以下技术细节。

本发明的实施例提供了一种基于A*算法的无人机轨迹规划方法、设备及存储设备。请参阅图1，图1是本发明实施例中基于A*算法的无人机轨迹规划方法的整体流程图，所述方法由硬件设备实现，具体步骤包括：

S101：加载数据，具体为：从XML文件中读取参数及起止点数据信息。

S102：对无人机轨迹规划中的参数进行预处理，包括：坐标变换、跨带修正、数字地图加载及威胁处理。

1021.坐标转换

FlightGear使用测地学坐标系定位飞行器的位置，但是测地学坐标系是一个角度坐标系，经度、纬度的度量单位都是度，不能用于计算长度和面积。动力学方程使用直角坐标系进行分析和测算，这便需要进行两个坐标系统的转换。测地学坐标可以通过投影算法归算到平面上来，投影方法有等角投影、等面积投影和任意投影。平面直角坐标通过投影反算可以转换为测地学坐标。本项目采用高斯-克吕格尔投影简称高斯投影来实现坐标系的转换。其中，高斯正算将经纬度转换为高斯投影坐标；高斯反算将高斯投影坐标转换为经纬度。

1022.跨带修正

由于任何一种球面都不可能完全展开平面，因此在将球面坐标转换成平面坐标时必须根据一定指标进行最佳逼近。而高斯投影是一种等角投影，即投影变换后的平面坐标系中的角度关系与原球面坐标系中的角度关系保持不变。高斯投影具有“离中央子午线越远，变形越大”的特点，国际上普遍采用投影分带的方法。把地球360度经度按照6度或3度进行分割，这样整个地球就有很多条中央子午线，任何一个点都不会因离中央子午线太远而有太大变形。

本项目采用6度带划分法，处理跨带计算时，将问题进行查分，把跨带计算转换成带内计算，将一个跨带计算拆分成若干个带内计算，当A、B两点跨多带时，在直线AB遇到带去分界线的地方进行截断，即在点a,b,c,…,j处截断。截断是指分别求出这10个点的经纬度。例如，对于Aa段，a点的经度近似取5.999999度，b点经度近似6.000001度(这样就避开了求取边界点的坐标，避免高斯投影带来的跨带误差)，截断点的纬度则用纬度差除以经度差计算求得。此处对于a,b两点的经度差造成的误差可以忽略不计，因为再同一纬度，经度差0.000002度的两点的最大距离位于赤道上，这个距离为0.222638912m，与计算值相差几个数量级，因此可以忽略不计。

1023.数字地图加载

本项目需要考虑实际的地形因素，因此需要加载地图中的高程信息。本文的地图文件为TIFF文件，来源于google地图。

标签图像文件格式(Tagged Image File Format，简写为TIFF)是一种主要用来存储包括照片和艺术图在内的图像的文件格式。TIFF文件以.GIF为扩展名。其数据格式是一种3级体系结构，从高到低依次为：文件头、一个或多个称为IFD的包含标记指针的目录和数据。在每一个TIFF文件中第一个数据结构称为图像文件头或IFH，它是图像文件体系结构的最高层。文件头结构在一个TIFF文件中是惟一的，有固定的位置。它位于文件的开始部分，包含了正确解释TIFF文件的其他部分所需的必要信息；文件目录(IFD)是TIFF文件中第2个数据结构，它是一个名为标记(tag)的用于区分一个或多个可变长度数据块的表，标记中包含了有关于图像的所有信息。IFD提供了一系列的指针(索引)，这些指针告诉我们各种有关的数据字段在文件中的开始位置，并给出每个字段的数据类型及长度；图像数据是根据IFD所指向的地址.存储相关的图像信息。

1024.威胁处理

地形数据在地图数据加载模块已经处理，此处的威胁处理是指对于建筑物等新威胁的建模和处理，本文将威胁看做数学模型中的球体或者柱体(可拓展模型库)，以此建立威胁模型。

在航迹规划之前，需要对威胁进行预处理，一方面，剔除不在搜索空间内的威胁点，提高效率；另一方面，根据起点坐标，进行计算，将威胁放入到三维网格空间中。

S103：将任务轨迹点载入OPEN表中。

S104：判断当前航迹点的OPEN表是否为空。

S105：如果为空，则无人机到达目标点。

S106：如果不为空，则取当前航迹点的OPEN表中代价最小点放入CLOSE表中并删除OPEN表中的代价最小点。

S107：判断无人机是否到达终点。

S108：如果是，则输出轨迹点完成无人机轨迹规划。

S109：如果否，扩展当前航迹点的邻节点。

S110：判断扩展后的邻节点是否为空，如果不为空，则返回S106继续执行。

S111：如果为空，则取所述空白邻节点的父节点。

S112：判断父节点的OPEN表是否为空。如果为空，则返回S111继续执行。如果不为空，则返回S106继续执行。

S113：如果扩展后的邻节点不为空则执行约束处理，所述约束处理的约束项具体包括：垂直方向最大转弯角约束、水平方向最大转弯角约束、最小转弯半径约束、最远飞行距离约束、飞行高度约束及威胁约束。所述威胁约束具体为实物威胁；对所述实物威胁的模型进行简化，具体包括：球体模型威胁和柱体模型威胁。

1131.垂直方向最大转弯角约束

由于无人机机动性能的限制，无人机在垂直机身的方向上进行的抬头或低头动作应该是在一定的角度范围之内，超过该角度会影响到无人机的飞行安全，因此规划航迹在竖直方向上的转弯角度应该小于该角度。垂直方向最大转弯角约束可以表示为

式中，i表示当前为第i段航迹，(x_i,y_i,z_i)与(x_(i+1),y_(i+1),z_(i+1))分别表示当前航路点与待选航路点在规划空间中的位置坐标，θ_max表示无人机在垂直方向上的最大转弯角度。

1132.水平方向最大转弯角约束

与垂直方向相同，无人机在水平方向上转弯时也受到一定的角度限制，水平方向最大转弯角约束可以表示为

式中，i表示当前为第i段航迹，(x_i,y_i)与(x_(i+1),y_(i+1))分别表示当前航路点与待选航路点在规划空间中水平位置坐标，θ_max表示无人机在水平方向上的最大转弯角度。

1133.最小转弯半径约束

无人机在进行转弯时由于机动性能的限制只能进行一定半径的转弯，其转弯半径不能小于一定值，因此在规划出的航迹的进行转弯时应该由一定的转弯半径约束，最小转弯半径约束可以表示为

R_i≥R_min，(i＝2，...，n)

式中，R_i为规划航迹进行第i次转弯时的转弯半径，R_min为无人机的最大转弯半径。R_min计算公式为

1134.最远飞行距离约束

由于无人机只能携带一定数量的燃料，且无人机所执行的任务也需要在一定的时间内完成，因此无人机所飞行的距离应该有小于一个最远的距离。最远飞行距离约束可以表示为

1135.飞行高度约束

为了避开敌方预警雷达的搜索，无人机的飞行高度应该尽量的低，然而如果飞行高度过低的话则又容易与地面发生撞击，所以无人机的飞行高度应该在一定的范围只之内，飞行高度约束可表示为

H_min≤H_i≤H_max，(i＝1，...n)

式中，H_i为当前飞行高度，H_min为最低可飞高度，H_max为最高可飞高度。

1136.威胁约束

对于威胁约束，首先进行判断，当点到威胁中心的距离小于半径时，此点不安全；大于威胁半径时，即点在威胁外，判断威胁是否会影响到拓展点与当前点连接的线段，若存在，则进行下述判断，若不存在，则安全。

S114：判断扩展后的邻节点是否存在于CLOSE表中。

S115：如果存在，则该邻节点已经遍历，返回S107继续执行。

S116：如果不存在，则判断扩展后的邻节点是否存在于OPEN表中。

S117：如果存在于OPEN表中，则计算到达邻节点的航迹的评价函数，返回S104继续执行。

S118：如果不存在于OPEN表中，则计算到达邻节点的航迹的评价函数，返回S103继续执行。所述评价函数具体公式为：

L_F(N)＝L_G(N)+L_H(N)

其中，L_G(N)为从起始点到当前规划点的航迹长度，L_H(N)为从当前规划点到目标点的估计长度。

参见图2，图2是本发明实施例中球体模型威胁的判断步骤流程图，包括：

S201：建立当前航迹点与当前航迹点扩展后的邻节点的连线。

S202：计算球体模型的中心到所述连线的距离。

S203：判断所述距离是否大于球体模型半径。

S204：若大于，则判断无威胁，航迹可用。

S205：若小于等于，则判断有威胁，航迹不可用。

参见图3，图3是本发明实施例中柱体模型威胁的判断步骤流程图，包括：

S301：将三维坐标系xyz中柱体模型及当前航迹点及扩展后的邻节点投影到二维平面xy上，所述二维平面上为一圆及两点。

S302：在投影平面上建立当前航迹点与当前航迹点扩展后的邻节点的连线。

S303：计算圆的中心到所述连线的距离。

S304：判断所述距离是否大于圆的半径。

S305：若大于，则判断无威胁，航迹可用。

S306：若小于等于，则判断所述连线与柱体是否有交点。

S307：若有交点，则有威胁，航迹不可用。

S308：若无交点，则无威胁，航迹可用。

参见图4，图4是本发明实施例中球体模型威胁的说明示意图，包括：当前航迹点401、扩展后的邻节点402、航迹403、球体模型中心到航迹距离404、球体模型405及球体模型半径406。由图中可见，当球体模型中心到航迹距离404大于球体模型半径406时，航迹403为有效航迹。当球体模型中心到航迹距离404小于等于球体模型半径406时，航迹403为无效航迹。

参见图5，图5是本发明实施例中柱体模型威胁判断二维平面投影示意图，包括：当前航迹点501、扩展后邻节点502、航迹503、柱体投影圆心到航迹距离504、柱体投影圆半径505、柱体投影圆心506及柱体投影圆507。由图中可见，当柱体投影圆心到航迹距离504大于柱体投影圆半径505时，航迹503可用。当柱体投影圆心到航迹距离504小于等于柱体投影圆半径505时，就要判断航迹503与真实的柱体是否有交点。若有，则航迹503不可用。若没有，则航迹503可用。

参见图6，图6是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种基于A*算法的无人机轨迹规划设备601、处理器602及存储设备603。

基于A*算法的无人机轨迹规划设备601：所述一种基于A*算法的无人机轨迹规划设备601实现所述一种基于A*算法的无人机轨迹规划方法。

处理器602：所述处理器602加载并执行所述存储设备603中的指令及数据用于实现所述的一种基于A*算法的无人机轨迹规划方法。

存储设备603：所述存储设备603存储指令及数据；所述存储设备603用于实现所述的一种基于A*算法的无人机轨迹规划方法。

通过执行本发明的实施例，本发明权利要求里的所有技术特征都得到了详尽阐述。

区别于现有技术，本发明的实施例提供了一种基于A*算法的无人机轨迹规划方法、设备及存储设备，通过在A*算法中引入约束条件和预处理机制，可以有效规划无人机在飞行过程中的轨迹，为无人机轨迹规划提供了一种新思路。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于A*算法的无人机轨迹规划方法，所述方法由硬件设备实现，其特征在于：包括以下步骤：S1：加载数据；S2：对无人机轨迹规划中的参数进行预处理；S3：将任务轨迹点载入OPEN表中；S4：判断当前航迹点的OPEN表是否为空；S5：如果为空，则无人机到达目标点；S6：如果不为空，则取当前航迹点的OPEN表中代价最小点放入CLOSE表中并删除OPEN表中的代价最小点；S7：判断无人机是否到达终点；S8：如果是，则输出轨迹点完成无人机轨迹规划；S9：如果否，扩展当前航迹点的邻节点；S10：判断扩展后的邻节点是否为空；S11：如果为空，则取所述空白邻节点的父节点并判断父节点的OPEN表是否为空；S12：如果为空，则继续取父节点的父节点并判断新父节点的OPEN表是否为空；S13：如果不为空，则返回S6继续执行；S14：如果扩展后的邻节点不为空则执行约束处理；S15：判断扩展后的邻节点是否存在于CLOSE表中；S16：如果存在，则该邻节点已经遍历，返回S7继续执行；S17：如果不存在，则判断扩展后的邻节点是否存在于OPEN表中；S18：如果存在于OPEN表中，则计算到达邻节点的航迹的评价函数并返回S4继续执行；S19：如果不存在于OPEN表中，则计算到达邻节点的航迹的评价函数并返回S3继续执行。

2.如权利要求1所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述加载数据具体为：从XML文件中读取参数及起止点数据信息。

3.如权利要求1所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述预处理包括：坐标变换、跨带修正、数字地图加载及威胁处理。

4.如权利要求1所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述约束处理中的约束项具体包括：垂直方向最大转弯角约束、水平方向最大转弯角约束、最小转弯半径约束、最远飞行距离约束、飞行高度约束及威胁约束。

5.如权利要求4所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述威胁约束具体为实物威胁；对所述实物威胁的模型进行简化，具体包括：球体模型威胁和柱体模型威胁。

6.如权利要求5所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述球体模型威胁的判断步骤为：建立当前航迹点与当前航迹点扩展后的邻节点的连线；计算球体模型的中心到所述连线的距离；判断所述距离是否大于球体模型半径；若大于，则判断无威胁，航迹可用；若小于等于，则判断有威胁，航迹不可用。

7.如权利要求5所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述柱体模型威胁的判断步骤为：将三维坐标系xyz中柱体模型及当前航迹点及扩展后的邻节点投影到二维平面xy上，所述二维平面上为一圆及两点；在投影平面上建立当前航迹点与当前航迹点扩展后的邻节点的连线；计算圆的中心到所述连线的距离；判断所述距离是否大于圆的半径；若大于，则判断无威胁，航迹可用；若小于等于，则判断所述连线与柱体是否有交点；若有交点，则有威胁，航迹不可用；若无交点，则无威胁，航迹可用。

8.如权利要求1所述的一种基于A*算法的无人机轨迹规划方法，其特征在于：所述评价函数具体公式为：

L_F(N)＝L_G(N)+L_H(N)

其中，L_G(N)为从起始点到当前规划点的航迹长度，L_H(N)为从当前规划点到目标点的估计长度。

9.存储设备，其特征包括：所述存储设备存储指令及数据用于实现权利要求1～8所述的任意一种方法。

10.一种基于A*算法的无人机轨迹规划设备，其特征在于：包括：处理器及所述存储设备；所述处理器加载并执行权利要求9中所述存储设备中的指令及数据用于实现权利要求1～8所述的任意一种方法。