CN108204814A - 无人机三维场景路径导航平台及其三维改进路径规划方法 - Google Patents

Abstract

本发明公开了一种无人机三维场景路径导航平台及其三维改进路径规划方法，该平台基于Unity3d加载DEM地图数据，进行三维场景的显示；通过VS2010C++编程，实现了基于DEM的三维改进路径规划算法，获取无人机可飞行安全路径，并通过Matlab2012a绘制规划得到的路径检测规划效果；导入无人机模型，结合miniAHRS姿态传感器，获取当前无人机导航信息；融合路径规划算法到三维场景中，对无人机飞行状态和避障过程进行实时三维显示。所述三维改进路径规划方法，通过基于DEM的改进A*算法获取初始路径；结合Bresenham直线生成算法对所得到的初步规划路径进行直线化；再对直线化后的路径进行Bézier曲线平滑优化。本发明路径规划耗时短，能够实时显示飞行状态，动态三维显示路径规划和避障过程。

Description

无人机三维场景路径导航平台及其三维改进路径规划方法

技术领域

本发明涉及导航技术领域，特别是一种无人机三维场景路径导航平台及其三维改进路径规划方法。

背景技术

数字高程图(Digital Elevation Map),简称DEM。它是将地面高程用一组有序数值阵列形式表示的一种实体地面模型，是一定范围内规则网格点的平面坐标及其高程或经纬度及其海拔的数据集。随着卫星技术，传感器技术等一系列高新技术的快速发展，DEM数字高程数据的分辨率越来越高，这就为DEM数据的应用奠定了良好的基础。DEM在测绘、水文、气象、地貌、地质、土壤、工程建设、通讯、军事等国民经济和国防建设以及人文和自然科学领域有着广泛的应用。

无人机是通过无线电遥控操纵或由自身程序操纵控制的一种无人驾驶飞机。由于无人驾驶，所以无人机机型的尺寸可以做到很小、重量较轻、做高机动动作、雷达反射面积小、可以适应各种环境和地形等特点。目前无人机在作战，救生，森林火险监测，救灾和天气预报方面的广泛应用。并且随着无人机自性的提高,它在未来实际应用中的角色将越来越重要。

路径规划作为无人机导航至关重要的一部分，是无人机完成预定任务不可缺少的前提条件。所谓路径规划就是为无人机设计出一条安全可飞行的航线。随着路径规划的发展，目前无人机的路径规划开始考虑无人机机自身的物理约束。根据已知的地图数据和测量信息在无人机自身物理约束、安全约束以及其它一些不确定因素的条件下，得到一条起始位置点到终止位置点的最优可飞行安全路径。

但由于目前对于无人机的路径规划考虑无人机自身物理约束并不充分，得到的路径看似可以避开障碍物，实际上无人机根本无法沿此路径飞行。尤其是对于自身物理约束很强的固定翼无人机。算法耗时也相对较长，得到的路径也只是一条空间曲线，并不包含无人机的姿态信息，无法动态显示无人机避障过程。

发明内容

本发明的目在于提供一种三维改进路径规划方法，得到满足各种约束条件下的无人机可飞行三维安全路径；并构建一种无人机三维场景路径导航平台，有效地展示无人机三维空间导航和避障过程。

实现本发明目的的技术解决方案为：一种无人机三维场景路径导航平台，该平台基于Unity3d加载DEM地图数据，进行三维场景的显示；通过VS2010C++编程，实现了基于DEM的三维改进路径规划算法，获取无人机可飞行安全路径，并通过Matlab2012a绘制规划得到的路径并检测规划效果；导入无人机模型，并结合miniAHRS姿态传感器，获取当前无人机导航信息；融合路径规划算法到三维场景中，对无人机飞行状态和避障过程进行实时三维显示。

一种基于所述无人机三维场景路径导航平台的三维改进路径规划方法，步骤如下：首先通过基于DEM的改进A*算法获取初始路径，在此基础上考虑无人机的安全性和动力学约束；结合Bresenham直线生成算法对所得到的初步规划路径进行直线化；再对直线化后的路径进行Bézier曲线平滑优化，最终得到满足约束条件的无人机可飞行三维安全路径。

本发明与现有技术相比，其显著优点为：(1)具有地图纹理，天空盒子和光照效果，逼真的显示了三维场景；(2)改进的路径规划耗时短，并考虑了无人机动力学约束；(3)结合miniAHRS姿态传感器，获取无人机导航信息，实时显示飞行状态；(4)融合路径规划算法到三维场景中，动态三维显示路径规划和避障过程。

附图说明

图1为本发明无人机三维场景路径导航平台的***结构框图。

图2为Unity3d加载DEM地图三维场景示意图。；

图3为基于DEM改进路径规划算法流程图。

图4为改进路径算法得到可安全飞行路径示意图。

图5为融合了路径规划的三维场景示意图。

图6为无人机三维路径导航软件界面图。

具体实施方式

结合图1，本发明无人机三维场景路径导航平台，该平台基于Unity3d加载DEM地图数据，进行三维场景的显示；通过VS2010C++编程，实现了基于DEM的三维改进路径规划算法，获取无人机可飞行安全路径，并通过Matlab2012a绘制规划得到的路径并检测规划效果；导入无人机模型，并结合miniAHRS姿态传感器，获取当前无人机导航信息；融合路径规划算法到三维场景中，对无人机飞行状态和避障过程进行实时三维显示。

进一步地，所述基于Unity3d加载DEM地图数据，进行三维场景的显示，其中三维场景为DEM添加了相应的地图纹理信息，并具有天空盒子和光照效果，逼真的显示了三维场景。

进一步地，所述基于DEM的三维改进路径规划算法，具体为：

首先通过基于DEM的改进A*算法获取初始路径，在此基础上考虑无人机的安全性和动力学约束，结合Bresenham直线生成算法对所得到的初步规划路径进行直线化，再对直线化后的路径进行Bézier曲线平滑优化，最终得到满足约束条件的无人机可飞行三维安全路径。

进一步地，所述miniAHRS姿态传感器包括MPU-6050陀螺仪加速度计、HMC5883磁力计、BPM180气压计模块，通过串口从miniAHRS姿态传感器读取俯仰角、滚转角、航向角的导航信息，对应到飞机模型上，实时显示无人机的飞行状态，并在界面显示导航信息。

进一步地，所述融合路径规划算法到三维场景中，具体为：将改进的基于DEM的改进路径规划算法由.cpp文件转化为动态链接库.dll文件；通过在Unity3d中调用该动态链接库，将改进的A*算法融合到三维场景之中，动态三维显示路径规划和避障过程，并在界面设置地图，展现无人机沿着路径飞行的整体效果。

本发明基于所述无人机三维场景路径导航平台的三维改进路径规划方法，步骤如下：首先通过基于DEM的改进A*算法获取初始路径，在此基础上考虑无人机的安全性和动力学约束；结合Bresenham直线生成算法对所得到的初步规划路径进行直线化；再对直线化后的路径进行Bézier曲线平滑优化，最终得到满足约束条件的无人机可飞行三维安全路径。

进一步地，所述基于DEM的改进A*算法获取初始路径，利用DEM的高程数据，并根据固定翼无人机的曲率约束和俯仰角约束，得到无人机在对应点的飞行高度，最终得到满足固定翼无人机动力学约束和安全性约束的初始路径，具体如下：

无人机曲率约束通过限制改进A*算法下一点的检测方向来实现，前、后两段直线路径在水平面投影向量的内积大于等于零的加入改进A*算法的检测，小于零的不考虑；

无人机对应点的飞行高度，依据无人机的俯仰角约束和安全性约束得到，当下一个可能路径点，在俯仰角限定范围之内能够到达的最优高度fh_best，即：

记录该点飞行高度为fh_i+1＝fh_best，并进行下一步检测判断处理；

其中，表示最大俯仰角，-ψ_max表示最小俯角；fh_i、fh_i+1分别表示第i个路径点和第i+1个可能路径网格点无人机飞行的离地高度；h_i、h_i+1分别表示第i个路径点和第i+1个可能路径网格点的DEM高程；表示这两个网格点的水平距离；

当fh_best不在俯仰角限定范围之内，且时，若满足：

考虑该点，记录该点飞行高度为并进行下一步判断处理；不满足该判断条件的，不考虑该点；其中fh_{min s}表示预留安全距离后无人机的最低飞行高度；对于情况类似；

DEM高程数据的利用，还通过在改进A*算法的代价函数中考虑了高程的影响：

代价函数表示为：

F＝G+H

其中，G表示从启示网格点移动到指定网格点的损耗，H表示从指定网格点移动到终止网格点的估计损耗，F表示总的估计损耗；

为得到最短的飞行路径，所建立的代价函数与距离相关，代价函数G设计为：

如果则

如果则

其中G_i+1、G_i分别表示第i+1，i个网格点的G值，h_i+1、h_i分别表示第i+1、i个网格点的高程；fh_i+1、fh_i分别表示在第i+1、i个网格点无人机的飞行高度；表示第i+1与i个网格点行距离差，表示第i+1与i个网格点列距离差；λ对应DEM数据的分辨率，对于30m分辨的DEM数据取30；λ_d对应DEM数据分辨率对角线的值，对于30m分辨的DEM数据，取

从指定网格点移动到终止网格点的代价函数H表示为：

如果则

如果则

其中表示指定点到终止点行距离差、列距离差；h_f、fh_f分别表示终止点的高程和无人机的飞行高度。

上述基于DEM的改进A*算法，计算量大幅减小，节约了78.1％的计算时间；并且考虑了固定翼无人机的曲率和俯仰角约束，得到的初始路径基本满足无人机的动力学要求

进一步地，所述结合Bresenham直线生成算法对所得到的初步规划路径进行直线化，去除了不必要的路径点，路径点数量减少了78.8％，路径长度相比初始路径减少了13.63％。算法完全是基于整数运算的，因此该直线化算法效率极高，几乎没有增加整个算法的计算时间。模拟实验中，单单直线化的计算时间不超过1ms。具体为：

该基于Bresenham算法的路径直线化，结合Bresenham直线生成算法对A*算法得到的初始路径进行直线化，获取直线化过程中需要检测的点；若检测的所有点和直线化后的路径满足无人机的动力学和安全性约束，进行直线化去除不必要的路径点，否则放弃直线化。直线化去除了不必要的路径点，减少了路径长度，并且算法完全是基于整数运算的，因此该直线化算法效率极高。

进一步地，所述对直线化后的路径进行Bézier曲线平滑优化，得到了光滑连续路径，是满足无人机动力学和安全性约束的最短可飞行路径，路径长度相比直线化路径减少了2.47％。所设计的Bézier曲线，有闭合的求解公式，无需递归计算，几乎不增加计算时间。模拟实验中，单单Bézier曲线平滑的计算时间不超过1ms。具体为：

该Bézier曲线路径平滑，设计了一种C2连续的5次正则Bézier曲线r(q)进行路径平滑；五次Bézier的控制点b_k且k＝0,1,2在前一段直线路径上，控制点b_k且k＝3,4,5在后一段直线路径上；并且取对称结构其中O为两直线段路径的交点；

证明所设计的Bézier曲线为正则曲线，即证明它的一阶导数始终不为零：

由于在所建立坐标系x轴方向的投影都是沿x轴负方向，且这五个向量都不为零向量，所以曲线的一阶导数不为零；因而所设计的Bézier曲线是正则曲线，因此所设计Bézier曲线的长度S通过如下公式求得：

考虑到固定翼无人机的曲率约束，安全性约束，求取无人机最短可飞行路径表示为：

|Δ|≤L_safe

其中，κ_max表示最大可飞行曲率，|Δ|表示曲线化后的路径相比于直线化后的路径的偏移量；L_safe表示改进A*算法中预留的安全距离；l₀、l₁分别表示前一段直线路径和后一段直线路径的长度；为保证率不出现波动和剧烈变化，设定在Bézier曲线中间取得曲率最大点：

其中

要保证Bézier曲线中间取得曲率最大点，即保证q∈(0,0.5)v(q)正负性不变，此时根据对称性q∈(0.5,1)时v(q)正负性也不变；

经过多次模拟实验，在保证最大曲率尽可能小的情况下得到经验值：

得到是关于q，的二元多项式函数，该二元多项式函数在q∈[0,0.5)，时小于0，q∈[0.5,1)，时大于0；满足要求，则得到平滑曲线的最大曲率为：

对于特定的两直线段的平滑拼接，夹角α是已知的，得到的m₂最优值如下：

其他相关的值随即确定，最优的Bézier平滑曲线便随即确定。通过Bézier曲线平滑，最终得到的路径光滑连续，是满足无人机动力学和安全性约束的最短可飞行路径。

Matlab2012a绘制规划得到的路径，对得到的路径进行统计分析，检测改进路径规划的可行性和可靠性。证实了最终的路径满足固定翼无人机的动力学和安全性约束，是一条可飞行的安全路径。

下面将结合附图及具体实施例，对本发明的技术方案进行详细的说明。

实施例1

如图1～6所示，一种无人机三维路径导航平台，基于Unity3d加载DEM地图数据，进行三维场景的显示；通过VS2010C++编程，实现了基于DEM的三维改进路径规划算法，获取无人机可飞行安全路径，并通过Matlab2012a绘制规划得到的路径检测规划效果；导入无人机模型，并结合miniAHRS姿态传感器，获取当前无人机飞行姿态；融合路径规划算法到三维场景中，对无人机飞行状态和避障过程进行实时三维显示；

一种无人机三维路径导航平台如图1所示的步骤实现的。

步骤一、场景绘制：Unity3d加载DEM数据，为DEM添加相应的地图纹理信息，并添加天空盒子和光照效果。如图2所示，逼真的显示了三维场景。步骤二、路径规划：VS2010C++编程，实现了基于DEM的三维改进路径规划算法。如图3所示，展现了算法发的具体流程。如图4所示，通过Matlab2012a绘制了所得到的路径。仿真试验证实了该路径是满足固定翼无人机动力学和安全性约束的可飞行安全路径。其中路径规划具体为：

(1)通过基于DEM的改进A*算法获取初始路径：充分利用DEM的高程数据，并考虑了固定翼无人机的曲率约束和俯仰角约束，得到了无人机在对应点的飞行高度，最终得到了基本满足固定翼无人机动力学约束和安全性约束的初始路径。

无人机曲率约束通过限制改进A*算法下一点的检测方向来实现，前一小段直线路径后一小段直线路径在水平面投影向量内积大于等于零的可加入改进A*算法的检测，小于零的暂时不考虑。

无人机对应点的飞行高度，主要是依据无人机的俯仰角约束和安全性约束得到的。当下一个可能路径点，在俯仰角限定范围之内可以到达最优高度fh_best，即

记录该点飞行高度为fh_i+1＝fh_best，并进行下一步检测判断处理。其中表示最大俯仰角，-ψ_max表示最小俯角。fh_i、fh_i+1分别表示第i个路径点和第i+1个可能路径网格点无人机飞行的离地高度。h_i、h_i+1分别表示第i个路径点和第i+1个可能路径网格点的DEM高程。表示这两个网格点的水平距离。

当fh_best不在俯仰角限定范围之内，且时，

若满足考虑该点，记录该点飞行高度为并进行下一步判断处理。不满足该判断条件的，说明即使按最大仰角，也不能使无人机达到最低离地飞行高度要求，所以暂时不考虑该点。其中fh_{min s}表示预留安全距离后无人机的最低飞行高度。对于情况类似。

DEM高程数据的充分利用，还通过在改进A*算法的代价函数中考虑了高程的影响。

代价函数可表示为：

F＝G+H

其中G表示从启示网格点移动到指定网格点的损耗，H表示从指定网格点移动到终止网格点的估计损耗，F表示总的估计损耗。

为得到最短的可飞行路径，所建立的代价函数都是与距离相关的量，并将DEM高程进行考虑。代价函数G设计为。

如果则

如果则

其中G_i+1、G_i分别表示第i+1，i个网格点的G值，h_i+1、h_i分别表示第i+1、i个网格点的高程。fh_i+1、fh_i分别表示在第i+1、i个网格点无人机的飞行高度。表示第i+1与i个网格点行距离差，表示第i+1与i个网格点列距离差。λ对应DEM数据的分辨率，对于30m分辨的DEM数据，取30。λ_d对应DEM数据分辨率对角线的值，对于30m分辨的DEM数据，取

从指定网格点移动到终止网格点的代价函数H可表示为：

如果则

如果则

其中表示指定点到终止点行距离差、列距离差。h_f、fh_f分别表示终止点的高程和无人机的飞行高度。

(2)基于Bresenham算法的路径直线化：结合Bresenham直线生成算法对A*算法得到的初始路径进行直线化，获取直线化过程中需要检测的点。若检测的所有点和直线化后的路径满足无人机的动力学和安全性约束，进行直线化，否则放弃直线化。直线化去除了不必要的路径点，减少了路径长度，并且算法完全是基于整数运算的，因此该直线化算法效率极高。

(3)基于Bézier曲线路径平滑，设计了一种C2连续的5次正则Bézier曲线r(q)进行路径平滑。五次Bézier的控制点b_k(k＝0,1,2)在前一段直线路径上，控制点b_k(k＝3,4,5)在后一段直线路径上。并且取对称结构 其中O为两直线段路径的交点。

证明所设计的Bézier曲线为正则曲线，即证明它的一阶导数始终不为零。

由于在所建立坐标系x轴方向的投影都是沿x轴负方向，且这五个向量都不为零向量，所以曲线的一阶导数不为零。因而所设计的Bézier曲线是正则曲线。因此所设计Bézier曲线的长度可通过如下公式求得。

考虑到固定翼无人机的曲率约束，安全性约束。求取无人机最短可飞行路

径可表示为。

|Δ|≤L_safe

κ_max表示最大可飞行曲率，|Δ|表示曲线化后的路径相比于直线化后的路径的偏移量。L_safe表示改进A*算法中预留的安全距离。l₀、l₁分别表示前一小段直线路径和后一小段直线路径的长度。为保证率不出现波动和剧烈变化，设定在Bézier曲线中间取得曲率最大点。

其中

要保证Bézier曲线中间取得曲率最大点，即保证q∈(0,0.5)v(q)正负性不变，此时根据对称性q∈(0.5,1)时v(q)正负性也不变。经过多次模拟实验，在保证最大曲率尽可能小的情况下得到经验值。

将可以得到是关于q，的二元多项式函数。该二元多项式函数，该函数在q∈[0,0.5)，时小于0，q∈[0.5,1)，时大于0。满足要求。可以得到平滑曲线的最大曲率为。

对于特定的两直线段的平滑拼接，夹角α是已知的，可以得到的m₂最优值。

其他相关的值随即确定，最优的Bézier平滑曲线便随即确定。通过Bézier曲线平滑，最终得到的路径光滑连续，是满足无人机动力学和安全性约束的最短可飞行路径。

(4)路径规划仿真实验：仿真实验实现了改进路径规划算法发，Matlab2012a绘制规划得到的路径(如图3所示)，检测改进路径规划的可行性和可靠性。对得到的路径进行统计分析，证实了通过改进A*算法得到初始路径，再经过直线化和曲线平滑的路径满足无人机的动力学和安全性约束。最终得到的路径是一条安全看飞行路径。

表1路径规划对比表

通过表1可以看出，相比于传统A*，改进的A*的计算时间极大的减少，减少了78.1％。为得到基本满足约束条件的路径，要避开一些无法通过的点，因而路径长度增加了11.6％，路径点数增加了10.7％。从最大俯仰角可以看出，传统A*得到的路径并不满足俯仰角约束，而改进的A*满足这一约束。

对改进A*算法得到的初始路径进行进一步优化。从表中可以看出，直线化和曲线平滑几乎没有增加计算时间，并且，直线化后路径长度有所减少，路径长度减少了13.63％。曲线插值后的路径长度又减少了2.47％。最重要的是通过路径直线化和曲线平滑，得到了满足无人机动力学约束的可飞行路径。

步骤三、导航信息：miniAHRS姿态传感器主要由MPU-6050陀螺仪加速度计、HMC5883磁力计、BPM180气压计模块构成。通过串口读取miniAHRS姿态传感器数据，获取无人机俯仰角，滚转角，航向角等导航信息。将获取的无人机导航信息，对应到飞机模型上，实时的显示无人机的飞行状态。如图6所示，三维动态显示无人机的飞行状态，并在界面左上方显示了有关的导航信息。

步骤四、场景算法融合：融合改进路径规划算法到三维场景中，将改进的基于DEM的A*算法(包括直线化和曲线平滑)由.cpp文件转化为动态链接库.dll文件。通过在Unity3d中调用该动态链接库，将改进路径规划算法融合到三维场景之中，动态三维显示路径规划和避障过程。如图5所示，展现了三维场景中的路径规划。如图6所示，用亮绿色的线绘制了规划得到的可飞行安全路径，橘黄色的箭头指示了无人机的飞行方向，并在界面右上方设置小地图，展现无人机沿着路径飞行的整体效果。

以上实施例是对本发明的具体实施方式的说明，而非对本发明的限制，有关技术领域的技术人员在不脱离本发明的精神和范围的情况下，还可以做出各种变换和变化而得到相对应的等同的技术方案，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (9)

1.一种无人机三维场景路径导航平台，其特征在于，该平台基于Unity3d加载DEM地图数据，进行三维场景的显示；通过VS2010C++编程，实现了基于DEM的三维改进路径规划算法，获取无人机可飞行安全路径，并通过Matlab2012a绘制规划得到的路径并检测规划效果；导入无人机模型，并结合miniAHRS姿态传感器，获取当前无人机导航信息；融合路径规划算法到三维场景中，对无人机飞行状态和避障过程进行实时三维显示。

2.根据权利要求1所述的无人机三维场景路径导航平台，其特征在于，所述基于Unity3d加载DEM地图数据，进行三维场景的显示，其中三维场景为DEM添加了相应的地图纹理信息，并具有天空盒子和光照效果。

3.根据权利要求1所述的无人机三维场景路径导航平台，其特征在于，所述基于DEM的三维改进路径规划算法，具体为：

首先通过基于DEM的改进A*算法获取初始路径，在此基础上考虑无人机的安全性和动力学约束，结合Bresenham直线生成算法对所得到的初步规划路径进行直线化，再对直线化后的路径进行Bézier曲线平滑优化，最终得到满足约束条件的无人机可飞行三维安全路径。

4.根据权利要求1所述的无人机三维场景路径导航平台，其特征在于，所述miniAHRS姿态传感器包括MPU-6050陀螺仪加速度计、HMC5883磁力计、BPM180气压计模块，通过串口从miniAHRS姿态传感器读取俯仰角、滚转角、航向角的导航信息，对应到飞机模型上，实时显示无人机的飞行状态，并在界面显示导航信息。

5.根据权利要求3所述的无人机三维场景路径导航平台，其特征在于，所述融合路径规划算法到三维场景中，具体为：将改进的基于DEM的改进路径规划算法由.cpp文件转化为动态链接库.dll文件；通过在Unity3d中调用该动态链接库，将改进的A*算法融合到三维场景之中，动态三维显示路径规划和避障过程，并在界面设置地图，展现无人机沿着路径飞行的整体效果。

6.一种基于权利要求1所述无人机三维场景路径导航平台的三维改进路径规划方法，其特征在于，步骤如下：首先通过基于DEM的改进A*算法获取初始路径，在此基础上考虑无人机的安全性和动力学约束；结合Bresenham直线生成算法对所得到的初步规划路径进行直线化；再对直线化后的路径进行Bézier曲线平滑优化，最终得到满足约束条件的无人机可飞行三维安全路径。

7.根据权利要求6所述的基于无人机三维场景路径导航平台的三维改进路径规划方法，其特征在于，所述基于DEM的改进A*算法获取初始路径，利用DEM的高程数据，并根据固定翼无人机的曲率约束和俯仰角约束，得到无人机在对应点的飞行高度，最终得到满足固定翼无人机动力学约束和安全性约束的初始路径，具体如下：

无人机曲率约束通过限制改进A*算法下一点的检测方向来实现，前、后两段直线路径在水平面投影向量的内积大于等于零的加入改进A*算法的检测，小于零的不考虑；

无人机对应点的飞行高度，依据无人机的俯仰角约束和安全性约束得到，当下一个可能路径点，在俯仰角限定范围之内能够到达的最优高度fh_best，即：

记录该点飞行高度为fh_i+1＝fh_best，并进行下一步检测判断处理；

其中，表示最大俯仰角，-ψ_max表示最小俯角；fh_i、fh_i+1分别表示第i个路径点和第i+1个可能路径网格点无人机飞行的离地高度；h_i、h_i+1分别表示第i个路径点和第i+1个可能路径网格点的DEM高程；表示这两个网格点的水平距离；

当fh_best不在俯仰角限定范围之内，且时，若满足：

考虑该点，记录该点飞行高度为并进行下一步判断处理；不满足该判断条件的，不考虑该点；其中fh_mins表示预留安全距离后无人机的最低飞行高度；对于情况类似；

DEM高程数据的利用，还通过在改进A*算法的代价函数中考虑了高程的影响：

代价函数表示为：

F＝G+H

其中，G表示从启示网格点移动到指定网格点的损耗，H表示从指定网格点移动到终止网格点的估计损耗，F表示总的估计损耗；

为得到最短的飞行路径，所建立的代价函数与距离相关，代价函数G设计为：

如果则

如果则

其中G_i+1、G_i分别表示第i+1，i个网格点的G值，h_i+1、h_i分别表示第i+1、i个网格点的高程；fh_i+1、fh_i分别表示在第i+1、i个网格点无人机的飞行高度；表示第i+1与i个网格点行距离差，表示第i+1与i个网格点列距离差；λ对应DEM数据的分辨率，对于30m分辨的DEM数据取30；λ_d对应DEM数据分辨率对角线的值，对于30m分辨的DEM数据，取

从指定网格点移动到终止网格点的代价函数H表示为：

如果则

如果则

其中表示指定点到终止点行距离差、列距离差；h_f、fh_f分别表示终止点的高程和无人机的飞行高度。

8.根据权利要求6所述的基于无人机三维场景路径导航平台的三维改进路径规划方法，其特征在于，所述结合Bresenham直线生成算法对所得到的初步规划路径进行直线化，具体为：

该基于Bresenham算法的路径直线化，结合Bresenham直线生成算法对A*算法得到的初始路径进行直线化，获取直线化过程中需要检测的点；若检测的所有点和直线化后的路径满足无人机的动力学和安全性约束，进行直线化去除不必要的路径点，否则放弃直线化。

9.根据权利要求6所述的基于无人机三维场景路径导航平台的三维改进路径规划方法，其特征在于，所述对直线化后的路径进行Bézier曲线平滑优化，具体为：

该Bézier曲线路径平滑，设计了一种C2连续的5次正则Bézier曲线r(q)进行路径平滑；五次Bézier的控制点b_k且k＝0,1,2在前一段直线路径上，控制点b_k且k＝3,4,5在后一段直线路径上；并且取对称结构其中O为两直线段路径的交点；

证明所设计的Bézier曲线为正则曲线，即证明它的一阶导数始终不为零：

由于在所建立坐标系x轴方向的投影都是沿x轴负方向，且这五个向量都不为零向量，所以曲线的一阶导数不为零；因而所设计的Bézier曲线是正则曲线，因此所设计Bézier曲线的长度S通过如下公式求得：

考虑到固定翼无人机的曲率约束，安全性约束，求取无人机最短可飞行路径表示为：

|Δ|≤L_safe

其中，κ_max表示最大可飞行曲率，|Δ|表示曲线化后的路径相比于直线化后的路径的偏移量；L_safe表示改进A*算法中预留的安全距离；l₀、l₁分别表示前一段直线路径和后一段直线路径的长度；为保证率不出现波动和剧烈变化，设定在Bézier曲线中间取得曲率最大点：

其中

要保证Bézier曲线中间取得曲率最大点，即保证q∈(0,0.5)v(q)正负性不变，此时根据对称性q∈(0.5,1)时v(q)正负性也不变；

经过多次模拟实验，在保证最大曲率尽可能小的情况下得到经验值：

得到是关于q，的二元多项式函数，该二元多项式函数在q∈[0,0.5)，时小于0，q∈[0.5,1)，时大于0；满足要求，则得到平滑曲线的最大曲率为：

对于特定的两直线段的平滑拼接，夹角α是已知的，得到的m₂最优值如下：

通过Bézier曲线平滑，最终得到的路径光滑连续，是满足无人机动力学和安全性约束的最短可飞行路径。