方向和时间约束下的三维无人机航迹规划

俞瑞，沈亮

（1.四川大学计算机学院，成都 610065；2.江西洪都航空工业集团有限责任公司，南昌 330000）

0 引言

无人机航迹规划，指的是无人机在满足性能、地形环境以及任务约束的条件下，寻找到从起始点到目标点的可行航迹[1-2]。由于战场敌对环境的复杂，以及无人机自身性能存在约束，为无人机规划航迹能够更好地适应环境，避开威胁，提高无人机的安全性，减少航程，降低油耗，减少机动操作，保证预定任务的完成。

目前的路径规划用到的启发式算法有A*算法、遗传算法、粒子群算法、蚁群算法等[3]，其中A*算法更为简单高效。传统的A*算法[4]是在规划环境网格化的基础上根据设定的代价函数寻找最小代价的路径，稀疏A*算法[5]是在传统A*算法的基础上，在扩展节点时，通过考察各种约束条件，有效地缩减搜索空间和缩短搜索时间，能够快速收敛得到所求路径。

根据任务的特殊性，存在针对无人机在飞行方向和时间上的要求，但传统规划算法通常并不考虑。针对此问题，文献[6]在起点/终点附近设置高代价区域，但该方法并不能保证无人机一定以规定角度飞入或者飞出，只能控制在一定范围内，且会造成可行解空间的损失；文献[7]加入引导点的概念，但若设置不当也会产生副作用；文献[8]通过调整速度来满足时间维度的方法，但并未考虑速度的调整仍然需要时间，过于理想化，不符合实际情况。本文针对方向和时间约束的三维无人机航迹规划问题，提出一种基于改进A*算法的有效解决此问题的航迹规划方法，能使规划出的航迹更符合实际情况且较优。

1 环境构建

1.1 三维环境构建

在三维空间中进行无人机航迹规划，建立笛卡尔坐标系，设(x,y,z)为空间中节点的坐标，节点n的坐标为(xn,yn,zn)，其中xn、yn为横向和纵向坐标，zn为该点的地形高度，则该搜索空间可表示为：

整个三维空间栅格化，每次扩展需要搜索相邻的26 个节点，如图1 所示，因此需要根据无人机自身性能约束以及对地形等障碍判断作为限制条件来缩小搜索空间的范围。

图1 三维空间搜索扩展原理

1.2 无人机性能约束

受无人机硬件设施的性能限制，其自身性能存在一定的约束，满足自身性能条件的航迹规划也是为了保证安全。

（1）最小步长约束

要求无人机航迹段长度不小于某一个值，该值是无人机在改变姿态时必须直飞的一个距离，也是每次扩展节点的最小距离lmin。在三维网格空间中，无人机在相邻的顶点之间飞行，故可以顶点间最小航行距离划分。设无人机有N 段航迹ln，满足:

（2）最大航程约束

由于无人机只能携带一定数量的燃油，因此航迹段之和应不超过最大航程的值ln，即：

（3）最大偏航角约束

无人机的飞行高度变化是有限制的，设允许的最大偏航角为θmax，相邻节点的高度分别为zn和zn-1，则满足：

（4）最大飞行高度和最小离地高度约束

受制于无人机自身性能，需要无人机在不超过某一个高度的范围内飞行，同样，飞行高度过低的话，无人机容易与地面发生碰撞，因此设定最大飞行高度zmax和最小离地高度zmin为一定值，确保无人机安全，设节点高度为zn，则满足：

1.3 地形障碍和雷达威胁

真实环境中无人机需要对其航行空间中的地形障碍进行躲避，因此本文设定节点值c表示该点是否可行。山体或障碍所在的节点其c值为1，表示其为不可到达的点，值为0 则表示可到达的点。

预警雷达在战场中应用广泛，其威胁程度与雷达的最小半径和最大半径的距离有关，小于最小半径的范围属于禁飞区，无人机是不能穿过的，否则被发现则此次任务失败，大于最大半径的范围是雷达检测不到的，属于安全区，而在两个半径之间的范围，雷达有一定的概率探测到无人机，这个概率与无人机距离威胁中心的远近有关。雷达威胁度Tr表示为：

式（6）中，d为无人机到威胁中心的欧氏距离，Rmax为该威胁所影响的最大范围半径，Rmin为一定会受到威胁的区域半径，在Rmin内的区域则为禁飞区，该区域内无人机不可达，在Rmax和Rmin内的区域属于威胁区，该区域内无人机存在着一定概率的威胁。

2 改进A＊ 算法航迹规划

2.1 基本思想

A*算法是一种启发式搜索算法，传统的A*算法是在规划环境网格化的基础上根据设定的估价函数寻找最小代价的航迹，能够有效地引导无人机飞向目标点。稀疏A*算法是在传统A*算法的基础上，在扩展节点时，通过考察各种约束条件，有效地缩减搜索空间和缩短搜索时间，能够快速收敛得到所求航迹。构建代价函数[9-10]：

式（7）中，n表示当前所在的节点，f(n)为从初始点经由节点n到达目标点所估计的总代价函数，g(n)是从起点到节点n的实际代价，h(n)是从节点n到目标点的估计代价。

2.2 改进A★算法航迹规划

标准A*算法规划的航迹存在不足：节点只能反映无人机的位置信息，无法体现无人机的姿态信息；仅通过比较各相邻扩展节点的代价大小来判断航迹点是否可行，未考虑到雷达探测概率；冗余点和拐点多，航迹不平滑；无法满足方向和时间上的约束条件。

因此对传统的A*算法进行改进，对航迹点的扩展搜索方式、代价函数的设定、新航迹点的可行性判定、航迹优化以及运算时间等进行优化改进。

（1）变权值评估

在搜索过程中，g(n)和h(n)对评估的影响是不同的，应当选择加权评估方法，从而根据实际情况更改各代价值的比重，保证路径的合理性，即：

其中，wg和wh分别为实际代价g(n)和估计代价h(n)对应的权值，且满足wg+wh=1。

（2）引入加速度代价

为了满足任务要求中对于到达目标点时间的要求，引入加速度代价，以确定到达时间为准，在每一次扩展搜索节点时，通过更改无人机的加速度值来调节其飞行速度，从而达到按时到达目标点的任务要求。节点的加速度值的计算方法：设定当前点作匀变速直线运动到扩展点后，再匀速运动到目标点，运动总时间为预计到达时间，假设当前点的速度为v0，到飞行当前点的所花时间t0，当前点到扩展点的距离为d、所花时间为t1，扩展点的加速度为a，扩展点到目标点的估计距离为s、所花时间为t2，预计到达时间为T，根据方程组（9）求解得出扩展点的加速度，取其绝对值为该点的加速度代价a(n)。

（3）引入威胁代价

为了保障无人机执行任务的安全，避免被敌方雷达发现，在实际代价函数中引入威胁度的评估。根据雷达威胁模型，节点的威胁代价取值w(n)为该点的雷达威胁度Tr。改进A*算法的代价函数f(n)为：

其中，g(n)为初始节点到该点的实际代价，h(n)为该点到目标节点的估计代价，a(n)为该点的加速度代价，w(n)为该点的威胁代价，并针对各项参数的单位以及数量级上的差异进行归一化的处理，w1、w2、w3、w4为对应的权重系数。

针对各项参数的单位以及数量级上的差异进行如下归一化的处理：

其中，n表示当前航迹节点，i表示取扩展节点集合中的一个扩展节点，L(n)i为从初始节点到扩展点i的航迹段之和，L(n)为从初始节点到所有扩展点的航迹段之和的集合，D(n)i为扩展点i到目标节点的距离，D(n)为所有扩展节点到目标节点的距离的集合，A(n)i为扩展点i的加速度值，A(n)为所有扩展点的加速度值的集合，W(n)i为扩展点i的威胁值，W(n)为所有扩展点的威胁值的集合，max 表示取集合中的最大值。

（4）计算新的起点和终点

为了满足无人机以预定角度出发和到达的端点方向约束，分别沿起点和终点的朝向角反方向计算出新的起点和终点。设起点坐标位置(x0,y0,z0)以及其朝向角分别为转弯角α1和俯仰角β1，输入终点的坐标位置(xg,yg,zg)以及其朝向角分别为转弯角α2和俯仰角β2，以最小步长lmin为单位长度，沿起点朝向角方向计算出新的起点(x1,y1,z1)，沿终点朝向角反方向计算出新的终点(xg-1,yg-1,zg-1)，方程组（12）-（13）为计算方法。

（5）双向搜索对比

将起点和终点的坐标位置和对应朝向角对调后输入，重新进行航迹规划，得到另一条规划航迹；对比所得两条航迹，选其中总航迹代价值较小的一条作为最终航迹输出。

因此，本文提出的无人机航迹规划流程如图2所示。

3 仿真结果与分析

3.1 规划区域

设定三维环境大小为 100 × 100 × 50，即MAX_X、MAX_Y、MAX_Z分别为100、100、50，建立一个飞行环境，取某区域的真实地形导入，每单位长度为100m，无人机的起点坐标（10,10,10），终点坐标（90,80,12），初始速度为100m/s，预计到达时间为150s，代价函数中w1、w2、w3、w4分别取值0.2、0.3、0.3、0.2。该实验在CPU 为Intel Core i7-4500U，RAM 为7.89GB 的计算机上实现，软件是MATLAB R2016a。规划区域已有雷达参数如表1 所示，无人机性能约束如图2 所示。

图2 无人机航迹规划流程

表1 雷达参数

表2 无人机性能约束

3.2 仿真对比

为了验证本文所提出后的方法的有效性，在上述规划的三维空间中进行航迹规划仿真实验。

（1）改进算法与稀疏A*算法比较

稀疏A*算法实验中，在起点和终点附近设置高代价区，代价函数为实际到达时间与预计到达时间的差值，其余约束条件不变，假设无人机匀速运动，仿真结果如图3。

本文所提的改进A*算法航迹规划方法仿真结果如图4。

由图3 和图4 可以看出，本文所提方法在起点和终点的朝向更满足预设条件。

图3

图4

统计两种方法的端点角度和实际到达时间与实验约束条件对比，结果如表3 所示。结果显示，本文所提出的方法更加满足端点角度约束且到达时间更接近预计到达时间。

表3 2 种算法航迹规划结果比较

（2）双向搜索结果对比

对调起点和终点进行双向搜索实验，仿真结果如图5 所示，得到的两条航迹（a）和（b）的总航迹代价值分别为77.95、82.87，因此从两条航迹中选出总航迹代价值较小的一条航迹（a）作为最终航迹输出，即为图4所示航迹。从该对比图中也可以看出，双向搜索后选出的航迹在总航迹代价值较小的前提下，规划出的航迹较平滑。

图5

3.3 分析

本文提出的方向和时间约束下的改进A*算法三维无人机航迹规划方法，能够较为精确地满足任务中对于起始和到达时方向以及飞行时间的约束，使无人机以特定角度和在预定时间到达终点，并且规划出的较优航迹。

4 结语

实际战场中环境是复杂的，任务要求是多样的，针对真实战场的复杂环境，在构建三维环境模型以及满足无人机性能约束的基础上，提出了一种能够满足方向和时间约束的基于A*算法的无人机航迹规划方法。根据朝向角计算出新的起点和终点，更满足对方向上的约束；改进稀疏A*算法，采用变权值代价函数并引入加速度代价和威胁代价，满足时间约束条件，且提高了航迹的安全性；双向搜索，选出较优航迹输出。仿真结果表明，该方法能够解决无人机在方向和时间约束下的三维航迹规划问题，并能在满足无人机性能限制的条件下有效地规避地形障碍和雷达威胁，提高作战安全性。