CN111024085B - 一种具有端点方向和时间约束的无人机航迹规划方法 - Google Patents

Abstract

本发明提出了一种具有端点方向和时间约束的无人机航迹规划方法。其步骤包括：构建环境，划分空间；输入起点和终点的坐标位置及其对应朝向角并计算出新的起点和终点；改进稀疏A*算法，在代价函数中增加加速度代价，以在规定时间到达终点为准调整节点加速度代价值，并用该算法进行航迹规划；将原始起点和终点加入航迹；双向搜索，选出较优航迹输出，完成航迹规划。本发明基于改进稀疏A*算法，能够解决无人机在复杂约束下的航迹规划问题，其中复杂约束包括以预定角度出发和到达的端点方向约束、时间约束、无人机运动学约束以及障碍威胁规避约束。

Description

一种具有端点方向和时间约束的无人机航迹规划方法

技术领域

本发明涉及一种具有端点方向和时间约束的无人机航迹规划方法，属于航迹规划领域。

背景技术

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

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

目前已有的解决具有端点方向约束的方法为在起点/终点附近设置高代价区域，但该方法并不能保证无人机一定以规定角度飞入或者飞出，只能控制在一定范围内，且会造成可行解空间的损失；目前已有的解决具有时间约束的方法为通过调整速度来满足时间维度的方法，但并未考虑速度的调整仍然需要时间，过于理想化，不符合实际情况，本发明可以解决上述问题。

稀疏A*算法在面临比较复杂的环境时，由于每一步只进行一次扩展，对于环境的感知能力是有限的，比如近似“凹”字形区域，很有可能会出现绕径，或者不是全局最优的情况，针对该情况，本发明可以解决此类问题。

发明内容

本发明的目的在于解决具有端点方向和时间约束的无人机航迹规划问题，使规划出的航迹更符合实际情况且较优。

本发明的技术方案如下：

一种具有端点方向和时间约束的无人机航迹规划方法，其特征在于，包括以下步骤：

步骤一，构建环境，划分空间；

步骤二，将要规划航迹的无人机的起点和终点的坐标位置和对应朝向角输入，以最小步长为距离，沿起点朝向角方向计算出新的起点，沿终点朝向角反方向计算出新的终点；

步骤三，利用改进稀疏A*算法进行航迹求解，在该算法的代价函数中增加加速度代价，以在规定时间到达终点为准调整加速度代价值，并求解得到航迹点集合；

步骤四，将原始起点和终点加入航迹点集合，得到一条规划航迹；

步骤五，将第二步中起点和终点的坐标位置和对应朝向角对调后输入，重复第二至第四步，得到另一条规划航迹；

步骤六，对比所得两条航迹，选其中总航迹代价较小的一条作为最终航迹输出。

上述步骤三中，改进稀疏A*算法中的扩展节点时的约束条件包括以下几点：地形约束、最小步长约束、最大航程约束、最大飞行高度约束、最小离地高度约束、雷达威胁约束以及时间约束。其中：

最小步长约束:要求无人机航迹段长度不小于某一个值，该值是无人机在改变姿态时必须直飞的一个距离，也是每次扩展节点的最小距离；

最大航程约束：由于无人机只能携带一定数量的燃油，因此无人机应该有一个最大航程距离的约束；

最大飞行高度约束：受制于无人机自身性能，需要无人机在不超过某一个高度的范围内飞行，以确保一定的安全性，应该确定最大飞行高度的约束；

最小离地高度约束：飞行高度过低的话，无人机容易与地面发生碰撞，因此确定最小离地高度约束，同样也是确保安全性；

雷达威胁约束：在敌对环境中存在着预警雷达，首先计算当前点到威胁中心的欧式距离，并将这个距离与雷达的最小半径R_min和最大半径R_max进行比较，小于最小半径的范围属于禁飞区，无人机是不能穿过的，否则被发现则此次任务失败，大于最大半径的范围是雷达检测不到的，属于安全区，而在两个半径之间的范围，雷达有一定的概率探测到无人机，这个概率与无人机距离威胁中心的远近有关，因此设定雷达威胁约束，保障无人机的安全；

时间约束：无人机在规定的时间范围到达目标点，以满足对时间维度有要求的飞行任务。

上述步骤三中，改进稀疏A*算法中的节点的代价函数f(n)为：f(n)＝c₁*g(n)+c₂*h(n)+c₃*a(n)+c₄*w(n)，其中，g(n)为初始节点到该点的实际代价，h(n)为该点到目标节点的估计代价，a(n)为该点的加速度代价，w(n)为该点的雷达的威胁代价，并针对各项参数的单位以及数量级上的差异进行归一化的处理，c₁,c₂,c₃,c₄为对应的权重系数。

上述步骤三中，改进稀疏A*算法中的节点的加速度值的计算方法：设定当前点作匀变速直线运动到扩展点后，再匀速运动到目标点，运动总时间为预计到达时间，假设当前点的速度为v₀,到飞行当前点的所花时间为t₀,当前点到扩展点的距离为d、所花时间为t₁，扩展点的加速度为a，扩展点到目标点的估计距离为s、所花时间为t₂，预计到达时间为T，则根据

求得扩展点的加速度，取其绝对值。

本发明的有益效果：

1.本发明提供的一种具有端点方向和时间约束的无人机航迹规划方法，以起点和终点的坐标位置、朝向角和最小步长计算出新的起点和终点，满足无人机的端点方向的精确约束；

2.本发明提供的一种具有端点方向和时间约束的无人机航迹规划方法，提出改进稀疏A*算法，在代价函数的设计上，考虑到在实际情况中调整速度是需要时间的，增加了加速度代价，并以在规定时间到达终点为准调整加速度代价值，满足时间维度约束，使无人机在预定时间范围到达；

3.本发明提供的一种具有端点方向和时间约束的无人机航迹规划方法，采用双向搜索的方法，能选出较优路径，改善A*算法存在的绕径问题。

附图说明

图1为一种具有端点方向和时间约束的无人机航迹规划方法流程图

图2为一种具有端点方向和时间约束的无人机航迹规划方法中改进稀疏A*算法流程图

图3为稀疏A*算法实验结果截图

图4为一种具有端点方向和时间约束的无人机航迹规划方法实验结果截图

图5为一种具有端点方向和时间约束的无人机航迹规划方法中双向搜索实验结果对比图

具体实施方式

为了使本发明实施例的目的、技术方案、优点更加清晰、明确，下面结合附图和实施案例，对本发明做进一步详细说明。

本发明提出了一种具有端点方向和时间约束的无人机航迹规划方法，其流程图如图1所示，具体步骤如下：

(1)构建环境，划分空间：加载.mat地形文件，该地形文件存储的是地形的三维坐标点，X、Y、Z轴方向的最大坐标值MAX_X、MAX_Y、MAX_Z，以及雷达的数量、坐标、最小半径R_min和最大半径R_max，并生成需要的地形曲面图，之后将空间按照无人机飞行的最小步长划分为规则的网格单元，网格单元的各顶点为空间中的节点，方格的边长为最小步长L_min，地形和雷达的边缘以及内部的点为不可到达的节点，地形和雷达外部的点为可到达的节点，不可到达的节点标记为-1，可到达的节点标记为2，所有标记按坐标存入矩阵MAP中，因此MAP矩阵为存储MAX_X×MAX_Y×MAX_Z范围内所有点的标记的三维矩阵；

(2)输入起点坐标位置(x₀,y₀,z₀)以及对应的转弯角α₁和俯仰角β₁，输入终点的坐标位置(x_g,y_g,z_g)以及对应的转弯角α₂和俯仰角β₂，沿起点朝向角方向计算出新的起点(x₁,y₁,z₁)，沿终点朝向角反方向计算出新的终点(x_g-1,y_g-1,z_g-1)；

(3)以改进稀疏A*算法进行航迹规划；

实例中采用的改进稀疏A*算法的流程图如图2所示。

实例中采用的改进稀疏A*算法中节点的代价函数f(n)计算公式为：f(n)＝c₁*g(n)+c₂*h(n)+c₃*a(n)+c₄*w(n)，其中，g(n)为初始节点到该点的实际代价，h(n)为该点到目标节点的估计代价，a(n)为该扩展点的加速度代价，w(n)为该点的雷达的威胁代价，c₁,c₂,c₃,c₄为对应的权重系数，针对各项参数的单位以及数量级上的差异进行如下归一化的处理：

其中，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表示取集合中的最大值。

实例中改进稀疏A*算法中节点的加速度值的计算方法：设定当前点作匀变速直线运动到扩展点后，再匀速运动到目标点，运动总时间为预计到达时间，假设当前点的速度为v₀,到飞行当前点的所花时间为t₀,当前点到扩展点的距离为d、所花时间为t₁，扩展点的加速度为a，扩展点到目标点的估计距离为s、所花时间为t₂，预计到达时间为T，则根据

求得扩展点的加速度，取其绝对值。

实例中改进稀疏A*算法中节点的威胁值t(n)与该点到威胁中心的距离d的关系为：

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

(4)在航迹前后加入原始起点(x₀,y₀,z₀)和终点(x_g,y_g,z_g)，返回航迹；

(5)以终点为初始节点，起点为目标节点，重复(2)至(4)；

(6)对比双向搜索后得到的两条航迹，选取总航迹代价值较小的一条为最终航迹输出。

总航迹代价值的计算方法为：将航迹中节点的代价值f(n)累加。

本实施案例分别采用稀疏A*算法和本发明的一种具有端点方向和时间约束的无人机航迹规划方法进行航迹规划。设定三维环境大小为100*100*50，即MAX_X、MAX_Y、MAX_Z分别为100、100、50，每单位长度为100m，设定雷达数量为3，位置为radar1(30,20,10)、radar2(50,50,12)、radar3(65,60,12)，最小半径和最大半径分别为R_min＝8，R_max＝10，无人机飞行的最小步长L_min为100m，最大航程为20km，最大飞行高度为4500m，最小离地高度为100m，初始速度为100m/s，预计到达时间为150s，c₁,c₂,c₃,c₄分别取值0.2、0.3、0.3、0.2，起点坐标(10,10,12)，转弯角为45°，俯仰角为30°，终点坐标(90,80,12)，转弯角为0,俯仰角为135°。该实验在CPU为Intel(R)Core(TM)i7-4500U，RAM为7.89GB的计算机上实现，软件是Matlab R2016a。

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

本发明的航迹规划方法实验结果如图4。

统计两种方法的端点角度和实际到达时间与实验约束条件对比，结果如下表1所示。

表1

	起点转弯角/度	起点俯仰角/度	终点转弯角/度	终点俯仰角/度	到达时间/s
						约束条件	45	30	0	135	150
稀疏A*	45	0	45	144.74	114.22
						本发明方法	45	35.26	0	135	152.82

表1所示结果显示，本发明所提出的方法与更加满足端点角度约束且到达时间更接近预计到达时间。

该实施案例中，双向搜索实验结果如图5所示，两条航迹的总航迹代价值分别为77.95、82.87，从得到的两条航迹中选出总航迹代价值较小的一条，作为最终航迹输出，即为图4所示航迹，结果表明通过双向搜索能选出较优的航迹。

本发明的益处在于：以起点和终点的坐标位置、朝向角和最小步长计算出新的起点和终点，满足无人机的端点方向的精确约束；提出改进稀疏A*算法，在代价函数的设计上，考虑到在实际情况中调整速度是需要时间的，增加了加速度代价，并以在规定时间到达终点为准调整加速度代价值，满足时间维度约束，使无人机在预定时间范围到达；采用双向搜索的方法，能选出较优航迹，改善A*算法存在的绕径问题。

以上所述具体实施案例只是对本发明的进一步解释和说明，并不用以限制本发明，凡在本发明的精神和原则之内所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种具有端点方向和时间约束的无人机航迹规划方法，其特征在于，包括如下步骤：

步骤一，构建环境，划分空间；

步骤二，输入将要规划航迹的无人机的起点和终点的坐标位置和对应朝向角，以最小步长为距离，沿起点朝向角方向计算出新的起点，沿终点朝向角反方向计算出新的终点；

步骤三，利用改进稀疏A*算法进行航迹求解，在该算法中的代价函数中增加加速度代价，以在规定时间到达新的终点为准调整加速度代价值，并求解得到航迹点集合；

步骤四，将原始起点和终点加入航迹点集合，得到一条规划航迹；

步骤五，将步骤二中起点和终点的坐标位置和对应朝向角对调后输入，重复步骤二至步骤四，得到另一条规划航迹；

步骤六，对比所得两条航迹，选其中总航迹代价值较小的一条作为最终航迹输出。

Images