CN114020009B - 一种小型固定翼无人机地形突防规划方法 - Google Patents

Abstract

本发明提供了一种小型固定翼无人机地形突防规划方法，基于数字地图的高程数据，构建与无人机机动性能相匹配的地形威胁模型；采用基于A^*算法改进的航路规划方法，生成初始航路；为提升航路可飞行，删除冗余航点，生成优化航路。本发明能够在复杂地形环境下使用，生成的航路飞行效率高、安全性好，工程化程度高，能够有力支撑无人机小型固定翼无人机完成地形突防。本发明在构建地形威胁模型的基础上，采用改进的A^*算法生成航路并进行优化，以提升地形突防的飞行效率和安全性。

Description

一种小型固定翼无人机地形突防规划方法

技术领域

本发明涉及无人机任务控制技术领域，尤其是一种无人机地形突防规划方法。

背景技术

小型固定翼无人机因其巡航速度快、使用成本低，在各种环境下被广泛使用。在高原、山地等复杂地形环境下，小型固定翼无人机常被用于前突侦察，为后方的直升机、地面车辆或人员提供前方态势。传统上，操作员在小型固定翼无人机起飞前，依据粗略地形为无人机加载预设航路，在无人机飞行过程中实时操控调整，以避免与威胁地形相撞。这种方法航路安全性低，人员操作负担重。有人对地形进行复杂的滤波，处理为平滑的威胁曲面，存在较大失真，且此过程中未考虑无人机的机动性能，实际飞行时会出现无人机转弯过程中撞山的危险。有人以地形跟随为目的，设计了三维曲线航路，但计算量大，航路难以被无人机跟随，工程化实施困难。有人引入了遗传算法、模拟退火算法进行航路的求解，但此类算法在嵌入式环境下运行缓慢，耗时几秒甚至几十秒，且存在无解的可能，不适用于无人机在线规划。由于计算资源和机动性能受限，尚未看到小型固定翼无人机实施地形突防的成功工程案例。

发明内容

为了克服现有技术的不足，本发明提供一种小型固定翼无人机地形突防规划方法，属于无人机任务控制技术领域，用以解决小型固定翼无人机复杂地形环境下计算效率低、安全性不高问题。本发明基于数字地图的高程数据，构建与无人机机动性能相匹配的地形威胁模型；考虑飞行效率和安全性，采用基于A^*算法改进的航路规划方法，生成初始航路；为提升航路可飞行，删除冗余航点，生成优化航路。相较于传统的无人机威胁规避规划问题，本发明能够在复杂地形环境下使用，生成的航路飞行效率高、安全性好，工程化程度高，能够有力支撑无人机小型固定翼无人机完成地形突防。本发明在构建地形威胁模型的基础上，采用改进的A^*算法生成航路并进行优化，以提升地形突防的飞行效率和安全性。

本发明解决其技术问题所采用的技术方案采用如下步骤：

步骤(一)基于数字地图的高程数据，构建与无人机机动性能相匹配的地形威胁模型；

突防区域的数字地图的高程数据的行数为n_row，列数为n_col，每个高程数据点内容包括坐标(u,v)和高度H_u,v，其中u为行坐标，1≤u≤n_row；v为列坐标，1≤v≤n_col，高程数据行之间的距离为t_V，列之间的距离为t_H；

小型固定翼无人机飞行高度为H₀，进行地形突防的起始点坐标为(u_S,v_S)，终点坐标为(u_E,v_E)，可用最小转弯半径为R；

每个高程点对应一个威胁栅格点，一个威胁栅格点包括坐标(u,v)和威胁度T_u,v，威胁栅格的坐标与高程数据一致，根据每个高程数据点的高度H_u,v生成威胁度T_u,v；

步骤(二)考虑飞行效率和安全性，采用基于A*算法改进的航路规划方法，生成初始航路；

每个威胁栅格点对应一个规划栅格点，一个规划栅格点内容包括坐标(u,v)、父栅格坐标(u^P,v^P)、历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>和综合代价f_u,v；其中，规划栅格点的坐标(u,v)与威胁栅格点一致；父栅格坐标(u^P,v^P)为到该规划栅格点的前一个规划栅格点的坐标；历史代价g_u,v是指从起始点(u_S,v_S)到该规划栅格点的代价；预估代价h_u,v是指从该规划栅格点到终点(u_E,v_E)的可能代价；在开集标志/>指示着该规划栅格点是否在待选的航路点集合内，/>表示该规划栅格点在待选的航路点集合内，表示该规划栅格点不在待选的航路点集合内；在闭集标志/>指示着该规划栅格点是否已被确认为是航路点或不是航路点，/>表示该规划栅格点已被确认为是航路点或不是航路点，/>表示该规划栅格点未被确认为是航路点或不是航路点。综合代价f_u,v为历史代价g_u,v与预估代价h_u,v之和，即：

f_u,v＝g_u,v+h_u,v (4)

步骤(三)优化航路生成

生成的初始航路需要进行裁剪优化，裁剪优化的步骤如下：

设初始航路上任意两个航路点的规划栅格点坐标分别为两点之间连线的距离为d_p,q，d_p,q按式(13)计算：

离散化这两点之间连线，离散化步长设为τ；

τ＝min(t_H,t_V) (14)

离散化段数设为n_d，按式(15)计算获得：

n_d＝ceil(d_p,q/τ) (15)

设离散化的第δ段右端点的规划栅格点坐标为按式(16)计算获得：

设规划栅格点对应的威胁度为/>如果/>则认为/>与/>的连线穿过了威胁；如果第1、2、…、n_d-1段右端点的威胁度均等于0，则认为/>与/>的连线未穿过威胁；

将起始点(u_S,v_S)设为优化航路的第一个点，终点(u_E,v_E)为优化航路的最后一个点；使用两重循环方式检查初始航路的其他点；两重循环的变量均为初始航路的序列号m(1≤m≤n_h)；为便于区分，设第一重循环的变量为m_o，初始值设置为1；设第二重循环的变量为m_s，初始值设置为n_h；按以下步骤处理初始航路，获取优化航路：

a)判断m_o对应的规划栅格点与m_s对应的规划栅格点/>的连线是否穿过威胁，如果穿过了威胁，转入b)；否则，将/>加入到优化航路，第一重循环变量m_o的数值设置为m_s的数值，第二重循环变量m_s的数值重设为n_h，转入c)；

b)第二重循环变量m_s的数值减去1，转入步骤c)；

c)如果第一重循环变量m_o等于n_h-1，结束处理，获得优化航路；如果第二重循环变量m_s的数值等于第一重循环变量m_o加1，将加入到优化航路，第一重循环变量m_o的数值加1，第二重循环变量m_s的数值重设为n_h，转入a)；其他情况，直接转入步骤a)。

两重循环完成后，即可得到优化航路；优化航路的航点数目为n_z，优化航路中的规划栅格点坐标表示为其中1≤β≤n_z；

优化航路为该方法的最终计算结果，将优化航路中的每个规划栅格点对应的经度、纬度发送到无人机飞行控制系统中，无人机即可按照优化航路进行地形突防。

所述生成威胁度T_u,v的具体步骤如下：

最小转弯半径为R对应的威胁栅格数为：

n_R＝ceil(R/min(t_H,t_V)) (1)

其中min(t_H,t_V)为取t_H、t_V的最小值，ceil(.)为向上取整；

对于每一个高程数据点(u,v)，按式(2)进行第一次处理，获得每个威胁栅格点的初始威胁度T′_u,v：

初始威胁度等于255的威胁栅格点坐标为(u′,v′)，对每个栅格点按式(3)进行第二次处理，得到威胁度T_u,v：

所述生成初始航路的具体步骤为：

1)算法初始化

对每个规划栅格点(u,v)，完成对每个规划栅格点的历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>综合代价f_u,v的初始化，初始化步骤如下：

a)历史代价g_u,v设为0；

b)预估代价h_u,v按式(5)计算：

h_u,v＝t_V|u-u_E|+t_H|v-v_E| (5)

c)按照式(4)计算综合代价f_u,v；

d)如果T_u,v＝255或T_u,v＝2，设置规划栅格点(u,v)在闭集标志否则

e)在开集标志

2)栅格扩展

栅格扩展是一个迭代寻优过程，从起始点开始(u_S,v_S)逐步扩展到终点(u_E,v_E)。设置起始点(u_S,v_S)的在开集标志在每一次迭代中，按以下步骤处理：

a)获取当前栅格(u_C,v_C)：遍历所有的规划栅格点，选择综合代价f_u,v最小的栅格点作为当前栅格(u_C,v_C)；

b)退出迭代判断，如存在以下两种情况，退出迭代，否则继续：

1)当前栅格为终点(u_E,v_E)，则栅格扩展成功；

2)当前栅格为空，则栅格扩展失败；

c)八方向扩展：尝试向相邻的八个栅格扩展；

d)设置当前栅格的在闭集标志

3)航路提取

为提升航路可飞行，裁剪冗余航点，生成优化航路；

如果栅格扩展成功，则从起始点到终点的航路存在；从终点(u_E,v_E)开始，首先获取终点的父节点坐标，再获取终点的父节点的父节点的坐标，以此类推，直到获取的点为起始点(u_S,v_S)；将这一系列的栅格点从起始点(u_S,v_S)到终点(u_E,v_E)排列起来，即得到了原始航路；原始航路点的个数为n₀，原始航路中的规划栅格点坐标表示为其中1≤k≤n₀；

原始航路点数目较多，需要提取出关键点；为规划栅格点/>的方向改变代价，按以下步骤处理获取初始航路：

a)将起始点(u_S,v_S)作为初始航路第一个航路点；

b)根据式(8)-(11)逐个计算规划栅格点的方向改变代价

c)当时，提取该规划栅格点的父节点作为初始航路的一个航点；

d)将终点(u_E,v_E)作为初始航路的最后一个航点。

设初始航路的航点数目为n_h，初始航路中的规划栅格点坐标表示为其中1≤m≤n_h；

所述八方向扩展的具体步骤如下：

对于当前栅格(u_C,v_C)，它的相邻八个栅格的坐标表示为(u_C+i,v_C+j)，其中-1≤i≤1，-1≤j≤1且i、j不同时为0，对于每一个相邻栅格，设它的在开集标志为原有的历史代价为/>执行以下处理：

a)如果该相邻栅格点的在闭集标志退出处理，否则继续；

b)设该相邻栅格点的历史代价为可按下式计算获得

其中为当前栅格(u_C,v_C)的历史代价，/>为从当前栅格到该相邻栅格点的长度代价，/>为从当前栅格到该相邻栅格点的长度改变代价，/>为从当前栅格到该相邻栅格点的威胁代价；

c)如果且/>则将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)；如果/>且/>将该相邻栅格的历史代价还原为/>如果/>将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)，并设置/>

所述从当前栅格到该相邻栅格点的长度代价按式(7)计算：

当前栅格的父节点为从当前栅格(u_C,v_C)到当前栅格的父节点/>的方向为θ_CP，θ_CP由式(8)确定；

设从该相邻栅格点(u_C+i,v_C+j)到当前栅格(u_C,v_C)的方向为θ_NC，θ_NC由式(9)确定；

设方向θ_NC与方向θ_CP之差为Δθ，Δθ由式(10)确定；

则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的方向改变代价由式(11)计算得到；

设相邻栅格点(u_C+i,v_C+j)的威胁度为则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的威胁代价/>由式(12)确定；

本发明的有益效果在于所提出的地形突防规划方法，基于数字高程，综合考虑了航路的飞行效率、安全性、可飞行，并且计算实时性好，可在线执行，能够适用于各种复杂地形环境，具有较强的工程实现价值，可提升无人机执行地形突防的自动化水平。使用某小型固定翼无人机对该发明提出的方法进行了三个架次的试飞验证。试飞结果表明，该方法计算效率高、实时性好，小型固定翼无人机能够在200ms内完成规划，耗时比现有方法减少了80％以上；生成的航路可飞性好、安全性高，小型固定翼无人机按照生成的航路飞行，可以穿过复杂的地形，顺利完成突防。

附图说明

图1为本发明一种小型固定翼无人机地形突防规划流程图。

图2为本发明初始航路生成流程图。

图3为本发明实施例1高程数据示意图。

图4为本发明实施例1地形威胁建模示意图。

图5为本发明实施例1初始航路和优化航路示意图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

实施例的流程如图1所示，按以下步骤计算。

(一)基于数字地图的高程数据，构建与无人机机动性能相匹配的地形威胁模型；

突防区域的数字地图的高程数据的行数为n_row，列数为n_col，每个高程数据点内容包括坐标(u,v)和高度H_u,v，其中u为行坐标，1≤u≤n_row；v为列坐标，1≤v≤n_col，高程数据行之间的距离为t_V，列之间的距离为t_H；

小型固定翼无人机飞行高度为H₀，进行地形突防的起始点坐标为(u_S,v_S)，终点坐标为(u_E,v_E)，可用最小转弯半径为R；

每个高程点对应一个威胁栅格点，一个威胁栅格点包括坐标(u,v)和威胁度T_u,v，威胁栅格的坐标与高程数据一致，根据每个高程数据点的高度H_u,v生成威胁度T_u,v；具体步骤如下：

最小转弯半径为R对应的威胁栅格数为：

n_R＝ceil(R/min(t_H,t_V)) (1)

其中min(t_H,t_V)为取t_H、t_V的最小值，ceil(.)为向上取整；

对于每一个高程数据点(u,v)，按式(2)进行第一次处理，获得每个威胁栅格点的初始威胁度T′_u,v：

初始威胁度等于255的威胁栅格点坐标为(u′,v′)，对每个栅格点按式(3)进行第二次处理，得到威胁度T_u,v：

(二)考虑飞行效率和安全性，采用基于A*算法改进的航路规划方法，生成初始航路；

A^*算法是一种高效的启发式算法，被广泛应用于机器人路径规划、无人机航路规划。本发明采用改进的A^*算法生成初始航路，流程如图2所示。

每个威胁栅格点对应一个规划栅格点，一个规划栅格点内容包括坐标(u,v)、父栅格坐标(u^P,v^P)、历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>和综合代价f_u,v；其中，规划栅格点的坐标(u,v)与威胁栅格点一致；父栅格坐标(u^P,v^P)为到该规划栅格点的前一个规划栅格点的坐标；历史代价g_u,v是指从起始点(u_S,v_S)到该规划栅格点的代价；预估代价h_u,v是指从该规划栅格点到终点(u_E,v_E)的可能代价；在开集标志/>指示着该规划栅格点是否在待选的航路点集合内，/>表示该规划栅格点在待选的航路点集合内，表示该规划栅格点不在待选的航路点集合内；在闭集标志/>指示着该规划栅格点是否已被确认为是航路点或不是航路点，/>表示该规划栅格点已被确认为是航路点或不是航路点，/>表示该规划栅格点未被确认为是航路点或不是航路点。综合代价f_u,v为历史代价g_u,v与预估代价h_u,v之和，即：

f_u,v＝g_u,v+h_u,v (4)

具体算法步骤如下：

1)算法初始化

对每个规划栅格点(u,v)，完成对每个规划栅格点的历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>综合代价f_u,v的初始化，初始化步骤如下：

a)历史代价g_u,v设为0；

b)预估代价h_u,v按式(5)计算：

h_u,v＝t_V|u-u_E|+t_H|v-v_E| (5)

c)按照式(4)计算综合代价f_u,v；

d)如果T_u,v＝255或T_u,v＝2，设置规划栅格点(u,v)在闭集标志否则/>

e)在开集标志

2)栅格扩展

栅格扩展是一个迭代寻优过程，从起始点开始(u_S,v_S)逐步扩展到终点(u_E,v_E)。设置起始点(u_S,v_S)的在开集标志在每一次迭代中，按以下步骤处理：

a)获取当前栅格(u_C,v_C)：遍历所有的规划栅格点，选择综合代价f_u,v最小的栅格点作为当前栅格(u_C,v_C)；

b)退出迭代判断，如存在以下两种情况，退出迭代，否则继续：

1)当前栅格为终点(u_E,v_E)，则栅格扩展成功；

2)当前栅格为空，则栅格扩展失败；

c)八方向扩展：尝试向相邻的八个栅格扩展；

d)设置当前栅格的在闭集标志

所述八方向扩展的具体步骤如下：

对于当前栅格(u_C,v_C)，它的相邻八个栅格的坐标表示为(u_C+i,v_C+j)，其中-1≤i≤1，-1≤j≤1且i、j不同时为0，对于每一个相邻栅格，设它的在开集标志为原有的历史代价为/>执行以下处理：

a)如果该相邻栅格点的在闭集标志退出处理，否则继续；

b)设该相邻栅格点的历史代价为可按下式计算获得

其中为当前栅格(u_C,v_C)的历史代价，/>为从当前栅格到该相邻栅格点的长度代价，/>为从当前栅格到该相邻栅格点的长度改变代价，/>为从当前栅格到该相邻栅格点的威胁代价；

c)如果且/>则将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)；如果/>且/>将该相邻栅格的历史代价还原为如果/>将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)，并设置

所述从当前栅格到该相邻栅格点的长度代价按式(7)计算：

设当前栅格的父节点为从当前栅格(u_C,v_C)到当前栅格的父节点/>的方向为θ_CP，θ_CP由式(8)确定；

设从该相邻栅格点(u_C+i,v_C+j)到当前栅格(u_C,v_C)的方向为θ_NC，θ_NC由式(9)确定；

设方向θ_NC与方向θ_CP之差为Δθ，Δθ由式(10)确定；

则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的方向改变代价由式(11)计算得到；

设相邻栅格点(u_C+i,v_C+j)的威胁度为则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的威胁代价/>由式(12)确定；

3)航路提取

为提升航路可飞行，裁剪冗余航点，生成优化航路；

如果栅格扩展成功，则从起始点到终点的航路存在；从终点(u_E,v_E)开始，首先获取终点的父节点坐标，再获取终点的父节点的父节点的坐标，以此类推，直到获取的点为起始点(u_S,v_S)；将这一系列的栅格点从起始点(u_S,v_S)到终点(u_E,v_E)排列起来，即得到了原始航路；原始航路点的个数为n₀，原始航路中的规划栅格点坐标表示为其中1≤k≤n₀；

原始航路点数目较多，需要提取出关键点；为规划栅格点/>的方向改变代价，按以下步骤处理获取初始航路：

a)将起始点(u_S,v_S)作为初始航路第一个航路点；

b)根据式(8)-(11)逐个计算规划栅格点的方向改变代价

c)当时，提取该规划栅格点的父节点作为初始航路的一个航点；

d)将终点(u_E,v_E)作为初始航路的最后一个航点。

设初始航路的航点数目为n_h，初始航路中的规划栅格点坐标表示为其中1≤m≤n_h；

(三)优化航路生成

生成的初始航路存在一些不必要的弯曲，可飞性不够好，需要进行裁剪优化，裁剪优化的步骤如下：

设初始航路上任意两个航路点的规划栅格点坐标分别为两点之间连线的距离为d_p,q，d_p,q按式(13)计算：

离散化这两点之间连线，离散化步长设为τ；

τ＝min(t_H,t_V) (14)

离散化段数设为n_d，按式(15)计算获得：

n_d＝ceil(d_p,q/τ) (15)

设离散化的第δ段右端点的规划栅格点坐标为按式(16)计算获得：

设规划栅格点对应的威胁度为/>如果/>则认为/>与/>的连线穿过了威胁；如果第1、2、…、n_d-1段右端点的威胁度均等于0，则认为/>与的连线未穿过威胁；

将起始点(u_S,v_S)设为优化航路的第一个点，终点(u_E,v_E)为优化航路的最后一个点；使用两重循环方式检查初始航路的其他点；两重循环的变量均为初始航路的序列号m(1≤m≤n_h)；为便于区分，设第一重循环的变量为m_o，初始值设置为1；设第二重循环的变量为m_s，初始值设置为n_h；按以下步骤处理初始航路，获取优化航路：

a)判断m_o对应的规划栅格点与m_s对应的规划栅格点/>的连线是否穿过威胁，如果穿过了威胁，转入b)；否则，将/>加入到优化航路，第一重循环变量m_o的数值设置为m_s的数值，第二重循环变量m_s的数值重设为n_h，转入c)；

b)第二重循环变量m_s的数值减去1，转入步骤c)；

c)如果第一重循环变量m_o等于n_h-1，结束处理，获得优化航路；如果第二重循环变量m_s的数值等于第一重循环变量m_o加1，将加入到优化航路，第一重循环变量m_o的数值加1，第二重循环变量m_s的数值重设为n_h，转入a)；其他情况，直接转入步骤a)。

两重循环完成后，即可得到优化航路；优化航路的航点数目为n_z，优化航路中的规划栅格点坐标表示为其中1≤β≤n_z；

优化航路为该方法的最终计算结果，将优化航路中的每个规划栅格点对应的经度、纬度发送到无人机飞行控制系统中，无人机即可按照优化航路进行地形突防。

实施例1：

突防区域的数字地图的高程数据的行数为n_row＝60，列数为n_col＝60，高程数据行之间的距离为t_V＝100米，列之间的距离为t_H＝100米，高程数据如图3所示。小型固定翼无人机飞行高度为H₀＝700米，进行地形突防的起始点坐标为(10,10)，终点坐标为(50,50)，可用最小转弯半径R＝200米。

地形威胁建模如图4所示，其中“*”表示威胁度为255，“×”表示威胁度为2，“●”表示威胁度为1，空白处表示威胁度为0。

初始航路和优化航路如图5所示。初始航路有11个点，坐标分别为(10，10)，(14，14)，(22，14)，(25，17)，(30，17)，(34，21)，(34，30)，(26，38)，(26，39)，(37，50)，(50，50)。优化航路有7个点，坐标分别为(10，10)，(30，17)，(34，21)，(26，38)，(26，39)，(37，50)，(50，50)。可以看出，最终生成的优化航路不但能够有效避开地形威胁，而且路程短、可飞性好。

Claims (6)

1.一种小型固定翼无人机地形突防规划方法，其特征在于包括下述步骤：

步骤(一)基于数字地图的高程数据，构建与无人机机动性能相匹配的地形威胁模型；

突防区域的数字地图的高程数据的行数为n_row，列数为n_col，每个高程数据点内容包括坐标(u,v)和高度H_u,v，其中u为行坐标，1≤u≤n_row；v为列坐标，1≤v≤n_col，高程数据行之间的距离为t_V，列之间的距离为t_H；

小型固定翼无人机飞行高度为H₀，进行地形突防的起始点坐标为(u_S,v_S)，终点坐标为(u_E,v_E)，可用最小转弯半径为R；

每个高程点对应一个威胁栅格点，一个威胁栅格点包括坐标(u,v)和威胁度T_u,v，威胁栅格的坐标与高程数据一致，根据每个高程数据点的高度H_u,v生成威胁度T_u,v；

步骤(二)考虑飞行效率和安全性，采用基于A*算法改进的航路规划方法，生成初始航路；

每个威胁栅格点对应一个规划栅格点，一个规划栅格点内容包括坐标(u,v)、父栅格坐标(u^P,v^P)、历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>和综合代价f_u,v；其中，规划栅格点的坐标(u,v)与威胁栅格点一致；父栅格坐标(u^P,v^P)为到该规划栅格点的前一个规划栅格点的坐标；历史代价g_u,v是指从起始点(u_S,v_S)到该规划栅格点的代价；预估代价h_u,v是指从该规划栅格点到终点(u_E,v_E)的可能代价；在开集标志/>指示着该规划栅格点是否在待选的航路点集合内，/>表示该规划栅格点在待选的航路点集合内，/>表示该规划栅格点不在待选的航路点集合内；在闭集标志/>指示着该规划栅格点是否已被确认为是航路点或不是航路点，/>表示该规划栅格点已被确认为是航路点或不是航路点，/>表示该规划栅格点未被确认为是航路点或不是航路点；综合代价f_u,v为历史代价g_u,v与预估代价h_u,v之和，即：

f_u,v＝g_u,v+h_u,v (4)

步骤(三)优化航路生成

生成的初始航路需要进行裁剪优化，裁剪优化的步骤如下：

设初始航路上任意两个航路点的规划栅格点坐标分别为两点之间连线的距离为d_p,q，d_p,q按式(13)计算：

离散化这两点之间连线，离散化步长设为τ；

τ＝min(t_H,t_V) (14)

离散化段数设为n_d，按式(15)计算获得：

n_d＝ceil(d_p,q/τ) (15)

设离散化的第δ段右端点的规划栅格点坐标为按式(16)计算获得：

设规划栅格点对应的威胁度为/>如果/>则认为/>与/>的连线穿过了威胁；如果第1、2、…、n_d-1段右端点的威胁度均等于0，则认为/>与/>的连线未穿过威胁；

将起始点(u_S,v_S)设为优化航路的第一个点，终点(u_E,v_E)为优化航路的最后一个点；使用两重循环方式检查初始航路的其他点；两重循环的变量均为初始航路的序列号m(1≤m≤n_h)；为便于区分，设第一重循环的变量为m_o，初始值设置为1；设第二重循环的变量为m_s，初始值设置为n_h；

两重循环完成后，即可得到优化航路；优化航路的航点数目为n_z，优化航路中的规划栅格点坐标表示为其中1≤β≤n_z；

优化航路为该方法的最终计算结果，将优化航路中的每个规划栅格点对应的经度、纬度发送到无人机飞行控制系统中，无人机即可按照优化航路进行地形突防。

2.根据权利要求1所述的小型固定翼无人机地形突防规划方法，其特征在于：

所述生成威胁度T_u,v的具体步骤如下：

最小转弯半径为R对应的威胁栅格数为：

n_R＝ceil(R/min(t_H,t_V)) (1)

其中min(t_H,t_V)为取t_H、t_V的最小值，ceil(·)为向上取整；

对于每一个高程数据点(u,v)，按式(2)进行第一次处理，获得每个威胁栅格点的初始威胁度T′_u,v：

初始威胁度等于255的威胁栅格点坐标为(u′,v′)，对每个栅格点按式(3)进行第二次处理，得到威胁度T_u,v：

3.根据权利要求1所述的小型固定翼无人机地形突防规划方法，其特征在于：

所述生成初始航路的具体步骤为：

1)算法初始化

对每个规划栅格点(u,v)，完成对每个规划栅格点的历史代价g_u,v、预估代价h_u,v、在开集标志在闭集标志/>综合代价f_u,v的初始化，初始化步骤如下：

a)历史代价g_u,v设为0；

b)预估代价h_u,v按式(5)计算：

h_u,v＝t_V|u-u_E|+t_H|v-v_E| (5)

c)按照式(4)计算综合代价f_u,v；

d)如果T_u,v＝255或T_u,v＝2，设置规划栅格点(u,v)在闭集标志否则/>

e)在开集标志

2)栅格扩展

栅格扩展是一个迭代寻优过程，从起始点开始(u_S,v_S)逐步扩展到终点(u_E,v_E)；设置起始点(u_S,v_S)的在开集标志在每一次迭代中，按以下步骤处理：

a)获取当前栅格(u_C,v_C)：遍历所有的规划栅格点，选择综合代价f_u,v最小的栅格点作为当前栅格(u_C,v_C)；

b)退出迭代判断，如存在以下两种情况，退出迭代，否则继续：

1)当前栅格为终点(u_E,v_E)，则栅格扩展成功；

2)当前栅格为空，则栅格扩展失败；

c)八方向扩展：尝试向相邻的八个栅格扩展；

d)设置当前栅格的在闭集标志

3)航路提取

为提升航路可飞行，裁剪冗余航点，生成优化航路；

如果栅格扩展成功，则从起始点到终点的航路存在；从终点(u_E,v_E)开始，首先获取终点的父节点坐标，再获取终点的父节点的父节点的坐标，以此类推，直到获取的点为起始点(u_S,v_S)；将这一系列的栅格点从起始点(u_S,v_S)到终点(u_E,v_E)排列起来，即得到了原始航路；原始航路点的个数为n₀，原始航路中的规划栅格点坐标表示为其中1≤k≤n₀；

原始航路点数目较多，需要提取出关键点；为规划栅格点/>的方向改变代价，按以下步骤处理获取初始航路：

a)将起始点(u_S,v_S)作为初始航路第一个航路点；

b)根据式(8)-(11)逐个计算规划栅格点的方向改变代价

c)当时，提取该规划栅格点的父节点作为初始航路的一个航点；

d)将终点(u_E,v_E)作为初始航路的最后一个航点；

设初始航路的航点数目为n_h，初始航路中的规划栅格点坐标表示为其中1≤m≤n_h。

4.根据权利要求3所述的小型固定翼无人机地形突防规划方法，其特征在于：

所述八方向扩展的具体步骤如下：

对于当前栅格(u_C,v_C)，它的相邻八个栅格的坐标表示为(u_C+i,v_C+j)，其中-1≤i≤1，-1≤j≤1且i、j不同时为0，对于每一个相邻栅格，设它的在开集标志为原有的历史代价为/>执行以下处理：

a)如果该相邻栅格点的在闭集标志退出处理，否则继续；

b)设该相邻栅格点的历史代价为可按下式计算获得

其中为当前栅格(u_C,v_C)的历史代价，/>为从当前栅格到该相邻栅格点的长度代价，/>为从当前栅格到该相邻栅格点的长度改变代价，/>为从当前栅格到该相邻栅格点的威胁代价；

c)如果且/>则将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)；如果/>且/>将该相邻栅格的历史代价还原为/>如果/>将该相邻栅格的父栅格坐标设置为当前栅格(u_C,v_C)，并设置/>

5.根据权利要求4所述的小型固定翼无人机地形突防规划方法，其特征在于：

所述从当前栅格到该相邻栅格点的长度代价按式(7)计算：

当前栅格的父节点为从当前栅格(u_C,v_C)到当前栅格的父节点/>的方向为θ_CP，θ_CP由式(8)确定；

设从该相邻栅格点(u_C+i,v_C+j)到当前栅格(u_C,v_C)的方向为θ_NC，θ_NC由式(9)确定；

设方向θ_NC与方向θ_CP之差为Δθ，Δθ由式(10)确定；

则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的方向改变代价由式(11)计算得到；

设相邻栅格点(u_C+i,v_C+j)的威胁度为则从当前栅格(u_C,v_C)到该相邻栅格点(u_C+i,v_C+j)的威胁代价/>由式(12)确定；

6.根据权利要求1所述的小型固定翼无人机地形突防规划方法，其特征在于：

处理初始航路，获取优化航路的具体步骤为：

a)判断m_o对应的规划栅格点与m_s对应的规划栅格点/>的连线是否穿过威胁，如果穿过了威胁，转入b)；否则，将/>加入到优化航路，第一重循环变量m_o的数值设置为m_s的数值，第二重循环变量m_s的数值重设为n_h，转入c)；

b)第二重循环变量m_s的数值减去1，转入步骤c)；

c)如果第一重循环变量m_o等于n_h-1，结束处理，获得优化航路；如果第二重循环变量m_s的数值等于第一重循环变量m_o加1，将加入到优化航路，第一重循环变量m_o的数值加1，第二重循环变量m_s的数值重设为n_h，转入a)；其他情况，直接转入步骤a)。