CN110262548B

CN110262548B - 一种考虑抵达时间约束的无人机航迹规划方法

Info

Publication number: CN110262548B
Application number: CN201910541621.3A
Authority: CN
Inventors: 龙腾; 曹严; 王仰杰; 王祝
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2019-06-21
Filing date: 2019-06-21
Publication date: 2020-10-09
Anticipated expiration: 2039-06-21
Also published as: CN110262548A

Abstract

本发明涉及一种考虑抵达时间约束的无人机航迹规划方法，属于航迹规划技术领域。本发明针对无人机指定时刻抵达目标区域问题，建立抵达时间约束模型和航迹规划模型。在SAS算法思想的基础上，提出一种指定航程稀疏A*搜索(GRC‑SAS)算法，该方法通过对SAS算法的代价函数和收敛条件进行设计，使得航迹结果满足抵达时间约束，并对节点扩展方案进行改进以进一步提高算法的搜索效率，从而快速生成满足约束的无人机飞行航迹。本发明要解决的技术问题为：根据实际任务需要，基于指定航程稀疏A*搜索算法获得无人机飞行航迹，具有满足复杂约束、短时间内生成可行航迹的优点，其中，复杂约束包括抵达时间约束、无人机运动学约束和障碍规避约束。

Description

一种考虑抵达时间约束的无人机航迹规划方法

技术领域

本发明涉及一种考虑抵达时间约束的无人机航迹规划方法，属于航迹规划技术领域。

背景技术

在现代战争中，战场环境复杂，无人机执行任务时需满足抵达时间约束的需求，并规避战场环境中的禁飞区，这是最大化作战效能，成功完成任务的基础。

为了满足无人机抵达时间约束，其处理方式主要包括速度调节和航程调节两类方法。速度调节法是利用无人机速度的可调范围对无人机进行航迹规划，以满足给定的抵达时间约束。航迹规划方法主要包括图搜索算法(A*、Dijkstra、k最优路径等)、数值优化算法(蚁群优化、粒子群优化、遗传算法等)以及势场法。但由于无人机速度调节范围有限，巡航速度的调节同时会带来经济上的损失，因此该方式难以满足无人机抵达时间约束。

若无人机飞行速度固定，则抵达时间约束可转化为航程约束。航程调节法即是在固定飞行速度的条件下，直接通过调节无人机航程以满足抵达时间约束。典型的研究包括弹簧链法和盘旋等待策略。这类方法通过在无人机航迹中插入额外的机动动作来调整飞行航程，却难以考虑航迹附近禁飞区的影响。基于数值优化的航程耦合规划方法可直接将航程需求建模为等式约束或不等式约束条件。然而，建立的约束优化模型是一个复杂的高维强非线性问题，难以保证航迹规划的时效性。

发明内容

本发明公开的一种考虑抵达时间约束的无人机航迹规划方法，要解决的技术问题为：根据实际任务需要，基于指定航程稀疏A*搜索算法获得无人机飞行航迹，具有满足复杂约束、短时间内生成可行航迹的优点。复杂约束除包括抵达时间约束外，还包括无人机运动学约束和障碍规避约束。

本发明基于稀疏A*搜索(Sparse A*Search,SAS)算法进行改进定制，提出一种考虑抵达时间约束的无人机航迹规划方法，通过规划无人机的飞行航程，满足无人机抵达时间约束。稀疏A*算法引入启发信息提高了搜索效率，并能够在一定的假设条件下保证解的最优性和完备性。因此，在稀疏A*搜索算法的代价函数和收敛条件中考虑指定航程信息，改变其搜索趋势和收敛条件，是无人机航迹规划过程中满足抵达时间约束的可行方法。

本发明的目的是通过下述技术方案实现的：

本发明公开的一种考虑抵达时间约束的无人机航迹规划方法，针对无人机指定时刻抵达目标区域问题，建立抵达时间约束模型和航迹规划模型。在SAS算法思想的基础上，提出一种指定航程稀疏A*搜索(GRC-SAS)算法，该方法通过对SAS算法的代价函数和收敛条件进行设计，使得航迹结果满足抵达时间约束，并对节点扩展方案进行改进以进一步提高算法的搜索效率，从而快速生成满足约束的无人机飞行航迹。

本发明公开的一种考虑抵达时间约束的无人机航迹规划方法，包括如下步骤：

步骤一：获得无人机飞行性能参数信息、航迹约束信息和任务环境信息；所述的无人机飞行性能参数信息包括无人机飞行速度、最大转弯角和最小航迹段长度；所述的航迹约束信息包括无人机的飞行起点位置和目标点位置；所述的任务环境信息包括禁飞区的位置、半径、无人机的指定航程L^*和航迹长度的相对误差限ε_L；

步骤二：建立抵达时间约束和航迹规划数学模型；

(1)建立抵达时间约束模型

抵达时间约束要求无人机抵达目标点时间的偏差在无人机抵达时间偏差限ε_t以内；由于航迹点不包括准确的时间信息，为了满足无人机抵达时间约束，则根据飞行航程计算得到每个航迹点对应的近似时间；然后，以近似时间为基准，建立无人机的抵达时间约束模型；

无人机的飞行航迹表示为：

其中，P表示无人机的航迹点序列，n表示无人机的航迹所包含航迹点总数量；

表示航迹P中的第1个航迹点；

表示航迹P中的第2个航迹点；

表示航迹P中的第k个航迹点；

表示航迹P中的第n个航迹点；x₁,y₁表示第1个航迹点的x、y方向坐标；

表示无人机飞抵第1个航迹点的近似时刻；x₂,y₂表示第2个航迹点的x、y方向坐标；

表示无人机飞抵第2个航迹点的近似时刻；x_k,y_k表示第k个航迹点的x、y方向坐标；

表示无人机飞抵第k个航迹点的近似时刻；x_n,y_n表示第n个航迹点的x、y方向坐标；

表示无人机飞抵第n个航迹点的近似时刻；

设无人机的巡航速度为V，则无人机抵达各个航迹点的近似时间为：

其中，x_k+1,y_k+1表示第k+1个航迹点的x、y方向坐标；

表示无人机飞抵第k+1个航迹点的近似时刻；

基于式(1)，无人机的抵达时间约束表示为：

其中，t^*表示指定无人机抵达时间，ε_t表示无人机抵达时间偏差限。

由于无人机抵达航迹点的时间是以巡航速度近似计算得到的，因此无人机抵达时间约束也能够转换为无人机航程约束，即要求无人机的航迹长度误差在允许的范围内，如式(4)所示。

|L-L^*|/|L^*|≤ε_L (4)

其中，ε_L为航迹长度的相对误差限，L^*是指定航程，L为无人机的实际航迹长度，其表达式为

(2)航迹规划模型

无人机航迹规划的优化目标根据应用不同而有所不同，本发明以无人机实际飞行航程与指定航程的误差作为优化目标，如下式所示

min|L-L^*| (6)

无人机航迹规划约束不仅包括抵达时间约束，还需考虑无人机机动能力约束和禁飞区约束；所述机动能力约束包括：最小航迹段长度和最大转弯角；

最小航迹段长度约束：受机动性能限制，无人机每次改变航迹方向前，必须沿原方向飞行一段距离，即要求每一段航迹段不小于最短直飞距离l_min，最小航迹段长度约束的表达式为：

其中，l_k为无人机第k段航迹的长度，其表达式如下所示

最大转弯角约束：受无人机机动能力的约束，规划的航迹需要避免过大的转弯角，以保证航迹可行。设无人机的最大转弯角为Δχ_max，则要求

其中，Δχ_k为无人机在第k个航迹点处的转弯角。

禁飞区约束：无人机飞行过程中，需对环境中的禁飞区进行规避，即要求无人机的航迹不与禁飞区相交，表示为

其中，dis_j表示无人机的航迹与禁飞区j之间的最小距离，n_NFZ为禁飞区的数量。

步骤三：通过指定航程稀疏A*搜索GRC-SAS算法对无人机进行抵达时间约束的航迹规划。

步骤1)：初始化算法中的OPEN表和CLOSED表。创建OPEN表和CLOSED表，同时将规划的起点插入OPEN表，此时CLOSED表为空。

步骤2)：大步长预先采点。以设置步长N倍的值作为大步长，从起点开始进行节点扩展，对大步长扩展得到的节点不进行收敛条件判断，但需进行约束检测，然后将扩展得到的可行节点全部放入OPEN表中。

所述N为3～5；

步骤3)：判断OPEN表是否为空。若OPEN表为空，则结束搜索；若OPEN表非空，则执行步骤4)。

步骤4)：更新当前节点。从当前的OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从OPEN表中删除，并放入CLOSED表。

节点代价值计算的具体实现如下：

GRC-SAS算法不再以最小化航迹长度为目标，而是以最小化航迹长度与指定航程的差值为目标，因此节点的代价函数f(k)表示为

其中，

为从起点经过节点k并到达目标点的估计航迹长度，其表达式为

其中，L_g(k)为无人机从起点到节点k的真实航迹长度，L_h(k)为从节点k到目标点的估计航迹长度，w_h为估计航迹长度的比例系数。

步骤5)：判断当前节点能否满足收敛条件。若当前节点能够在满足所有约束的条件下到达目标节点，则结束节点扩展循环，转而执行步骤7)；否则执行步骤6)。

收敛条件为当前节点直线飞抵目标点时，在节点可行性的前提下，整条航迹满足指定航程约束，即将起点到当前节点的真实航迹长度与当前节点直线到达目标点的航迹长度之和与指定航程进行比较，比较值小于给定的ε_L。

步骤6)：节点扩展与储存。以当前节点为中心进行节点扩展，获得当前节点的子节点。判断扩展子节点的可行性，计算所有可行节点的代价值，并将可行节点存入OPEN表中。然后执行步骤3)。

所述节点扩展的方法为：采用GRC-SAS算法进行二维航迹规划时，节点扩展仅需在水平面内进行。因此，节点扩展包含平飞和转弯两种情况。平飞扩展对应于零转弯角飞行，即沿当前节点的速度方向，继续飞行一个步长得到一个子节点。转弯扩展包括左转弯和右转弯两组扩展节点。

设左转的扩展节点数量为m_L，则以当前节点为中心点，扩展步长为线段长度，分别以{Δχ_max/m_L,2Δχ_max/m_L,...,Δχ_max}为转弯角度，计算得到左转弯所对应的扩展节点。

设右转的扩展节点数量为m_R，则以当前节点为中心点，扩展步长为线段长度，分别以{Δχ_max/m_R,2Δχ_max/m_R,...,Δχ_max}为转弯角度，计算得到右转弯所对应的扩展节点。

通过上述节点扩展，得到当前节点的m_L+m_R+1个子节点。m_L和m_R的值越大，GRC-SAS算法在空间搜索过程中扩展出的节点越多，获得可行航迹的概率越大，但算法的内存消耗和搜索时间也随之增加。

所述节点可行性的判断方法为：考虑禁飞区约束，依次对新扩展子节点进行约束检验。由于扩展过程中已经保证了从起点到当前节点的航迹可行性，因此仅需检测当前节点到扩展子节点的航迹段的可行性即可。对于不满足约束的新扩展节点，直接舍弃。而对于满足约束的新扩展可行节点，判断其是否与OPEN表中的已有节点重复。若不存在重复节点，则计算所有可行节点代价值后，将所有可行节点放入OPEN表中；若存在重复节点，则仅保留代价值较小的节点。

步骤7)：创建目标节点，其父节点设置为当前节点，并将目标节点压入CLOSED表。

步骤8)：反溯最终规划航迹。根据目标节点和CLOSED表中的已扩展节点，利用节点间的扩展关系，从目标节点向上回溯直至起始节点，得到从起始点到目标点的航迹，该航迹即为满足抵达时间约束的无人机可行航迹。

有益效果

1、本发明公开的一种考虑抵达时间约束的无人机航迹规划方法，针对抵达时间约束的航迹规划问题，建立无人机运动学模型、抵达时间约束模型和航迹规划模型。在SAS算法思想的基础上，提出一种指定航程稀疏A*搜索(GRC-SAS)算法，该方法通过对搜索代价函数和收敛条件进行设计，使得航迹结果满足抵达时间约束，并实现对禁飞区的规避。

2、本发明公开的一种考虑抵达时间约束的无人机航迹规划方法，对算法节点扩展方案进行改进，引入大步长扩展思想，缓解SAS算法前期易陷入局部搜索而难以收敛的问题，进一步提高了GRC-SAS算法的搜索效率。

附图说明

图1为GRC-SAS算法流程图；

图2为GRC-SAS算法节点扩展示意图；

图3为抵达时间约束的二维航迹规划结果。

具体实施方式

为了更好的说明本发明的目的与优点，下面通过无人机航迹规划实例，结合附图与表格对本发明做进一步说明。

实施例1：

仿真硬件为Intel Core i5-6200 CPU 2.30GHz，8G内存，仿真环境为MATLABR2016b。无人机编队在10km×10km的二维环境中执行任务。航迹规划要求无人机从起始点抵达目标点过程中规避环境中的禁飞区。

本实施例公开的一种考虑抵达时间约束的无人机航迹规划方法，具体实现步骤如下：

步骤一：获得无人机飞行性能参数信息、航迹约束信息和任务环境信息。

设定无人机的飞行速度为35m/s，最大转弯角为90°，最小航迹段长度l_min＝1km。无人机的飞行起点/终点位置和任务环境中禁飞区的位置、半径如表1所列。无人机的指定航程为9.60km，航迹长度的相对误差限ε_L＝2％。

表1无人机和禁飞区信息

无人机信息	起点(km)	终点(km)	禁飞区信息	位置(km)	半径(km)
						无人机1	(3.72,1.38,0.35)	(8,4,0.055)	禁飞区1	[4.6,2.1]	0.65
无人机2	(3.28,3.47,0.47)	(8,4,0.055)	禁飞区2	[3.0,5.1]	0.70
						无人机3	(3.61,5.80,0.45)	(8,4,0.055)	禁飞区3	[7.2,3.1]	0.60
无人机4	(4.03,9.25,0.42)	(8,4,0.055)	禁飞区4	[7.0,7.5]	0.70
									禁飞区5	[8.0,7.3]	0.91
			禁飞区6	[6.9,5.4]	1.11

步骤二：根据上述具体实例的参数输入，建立抵达时间约束和航迹规划的数学模型，如式(13)-(22)所示。

(1)抵达时间约束模型

L-L^*/L^*≤2％ (16)

(2)航迹规划模型

无人机航迹规划的优化目标函数为

min|L-L^*| (18)

最小航迹段长度约束：

最大转弯角约束：

禁飞区约束：

步骤三：通过指定航程稀疏A*搜索(GRC-SAS)算法对四架无人机依次进行抵达时间约束的航迹规划，算法流程图如图1所示。

步骤四：初始化OPEN表和CLOSED表。创建OPEN表和CLOSED表，同时将规划的起点插入OPEN表，此时CLOSED表为空。

步骤五：大步长预先采点。以设置步长N倍的值作为大步长，从起点开始进行节点扩展，对大步长扩展得到的节点不进行收敛条件判断，但需进行约束检测，然后将扩展得到的可行节点全部放入OPEN表中。

所述N为3～5；

步骤六：判断OPEN表是否为空。若OPEN表为空，则结束搜索；若OPEN表非空，则执行步骤七。

步骤七：更新当前节点。从当前的OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从OPEN表中删除，并放入CLOSED表。

GRC-SAS算法是最小化航迹长度与指定航程的差值，因此节点的代价函数f(k)可表示为

其中，

表达式为

步骤八：判断当前节点能否满足收敛条件。若当前节点能够在满足所有约束的条件下到达目标节点，则结束节点扩展循环，转而执行步骤十；若不收敛，则执行步骤九。

步骤九：节点扩展与储存。以当前节点为中心进行节点扩展，获得当前节点的子节点，节点扩展示意图如图2所示。判断扩展子节点的可行性，计算所有可行节点的代价值，并将所有可行节点存入OPEN表中。然后执行步骤六。

所述节点扩展的方法为：设左转和右转的扩展节点数量都为2，则以当前节点为中心点，扩展步长为线段长度，分别以{90°/2,90°}为转弯角度，可计算得到左转弯和右转弯所对应的两组扩展节点。通过上述节点扩展，可得到当前节点的5个子节点。

所述节点可行性的判断方法为：节点可行性判断。考虑禁飞区约束，依次对新扩展的5个子节点进行约束检验，仅需检测当前节点到扩展子节点的航迹段的可行性即可。对于不满足约束的新扩展节点，直接舍弃。而对于满足约束的新扩展节点，判断其是否与OPEN表中的已有节点重复。若不存在重复节点，则计算其代价值后放入OPEN表中；若存在重复节点，则仅保留代价值较小的节点。

步骤十：创建目标节点，其父节点设置为当前节点，并将目标节点压入CLOSED表。

步骤十一：反溯最终规划航迹。根据目标节点和CLOSED表中的已扩展节点，利用节点间的扩展关系，从目标节点向上回溯直至起始节点，得到从起始点到目标点的航迹，该航迹即为满足抵达时间约束的无人机可行航迹。

抵达时间约束航迹规划结果如图3所示。图中各无人机的航迹长度分别为9.41km，9.41km，9.67km和9.60km。其中航程相对误差的最大值为1.98％，小于航迹长度的相对误差限。因此，航迹规划结果满足无人机抵达时间约束，且实现了对禁飞区的规避。

另外，航迹规划对算法具有严格的时效性要求。为检验本发明提出的指定航程航迹规划方法的效率，对上述航迹规划的时间进行统计，结果如表2所示。

表2抵达时间约束无人机航迹规划时间统计

	无人机1	无人机2	无人机3	无人机4
					抵达时间约束航迹规划耗时(s)	0.0544	0.0198	0.1774	0.0347

无人机的抵达时间约束航迹规划耗时均在0.2s以内，规划耗时较短，能够满足无人机航迹规划的时效性需求。

根据前述的无人机航迹规划实例仿真结果与分析可见，本实施例所述的指定航程航迹规划方法能够为无人机提供满足抵达时间约束的可行航迹，航迹生成速度具有较高的效率，因此本发明具有很强的工程实用性，并且能够实现预期的发明目的。

以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种考虑抵达时间约束的无人机航迹规划方法，其特征在于：包括如下步骤：

步骤二：建立抵达时间约束和航迹规划数学模型；

(1)建立抵达时间约束模型

无人机的飞行航迹表示为：

表示航迹P中的第1个航迹点；

表示航迹P中的第2个航迹点；

表示航迹P中的第k个航迹点；

表示航迹P中的第n个航迹点；x₁，y₁表示第1个航迹点的x、y方向坐标；

表示无人机飞抵第1个航迹点的近似时刻；x₂，y₂表示第2个航迹点的x、y方向坐标；

表示无人机飞抵第2个航迹点的近似时刻；x_k，y_k表示第k个航迹点的x、y方向坐标；

表示无人机飞抵第k个航迹点的近似时刻；x_n，y_n表示第n个航迹点的x、y方向坐标；

表示无人机飞抵第n个航迹点的近似时刻；

其中，x_k+1，y_k+1表示第k+1个航迹点的x、y方向坐标；

表示无人机飞抵第k+1个航迹点的近似时刻；

基于式(1)，无人机的抵达时间约束表示为：

其中，t^*表示指定无人机抵达时间，ε_t表示无人机抵达时间偏差限；

由于无人机抵达航迹点的时间是以巡航速度近似计算得到的，因此无人机抵达时间约束也能够转换为无人机航程约束，即要求无人机的航迹长度误差在允许的范围内，如式(4)所示；

|L-L^*|/|L^*|≤ε_L (4)

(2)建立航迹规划模型

以无人机实际飞行航程与指定航程的误差作为优化目标，如下式所示

min|L-L^*| (6)

无人机航迹规划约束不仅包括抵达时间约束，还需考虑无人机机动性能的约束和禁飞区约束；所述机动能力约束包括：最小航迹段长度和最大转弯角；

其中，l_k为无人机第k段航迹的长度，其表达式如下所示

最大转弯角约束：受无人机机动性能的约束，规划的航迹需要避免过大的转弯角，以保证航迹可行；设无人机的最大转弯角为Δχ_max，则要求

其中，Δχ_k为无人机在第k个航迹点处的转弯角；

其中，dis_j表示无人机的航迹与禁飞区j之间的最小距离，n_NFZ为禁飞区的数量；

步骤三：通过指定航程稀疏A*搜索GRC-SAS算法，对无人机进行抵达时间约束的航迹规划；

步骤1)：初始化GRC-SAS算法中的OPEN表和CLOSED表；创建OPEN表和CLOSED表，同时将规划的起点插入OPEN表，此时CLOSED表为空；

步骤2)：大步长预先采点；以设置步长N倍的值作为大步长，从起点开始进行节点扩展，对大步长扩展得到的节点不进行收敛条件判断，但需进行约束检测，然后将扩展得到的可行节点全部放入OPEN表中；

步骤3)：判断OPEN表是否为空；若OPEN表为空，则结束搜索；若OPEN表非空，则执行步骤4)；

步骤4)：更新当前节点；从当前的OPEN表中取出代价值最小的节点作为新的当前节点，将当前节点从OPEN表中删除，并放入CLOSED表；

节点代价值计算的具体实现如下：

GRC-SAS算法是以最小化航迹长度与指定航程的差值为目标，因此节点的代价函数f(k)表示为

其中，

其中，L_g(k)为无人机从起点到节点k的真实航迹长度，L_h(k)为从节点k到目标点的估计航迹长度，w_h为估计航迹长度的比例系数；

步骤5)：判断当前节点能否满足收敛条件；若当前节点能够在满足所有约束的条件下到达目标节点，则结束节点扩展循环，转而执行步骤7)；否则执行步骤6)；

收敛条件为当前节点直线飞抵目标点时，在节点可行性的前提下，整条航迹满足指定航程约束，即将起点到当前节点的真实航迹长度与当前节点直线到达目标点的航迹长度之和与指定航程进行比较，比较值小于给定的ε_L；

步骤6)：节点扩展与储存；以当前节点为中心进行节点扩展，获得当前节点的子节点；判断扩展子节点的可行性，计算所有可行节点的代价值，并将可行节点存入OPEN表中；然后执行步骤3)；

所述节点扩展的方法为：采用GRC-SAS算法进行二维航迹规划时，节点扩展仅需在水平面内进行；因此，节点扩展包含平飞和转弯两种情况；平飞扩展对应于零转弯角飞行，即沿当前节点的速度方向，继续飞行一个步长得到一个子节点；转弯扩展包括左转弯和右转弯两组扩展节点；

设左转的扩展节点数量为m_L，则以当前节点为中心点，扩展步长为线段长度，分别以{Δχ_max/m_L，2Δχ_max/m_L，...，Δχ_max}为转弯角度，计算得到左转弯所对应的扩展节点；

设右转的扩展节点数量为m_R，则以当前节点为中心点，扩展步长为线段长度，分别以{Δχ_max/m_R，2Δχ_max/m_R，...，Δχ_max}为转弯角度，计算得到右转弯所对应的扩展节点；

通过上述节点扩展，得到当前节点的m_L+m_R+1个子节点；

所述节点可行性的判断方法为：考虑禁飞区约束，依次对新扩展子节点进行约束检验；由于扩展过程中已经保证了从起点到当前节点的航迹可行性，因此仅需检测当前节点到扩展子节点的航迹段的可行性即可；对于不满足约束的新扩展节点，直接舍弃；而对于满足约束的新扩展可行节点，判断其是否与OPEN表中的已有节点重复；若不存在重复节点，则计算所有可行节点代价值后，将所有可行节点放入OPEN表中；若存在重复节点，则仅保留代价值较小的节点；

步骤7)：创建目标节点，目标节点的父节点设置为当前节点，并将目标节点压入CLOSED表；

步骤8)：反溯最终规划航迹；根据目标节点和CLOSED表中的已扩展节点，利用节点间的扩展关系，从目标节点向上回溯直至起始节点，得到从起始点到目标点的航迹，该航迹即为满足抵达时间约束的无人机可行航迹。

2.如权利要求1所述的一种考虑抵达时间约束的无人机航迹规划方法，其特征在于：所述N为3～5。