多约束条件下智能无人飞行器航迹快速规划方法与流程

本发明属于智能无人飞行器控制领域，特别涉及多约束条件下智能无人飞行器航迹快速规划方法。

背景技术：

随着人工智能和无人飞行器技术的不断发展，两者之间的深度融合必对我国通用航空事业发展产生不可低估的影响。其中，复杂环境下航迹快速规划是智能无人飞行器控制要解决的重要问题之一。而且在飞行过程中，这类飞行器需要对定位误差进行校正，从而实现精准定位，以免定位误差积累到一定程度导致任务失败。

目前，无人飞行器航迹规划算法主要有启发式算法和智能优化算法。其中，启发式算法主要包括人工势场法和a*算法，但前者存在局部最优点问题，后者随着搜索空间的增大，无法保证实时性；智能优化算法包括很多，如遗传算法、蚁群算法和粒子群算法等，但计算复杂度较高，且在复杂环境下难以规划出满足飞行性能约束的航迹。

然而，以上研究都没有考虑无人飞行器能智能地进行定位误差校正，没有形成一套对复杂环境下智能无人飞行器航迹快速规划进行分析研究的系统方法。要在复杂环境下智能无人飞行器实现航迹快速规划，主要存在以下挑战：

(1)地形限制：飞行器任务区域地形多种多样，环境复杂多变。

(2)载重限制：由于自身体积和结构系统的原因，飞行器的载重能力是有限的。

(3)续航限制：有限的载重决定了智能无人飞行器携带的能源有限，而能源的多少又决定了飞行器持续航行的能力。所以，在保证完成任务的前提下，航迹长度要尽可能小。

(4)计算能力限制：有限的载重和有限的能源决定了智能无人飞行器的“大脑”的计算能力。同时，环境的复杂性和不确定性地大幅度增加，更是加剧了计算资源的消耗；而且为了保证实时性，这就要求航迹规划算法的计算复杂度不能太大，以免贻误时机。

(5)实时精准定位：飞行器在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。并且在飞行过程中，由于飞行器速度变化快和振动频率大等因素，定位误差较大，其定位系统无法对自身进行精准定位，这就需要飞行器对定位误差进行校正。

(6)转弯半径限制：飞行器在转弯时受到结构和控制系统的限制，前进方向无法突然改变，无法完成即时转弯。

综上所述，复杂环境下智能无人飞行器航迹快速规划具有很大的研究前景与应用价值。

技术实现要素：

针对上述存在的不足，从复杂环境下智能无人飞行器航迹快速规划的需求出发，本发明提出了一种多约束条件下智能无人飞行器航迹快速规划方法。其中，飞行器航迹约束具备如下特征：

(a)飞行误差增量：飞行器定位误差包括垂直误差和水平误差，其每飞行1m，垂直误差和水平误差将各增加δ个专用单位，以下简称单位。

(b)定位误差校正：飞行器到达用于误差校正的安全位置(称之为校正点)时，能够根据该位置的误差校正类型进行误差校正。1)当飞行器在垂直误差校正点进行垂直误差校正时，只有垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正，且校正之后垂直误差将变为0，水平误差保持不变；2)当飞行器在水平误差校正点进行水平误差校正时，只有垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正，且校正之后其水平误差将变为0，垂直误差保持不变。

(c)按照规划航迹飞行：若垂直误差、水平误差都能够得到及时校正，且到达校正点的垂直误差和水平误差均小于θ个单位时，则无人飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。其中，在出发地的无人飞行器的垂直和水平误差均为0，到达终点的垂直和水平误差均应小于θ个单位。

(d)最小转弯半径：无人飞行器在转弯时受到结构和控制系统的限制，前进方向无法突然改变，无法完成即时转弯，故无人飞行器的最小转弯半径应不小于r0m。

本发明的技术方案：

多约束条件下智能无人飞行器航迹快速规划方法，首先根据地形，在航迹规划前找出飞行区域中的安全位置，确定可进行定位误差(包括垂直误差和水平误差)校正的校正点坐标及其类型；其次根据目标确定约束条件，即飞行误差增量约束、垂直误差校正约束、水平误差校正约束、按照规划航迹飞行约束以及最小转弯半径约束，并对约束条件的相互作用机理进行分析，然后以完成任务时间最小、能量消耗最少为目标，建立无人飞行器航迹快速规划模型；最后求解模型，确定满足多约束条件的航迹规划路径；具体步骤如下：

(1)分析飞行区域地形，确定校正点坐标及其类型：

令i＝{i|i＝0,1,2,...,n}，表示校正点编号集，其中当i＝0时，表示起点，i＝n时，表示终点；坐标表示校正点i的坐标，其中当i＝0时，(x0,y0,z0)表示起点坐标，i＝n时，(xn,yn,zn)表示终点坐标；则任意两校正点i和j之间的距离dij表示为

误差校正类型有2种：垂直误差和水平误差，所以引入0-1函数

同时，令vi,v′i分别表示校正点i∈i校正之前和校正之后的垂直误差，hi,h′i分别表示校正点i∈i校正之前和校正之后的水平误差。

(2)以完成任务时间最少、能量消耗最少为优化目标，建立满足约束条件(a)～(d)的智能无人飞行器航迹规划模型，其中参数α1,α2,β1,β2,θ,δ,r0由飞行器本身决定；具体如下：

为规划出无人飞行器的航迹，引入0-1函数

令e＝{e1,e2,…,ej,…}表示无人飞行器航迹的节点集，其中j表示校正点纳入航迹节点集e的顺序，节点ej存放着第j个纳入航迹节点集e的校正点编号，则有

其中，ej(i)表示航迹节点集e的第j位置存放着校正点编号i，

对任意航迹节点ej-1(i0),ej(i1),ej+1(i2)∈e＝{e1,e2,…,ej,…}，i0,i1，i2∈i,j∈{1,2,…,m}，其中ej-1(i0),ej(i1),ej+1(i2)分别表示航迹e中相邻3个位置即第j-1,j,j+1个位置上分别存放着校正点编号i0,i1,i2；同时，引入虚拟节点e0(0)，它具有起点的所有信息，包括校正点编号、三维坐标和校正点类型；令和表示航迹节点ej(i1)校正之前的垂直误差和水平误差，和表示航迹节点ej(i1)校正之后的垂直误差和水平误差；同理，和表示航迹节点ej+1(i2)校正之前以及校正之后的垂直误差和水平误差，并且满足如下约束条件：

(i)飞行器飞行误差增量

由于飞行器每飞行1m，垂直误差和水平误差将各增加δ个专用单位；因此，航迹节点ej+1(i2)校正之前的垂直误差和水平误差等于上一航迹节点ej(i1)校正之后的垂直误差和水平误差加上飞行误差增量，即

其中，m为航迹节点的个数，表示航迹节点ej(i1)与ej+1(i2)之间的距离。

(ii)飞行器按照规划路径飞行

另外，只有垂直误差和水平误差均小于θ个单位时，飞行器才能够按照规划航迹飞行；所以，航迹节点ej(i1)校正之前的垂直误差和水平误差均应小于θ，即

(iii)飞行器定位误差校正

无人飞行器在空间飞行过程中需要根据定位误差进行实时定位，此时存在2种情形：

情形1：飞行器垂直误差校正

当飞行器在水平误差校正点进行垂直误差校正时，只有垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正，所以令

其中，当rvi＝1时表示校正点i可进行垂直误差校正，当rvi＝0时，表示校正点i不可进行垂直误差校正；令

svi＝ti·rvi

其中，svi表示为校正点i∈i垂直误差是否校正成功，当校正点i为垂直校正点，即校正类型ti＝1时，此时svi＝1，校正点i垂直误差校正成功；当校正点i为水平校正点，即校正类型ti＝0时，此时svi＝0，校正点i垂直误差校正失败。

由于飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差变为0，水平误差保持不变，因此，航迹节点ej(i1)校正之后的垂直误差和水平误差表示为

其中，为校正点i1的校正类型，表示为校正点i1是否可进行垂直误差校正；此时，当校正点i1为垂直校正点且时，即垂直误差校正成功，且航迹节点ej(i1)校正之后的垂直误差和水平误差为

情形2：飞行器水平误差校正

当飞行器在水平误差校正点进行水平误差校正时，只有垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正，所以令

其中，当rhi＝1时，表示校正点i可进行水平误差校正，当rhi＝0时，表示校正点i不可进行水平误差校正；令

其中，shi表示为校正点i∈i水平误差是否校正成功，当校正点i为水平校正点，即校正类型ti＝0时，此时shi＝1，校正点i水平误差校正成功；当校正点i为垂直校正点，即校正类型ti＝1时，此时shi＝0，校正点i水平误差校正失败。

由于飞行器在垂直误差校正点进行水平误差校正后，其水平误差变为0，垂直误差保持不变，因此，航迹节点ej(i1)校正之后的垂直误差和水平误差表示为

其中，为校正点i1的校正类型，表示为校正点i1是否可进行垂直误差校正，此时，当校正点i1为水平校正点且时，即水平误差校正成功，且航迹节点ej(i1)校正之后的垂直误差和水平误差表示为

(iv)飞行器飞行转弯半径

由于无人飞行器在转弯时受到结构和控制系统的限制，前进方向无法突然改变，无法完成即时转弯，这就要求无人飞行器飞行的转弯半径r最小为r0，如图1所示；点ej-1,ej,ej+1表示无人飞行器航迹中任意相邻的3个节点，两节点之间的连线代表飞行方向，点ej-1,ej,ej+1的坐标分别是则无人飞行器从节点ej-1飞到节点ej的飞行方向即方向向量为

无人飞行器从节点ej飞到节点ej+1的飞行方向即方向向量为

所以，无人飞行器从节点ej-1飞到节点ej与从节点ej飞到节点ej+1的转弯角度为方向向量与的夹角γ∈[0,π]，则有

其中，表示节点ej与ej+1之间的距离，表示节点ej+1与ej+2之间的距离，r为无人飞行器的转弯半径，故

其中，当方向向量与共线时，夹角γ＝0或π，此时，γ＝0表示无人飞行器继续沿着当前飞行方向飞行，不用转弯；γ＝π表示无人飞行器与当前飞行方向的完全相反方向飞行，转弯半径当γ∈(0,π)时，有

考虑到在复杂环境下，智能无人飞行器必须在规定的时间内完成任务，并且尽可能提高效率和减少能量消耗，这就要求航迹长度尽可能小，以及经过校正区域进行校正的次数尽可能少。换句话说，也就是相邻航迹节点之间的距离之和尽可能小，即最小化；以及要求航迹经过校正点，且在该校正点定位误差校正成功，即最小化；所以，确定目标函数为：

则无人飞行器航迹规划模型如下：

(3)采用基于最速下降的改进dijkstra算法对无人飞行器航迹规划模型进行求解，确定满足多约束条件的航迹规划路径。其基本思想是：从起点出发，以可达步长为搜索半径，以投影方向为搜索方向，逐步地向终点探寻最短的航迹，其中可达步长为满足约束条件的最长距离，投影方向为到达点与终点之间连线投影在起点—终点连线上的距离(带有方向)最大的方向。在执行的过程中，与每个校正点对应，找出满足搜索半径和搜索方向的最佳校正点，记录下该点的信息作为返回值，它包含该校正点的编号，到上一出发点的距离，以及校正之前和之后的定位误差(垂直误差、水平误差)；具体步骤如下：

给定校正点编号i＝{i|i＝0,1,2,...,n}，坐标(xi,yi,zi)i∈i，参数α1,α2,β1,β2,θ,δ,r0,e1＝0,以及航迹节点集e，航迹节点校正之前的定位误差集vh，校正之后的定位误差集vh′；

开始：j＝1

step1：令ej＝{e1,e1,...,ej}，ij＝i\ej，

step2:如果n∈ej，算法终止，转入step7；否则转入step3；

step3:对计算ij与出发点ej的连线投影在起点与终点连线上的长度td(ej,ij)，令表示校正点编号ij(1)与出发点ej之间连线投影在起点与终点连线上的长度最大值；

step4:记

1)当校正点编号ij(1)的类型时，令若且则转入step7；否则，ij＝ij\{ij(1)}，转入step3；

2)当校正点编号ij(1)的类型令若且则转入step7；否则，ij＝ij\{ij(1)}，转入step3；

step5：记

1)当|cosγ|＝1时，转入step6；

2)当|cosγ|≠1时，①转弯半径r≥r0时，转入step6；②转弯半径r<r0时，ij＝ij\{ij(1)}，转入step3；

step6：当j≥n时，算法结束，转入step7；否则j＝j+1，转入step1；

step7：输出j，ej＝ij(1)，ej＝{e1,e1,...,ej},

本发明的有益效果：通过本发明的技术可以实现多约束条件下智能无人飞行器航迹快速规划，且具有较高的有效性和鲁棒性；同时，飞行器飞行时间少、能源消耗低。

附图说明

图1是本发明中的无人飞行器飞行转弯半径示意图。

图2是本发明的基于最速下降的改进dijkstra算法流程图。

图3是本发明实例中的无人飞行器航迹规划区域示意图。

图4是本发明实例中的航迹规划结果轨迹图。

具体实施方式

以下结合附图和技术方案，进一步说明本发明的具体实施方式。

本发明的多约束条件下智能无人飞行器航迹快速规划方法，具体包括：

(1)分析飞行区域地形，找出区域中存在的可用于误差校正的安全位置即校正点，确定校正点坐标及其类型；校正点位置分布依赖于地形，无统一规律。

(2)根据目标确定约束条件，并对约束条件的相互作用机理进行分析，以完成任务时间最小少、能量消耗最少为目标，建立无人飞行器航迹规划模型。

(3)设计算法进行求解，确定满足多约束条件的航迹规划路径。

取智能无人飞行器的飞行区域如图3所示，其中出发点为a点，目的地为b点，黑色曲线代表一条航迹。校正点的数据集的参数为：

α1＝25，α2＝15，β1＝20，β2＝25，θ＝30，δ＝0.001,r0＝200.

基于本发明的模型与算法，利用matlab编程来求解无人飞行器航迹规划模型，其运行结果数据和航迹规划轨迹图详见表1和图4，算法流程图见图2。

对表1和图4的实验结果数据分析可得：

(1)数据集的航迹规划路径从起点a点出发，经过8个校正点到达终点b点，其航迹长度为112329.75米，校正次数为8次；

(2)易得数据集中出发点a到终点b的距离约为100460米，与规划的航迹长度相差约12000米，根据定位误差的约束条件，当α1＝25，α2＝15，β1＝20，β2＝25，θ＝30，δ＝0.001,r0＝200时，出发点a到终点b的误差增量约为100个单位，出发点a到终点b的航迹大概至少需要经过4～6个校正点，而本发明规划的航迹经过8个，这在很大程度上说明了本发明的模型算法和结果的有效性。

(3)对比观察每个校正点的误差校正前后数据变化以及对应的校正点类型，可以明显看出，当校正点类型为1时，垂直误差校正为0；当校正点类型为0时，水平误差变为0，该数据变化规律符合前面的约束要求，表明了该模型在实验数据集上场景中实现了准确的应用。

(4)本实施例的校正点数据集中，可以看出其校正点类型顺序为“0”(水平校正)-“1”(垂直校正)-“0”(水平校正)-“1”(垂直校正)这样的交错循环校正规律，这样可以不断修正需校正的定位误差，满足约束条件且合乎常理，更加验证了该模型的实际可操作性。

(5)图4中实线为无人飞行器航迹路径，虚线为起点a与终点b之间的连线，对比发现，无人飞行器航迹路径“震荡”趋近于ab连线，此“拟合”现象更加形象地说明了该模型可以更加准确的探寻到如何去“拟合”起始点与目的点连线。

实例结果表明，本发明能够快速规划出满足多约束条件的较优航迹，且具有较高的规划效率和较强的鲁棒性，同时飞行器飞行时间少、能源消耗低。

表1是本发明实例中的航迹规划结果表。

表1