CN115933736A - 一种航迹快速规划精准控制飞行器的方法 - Google Patents

Abstract

一种航迹快速规划精准控制飞行器的方法，建立了完整的仅存在定位信息限制的飞行器航迹路线模型，确定飞行器的可行集、提出信息素的更新法则、启发函数，得出基于最大优化在飞行器即时转弯中的航迹长度和校正次数得解。考虑飞行器在转弯时会受到结构和控制系统的限制，无法完成即时转弯，并存在一转弯半径，继而在飞行器航迹校正中引入切线圆轨迹模型，在保证相邻节点间最小化航迹路线长度的情况下以最小化校正次数，得到在飞行器即时转弯中成功以最短距离到达终点的方法。

Description

一种航迹快速规划精准控制飞行器的方法

技术领域

本发明涉及飞行器航迹规划技术领域，具体涉及一种航迹快速规划精准控制飞行器的方法。

背景技术

智能飞行器因具有起降简单、操作灵活、自动控制智能化、系统配置多样化等特点，在农业灌溉、地理测绘、气象预报、安全报警以及军事作战等生产生活多个领域有着广泛的应用。智能飞行器在现实生活中扮演着重要的角色，未来的发展方向也更加趋于智能化、自动化。

但是，这类飞行器由于自身结构的限制，更多依靠人力对其飞行轨迹进行操纵控制，无法自发性根据定位系统对自身进行全方位的精准定位，当误差积累到一定程度可能产生严重后果。因此，飞行器能否依据已知飞行的位置信息对航迹作出快速而准确的规划是飞行器脱离人力控制、迈向智能化的一项重点任务，也是国内外着力攻克的难点之一，更是智能飞行器能被准确控制的基本保障。

发明内容

本发明为了克服以上技术的不足，提供了一种实现飞行器在已知全部位置坐标等信息的前提下，在最短时间内自行完成航迹规划、位置坐标校正等任务的方法，该方法脱离人为控制，以实现智能化、自动化、快速化航迹规划功能。

本发明克服其技术问题所采用的技术方案是：

一种航迹快速规划精准控制飞行器的方法，其特征在于，包括如下步骤：

a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划；

b)计算得到航迹模型

得到最优航迹；

c)计算得到航迹模型

得到飞行器转弯受结构和控制系统的限制下的航迹规划。

进一步的，步骤a)包括如下步骤：

a-1)飞行器从A地到B地飞行时，飞行器飞行过程中实时定位约束为：ε^h＜θ且ε^v＜θ，其中ε^h为水平误差，ε^v为垂直误差；

a-2)如果飞行器从A地到B地的路径为直线，则无需校正，如果飞行器从A地到B地的路径不为直线，则飞行器采取直线行驶到最近一个校正点的方式校正飞行器误差，飞行器从从A地到B地的路径由多个节点连成的线段组成，将飞行器的路径简化为一个有向图，每一个校正点均为有向图中的一个点，ω_ij为有向图中是否存在一条由点i到点j的线路，当ω_ij＝0时表示路径规划中飞行器不可由点i飞向点j，当ω_ij＝1时表示路径规划中飞行器可由点i飞向点j；

a-3)飞行器从节点i到节点j时向量ij与单位向量iB的夹角θ满足

飞行器满足约束条件为：

其中

为飞行器在节点i处的水平误差，

为飞行器在节点i处的垂直误差，L_ij为节点i与节点j之间的距离，

X_i为节点i在三维坐标系中X轴坐标，X_j为节点j在三维坐标系中X轴坐标，Y_i为节点i在三维坐标系中Y轴坐标，Y_j为节点j在三维坐标系中Y轴坐标，Z_i为节点i在三维坐标系中Z轴坐标，Z_j为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，α₁为飞行中垂直方向误差的上限值，α₂为飞行中垂直方向误差的下限值，β₁为飞行中水平方向误差的上限值，β₂为飞行中水平方向误差的下限值；

a-4)对有向图上的任意一节点i，在

且

时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：

进一步的，步骤b)包括如下步骤：

b-1)通过公式

建立航迹模型

其中L_min为飞行器从A地到B地的最短路径，V为权重系数，Ω为飞行器航迹，

n为经过的校正点的数量，n_min为需要校正点的最少个数，

d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离，

N为所有节点的总数，

d_ij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型

满足约束条件为：

为飞行器在节点j处的水平误差，

为飞行器在节点j处的垂直误差，

为飞行器在节点i到节点j方向上的垂直误差，

为飞行器在节点i到节点j方向上的水平误差；

b-2)当飞行器从校正点i到达下一个校正点j时通过

确定集合J，在距终点B点从0到

的范围内，

时，集合J中有一个点j′，L_iB校正点i与点B之间的距离，

X_B为点B在三维坐标系中X轴坐标，Y_B为点B在三维坐标系中Y轴坐标，Z_B为点B在三维坐标系中Z轴坐标，点j′为使{L_ij+L_iB}最小的点；

b-3)飞行器从起点A飞到终点B得到一条航迹，通过公式τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τ_ij(t+1)，式中ρ为信息素更新系数，τ_ij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτ_ij(t)为信息素增量，

m为飞行器的总个数，

L_k(t)为经过t轮信息素更新后，各节点间距离之和，n_k(_t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τ_ij(t+1)最高的航迹作为最优航迹；

b-4)通过公式

计算得到启发式函数H_ij(t)，式中L_jB校正点j与点B之间的距离，

X_j为校正点j在三维坐标系中X轴坐标，Y_j为校正点j在三维坐标系中Y轴坐标，Z_j为校正点j在三维坐标系中Z轴坐标，

为威胁代价函数，式中f为可行点阈值，n_j为飞行器从节点i到节点j后，设在节点j的可行点个数，

b-5)通过公式

计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率

进一步的，步骤c)包括如下步骤：

c-1)飞行器到达节点i时的前进方向为向量v₁的方向，将飞行器经过节点i且方向为v₁的直线固定，利用节点j的坐标在三维空间中确定一平面ijv₁，以节点i为切点，以v₁为切线，在平面ijv₁内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v₁之间的夹角为γ；

c-2)当

时，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径；

c-3)当

时，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径；

c-4)v₁＝(a₁,b₁,c₁)，a₁为向量v₁在三维坐标系中X轴坐标，b₁为向量v₁在三维坐标系中Y轴坐标，c₁为向量v₁在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v₂的方向，v₂＝(a₂,b₂,c₂)，a₂为向量v₂在三维坐标系中X轴坐标，b₂为向量v₂在三维坐标系中Y轴坐标，c₂为向量v₂在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量v_ij的方向，v_ij＝(a_ij,b_ij,c_ij)，a_ij为向量v_ij在三维坐标系中X轴坐标，b_ij为向量v_ij在三维坐标系中Y轴坐标，c_ij为向量v_ij在三维坐标系中Z轴坐标，向量v₁、向量v₂、向量v_ij共面，行列式

通过公式

计算得到向量v₂；

c-5)计算得到节点i与节点j之间的距离L_ij后通过公式

建立航迹模型

航迹模型

满足约束条件为：

通过对航迹模型

的求解，得到飞行器在已转转弯半径R从节点i到节点j的最短路径。本发明的有益效果是：过递进计算方法，首先建立了完整的仅存在定位信息限制的飞行器航迹路线模型，确定飞行器的可行集、提出信息素的更新法则、启发函数的设计方案，在此基础上得出基于最大优化在飞行器即时转弯中的航迹长度和校正次数得解。之后考虑飞行器在转弯时会受到结构和控制系统的限制，无法完成即时转弯，并存在一转弯半径，继而在飞行器航迹校正中引入切线圆轨迹模型，在保证相邻节点间最小化航迹路线长度的情况下以最小化校正次数，得到在飞行器即时转弯中成功以最短距离到达终点的方法。

通过如上计算设计的控制器，可成功实现飞行器在已知全部位置坐标等信息的前提下，在最短时间内自行完成航迹规划、位置坐标校正等任务的功能。

附图说明

图1为飞行器可行路径图；

图2为飞行器在i点的航向与j点为锐角的最优航迹；

图3为飞行器在i点的航向与j点为钝角的最优轨迹。

具体实施方式

下面结合附图1、附图2、附图3对本发明做进一步说明。

本发明的航迹快速规划精准控制飞行器的方法，对现实环境的抽象化模型处理：1.所有位置的定位信息是已知的且固定不变的，不受天气等不可控因素干扰。飞行器在飞行途径每一个位置坐标点时会对垂直方向信息和水平方向信息进行校正，已改变航向，确保按照最优航线航行。这些需要校正的点我们称之为校正点，并在水平和垂直方向拆分为水平校正点和垂直校正点。飞行器每飞行1m，由于自身定位仪器精度不足，在校正过程中垂直误差和水平误差将各增加Δθ个专用单位(以下简称单位)。到达终点时垂直误差和水平误差均应小于θ个单位，根据现实情况，可假设当垂直误差和水平误差均小于θ单位时，飞行器仍能够按照规划路径飞行。

2.飞行器从任意一点A到点B飞行是一个连续过程。

3.想情况下不考虑飞机的功耗问题。

具体的，包括如下步骤：

a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划，飞行器在前进时方向可以任意改变。

b)计算得到航迹模型

得到最优航迹。

c)计算得到航迹模型

得到飞行器转弯受结构和控制系统的限制下的航迹规划，飞行器的最小转弯半径是已知的。

实施例1：

进一步的，步骤a)包括如下步骤：

1.设置飞行器飞行过程中实时定位约束：

飞行器的垂直误差和水平误差均应小于θ个单位，可以按照规划路径正常飞行，并且飞行器到达终点时垂直误差和水平误差均应小于θ个单位，对定位误差有如下约束：ε^h＜θ且ε^v＜θ，其中ε^h为水平误差，ε^v为垂直误差。

2.简化飞行器飞行路径：

若从任意一点到另一点的路径为直线，无需校正；否则，校正飞行器误差。飞行器采取直线驶向最近一个校正点的方式。将所经过的校正点作为节点，飞行器从A点到B点的整个路径可以看作由多个节点连城的线段所组成。将飞行器的路径简化为一个有向图，每一个校正点都为路径中的一个点，用ω_ij判断飞行器能否从点i飞向点j并在点j校正(即有向图中是否存在一条由i到j的线路)，当ω_ij＝0时表示路径规划中飞行器不可由点i飞向点j，当ω_ij＝1时表示路径规划中飞行器可由点i飞向点j。

3.引入飞行器在校正点处误差校正约束和飞行方向约束：

飞行器从节点i能否驶到节点j，与i点、j点之间的距离L_ij和飞行器飞行中自身产生的误差有关。规定向量ij与单位向量iB的夹角θ必须小于90度，即

即ij·iB＞0。飞行器满足约束条件为：

其中

为飞行器在节点i处的水平误差，

为飞行器在节点i处的垂直误差，L_ij为节点i与节点j之间的距离，

X_i为节点i在三维坐标系中X轴坐标，X_j为节点j在三维坐标系中X轴坐标，Y_i为节点i在三维坐标系中Y轴坐标，Y_j为节点j在三维坐标系中Y轴坐标，Z_i为节点i在三维坐标系中Z轴坐标，Z_j为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，由飞行器硬件决定，α₁为飞行中垂直方向误差的上限值，α₂为飞行中垂直方向误差的下限值，大于此误差飞行器将报错不可维持继续飞行，β₁为飞行中水平方向误差的上限值，β₂为飞行中水平方向误差的下限值，大于此误差飞行器将报错不可维持继续飞行。

4.确定所有可行点范围约束：

利用上面的约束条件，对有向图上的任意一节点i，在

且

时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：

利用该约束条件作出节点i与最大可能到达点集合J的连接情况，确定所有可行点的范围。

实施例2：

步骤b)包括如下步骤：

把飞行器经过校正区域校正次数降到最少，即求min(L,n)，其中L为航迹长度，n为经过的校正点的数量，V为引入的权重系数，具体步骤为：

1.对目标进行归一化处理：

飞行器从出发点A到终点B的最短路径为L_min，需要校正点的最少个数为n_min，L_min用L_AB代替，即令L_AB＝L_min，令

d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离。取

将目标函数可以改写为：

得到航迹模型

其中Ω为飞行器航迹，

N为所有节点的总数，

d_ij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型

满足约束条件为：

为飞行器在节点j处的水平误差，

为飞行器在节点j处的垂直误差，

为飞行器在节点i到节点j方向上的垂直误差，

为飞行器在节点i到节点j方向上的水平误差。

2.根据航迹模型

得到如下求解算法：

(1)确定校正点i的下一可达点集合J(可行集)：当飞行器到达下一个校正点j时通过

确定集合J。终点处对于误差的要求与水平校正点、垂直校正点处不同，在靠近终点时采取不同设计：因为终点处对于误差的要求为θ，在距终点B点从0到

的范围内，飞行器只需要分别进行一次水平校正和垂直校正即可到达终点，即当

时，可行集J中只有一个点j′，其中j′为上述使{L_ij+L_iB}最小的点，得出L_iB两种不同范围下的可行集，L_iB校正点i与点B之间的距离，

X_B为点B在三维坐标系中X轴坐标，Y_B为点B在三维坐标系中Y轴坐标，Z_B为点B在三维坐标系中Z轴坐标。

(2)更新信息素并记录信息素浓度：

在三维空间内进行路径规划，采用全局信息素更新，即一个飞行器从起点A飞到终点B得到一条航迹，以该航迹的长度和经过校正区域校正的次数作为评价来判断信息素的多少，然后从集合航迹中选择最优航迹，对该航迹上各点的信息素进行更新，完成第t次循环，飞行器在第t轮遍历集合J所含可行点的过程中，成功到达终点，此轮遍历路径上任意两点i和j之间路径的遍历次数的多少就是信息素的浓度。更新法则如下：

飞行器从起点A飞到终点B得到一条航迹，通过公式τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τ_ij(t+1)，式中ρ为信息素更新系数，τ_ij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτ_ij(t)为信息素增量，

m为飞行器的总个数，

L_k(t)为经过t轮信息素更新后，各节点间距离之和，n_k(_t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τ_ij(t+1)最高的航迹作为最优航迹。

(3)设计启发式函数，启发式函数H_ij(t)表示在t时刻，飞行器从节点i飞向终点B的期望度，其如附图1所示。

启发式函数第一项选择最短路径，由节点i到终点B的最短路径为L_iB，当经过节点j时最短路径为L_iB+L_ij，以

作为启发函数的第一项。启发函数的第二项引入校正点数量，从节点i到终点B出发，校正点数用

近似，从节点i到节点j再到终点B，所需要的校正点数用

近似，以

作为启发函数的第二项，得到启发式函数

引入威胁代价函数，消除无法到达终点的情况，令j≠B，从节点i到节点j后，设在节点j的可行点个数为n_j，可行点阈值为f，威胁代价函数表示为：

当f＝1时，可表示为：

最终得到引述威胁代价函数后的启发式函数为：

其中min(nj,1)的取值为0或1。

(4)计算转移概率：

具体的，通过公式

计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率

实施例3：

步骤c)包括如下步骤：

用L_ij来表示从节点节点i到节点j飞行器的轨迹长度，从节点i到节点j的轨迹不再是一条直线，引入CLC路径规划模式来计算和优化L_ij。这里C代表圆，L代表直线，并对飞行器在到达点的前进方向没有要求。

飞行器在起点i的方向固定，到达节点j的方向任意，如附图2所示和附图3所示，飞行器到达节点i时的前进方向为向量v₁的方向，将飞行器经过节点i且方向为v₁的直线固定，利用节点j的坐标在三维空间中确定一平面ijv₁，以节点i为切点，以v₁为切线，在平面ijv₁内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v₁之间的夹角为γ；

当

时，由余弦定理，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径。

c-3)当

时，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径；

下面要确定向量v₂的方向：

具体的，v₁＝(a₁,b₁,c₁)，a₁为向量v₁在三维坐标系中X轴坐标，b₁为向量v₁在三维坐标系中Y轴坐标，c₁为向量v₁在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v₂的方向，v₂＝(a₂,b₂,c₂)，a₂为向量v₂在三维坐标系中X轴坐标，b₂为向量v₂在三维坐标系中Y轴坐标，c₂为向量v₂在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量v_ij的方向，v_ij＝(a_ij,b_ij,c_ij)，a_ij为向量v_ij在三维坐标系中X轴坐标，b_ij为向量v_ij在三维坐标系中Y轴坐标，c_ij为向量v_ij在三维坐标系中Z轴坐标，向量v₁、向量v₂、向量v_ij共面，向量v₂可由二者线性表示，且有行列式

通过公式

计算得到向量v₂；

c-5)计算得到节点i与节点j之间的距离L_ij后通过公式

建立航迹模型

航迹模型

满足约束条件为：

通过对航迹模型

的求解，即可实现飞行器在已知转弯半径(即飞行器最小转弯半径为R)，从一个点i(起点或校正点)飞向另一个点j(校正点或终点)的最优航迹规划功能，并且点i到点j为最短路径。。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种航迹快速规划精准控制飞行器的方法，其特征在于，包括如下步骤：

a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划；

b)计算得到航迹模型

得到最优航迹；

c)计算得到航迹模型

得到飞行器转弯受结构和控制系统的限制下的航迹规划。

2.根据权利要求1所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤a)包括如下步骤：

a-1)飞行器从A地到B地飞行时，飞行器飞行过程中实时定位约束为：ε^h＜θ且ε^v＜θ，其中ε^h为水平误差，ε^v为垂直误差；

a-2)如果飞行器从A地到B地的路径为直线，则无需校正，如果飞行器从A地到B地的路径不为直线，则飞行器采取直线行驶到最近一个校正点的方式校正飞行器误差，飞行器从从A地到B地的路径由多个节点连成的线段组成，将飞行器的路径简化为一个有向图，每一个校正点均为有向图中的一个点，ω_ij为有向图中是否存在一条由点i到点j的线路，当ω_ij＝0时表示路径规划中飞行器不可由点i飞向点j，当ω_ij＝1时表示路径规划中飞行器可由点i飞向点j；

a-3)飞行器从节点i到节点j时向量ij与单位向量iB的夹角θ满足

飞行器满足约束条件为：

其中

为飞行器在节点i处的水平误差，

为飞行器在节点i处的垂直误差，L_ij为节点i与节点j之间的距离，

X_i为节点i在三维坐标系中X轴坐标，X_j为节点j在三维坐标系中X轴坐标，Y_i为节点i在三维坐标系中Y轴坐标，Y_j为节点j在三维坐标系中Y轴坐标，Z_i为节点i在三维坐标系中Z轴坐标，Z_j为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，α₁为飞行中垂直方向误差的上限值，α₂为飞行中垂直方向误差的下限值，β₁为飞行中水平方向误差的上限值，β₂为飞行中水平方向误差的下限值；

a-4)对有向图上的任意一节点i，在

且

时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：

3.根据权利要求2所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤b)包括如下步骤：

b-1)通过公式

建立航迹模型

其中L_min为飞行器从A地到B地的最短路径，V为权重系数，Ω为飞行器航迹，

n为经过的校正点的数量，n_min为需要校正点的最少个数，

d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离，

N为所有节点的总数，

d_ij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型

满足约束条件为：

为飞行器在节点j处的水平误差，

为飞行器在节点j处的垂直误差，

为飞行器在节点i到节点j方向上的垂直误差，

为飞行器在节点i到节点j方向上的水平误差；

b-2)当飞行器从校正点i到达下一个校正点j时通过

确定集合J，在距终点B点从0到

的范围内，

时，集合J中有一个点j′，L_iB校正点i与点B之间的距离，

X_B为点B在三维坐标系中X轴坐标，Y_B为点B在三维坐标系中Y轴坐标，Z_B为点B在三维坐标系中Z轴坐标，点j′为使{L_ij+L_iB}最小的点；

b-3)飞行器从起点A飞到终点B得到一条航迹，通过公式τ_ij(t+1)＝(1-ρ)τ_ij(t)+Δτ_ij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τ_ij(t+1)，式中ρ为信息素更新系数，τ_ij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτ_ij(t)为信息素增量，

m为飞行器的总个数，

L_k(_t)为经过t轮信息素更新后，各节点间距离之和，n_k(_t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τ_ij(t+1)最高的航迹作为最优航迹；

b-4)通过公式

计算得到启发式函数H_ij(t)，式中L_jB校正点j与点B之间的距离，

X_j为校正点j在三维坐标系中X轴坐标，Y_j为校正点j在三维坐标系中Y轴坐标，Z_j为校正点j在三维坐标系中Z轴坐标，

为威胁代价函数，式中f为可行点阈值，n_j为飞行器从节点i到节点j后，设在节点j的可行点个数，

b-5)通过公式

计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率

4.根据权利要求3所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤c)包括如下步骤：

c-1)飞行器到达节点i时的前进方向为向量v₁的方向，将飞行器经过节点i且方向为v₁的直线固定，利用节点j的坐标在三维空间中确定一平面ijv₁，以节点i为切点，以v₁为切线，在平面ijv₁内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v₁之间的夹角为γ；

c-2)当

时，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径；

c-3)当

时，通过公式

计算得到圆o的圆心到节点j的距离L_oj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ₂，

通过公式

计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ₁，通过公式

计算得到飞行器的最小路径；

c-4)v₁＝(a₁,b₁,c₁)，a₁为向量v₁在三维坐标系中X轴坐标，b₁为向量v₁在三维坐标系中Y轴坐标，c₁为向量v₁在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v₂的方向，v₂＝(a₂,b₂,c₂)，a₂为向量v₂在三维坐标系中X轴坐标，b₂为向量v₂在三维坐标系中Y轴坐标，c₂为向量v₂在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量v_ij的方向，v_ij＝(a_ij,b_ij,c_ij)，a_ij为向量v_ij在三维坐标系中X轴坐标，b_ij为向量v_ij在三维坐标系中Y轴坐标，c_ij为向量v_ij在三维坐标系中Z轴坐标，向量v₁、向量v₂、向量v_ij共面，行列式

通过公式

计算得到向量v₂；

c-5)计算得到节点i与节点j之间的距离L_ij后通过公式

建立航迹模型

航迹模型

满足约束条件为：

通过对航迹模型

的求解，得到飞行器在已转转弯半径R从节点i到节点j的最短路径。

Images