多源性能约束的无人机可靠动态航路规划算法

孙淑光，朱晓文

（1.中国民航大学 电子信息与自动化学院，天津 300300；2.中国民航大学 交通科学与工程学院，天津 300300）

随着无人机在航空物流运输、应急救援、森林防护以及农渔业作业等领域的广泛应用，低空空域交通流量快速增加，无人机的航路规划及运行安全日益成为人们关注的重点，有效的机载端动态航路规划是实现无人机自主运行安全的关键[1-4]。十四五期间，中国民航将以安全为要，推动无人机基于任务的自主航路规划能力和自主智能化复杂任务完成能力的实现。提高空中交通自适应、自诊断、自决策、重规划能力，实现交通管理全过程的数字化与自动化[5-9]。

现有的无人机航路规划方法中，大都是将无人机看作一个已知精确位置的质点进行航路规划，不考虑无人机机载导航系统的定位误差，对于运行于复杂低空空域，如城市建筑峡谷、复杂山区等区域的无人机来说，存在极大的安全隐患[10-12]。现有的无人机导航系统大多采用普通精度惯性测量单元/全球定位系统（Inertial Measurement Unit/Global Positioning System,IMU/GPS）导航模块来确定无人机的位置、速度和姿态信息。受无人机周围建筑物或地形的遮挡，机载导航系统极易处于卫星信号拒止的环境，接收的卫星信号数量大大减少，几何精度因子急剧增大，出现较大的定位误差及位置漂移[13-15]，无人机与空域障碍物之间的实际安全容限急剧下降，会导致所规划的航路不可行，或者即使航路可行，但与周围障碍物存在极大的碰撞风险，影响规划航路的可行性和安全性。

为提高规划航路的可行性和安全性，本文提出一种敏感无人机机载导航系统实际性能及无人机机动性能的动态航路规划算法，将无人机航路规划由基于等效质点的规划转变为基于多源性能约束的敏感区域路径规划，确保所规划航路的安全性和可行性。

1 无人机航路规划系统架构及约束条件分析

1.1 系统架构

根据无人机的任务需求确定航路的起始点和目的地，利用机载地形数据库信息建立三维地形网格，将机载导航系统的实际导航性能和无人机机动性能作为航路规划的约束条件，根据机载导航系统提供的无人机实时位置信息动态更新航路规划的起始点，实现机载端的动态航路规划。系统整体架构如图1 所示。

图1 无人机动态航路规划系统架构Fig.1 UAV dynamic flight path planning architecture

1.2 航路规划约束条件

1.2.1 机载导航系统导航性能约束

利用机载导航系统实时评估自身实际导航性能，计算导航系统定位误差包括，也即导航系统位置不确定度（Estimated Position Uncertainty,EPU），并将其与无人机机动性能一起作为航路规划的搜索代价函数来约束航路规划，EPU为机载导航系统的总系统误差。

在充分考虑机载导航系统定位误差及无人机机动性能的基础上进行航路规划，可以有效提高所规划航路的安全性。航路规划中，计算当前节点与临近障碍物之间的距离，与EPU 相比较确定安全节点。当节点与障碍物之间距离的最小值大于EPU 时，该节点被定义为安全节点。

1.2.2 无人机机动性能约束

无人机自身的机动性能是制约航路规划的另一关键要素，直接关系到所规划航路的可行性，需要在航路规划的过程中将其作为重要的判定依据。影响规划航路可行性的无人机机动性能包括无人机的最大转弯角度和飞行高度。

1）最大转弯角约束

实际飞行中，受无人机空气动力学特性及飞行控制系统能力所限，无人机尤其是固定翼无人机，规划航路的转弯角度最大不能超过无人机可能实现的最大允许转弯角。

航路规划中，将无人机最大转弯角作为约束，可以减少搜索子节点数量，缩短航路规划时间。如图2所示，根据无人机规划航路前序节点与当前节点的位置关系计算航迹角，作为无人机当前位置的航迹。在忽略侧风影响的情况下，可近似认为是无人机的当前航向。以此航迹作为基准向量，筛选无人机最大机动转弯角覆盖范围内的节点，作为后续航路搜索节点。图2 中，虚线为无人机当前航迹，黑色节点为不满足无人机最大转弯角限制的节点，灰色节点为满足最大转弯角限制节点。

图2 基于导航系统性能的可行航路Fig.2 Feasible trajectories based on navigation system performance

图3 最大转弯角约束Fig.3 Maximum turning angle constraint

航路规划中，计算航路的最大转弯角并与无人机最大允许转弯角相比较，以确保无人机能够实现一定距离容限内的安全转弯。

式中：θrp为规划航路的最大转弯角，θuav-max为无人机飞行时的最大允许转弯角。

2）最大飞行高度约束

在不考虑空域限制的情况下，飞行高度越高，无人机受地形障碍物的影响越小，航路规划越经济简单。但无人机受到自身高度升限的局限，在不同的飞行任务需求下，需要综合评估无人机高度升限、续航时间与规划航路之间的制约关系，在确保规划航路可行的前提下实现经济性。规划航路高度要限定在无人机的高度升限之下：

式中：Hrp为规划航路最大飞行高度，Huav-max为无人机高度升限。将所有高度高于Huav-max的位置节点定义为障碍物，低于Huav-max的位置节点则根据无人机高度与地形高度之间的差值来确定各节点的属性（障碍、非障碍）。

2 改进A＊算法的无人机航路规划

常用的路径规划算法有：传统的模拟退火算法、人工势场法；包括A*算法、Dijkstra 算法的启发式搜索算法；以及蚁群算法、粒子群算法等智能仿生学算法[16-20]。相比较而言，A*算法以简单、有效、直观等优点在路径规划中得到广泛应用，并可以通过稀疏改进、双向搜索等方式进行算法改进，提高优化效果及运行效率。

A*算法最早由斯坦福大学的Peter Hart 等人于1968 年提出，是一种典型的图遍历启发式搜索算法。A*算法利用启发函数描述当前节点距离目标点的距离，在全局范围逐次搜索临近节点，计算代价函数，通过代价函数最小选定最优航路。传统A*算法通过搜索遍历实现最优航路规划，对环境反应迅速，搜索路径直接，并可以通过调节启发函数有效控制算法的速度和精确度，相比其他算法，所规划航路不仅满足航路规划的需求，且具有很好的稳定性，易于无人机实现遵循规划航路的飞行。本文对传统A*算法进行改进，将无人机机动性能作为约束条件，减小搜索空间，缩短算法运行时间；通过启发函数加权计算，综合考虑导航系统EPU、机动影响和航路长度权重因子，提供高效的无人机航路规划。

传统A*算法的代价函数如下：

式中：n表示当前节点，g(n)为从起点到当前节点的实际代价函数，h(n)为从当前节点到目标点的估计代价函数（启发函数），f(n) 为所规划航路的总代价函数。f(n) 最小时，所规划的航路最优。

假设当前节点的位置坐标为(xn,yn,zn)，目标点的位置坐标为(xg,yg,zg)，则：

其中，启发函数采用曼哈顿距离计算方式。

2.1 加权启发函数

A*算法中，可以通过加权系数调节实际代价函数g(n)和启发函数h(n)对航路规划的影响，启发函数h(n)的权重越大，算法收敛速度越快，反之则收敛速度变慢。

为确保所规划航路的安全性和可行性，需要对A*算法进行改进，综合考虑机载导航系统定位误差和无人机机动性能对航路规划的影响，利用机载导航系统EPU、无人机机动转弯角和曼哈顿距离加权计算启发函数h(n)。

式(4)中，Dw为导航系统EPU 代价，Dz为无人机机动转弯角代价，Ds为航程代价，w1,w2,w3分别为三种代价的权重系数，w1+w2+w3=1，可以根据任务需要调节权重，实现不同代价需求的规划航路。其中w2根据无人机的特性可以为0～1/3，对航向可以任意改变的旋翼无人机，权重系数为0，而对于航向改变受限的固定翼无人机，其权重可以根据受限程度改变，受限程度越高，权重系数越大，直至与EPU 和航程代价的权重相同。

2.1.1 导航系统EPU 代价函数 Dw

捷联惯性导航系统的定位精度与传感器精度及工作时长有关。随着飞行时间的增长，惯性导航系统的定位误差呈累加状态，极大地限制了惯性导航系统独立使用的时长。随着卡尔曼滤波技术的改进，GPS/IMU 组合导航的情况下，惯导系统的定位精度得到较大改善，但在GPS 卫星信号拒止，需要惯导系统独立工作的情况下，其定位误差随时间的积累相对还是比较大的。典型的机载惯导系统的误差变化变化如下式所示。

式(5)中：t为IMU 的工作时间，单位为小时。

GPS 接收模块的导航定位误差主要与接收机的用户等效测距误差和所接收卫星的空间几何布局（Dilution of Precision,DOP）有关。

GPS/IMU 组合导航系统的实际导航性能用EPU来描述，它以机载导航系统计算的无人机位置为圆心，根据GPS/IMU 组合导航所用的滤波算法，计算95%概率下的系统定位误差，即为机载导航系统EPU。假设GPS/IMU 采用松组合形式，机载导航系统EPU 的计算公式如下：

式(6)中：HFOM 是GPS 接收模块根据DOP 及测距误差计算的GPS 模块2σ（95%）定位误差；δPINS是惯性导航计算误差，与IMU 传感器的误差特性有关；Δlatency是组合导航算法处理的延迟时间，GS 为无人机地速。

以当前节点为核心，以图4 所示的方式顺序搜寻当前节点周围八个方向的最近障碍物节点，并分别计算二者之间的距离。假设当前节点坐标为(xi,yi)，第j个障碍物节点的坐标为(xnj,ynj)，则二者之间的距离为计算如式(7)所示：

图4 当前节点与临近障碍物之间的距离Fig.4 Distance between present position and adjacent obstacle

式(7)中，Se为当前节点到临近障碍物网格的最小距离，当Se≤EPU时，该节点不可用，只有当Se＞EPU时，才属于安全节点，代价函数是Se的倒数，Se越大，则规划航路的安全性越高，在启发函数中的影响力就越大。

2.1.2 无人机机动转弯角代价Dz

无人机的航路规划首先应满足无人机的机动性能需求，只有满足性能需求的航路才是可实施航路。假设无人机当前节点坐标为(xi,yi,zi)，前序节点坐标为(xi-1,yi-1,zi-1)，后序节点坐标为(xi+1,yi+1,zi+1)，θi为规划航路转弯角大小。则θi为：

当θi＞θuav-max时，后续节点导致的无人机机动转弯角大于无人机的最大允许转弯角，该节点为无效节点，只有当θi≤θuav-max时，该节点才可作为航路规划的下一节点，且机动转弯角越小，在启发函数中的影响力就越大。因此，无人机激动转弯角的代价函数为：

航程代价Ds为当前节点到目的地的距离代价。假设无人机当前节点坐标为(xi,yi,zi)，目的地节点坐标为(xd,yd,zd)，则航程代价为：

2.2 无人机动态航路规划

2.2.1 航路规划网格的构建

本文航路规划所用地形数据库为global mapper提供的数字高程模型（Digital Elevation Model,DEM）数据，分辨率为5×5×5 m3，航路规划使用该数据作为网格数据。

为提高航路规划的搜索效率，通过高度分层来降低搜索维度，缩短搜索时间。起降阶段按照5 m 高度分层进行航路规划，巡航阶段则根据机载导航系统提供的无人机高度信息，取整到其下方的5 m 高度层网格，动态调整DEM 高程数据地形障碍物的网格图。由于无人机起降阶段短，航向相对稳定，对整体规划航程的影响小，因此对改进算法的性能分析主要集中在特定高度的巡航阶段。

2.2.2 航路规划流程

如图5 所示，无人机起飞前首先按照默认EPU 进行航路规划，默认EPU 为GPS 接收机常规误差，所规划航路作为初始航路。

飞行过程中，根据机载导航系统提供的无人机位置、高度信息和EPU 决定系统是否启动新一轮航路规划，逻辑判定的依据是无人机高度偏离和水平偏离。

当无人机导航系统提示无人机的当前高度与前期所规划航路的高度层之间的偏差大于1/2 网格高时，调用当前高度临近的低高度层网格信息，启动新的航路规划。

利用导航系统提供的水平位置信息和EPU 计算无人机的水平偏离，当水平偏离大于1/2 网格宽度时，启用新的航路规划。其中无人机的水平偏离为航道偏离和导航系统EPU 之和。

其中L为无人机的水平偏离，L1为航道偏离。如果两个偏离都未超出阈值，则保持前期规划航路不变。

新的航路规划中，导航系统提供的无人机实时位置作为航路规划的新起始点，终点不变。导航系统EPU用于计算当前时刻导航系统EPU 代价函数。改进后的A*算法流程图如图5 所示。

对于确定飞行任务的无人机来说，只有在出现特殊情况时，如强侧风、雷雨区躲避、动目标的机动冲突解脱等原因，才可能会出现较大的航路偏离，需要重新规划航路，或无人机任务临时改变时需要重新规划航路。常规飞机密度的空域中，所规划航路通常会得到较好的遵循。利用高度偏离和水平偏离逻辑判定来启动航路重新规划功能可以有效避免规划的无效重复，节省计算资源。同时，初始航路规划使用默认EPU进行规划，可以确保所规划的初始航路具有较大的安全裕度，为后期的航路更新提供足够的规划时间。极端情况下，只要规划时间小于EPU 与无人机最大飞行速度的比值，就可以确保飞机的飞行安全性。

根据现有典型无人机飞行速度来看，商业用途的固定翼无人机最高航速一般在 28 m/s～55.5 m/s（100～200 km/h）之间，旋翼无人机的最高航速一般在14 m/s～28 m/s（50～100 km/h）之间。考虑导航系统定位不确定度（EPU）约为20 m～30 m，可以满足航路安全的实时性要求。另外，随着无人机的飞行，距离目的地的距离不断缩短，航路规划所用时间也将大大缩短。

3 实验对比与结果分析

为验证基于机载导航系统实际导航性能的改进A*算法对无人机航路规划算法有效性和安全性的提升，利用Matlab2020a 软件对相关算法进行了仿真验证。模拟无人机在不同地形环境和不同导航性能的航路规划结果，并对所规划航路的安全性和规划时间进行了验证分析。

3.1 无约束下的航路规划分析

在不考虑无人机机动性能及导航系统EPU 的情况下，选定规划航路的起始点位置的经纬度为[-38.128554931 °,-7.477058516 °]，目的地经纬度为[-38.133504426 °,-7.503455822 °]。设定无人机分别以370 m，400 m，420 m 三个不同的巡航高度飞行，最大转弯角度为360 °，所规划航路如图6 所示。

可以看出，在不考虑无人机机动性能和导航系统EPU 的情况下，无人机航路规划主要受地形数据库中障碍物分布的影响，巡航高度越高，空域中障碍物的数量越少，分布越稀疏，所规划的航路长度越短，优化效果明显。

3.2 转弯角约束下的航路规划分析

不考虑导航系统EPU，考虑无人机的机动转弯性能，对航路规划效果及所规划航路的转弯角度进行分析。假设无人机起始点经纬度为[-38.128554931 °,-7.477058516 °]，目的地经纬度为[-38.129615537 °,-7.482715081 °]，运行于400 m 巡航高度。在无人机最大允许转弯角度分别为80 °和180 °，以及复杂场景最大允许转弯角度分别为30°和120°的情况下，进行无人机航路规划的对比分析，如图7 所示。

图7 不同场景/不同转弯角约束下的规划航路Fig.7 Planned trajectories under different maximum turning angle constraints

可以看出，在不同转弯角机动的约束下，无人机的规划航路出现了明显的差异。改进后的A*算法在80 °和180 °转弯角约束下所规划航路的转弯角度变化如图8 中所示。相较于有转弯角约束规划，无转弯角约束的规划航路中，无人机的航线转弯角变化幅度较大，航路可行性及稳定性不及有约束航路规划，对无人机的控制及飞行安全易造成较大影响。

图8 不同转弯角约束下的无人机转弯角Fig.8 UAV turning angle under different maximum turning angle constraints

表1 给出了传统A*算法和改进A*算法航路规划的用时，以及不同转弯角约束下规划航路的角度变化率。可以看出：改进后的A*算法航路规划用时大大缩短，规划时间效率提高一倍。相较于传统A*算法，最大允许转弯角较大的无人机规划航线的角度变化率也有一定程度的降低，但对于最大允许转弯角较小的无人机来说，由于某些特定的环境无法满足其转弯角度要求，所规划的航路有可能会更曲折，角度变化较频繁，但其整个航程的航路转弯角度都保持在较小值，符合其转弯角约束要求。

表1 有/无转弯角约束的仿真结果对比Tab.1 Simulation results comparison of with/without maximum turning angle constraints

3.3 导航系统EPU 约束下的航路规划分析

低成本无人机机载导航系统的误差较大，且极易受无人机复杂地形运行环境的影响。导航系统EPU 直接影响所规划航路的可行性和无人机运行的安全性。本文对不同导航系统EPU 影响下的航路规划进行了仿真，并利用无人机与障碍物之间的距离安全裕度分析所规划航路的安全性。

将无人机由质点转换为覆盖EPU 的空间结构，形成无人机包络，进行EPU 分别为15 m 和5 m 情况下的航路规划，飞行高度为390 m，最大转弯角度为360 °，规划结果如图9 所示。

图9 不同EPU 约束下的规划航路Fig.9 Planned trajectories under different EPU constraints

仿真结果显示，受机载导航系统位置不确定度的影响，相同性能的无人机在相同的运行环境下，所规划的航路有较大的差异，主要是处于不同导航系统EPU 下的航路安全需求，这种差异在低空复杂空域中表现得尤其明显。

以370 m 巡航高度为例，图10 和图11 给出了考虑导航系统EPU 和不考虑EPU 情况下的规划航路以及所规划航路与障碍物之间的距离。可以看出，不考虑导航系统EPU 的规划航路存在极大的安全风险。而基于无人机机载导航系统性能的改进A*算法考虑了安全裕度，所规划航路可以确保无人机到障碍物的距离都大于导航系统EPU，避免了无人机与障碍物的碰撞风险，增加了航路的安全性。

图10 EPU 对规划航路的影响Fig.10 Planned trajectories influenced by EPU

图11 考虑和不考虑EPU 规划航路的安全裕度Fig.11 Safety margins for trajectories with and without consideration of navigation system EPU

3.4 机动性能及EPU 多源约束下的航路规划

综合考虑无人机机动性能和导航系统性能的约束，进行多源敏感航路规划的仿真及效果分析，对比传统算法和改进后算法的航路规划及相关效能数据，确认改进后算法的有效性。设无人机起始点经纬度为[-38.1268897803 °,-7.478922489236 °]，目的地经纬度为[38.1303897803 °,-7.4978558225 °]，巡航高度360 m，最大允许转弯角60 °，导航系统EPU 为5 m。仿真结果如图12、图13 和表2 所示。

表2 算法改进前后数据对比Tab.2 Data comparison of different algorithm

图12 传统算法与改进算法所规划航路的对比Fig.12 Planned trajectories comparison of traditional A*algorithm and improved A* algorithm

图13 算法改进前后无人机航路转弯角对比Fig.13 UAV flight path turning angle comparison of different algorithm

表2 中数据显示，基于无人机机动性能和机载导航系统性能的改进A*算法所规划的航路，无论是航路距离障碍物的安全裕度方面，还是对无人机转弯机动的影响方面都有极大的改善，提高了所规划航路的安全性及可行性。

4 结论

为提升无人机航路规划的安全性，提出基于无人机机动性能和机载导航系统性能约束的改进A*航路规划算法，构建了融合无人机最大允许转弯角、导航系统EPU 和最短路径的加权启发函数，实现航路规划的多约束适应性，并通过机动约束条件减少搜索节点，提高航路规划速度。

利用数字高程模型模拟无人机运行环境，仿真不同最大允许转弯角和导航系统EPU 约束条件下的航路规划，将改进算法的规划航路与传统算法航路规划结果进行对比，实验结果表明，改进算法对所规划航路的安全性、可行性起到了良好的效果。由于大多数无人机体积小、重量轻，易受环境影响，后期需综合考虑环境强风对无人机飞行的扰动，以及导航卫星信号拒止环境下导航系统EPU 突然增大的情况对无人机运行安全性的影响，进一步提高航路规划的鲁棒性。