本发明涉及一种基于地形平滑的无人机近地三维路径规划方法及装置，属于无人机路径规划。
背景技术：
1、通常来说，路径规划的目的是生成一条最快、最简单的路径，摆脱复杂地形、障碍物和威胁源等所有不利因素。然而，当无人机在近地区域飞行时，由于飞行任务可能对地形有所限制和要求，如地形跟随(terrain following，tf)和地形回避(terrain avoidance，ta)。因此，威胁因素仍在考虑之中，此时三维路径规划的任务就变得艰巨而富有挑战性。
2、现有路径规划技术绝大多数改进的重点在于路径规划的寻优搜索算法本身，而并未研究如近地飞行这样特殊的实际应用场景，因此很多与地形相关的因素没有考虑。再者，有文献用拟合法生成地形，虽然简单快捷，但是不如直接采用地理信息系统真实测绘的数字地图进行处理更加具有实际意义。此外，也有不少文献使用了真实测绘的数字地图，但是轨迹优化的重点聚焦于优化数字地形本身，而基于数字地形的三维路径规划。
技术实现思路
1、本发明的目的在于提供一种基于地形平滑的无人机近地三维路径规划方法及装置，通过将地形平滑技术和路径搜索算法相结合，在安全飞行曲面的基础上避开特定的威胁，获得最佳的飞行路径，且满足无人机操纵性能的约束要求，为无人机近地三维路径规划的设计与实现提供了有效的新方法和新思路。
2、为达到上述目的，本发明采用的技术方案如下：
3、一方面，本发明提供一种基于地形平滑的无人机近地三维路径规划方法，包括：
4、获取无人机近地飞行任务的数字地形数据并进行平滑处理，生成安全飞行曲面；
5、在所述安全飞行曲面上，在满足无人机飞行约束的条件下，对给定起点到指定目的地的三维最优飞行路径进行规划。
6、进一步的，所述获取无人机近地飞行任务的数字地形数据，包括：
7、采用数字高程模型获取无人机近地飞行任务的数字地形数据，
8、或者，
9、通过对实际地形测绘的点进行插值或曲线拟合得到地形无人机近地飞行任务的数字地形数据；
10、还包括：
11、将获取的所述无人机近地飞行任务的数字地形数进行格式转换，转换成用于进行平滑处理的数据格式。
12、进一步的，所述进行平滑处理，包括：
13、读取经格式转换后的所述数字地形数据，通过网格函数展开生成平面中的点，通过插值函数生成高度方向上的点，得到包括数字地形和等高线的网格图；
14、对所述网格图设置地面以上的安全高度；
15、以满足无人机纵向飞行为约束，对所述网格图进行坡度修正；
16、在坡度修正完成后，对所述网格图进行曲率修正；
17、修正完成后的网格图即为安全飞行曲面。
18、进一步的，所述设置地面以上的安全高度，包括：
19、对于海域，设置地面以上的安全高度为15-30m；
20、对于平原区，设置地面以上的安全高度为60-80m；
21、对于平原区，设置地面以上的安全高度为120m。
22、进一步的，对所述网格图进行坡度修正，包括：
23、寻找所述网格图每行每列最高的山峰，标记为最高山峰点，从最高山峰点开始分别从正向和负向2个方向按如下方式进行坡度修正：
24、计算网格点坡度如下：
25、k＝(h(i+1)-h(i))/l，
26、其中，k表示网格点坡度，h(i+1)表示网格点i+1处对应的高程值，h(i)表示网格点i处对应的高程值，l为网格间距；
27、若k＞kmax，则修正网格点i处对应的高程值为：h(i)＝h(i+1)-l·kmax；其中kmax为无人机最大可飞坡度；
28、若k＜-kmax，则修正网格点i+1处对应的高程值为：h(i+1)＝h(i)-l·kmax；
29、除上述情况外网格点高程值保持不变。
30、进一步的，对所述网格图进行曲率修正，包括：
31、从最高山峰点开始分别对两侧按如下方式进行曲率修正：
32、在x方向上，计算网格点的一阶偏导数为：
33、h′x＝(hi+1,j-hi-1,j)/2l；
34、其中，h′x表示网格点在x方向的一阶偏导数，hi+1,j表示网格点(i+1,j)处的高程值，hi-1,j表示网格点(i-1,j)处的高程值，
35、计算网格点的二阶右偏导数为：
36、h″xr＝(2hi-1,j-5hi,j+4hi+1,j-hi+2,j)/l2；
37、计算网格点的二阶左偏导数为：
38、h″xl＝(-hi-2,j+4hi-1,j-5hi,j+2hi+1,j)/l2；
39、其中，h″xr表示网格点在x方向的二阶右偏导数，h″xl表示网格点在x方向的二阶左偏导数，hi,j表示网格点(i,j)处的高程值，hi+2,j表示网格点(i+2,j)处的高程值，hi-2,j表示网格点(i-2,j)处的高程值；
40、将所计算的一阶偏导数、二阶左偏导数或二阶右偏导数代入如下曲率公式，得到网格点x方向上的左、右曲率：
41、
42、其中，h″x表示网格点在x方向的二阶偏导数，计算左曲率时采用h″xl，计算右曲率时采用h″xr；
43、采用相同的方式计算网格点y方向上的左、右曲率；
44、若不满足网格点在x方向和y方向上的曲率的绝对值小于地形曲率最大值ρamax，则对网格点高程值进行修正，
45、其中，地形曲率最大值ρamax的计算方式为，
46、将无人机速度和最大法向加速度带入如下公式，得到地形曲率最大值：
47、
48、其中，ρa为地形曲率，nz为法向加速度，g为重力加速度，v为无人机速度。
49、进一步的，所述无人机飞行约束包括：
50、a、li＞lmin,i＝1,...n；
51、b、
52、c、
53、d、∑i||li||≤lmax；
54、其中，li表示第i段航迹，n为航迹段数，r为无人机飞行路径的曲率半径，rmin为无人机的最小转弯半径，vmin为无人机最小飞行速度，φmax为无人机最大滚转角，g为重力加速度，xi为网格点i横坐标值，hi表示网格点i处对应的高程值，hi-1表示网格点i-1处对应的高程值，γmax为无人机最大爬升/俯冲角，lmax为无人机飞行路径最大长度。
55、进一步的，在所述安全飞行曲面上，在满足无人机飞行约束的条件下，对给定起点到指定目的地的三维最优飞行路径进行规划，包括：
56、在满足无人机飞行约束的条件下，以飞行代价与威胁评估指标最小为目标，采用最优路径搜索算法求解从给定起点到指定目的地的最优路径；
57、所述飞行代价与威胁评估指标为：
58、
59、
60、其中，ti表示第i段航迹的飞行时间，为威胁代价，为高度代价，为燃油代价，ω1、ω2、ω3是各代价指标所占的权重，为一条航迹的代价值，c为威胁指标，表示第i段航迹的长度罚值，表示第i段航迹的平均飞行高度，表示飞行路径靠近已知危险的危险程度，ω4、ω5、ω6是权重系数，表示为：
61、
62、其中，kj代表第j个威胁源的强度，rsj表示飞行器距第j个威胁源的斜距，nt表示已知威胁的个数。
63、进一步的，所述最优路径搜索算法包括：基于人工势场法的最优路径算法、a*搜索算法和蚁群算法。
64、另一方面，本发明还提供一种基于地形平滑的无人机近地三维路径规划装置，用于实现上述的基于地形平滑的无人机近地三维路径规划方法，所述装置包括：
65、平滑处理模块，用于获取无人机近地飞行任务的数字地形数据并进行平滑处理，生成安全飞行曲面；
66、路径搜索模块，用于在所述安全飞行曲面上，在满足无人机飞行约束的条件下，对给定起点到指定目的地的三维最优飞行路径进行规划。
67、与现有技术相比，本发明所达到的有益效果：
68、本发明提供一种基于地形平滑的无人机近地三维路径规划方法，通过真实地图的数字高程模型作为无人机近地路径规划的地形数据来源，通过考虑安全高度、减小坡度和曲率等因素对原始数字地形进行预处理，可以有效的生成无人机的安全飞行曲面，形成对复杂三维地形的路径预规划。相对于现有技术，本发明针对的并不是平面，而是平滑后的地形曲面，所以，原本的最优搜索算法仍然可以使用，且地形曲面又是基于地理信息系统真实测绘的数字地图，并考虑无人机机动性能的约束而生成，相当于对优化过程进行了预处理，本发明将地形的优化处理和最优路径的生成这两个步骤形成相辅相成的结合，为无人机近地三维路径规划的设计与实现提供了新的方法和思路。
69、本发明所设计的gui程序可以加载优化后的安全飞行曲面，并通过最优算法避开威胁区域到达目标位置(其中威胁数量、威胁位置和目标位置可自由指定)，获得最佳的飞行路径。
1.一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，包括：
2.根据权利要求1所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，所述获取无人机近地飞行任务的数字地形数据，包括：
3.根据权利要求2所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，所述进行平滑处理，包括：
4.根据权利要求3所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，所述设置地面以上的安全高度，包括：
5.根据权利要求3所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，对所述网格图进行坡度修正，包括：
6.根据权利要求5所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，对所述网格图进行曲率修正，包括：
7.根据权利要求1所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，所述无人机飞行约束包括：
8.根据权利要求7所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，在所述安全飞行曲面上，在满足无人机飞行约束的条件下，对给定起点到指定目的地的三维最优飞行路径进行规划，包括：
9.根据权利要求8所述的一种基于地形平滑的无人机近地三维路径规划方法，其特征在于，所述最优路径搜索算法包括：基于人工势场法的最优路径算法、a*搜索算法和蚁群算法。
10.一种基于地形平滑的无人机近地三维路径规划装置，其特征在于，用于实现权利要求1至9任意一项所述的基于地形平滑的无人机近地三维路径规划方法，所述装置包括：
