[发明专利]多约束条件下智能无人飞行器航迹快速规划方法

权利要求书

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

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

令I＝{i|i＝0,1,2,...,n}，表示校正点编号集，其中当i＝0时，表示起点，i＝n时，表示终点；坐标表示校正点i的坐标，其中当i＝0时，(x₀,y₀,z₀)表示起点坐标，i＝n时，(x_n,y_n,z_n)表示终点坐标；则任意两校正点i和j之间的距离d_ij表示为

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

同时，令v_i,v′_i分别表示校正点i∈I校正之前和校正之后的垂直误差，h_i,h′_i分别表示校正点i∈I校正之前和校正之后的水平误差；

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

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

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

其中，e_j(i)表示航迹节点集E的第j位置存放着校正点编号i；M为航迹节点的个数，

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

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

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

其中，M为航迹节点的个数，表示航迹节点e_j(i₁)与e_j+1(i₂)之间的距离；

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

另外，只有垂直误差和水平误差均小于θ个单位时，飞行器才能够按照规划航迹飞行；

所以，航迹节点e_j(i₁)校正之前的垂直误差和水平误差均应小于θ，即

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

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

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

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

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

sv_i＝t_i·rv_i

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

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

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

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

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

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

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

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

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

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

点e_j-1,e_j,e_j+1表示无人飞行器航迹中任意相邻的3个节点，两节点之间的连线代表飞行方向，点e_j-1,e_j,e_j+1的坐标分别是则无人飞行器从节点e_j-1飞到节点e_j的飞行方向即方向向量为

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

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

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

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

相邻航迹节点之间的距离之和尽可能小，即最小化；要求航迹经过校正点，且在该校正点定位误差校正成功，即最小化；确定目标函数为：

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

(3)采用基于最速下降的改进Dijkstra算法对无人飞行器航迹规划模型进行求解，确定满足多约束条件的航迹规划路径；具体步骤如下：

给定校正点编号I＝{i|i＝0,1,2,...,n}，坐标(x_i,y_i,z_i)i∈I，参数α₁,α₂,β₁,β₂,θ,δ,R₀,e₁＝0,以及航迹节点集E，航迹节点校正之前的定位误差集VH，校正之后的定位误差集VH′；

开始：j＝1

Step1：令E_j＝{e₁,e₂,...,e_j}，I_j＝I\E_j，

Step2:如果n∈E_j，算法终止，转入Step7；否则转入Step3；

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

Step4:记

1)当校正点编号i_j(1)的类型时，令若且则转入Step7；否则，I_j＝I_j\{i_j(1)}，转入Step3；

2)当校正点编号i_j(1)的类型令若且则转入Step7；否则，I_j＝I_j\{i_j(1)}，转入Step3；

Step5：记

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

2)当|cosγ|≠1时，①转弯半径R≥R₀时，转入Step6；②转弯半径R＜R₀时，I_j＝I_j\{i_j(1)}，转入Step3；

Step6：当j≥n时，算法结束，转入Step7；否则j＝j+1，转入Step1；

Step7：输出j，e_j＝i_j(1)，E_j＝{e₁,e₁,...,e_j},