CN104075717A - 一种基于改进a*算法的无人机航线规划算法 - Google Patents

Abstract

本发明涉及一种无人机在大范围复杂三维空间的航线规划算法，包括以下步骤，首先基于2.5维网格(每个网格点包含经度、纬度、高程信息)划分三维空间；然后在雷达、灾害天气、禁飞区等约束条件下，综合考虑航线高度、被探测概率、航线长度等影响因子改进A*代价函数，基于算法搜索流程，确定初始航线；最后为了满足无人机性能约束(包括最小步长、转弯半径、爬升率、安全高度等)，进行一系列处理得到最终的可飞行航线。本发明涉及算法稳定、收敛性好、效率高、可满足大范围低空航线规划要求，可运用于军事国防、应急救灾等相关领域之中。

Description

一种基于改进A*算法的无人机航线规划算法

技术领域

本发明具体涉及一种算法，特别是一种用于无人机在大范围复杂三维空间的航线规划算法。 

背景技术

现代军事低空突防主要由无人机完成，为了提高无人机低空突防生存概率和作战效能，需要利用地形的遮蔽作用，在敌防御系统盲区内低空或超低空飞行。其中，关键问题是无人机的航线规划。对于该问题的研究已有多年的历史，有很多研究成果。一些学者试图在人工智能等相关领域寻找智能优化算法，但它们解决大范围无人机路径搜索问题都有一些常见的弊病。A*算法在二维图形搜索领域的成熟运用使得它解决该问题具有先天的优势，但目前成果存在以下问题：(1)目标空间为小范围人造简单地形图，障碍物少且规则，未考虑雷达、恶劣天气等复杂威胁体。无法体现算法在复杂环境中的适应性、收敛性及效率问题。(2)一些学者使用平面区域划分(Voronoi图等)的方法，这类空间划分是基于一定高度的，因此本质上依然是在二维空间中进行路径规划。但山区地形实时截面运算的计算量太大，而且无人机基于一定高度上飞行的理论前提不成立。(3)不考虑无人机的机动性能约束，不是可飞航线。 

发明内容

本发明目的在于提供一种表用于无人机在大范围复杂三维空间的航线规划算法，所涉及算法可满足大范围低空航线规划要求。 

本发明采用的技术方案如下： 

首先将三维连续空间使用离散二维空间表示，即规划空间表示为一定经差和纬差离散网格的网络图，并且为每个节点赋予当地地形的高程信息；然后在雷达、灾害天气、禁飞区等约束条件下，改进A*代价函数使航线高度、被探测概率、航线长度三项指标同方向作用关系，并进行归一化处理，在此基础上使用算法搜索流程，确定初始航线；在无人机最小步长、转弯半径、爬升率、安全高度约束下，建立航线保护区模型，进行一系列处理得到最终的可飞行航线，最后检验算法的稳定性和收敛性以及效率。 

本发明与现有技术相比具有以下优点： 

基于2.5维网格模型，每个节点的邻域仅仅存在8个可扩展方向，算法计算效率要远大于3维网格划分方式；在雷达、灾害天气、禁飞区等约束条件下，将航线高度、被探测概率、航线长度进行归一化处理，改进了A*算法；在无人机最小步长、转弯半径、爬升率、安全高度约束下，优化了航线。本发明涉及算法稳定、收敛性好、效率高、可满足大范围低空航线规划要求。 

具体实施方式

以下以航线规划算法流程对本发明作进一步的详细说明，但本发明的保护范围并不局限于此。 

1目标空间划分 

A*算法是一种启发式搜索算法，需要一个由点和边组成的网络空间，在此目标空间中引入启发信息，由定义好的代价函数确定节点拓展的规则，最终得到两点之间的最优路径。但基于DEM和DOM的三维环境本质上是一个连续状态的栅格空间，并不存在传统二维路径搜索中的路由网，从而无法利用路径搜索算法寻找最短路径中的节点和边，必须对其进行空间划分。 

一些研究尝试使用规则三维网格的空间划分方法。该方法的最大问题在于每个节点的可扩展方向在仅仅考虑邻域的情况下也有26个，要收敛到最优解会耗费大量的时间和内存资源，且随规划空间增大呈指数级增长。因此本发明提出如图1所示的2.5维网格模型，每个网格点包含经度、纬度、高程信息。此时每个节点的邻域仅仅存在8个可扩展方向，算法计算效率要远大于3维网格划分方式。 

2基于A*算法的初始航线确定 

2.1代价函数改进 

对于以上划分的目标网络空间，需要定义A*算法的代价函数对可拓展节点进行评估，从而挑选出最优路径中的节点-边列表。A*代价函数一般公式如下： 

f(n)＝g(n)+h(n)       (1) 

其中n是待扩展节点，g(n)是状态空间中从初始节点到节点n的实际代价，h(n)是从节点n到目标节点的估计代价。A*算法本身的性质决定了代价函数的表达式是影响搜索性能的主要因素。对于无人机低空航线规划问题，Asseo S.J给出了对代价函数的参考公式，如下所示： 

$J={\∫}_0^{t_f}({\mathrm{\ω}}_1{c_t}^2+{\mathrm{\ω}}_2{h}^2+{\mathrm{\ω}}_3{f}_{\mathrm{TA}})\mathrm{dt}---\left(2\right)$

其中c_t为飞行器偏离指定航线的距离，h为飞行高度，f_TA为当前位置的威胁指标，ω₁、ω₂、ω₃分别为相应的权重值。 

但该代价函数存在一定的问题。首先，c_t是相对距离值，h是绝对海拔，它们并不处于同一个数量级的作用关系；f_TA为到威胁点中心的距离，它对代价函数是正方向的作用关系， 该值越小越好，c_t、h则相反，因此很难分配对应项一个合理的权重值。其次，雷达并不是一个普通的威胁体，它对目标的威胁程度不能使用距离衡量，而是使用一个概率值来表达，它与其他两项的值将相差几个数量级，那么代价函数对ω₁、ω₂、ω₃的值就不够敏感，很难指定一个合理的值使得代价函数处于一个对搜索结果有意义的状态。 

为了解决以上问题，对代价函数进行了如下改进： 

$g\left(n\right)={\mathrm{\Σ}}_1^n({\mathrm{\ω}}_1{c}_i+{\mathrm{\ω}}_2{p}_i+{\mathrm{\ω}}_3{h}_i)---\left(3\right)$

其中，c_i为第i个节点与它前一个节点之间的地表距离，它是对航线长度的直接惩罚，可以使得航线以最快的速度向目标点靠近，而避免陷入局部最优和搜索死锁。p_i是节点i与i-1之间的被探测概率。该项参数是为了提高无人机的存活率。h_i为节点i与i-1之间的加权平均高度。 

该代价函数的三项指标是同方向的作用关系，要求航线长度越短、航线高度越低、探测概率越小。但是它们并不在一个数量级上，因此可以对各项惩罚指标进行归一化处理。采用0-1之间变换的关键是确定该指标变量的最大值和最小值。但节点之间的距离长短不一，地形也起伏不定，无法直接指定两点地表长度的最大值和最小值，但无论是真实的或人工设定的地形数据，最大和最小地形高度是已知的，或者也可以根据的地形数据计算出来，一旦确定了h_max和h_min，那么c_i的最大最小值也可以确定了，如图2所示。 

$\left\{\begin{array}{c}{c}_{\max }=\sqrt{\mathrm{\Δ}{d}^2+{(h_{\max }-h_{\min })}^2}*(n_i-1)\\ c_{\min }=\sqrt{d_i^2-\mathrm{\Δ}{h}_i^2}\end{array}\right.---\left(4\right)$

其中，它是节点i与i-1之间插值点的个数，d_i、Δh_i、Δd是节点i与i-1之间的欧式距离、高度差和采样间隔。 

p_i是节点i与i-1之间的雷达探测概率，它必须考虑多个雷达的叠加影响，公式如下： 

$p_i={\mathrm{\Σ}}_1^{m}p\left(R_i\right)+{\mathrm{\Σ}}_{1\≤i\≤j\≤k\≤m}^{m}p\left(R_i{R}_j{R}_k\right)-...+{(-1)}^{m}p(R_1{R}_2...R_m)---\left(5\right)$

其中，p(R_j)是节点i与i-1之间n_i个插值点被第j个雷达探测的加权平均概率，每个点探测概率可由雷达方程反演得出。这三项指标最终归一化结果如公式(6)： 

$\left\{\begin{array}{c}{C}_i=(c_i-c_{\min })/(c_{\max }-c_{\min })\\ H_i=(h_i-h_{\min })/(h_{\max }-h_{\min })\\ P_i=p_i\end{array}\right.---\left(6\right)$

根据归一化结果得到新的代价函数如公式(7)，如公式所示。其中ω₁+ω₂+ω₃＝1。该代 价函数将航线距离、高度、被探测距离归一化到同一个数量级，使得用户可以根据规划目标分配各自的权重，从而得到对应条件下的最优路径。 

$g\left(n\right)={\mathrm{\Σ}}_1^n({\mathrm{\ω}}_1{C}_i+{\mathrm{\ω}}_2{P}_i+{\mathrm{\ω}}_3{H}_i)---\left(7\right)$

公式(1)中的h(n)定义为节点n到目标点之间的地表距离，它可以促使节点n尽快向目标点靠近，从而使算法尽快收敛。 

h(n)＝(d_n-d_max)(d_max-d_min)     (8) 

其中d_n、d_max、d_min分别为节点n到目标点的地表距离、最大距离、最小距离，计算方法和公式(4)类似，唯一的差别是为了提高h(n)的计算效率，采样间隔ΔD往往比Δd大很多。 

2.2算法搜索流程 

定义好目标空间和代价函数，即可开始航线搜索。算法定义最小二叉堆open list和线性表closed list存放拓展节点,具体搜索流程如下： 

(1)把起点S放入open list，记f=h，将closed list置为空表，读入起飞时间。 

(2)重复下列过程，直至当前点与目标点距离小于阈值为止。若open list为空表，则提示用户在当前目标空间中该两点之间无满足初始条件的航路，航线搜索失败。 

(3)查找open list中未设置过的具有最小f值的节点为当前节点BN，并把它从open list中删除，且加入到closed list。 

(4)若BN距离目标节点小于一定阈值，则成功求得目标解，终止循环。 

(5)若BN不是目标节点，则对它进行拓展，得到相应的8领域节点。首先剔除已经在closed list中节点，然后剔除位于禁飞区和恶劣天气中的节点，将8领域节点中剩下的节点作为后继节点SUC。 

(6)对每个SUC依次进行下列过程： 

(a)建立从SUC返回BN的指针。 

(b)计算SUC的代价，g(SUC)=g(BN)+h(BN,SUC)。 

(c)如果SUC∈OPEN，则将此节点标记为当前临时节点OLD，并把它添至BN的后继节点表中。 

(d)比较新路径代价。如果g(SUC)<g(OLD)， 

则重新设置OLD的父辈节点为BN，记下较小代价g(OLD)，并修正f(OLD)值，否则停止扩展节点。 

(e)如果则判断其是否在closed list中，若SUC∈CLOSED，转向步 骤(c)，否则将它放入到open list中，并加入到BN的后裔表，然后转向(7)。 

(f)重新计算f值。 

(7)GO LOOP 

3航线优化算法 

2.2搜索结果可能不满足无人机性能约束，包括最小步长、转弯半径、爬升率、安全高度等，需要对其进行一系列算法处理才能得到最终的可飞行航线。 

最小步长、转弯半径是对航段在俯平面上的约束。前者要求航段长度大于一个特定值，后者则要求每一航段的长度需要同时满足无人机的前后两次转弯过程，因此必须对航线数据进行数据压缩，本发明采用FFP算法对其进行数据压缩，它能够找出尽可能最长的直线趋势，从而避免无人机频繁的转弯。一条由航路点构成的折线依然是无法提供给无人机飞行的，有学者提出使用曲线平滑的方法，例如贝塞尔曲线、B样条等，但这与最小步长约束的相冲突，实际上，无人机一般通过旁切、飞越或向点等方式进行转弯，本发明通过对应的数学描述对转弯保护区进行了建模和可视化。 

升降率和安全高度是对航线在纵剖面上的约束。升降率是衡量无人机性能很重要的一个方面，飞机总是尽量保持自身的姿态，而不是频繁转弯或者升降。如果两个航路点A、B高度不一样，飞机并不是直接从A点飞到B，而是依据A点的升降率，先完成高度的升降过程到达O点，再进行平飞到达航路点B，如图3所示。安全高度是指为了保障飞机安全飞行所规定的离地表的最小额定距离，一些论文考虑了该限定值从而对航线进行约束，但它们仅在航线纵剖面上考虑该约束，而由于在飞行过程中，由于各种误差的影响，飞机的实际飞行路径在俯平面上与预定的航线存在偏离，因此算法应该在这整个偏离区范围内考虑安全高度，该偏离区即为航线的保护区，其对应的数学模型如图4所示。保护区分主区和副区，其中主区提供全安全高度，副区的安全高度为从预定航线向两侧逐渐减少直至为0，计算公式如下： 

Δh＝h*(1-2*ΔL/L)       (9) 

其中h为安全高度（超障余度），ΔL为保护区内目标点到预定航线的距离，L为保护区宽度。 

根据以上数学模型，要使得在A、B航路点之间满足安全高度约束，可以求出保护区内的无人机高度与地形高度的最小有符号高度差ΔH_min，然后将航段AB的高度抬高|ΔH_min|即可，但考虑到在低空突防过程中，无人机的高度应尽可能小，因此可对算法稍作改进，计算步骤如下： 

(1)将AB段分为AO、OB两段，其中O点是按照A点的升降率算出的平飞点。 

(2)求出OB段最小有符号高度差ΔH₁。在OB之间按照进行插值（如等距插值），求出一系列插值点{P_i}，将P_i的横截面分为一个主区段和两个副区段，分别求出主区段和副区段的最小有符号高度差。 

(ΔH₁)_i＝min(min(h_B-Δh-h_j),min(h_B-h-h_k))   (10) 

其中(ΔH₁)_i是P_i横截面的最小有符号高度差，h_B是OB的航段高度，Δh是安全高度，h是副区安全高度，计算原理如公式(10)所示，h_j是主区第j个插值点的地形高度，h_k是副区第k个插值点的地形高度。遍历{P_i}，(ΔH₁)_i的最小值即为ΔH₁。 

(3)求出AO段的最小有符号高度差ΔH₂。现在AO之间进行插值，求得一系列插值点{Q_i}，Q_i的第i个横截面的最小有符号高度为： 

(ΔH₂)_i＝min(min(h_i-Δh-h_j),min(h_i-h-h_k))   (11) 

其中h_i＝h_A+AO*(h_o-h_A)/AQ_i，遍历{Q_i}，即可求得ΔH₂。 

(4)将O、B的高度调整到h_B+ΔH₁，将A的高度调整到h_A+ΔH₂，依据A点的升降率过A点作一条射线求得与OB的交点（可能在OB的延长线上），该交点为新的转折点O。 

4检验算法 

为了证明算法在不同距离和网格大小上表现的适应性、收敛性和效率问题，设计了如表1所示的几组坐标对照组。 

表1算法参数对照组 

通过表2的运行结果可以看出，算法在不同距离、不同网格大小的情况下均可以并且快速的计算出一条最优航线，说明算法是稳定、收敛的，并且能够满足实时性的要求。 

表2对照组运行结果 

附图说明：

图1是“空间划分模型”图； 

图2是“航段距离估算模型”图； 

图3是“无人机爬升模型”图； 

图4是“航线保护区模型”图。 

Claims (10)

1.一种无人机航线规划算法，其特征在于：包括以下步骤：首先基于2.5维网格划分三维空间；然后在雷达、灾害天气、禁飞区等约束条件下，综合考虑航线高度、被探测概率、航线长度等影响因子改进A*代价函数，基于算法搜索流程，确定初始航线；最后在最小步长、转弯半径、爬升率、安全高度约束下，进行一系列处理得到最终的可飞行航线；该算法稳定、收敛性好、效率高。

2.根据权利要求1所述一种无人机航线规划算法，其特征在于：所述2.5维网格为2.5维网格模型，每个网格点包含经度、纬度、高程信息，其每个网格点的邻域存在8个可扩展方向。

3.根据权利要求1所述一种无人机航线规划算法，其特征在于：基于2.5维网格划分三维空间，其每个网格点的邻域存在8个可扩展方向远小于规则三维网格的空间划分中每个网格点的邻域存在26个可扩展方向。

4.根据权利要求1所述一种无人机航线规划算法，其特征在于：A*代价函数中                                                首先改进为 ，使代价函数的航线高度、被探测概率、航线长度三项指标是同方向的作用关系。

5.根据权利要求1所述一种无人机航线规划算法，其特征在于：A*代价函数中进行归一化处理，进一步改进为。

6.根据权利要求1所述一种无人机航线规划算法，其特征在于：A*代价函数中。

7.根据权利要求1所述一种无人机航线规划算法，其特征在于：算法搜索基于2.5维空间划分和改进A*代价函数。

8.根据权利要求1所述一种无人机航线规划算法，其特征在于：在最小步长、转弯半径约束下采用FFP算法对航线数据进行数据压缩，使无人机能够找出尽可能最长的直线趋势，从而避免频繁的转弯。

9.根据权利要求1所述一种无人机航线规划算法，其特征在于：在爬升率、安全高度约束下，建立航线保护区模型，计算得到优化后的转折点，构成航线。

10.根据权利要求1所述一种无人机航线规划算法，其特征在于：该算法稳定、收敛性好为算法在不同距离、不同网格大小的情况下均可以并且快速的计算出一条最优航线。