CN111024092B - 一种多约束条件下智能飞行器航迹快速规划方法 - Google Patents

Abstract

本发明公开了一种多约束条件下智能飞行器航迹快速规划方法，属于飞行器路径规划技术领域，包括以下步骤：综合分析飞行器航迹规划特点，建立多目标多约束条件的数学优化模型；获取实例信息；运用启发式邻域搜索算法对实例进行求解，得到多种可行路径方案并对其进行表征，本发明针对飞行器在不同实际飞行环境下有效航迹快速规划问题，建立以最小化路径和最小化校正次数为目标函数，包含路径平衡约束，路径唯一性约束、遍历路径回路约束等多目标多约束的数学优化模型，同时提出一种启发式邻域搜索算法，该算法结合模拟退火思想寻找局部最优节点作为飞行器下一遍历节点，并规定相应搜索步长，以此提高算法的收敛性能，通过本发明可以快速获得较优的航迹路线。

Description

一种多约束条件下智能飞行器航迹快速规划方法

技术领域

本发明属于飞行器航迹规划与控制领域，具体涉及一种多约束条件下智能飞行器航迹快速规划方法。

背景技术

1917年英国人发明了第一台飞行器(Aerial Vehicle)以来，飞行器技术已经成为军事领域和民用领域中非常重要的发展项目。在军事领域中飞行器可以完美的完成对地形和敌人的侦察和对地攻击等在内的很多军事任务。在民用领域中飞行器在测绘、资源勘探和空中拍摄等方面也发挥了巨大的作用。随着计算机、信息及科学技术的快速发展，飞行器的性能、结果、操纵变得越来越复杂。而飞行器航迹规划是实现飞行器自主导航的一项关键技术，而且无论在理论上还是实际应用上都具有重要的意义，基于以上种种原因就需要寻求一种有效的航迹规划方法，使得飞行器不仅具有最短的飞行距离，而且能定位精确的完成任务，这也成为了当前迫切的要求。

航迹规划是路径规划的一种，但由于飞行器的飞行过程复杂性、执行任务时环境复杂性等，造成了航迹规划要比一般的路径规划更加困难复杂。一般飞行器航迹规划(Route Planning)就是为飞行器规划出一条或者多条满足要求的飞行轨迹。如现有专利(一种无人机航迹规划装置及方法，专利号CN201910556400.3；一种无人飞行器的航迹规划，专利号CN201810519664.7；一种低空飞行无人机的实时航迹规划，专利号CN201810519645.4)等等，但现有专利航迹规划方法中，对于飞行器在飞行过程中所遇到的各类实际环境并未作过多约束。而在实际应用中，由于飞行器的系统结构限制与复杂的环境影响和约束条件众多，使得在复杂环境下航迹快速规划成为飞行器控制的一个重要课题。为了使飞行器精确的在规定航线上飞行，避免由定位误差而造成飞行任务失败。因此，在飞行过程中需要考虑环境因素，择优选出满足水平误差约束、垂直误差约束、有效路径约束和飞行器转弯半径约束的空间节点，为使这些空间节点形成一条可行回路作为飞行器的有效航迹规划，本发明提供一种飞行器在系统定位精度限制下的航迹快速规划方法。

发明内容

针对背景技术中存在的问题，为了解决飞行器在实际飞行过程中遇到的各种情形，本发明提出了一种多约束条件下智能飞行器航迹快速规划方法，本方法具体如下：

一种多约束条件下智能飞行器航迹快速规划方法，具体包括以下步骤：

S1、分析飞行器航迹规划特点，建立多目标函数多约束条件的数学优化模型；

根据飞行器航迹规划特点，忽略飞行器的大小将飞行器简化为质点，将飞行器节点间飞行路径视为欧式距离，建立数学优化模型的目标函数如下：

最短路径：

经过最少校正点：

其中，N为飞行区域内校正点的节点集合，i、j为节点集合N中第i、j个节点的代号，d_ij为飞行器从节点i到节点j的距离；x_ij为飞行器是否从节点i飞行到节点j的二进制变量，如果飞行器从节点i飞行到节点j，则x_ij＝1，否则x_ij＝0；

该目标函数要满足以下约束条件：

1)路径平衡约束：这限制飞行器仅进行一次的飞入和飞出，即飞行器遍历该节点后就不会再遍历该节点，路径平衡约束方程为：

其中k为节点集合N中第k个节点的代号，n为节点集合N中节点总数量，x_ik为飞行器是否从节点i飞行到节点k的二进制变量，如果飞行器从节点i飞行到节点k，则x_ik＝1，否则x_ik＝0；x_kj为飞行器是否从节点k飞行到节点j的二进制变量，如果飞行器从节点k飞行到节点j，则x_kj＝1，否则x_kj＝0；

2)路径唯一性约束：这用于限制飞行器依次遍历各个节点有且只形成一条有效路径，路径唯一性约束方程为：

其中，y_i为节点i是否被飞行器遍历的二进制变量，当飞行器从该节点i飞出或飞入即表示节点i被飞行器遍历，则y_i＝1，否则为y_i＝0；

3)遍历路径回路约束：这用于限制飞行器在飞行过程中没有任何子回路解产生，遍历路径回路约束方程为：

S2、获取实例信息，包括飞行器起点坐标、终点坐标、飞行区域内所有的校正点的空间位置坐标及其误差校正类型，其中误差校正类型包括水平误差校正类型和垂直误差校正类型；

S3、运用启发式邻域搜索算法对实例进行求解，得到多个可选方案供选择。

进一步，所述约束条件还包括累积水平误差和累积垂直误差约束，根据飞行器从节点i到达节点j时能否进行理想的误差校正分为两种情况：

情况一：飞行器从节点i达到节点j，能够进行理想的误差校正时，限定累积水平误差和累积垂直误差在最大定位误差范围内，从而保证飞行路径的有效性，累积水平误差和累积垂直误差约束方程为：

其中：θ为累计误差阈值，h_j为飞行器到达节点j位置时总水平偏差量；v_j为飞行器到达节点j位置时总垂直偏差量；H为水平校正点集合，

为节点i是否属于集合H的二进制变量，若节点i∈H则

否则

V为垂直校正点集合，

为节点i是否属于集合V的二进制变量，若节点i∈V则

否则

δ为飞行器每飞行1m时水平误差与垂直误差增加量；

情况二：飞行器从节点i飞行到节点j，无法进行理想的误差校正时，则选择下一节点k进行误差校正，此时累积水平误差和累积垂直误差约束方程为：

其中：P_ijk为飞行器是否依次遍历节点i，j，k的二进制变量，当飞行器依次遍历节点i，j，k时，P_ijk＝1，否则P_ijk＝0；d_jk为飞行器从节点j到节点k的距离；

为节点k是否属于集合H的二进制变量，若节点k∈H，则

否则

η为校正余量，单位为m，η∈[2,8]；D为校正失败的节点集合；S为误差校正为0的概率是ξ的节点集合。

进一步，运用所述启发式邻域搜索算法进行求解具体包括以下步骤：

A、输入初始参数和所述实例信息，所述初始参数包括最大循环次数、累计误差阈值θ，垂直误差校正条件：垂直误差阈值α₁和水平误差阈值α₂、水平误差校正条件：垂直误差阈值β₁、水平误差阈值β₂、最小转弯半径R_min、飞行器每飞行1m误差增量δ；

B、确定相关候选点集合P₄，具体方法如下：

(1)在节点集合N中选定飞行器未遍历的节点作为备选下一节点，选定飞行器从当前节点到达备选下一节点时累积水平误差和累积垂直误差均小于累积误差阈值θ的节点，存储在集合P₁中；

(2)选定P₁中X轴坐标值比飞行器当前节点X轴坐标值大的节点，存储在候选点集合P₂中；

(3)为避免飞行器累计误差值较大，根据当前飞行器的累计水平误差值和垂直误差值，选定到达下一点需校准的误差值类型，并确定候选点集合P₄,确定的方式有两种：

方式一：

a、在集合P₂中选定满足垂直校正条件的节点组成集合P₃，所述垂直校正条件为：垂直误差不大于α₁，水平误差不大于α₂；如果P₂中不存在满足垂直校正条件的节点，那么选择P₂中满足水平校正条件的节点组成集合P₃，所述水平校正条件为：垂直误差不大于β₁，水平误差不大于β₂；

b、确定飞行器当前节点和上一节点位置坐标，随机选取所述P₃中任一点作为下一节点，这三个节点构成空间三角形，求解当前节点到达下一节点的转弯半径，若计算所得转弯半径大于最小转弯半径R_min，则从集合P₃中选择的该节点满足转弯半径约束，将该节点存储在候选点集合P₄中，从P₃中选择所有满足转弯半径约束的节点组成候选点集合P₄；

方式二：

选定集合P₂中能实现完全校准或满足校正余量η的垂直校正点作为候选点集合P₄，否则选定P₂中能完全校准或满足校正余量η的水平校正点作为候选点集合P₄；

C、从候选点集合P₄中任选一节点并前进一步作为当前节点，确定下一节点的候选点集合P₄，具体方法如下：

从步骤B中所得的候选点集合P₄中任取一节点并前进一步将其作为当前节点，转至步骤B选取其下一节点的候选点集合P₄，如果其下一节点的候选点集合P₄为空集，则返回步骤B中所得的候选点集合P₄，另取一节点作为当前节点，继续计算；若以候选点集合P₄中所有节点为当前节点，转至步骤B后获取的下一节点的候选点集合P₄均为空集，则获取原候选点集合P₄的当前节点为死点，返回该当前节点的上一节点，并以该上一节点为当前节点，重新计算；

在步骤B、C中，针对每个非空的候选点集合P₄判断是否包含终点坐标，若包含，则保存航迹并进入步骤D；否则，继续转至步骤B、C继续确定下一节点的候选点集合P₄；

D、每获得一条航迹则循环次数增加一次，判断累积循环次数是否达到最大循环次数，若未达到，则转至步骤A，继续探寻新的航迹，否则，输出所有航迹。

进一步，飞行器转弯半径的计算方式根据海伦公式得到，具体描述为：

如图2，假设飞行器依次遍历节点i，j，k时，节点i，j，k形成一个空间三角形，其内切圆半径为R,则将三角形分成分别以d_ij,d_jk,d_ik为底边的三个面积分别为s₁、s₂、s₃的三角形，根据海伦公式：

其中

s＝s₁+s₂+s₃,其中

因此得到转弯半径计算方法如下：

其中，R_ijk为转弯半径，s为节点i、j、k围成的三角形的面积，p为节点i、j、k围成的三角形半周长。

本发明针对飞行器在不同实际飞行环境下有效航迹快速规划问题，建立以最小化路径和最小化校正次数为目标函数，包含路径平衡约束，路径唯一性约束、遍历路径回路约束等多目标多约束的数学优化模型，同时提出一种启发式邻域搜索算法，该算法结合模拟退火思想寻找局部最优节点作为飞行器下一遍历节点，并规定相应搜索步长，以此提高算法的收敛性能，通过本发明可以快速获得较优的航迹路线。

附图说明

图1为海伦公式原理图；

图2为方案一、二航迹图；

图3为方案三、四航迹图；

具体实施方式

为了对本发明的技术特征、目的和有益效果有更加清楚的理解，结合附图对本发明的一个实施例作进一步描述。实施例只用于对本发明进行进一步的说明，不能理解为对本发明保护范围的限制，本领域的技术人员根据本发明的内容做出的一些非本质的改进和调整也属于本发明保护的范围。

具体地，本发明一种多约束条件下智能飞行器航迹快速规划方法包括以下步骤：

1、综合分析飞行器航迹规划特点，做出如下假设：

(1)假设忽略飞行器的大小将飞行器简化为质点，忽略地形以及障碍物对飞行器飞行过程中的影响；

(2)飞行器节点间飞行路径为欧式距离。

2、数学模型及约束条件

规划飞行器的最佳航迹路线问题，可以根据飞行区域中校正点将其转换为寻找多组路线，保证飞行器在满足最大误差范围经过最少的校正点对飞行器飞行过程中进行及时的垂直和水平误差校正，并使得飞行器从A点到达B点的航迹行程最短。建立如下目标函数：

约束条件如下：

d_ij≤d_max,其中

在目标函数中：

F₁为最短路径，F₂为最少校正点。

在约束条件中：

其中约束(1)～(2)为飞行器路径平衡约束，限制对任意的节点k而言，飞行器仅进行一次的飞入和飞出，即飞行器遍历该节点后就不会再遍历该节点；约束条件(3)为飞行器遍历节点个数与飞行路径条数的线性表达式，保证飞行路径的唯一性；约束条件(4)～(5)为遍历路径回路约束，保证了飞行器在飞行过程中没有任何的子回路解产生；约束条件(6)和(7)为校正误差约束，分别限制累积水平误差和累积垂直误差不大于允许的最大定位误差，保证飞行路径的有效性；约束条件(8)限制两个节点的距离不得大于最大的定位误差允许飞行的最大距离；式(9)约束飞行器依次遍历节点i、j、k；式(11)与式(12)为约束条件(6)和(7)的衍生约束，即考虑飞行器因环境因素能否成功校正，当能够成功校正选择式(11)与式(12)，否则选择(6)和(7)；式(10)为飞行器转弯半径约束。

符号说明：

N为飞行区域内校正点的节点集合；

i、j、k为节点集合N中第i、j、k个节点的代号；

n为节点集合N中节点总数量；

d_ij为飞行器从节点i到节点j的距离；

x_ij为飞行器是否从节点i飞行到节点j的二进制变量，如果飞行器从节点i飞行到节点j，则x_ij＝1，否则x_ij＝0；

x_ik为飞行器是否从节点i飞行到节点k的二进制变量，如果飞行器从节点i飞行到节点k，则x_ik＝1，否则x_ik＝0；

x_kj为飞行器是否从节点k飞行到节点j的二进制变量，如果飞行器从节点k飞行到节点j，则x_kj＝1，否则x_kj＝0；

y_i为节点i是否被飞行器遍历的二进制变量，当飞行器从该节点i飞出或飞入即表示节点i被飞行器遍历，则y_i＝1，否则为y_i＝0；

δ为飞行器每飞行1m时水平误差与垂直误差增加量

θ为累计误差阈值；

h_j为飞行器到达节点j位置时总水平偏差量；h_k为飞行器到达节点k位置时总水平偏差量；

v_j为飞行器到达节点j位置时总垂直偏差量；v_k为飞行器到达节点k位置时总水平偏差量；

H为水平校正点集合，

为节点i是否属于集合H的二进制变量，若节点i∈H则

否则

V为垂直校正点集合，

为节点i是否属于集合V的二进制变量，若节点i∈V则

否则

δ为飞行器每飞行1m时水平误差与垂直误差增加量；

P_ijk为飞行器是否依次遍历节点i，j，k的二进制变量，当飞行器依次遍历节点i，j，k时，P_ijk＝1，否则P_ijk＝0；

d_jk为飞行器从节点j到节点k的距离；

为节点k是否属于集合H的二进制变量，若节点k∈H，则

否则

η为校正余量，单位为m，η∈[2,8]；

D为校正失败的节点集合；S为误差校正为0的概率是ξ的节点集合；

R_ijk为转弯半径；R_min为最小转弯半径；

S_ijk为节点i、j、k围成的三角形的面积；

d_jk为节点j到节点k的距离；d_ik为节点i到节点k的距离

p为节点i、j、k围成的三角形半周长。

3、一种多约束条件下智能飞行器航迹快速规划方法的实例计算，原始实例信息见表2。其中A节点为起始位置坐标，B节点为终止位置坐标；校正点属性类型分为I、II类，其中，I类中：0表示水平误差校正点(飞行器经过该校正点自动将水平误差清0)，1表示垂直误差校正点(飞行器经过该校正点自动将垂直误差清0)；II类中，1表示能够成功将某个误差校正为0的概率ξ为0.8，0则表示能够成功将某个误差校正为0。

表1飞行区域内节点数据

4、以出发点为A点(0,50000,5000)，目的地为B点(100000，59652.34,5022)，在99个空间节点中寻找航迹长度尽可能小以及经过校正区域进行校正的次数尽可能少的航迹规划，对约束条件具体赋值进行线性化求解，下面是运用启发式邻域搜索算法进行求解具体步骤：

A、输入初始参数和实例信息，其中实例信息见表1，初始参数如下：

(1)最大循环次数100。

(2)累计误差阈值θ＝30，飞行器每飞行1m，误差增加δ，赋值为δ＝0.001。

(3)垂直误差校正条件：飞行器的垂直误差不大于α₁＝25m，水平误差不大于α₂＝15m。

(4)水平误差校正条件：飞行器垂直误差不大于β₁＝20m，水平误差不大于β₂＝25m。

(5)飞行器在转弯时受到结构和控制系统的限制，其最小转弯半径为R_min＝200m。

(6)校正余量η为5。

同时做出以下误差校正规定：

设定在出发地A点，飞行器的垂直和水平误差均为0；飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变；飞行器在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变。

B、确定相关候选点集合P₄，具体方法如下：

(1)在节点集合N中选定飞行器未遍历的节点作为备选下一节点，选定飞行器从当前节点到达备选下一节点时累积水平误差和累积垂直误差均小于累积误差阈值θ的节点，存储在集合P₁中；

(2)选定P₁中X轴坐标值比飞行器当前节点X轴坐标值大的节点，存储在候选点集合P₂中；

(3)为避免飞行器累计误差值较大，根据当前飞行器的累计水平误差值和垂直误差值，选定到达下一点需校准的误差值类型，并确定候选点集合P₄,确定的方式有两种：

方式一：

a、在集合P₂中选定满足垂直校正条件的节点组成集合P₃，所述垂直校正条件为：垂直误差不大于α₁，水平误差不大于α₂；如果P₂中不存在满足垂直校正条件的节点，那么选择P₂中满足水平校正条件的节点组成集合P₃，所述水平校正条件为：垂直误差不大于β₁，水平误差不大于β₂；

b、确定飞行器当前节点和上一节点位置坐标，随机选取所述P₃中任一点作为下一节点，这三个节点构成空间三角形，求解当前节点到达下一节点的转弯半径，若计算所得转弯半径大于最小转弯半径R_min，则从集合P₃中选择的该节点满足转弯半径约束，将该节点存储在候选点集合P₄中，从P₃中选择所有满足转弯半径约束的节点组成候选点集合P₄；

方式二：

选定集合P₂中能实现完全校准或满足校正余量η的垂直校正点作为候选点集合P₄，否则选定P₂中能完全校准或满足校正余量η的水平校正点作为候选点集合P₄；

C、从候选点集合P₄中任选一节点并前进一步作为当前节点，确定下一节点的候选点集合P₄，具体方法如下：

从步骤B中所得的候选点集合P₄中任取一节点并前进一步将其作为当前节点，转至步骤B选取其下一节点的候选点集合P₄，如果其下一节点的候选点集合P₄为空集，则返回步骤B中所得的候选点集合P₄，另取一节点作为当前节点，继续计算；若以候选点集合P₄中所有节点为当前节点，转至步骤B后获取的下一节点的候选点集合P₄均为空集，则获取原候选点集合P₄的当前节点为死点，返回该当前节点的上一节点，并以该上一节点为当前节点，重新计算。

在步骤B、C中，针对每个非空的候选点集合P₄判断是否包含终点坐标，若包含，则保存航迹并进入步骤D；否则，继续转至步骤B、C继续确定下一节点的候选点集合P₄。

D、每获得一条航迹则循环次数增加一次，判断累积循环次数否达到最大循环次数，若未达到，则转至步骤A，继续探寻新的航迹，否则，输出所有航迹。

5、计算结果

(1)采用4中所述的方法，选择方式一获取候选点集合P₄，经过求解计算得到多组互不占优的方案，选其中两组方案，见表2。

表2求解结果

可见方案一中飞行器经过除初末位置的9个节点，可从初始位置A到达终止位置B，遍历最小路径和为120787.00m，且到达每一节点的累计误差皆小于相关水平误差和垂直误差约束，且满足所设最小半径约束。再观方案二，飞行器遍历的节点虽然为10个，但所求遍历最小路径和为119754.84m，对比两种方案，可见多目标求解过程中会保留互不占优的解，各有所长。

(2)采用4中所述的方法，选择方式二获取候选点集合P₄，经过求解计算得到多组互不占优的方案，选其中两组方案，见表3。

表3求解结果

表3可见，在空间节点数为99个前提下，考虑校正不确定约束，方案三中，飞行器经过除起点、终点位置外的8个节点，遍历最小路径和为118132.68m；方案四中，飞行器经过除起点、终点位置外的9个节点，但所求遍历最小路径和为117006.88m，对比两种方案，可见多目标求解过程中会保留互不占优的解，各有所长。

以上对本发明的有关内容进行了说明，本领域普通技术人员在基于这些说明的情况下将能够实现本发明。基于本发明的上述内容，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

Claims (4)

1.一种多约束条件下智能飞行器航迹快速规划方法，其特征在于，包括以下步骤：

S1、分析飞行器航迹规划特点，建立多目标函数多约束条件的数学优化模型；

根据飞行器航迹规划特点，忽略飞行器的大小将飞行器简化为质点，将飞行器节点间飞行路径视为欧式距离，建立数学优化模型的目标函数如下：

最短路径：

经过最少校正点：

其中，N为飞行区域内校正点的节点集合，i、j为节点集合N中第i、j个节点的代号，d_ij为飞行器从节点i到节点j的距离；x_ij为飞行器是否从节点i飞行到节点j的二进制变量，如果飞行器从节点i飞行到节点j，则x_ij＝1，否则x_ij＝0；

该目标函数要满足以下约束条件：

1)路径平衡约束：这限制飞行器仅进行一次的飞入和飞出，即飞行器遍历该节点后就不会再遍历该节点，路径平衡约束方程为：

其中k为节点集合N中第k个节点的代号，n为节点集合N中节点总数量，x_ik为飞行器是否从节点i飞行到节点k的二进制变量，如果飞行器从节点i飞行到节点k，则x_ik＝1，否则x_ik＝0；x_kj为飞行器是否从节点k飞行到节点j的二进制变量，如果飞行器从节点k飞行到节点j，则x_kj＝1，否则x_kj＝0；

2)路径唯一性约束：这用于限制飞行器依次遍历各个节点有且只形成一条有效路径，路径唯一性约束方程为：

其中，y_i为节点i是否被飞行器遍历的二进制变量，当飞行器从该节点i飞出或飞入即表示节点i被飞行器遍历，则y_i＝1，否则为y_i＝0；

3)遍历路径回路约束：这用于限制飞行器在飞行过程中没有任何子回路解产生，遍历路径回路约束方程为：

S2、获取实例信息，包括飞行器起点坐标、终点坐标、飞行区域内所有的校正点的空间位置坐标及其误差校正类型，其中误差校正类型包括水平误差校正类型和垂直误差校正类型；

S3、运用启发式邻域搜索算法对实例进行求解，得到多个可选方案供选择。

2.根据权利要求1所述的一种多约束条件下智能飞行器航迹快速规划方法，其特征在于：所述约束条件还包括累积水平误差和累积垂直误差约束，根据飞行器从节点i到达节点j时能否进行理想的误差校正分为两种情况：

情况一：飞行器从节点i达到节点j，能够进行理想的误差校正时，限定累积水平误差和累积垂直误差在最大定位误差范围内，从而保证飞行路径的有效性，累积水平误差和累积垂直误差约束方程为：

其中：θ为累计误差阈值，h_j为飞行器到达节点j位置时总水平偏差量；v_j为飞行器到达节点j位置时总垂直偏差量；H为水平校正点集合，

为节点i是否属于集合H的二进制变量，若节点i∈H则

否则

V为垂直校正点集合，

为节点i是否属于集合V的二进制变量，若节点i∈V则

否则

δ为飞行器每飞行1m时水平误差与垂直误差增加量；

情况二：飞行器从节点i飞行到节点j，无法进行理想的误差校正时，则选择下一节点k进行误差校正，此时累积水平误差和累积垂直误差约束方程为：

其中：P_ijk为飞行器是否依次遍历节点i，j，k的二进制变量，当飞行器依次遍历节点i，j，k时，P_ijk＝1，否则P_ijk＝0；d_jk为飞行器从节点j到节点k的距离；

为节点k是否属于集合H的二进制变量，若节点k∈H，则

否则

η为校正余量，单位为m，η∈[2,8]；D为校正失败的节点集合；S为误差校正为0的概率是ξ的节点集合。

3.根据权利要求2所述的一种多约束条件下智能飞行器航迹快速规划方法，其特征在于：运用所述启发式邻域搜索算法进行求解具体包括以下步骤：

A、输入初始参数和所述实例信息，所述初始参数包括最大循环次数、累计误差阈值θ，垂直误差校正条件：垂直误差阈值α₁和水平误差阈值α₂、水平误差校正条件：垂直误差阈值β₁、水平误差阈值β₂、最小转弯半径R_min、飞行器每飞行1m误差增量δ；

B、确定相关候选点集合P₄，具体方法如下：

(1)在节点集合N中选定飞行器未遍历的节点作为备选下一节点，选定飞行器从当前节点到达备选下一节点时累积水平误差和累积垂直误差均小于累积误差阈值θ的节点，存储在集合P₁中；

(2)选定P₁中X轴坐标值比飞行器当前节点X轴坐标值大的节点，存储在候选点集合P₂中；

(3)为避免飞行器累计误差值较大，根据当前飞行器的累计水平误差值和垂直误差值，选定到达下一点需校准的误差值类型，并确定候选点集合P₄,确定的方式有两种：

方式一：

a、在集合P₂中选定满足垂直校正条件的节点组成集合P₃，所述垂直校正条件为：垂直误差不大于α₁，水平误差不大于α₂；如果P₂中不存在满足垂直校正条件的节点，那么选择P₂中满足水平校正条件的节点组成集合P₃，所述水平校正条件为：垂直误差不大于β₁，水平误差不大于β₂；

b、确定飞行器当前节点和上一节点位置坐标，随机选取所述P₃中任一点作为下一节点，这三个节点构成空间三角形，求解当前节点到达下一节点的转弯半径，若计算所得转弯半径大于最小转弯半径R_min，则从集合P₃中选择的该节点满足转弯半径约束，将该节点存储在候选点集合P₄中，从P₃中选择所有满足转弯半径约束的节点组成候选点集合P₄；

方式二：

选定集合P₂中能实现完全校准或满足校正余量η的垂直校正点作为候选点集合P₄，否则选定P₂中能完全校准或满足校正余量η的水平校正点作为候选点集合P₄；

C、从候选点集合P₄中任选一节点并前进一步作为当前节点，确定下一节点的候选点集合P₄，具体方法如下：

从步骤B中所得的候选点集合P₄中任取一节点并前进一步将其作为当前节点，转至步骤B选取其下一节点的候选点集合P₄，如果其下一节点的候选点集合P₄为空集，则返回步骤B中所得的候选点集合P₄，另取一节点作为当前节点，继续计算；若以候选点集合P₄中所有节点为当前节点，转至步骤B后获取的下一节点的候选点集合P₄均为空集，则获取原候选点集合P₄的当前节点为死点，返回该当前节点的上一节点，并以该上一节点为当前节点，重新计算；

在步骤B、C中，针对每个非空的候选点集合P₄判断是否包含终点坐标，若包含，则保存航迹并进入步骤D；否则，继续转至步骤B、C继续确定下一节点的候选点集合P₄；

D、每获得一条航迹则循环次数增加一次，判断累积循环次数是否达到最大循环次数，若未达到，则转至步骤A，继续探寻新的航迹，否则，输出所有航迹。

4.根据权利要求3所述的一种多约束条件下智能飞行器航迹快速规划方法，其特征在于：所述转弯半径计算方法如下：

其中，R_ijk为转弯半径，s为节点i、j、k围成的三角形的面积，p为节点i、j、k围成的三角形半周长。

Images