多约束条件下无人机快速航迹规划方法

本发明属于无人机应用技术领域，具体涉及多约束条件下无人机快速航迹规划方法，用于复杂环境下无人机的智能航迹快速规划。

背景技术：

在复杂环境下，无人机的快速航迹规划是新型智能无人机飞行控制一个很难攻克的课题。由于无人机的结构限制，定位系统无法对其自身进行精准定位，一旦定位误差积累到一定程度可能会导致航迹规划失败。因此，在飞行过程中对定位误差进行校正是智能无人机航迹规划中一项重要任务。

技术实现要素：

为克服上述现有技术的不足，本发明的目的在于提供多约束条件下无人机快速航迹规划方法，通过考虑限制无人机的主要因素和限制无人机飞行的环境因素，通过改进的dijkstra算法对无人机设计了一个快速航迹规划；具有简单、快速、动态及智能的特点。

为实现上述目的，本发明采用的技术方案是：多约束条件下无人机快速航迹规划方法，其特征在于，包括以下步骤：

步骤1，将无人机飞行环境中的各校正点简化为无向图中的0-1模型，简化无人机对校正点的选取；

步骤2，无人机航迹规划，设置无人机飞行网络的无向结构图，得到各校正点之间的距离；

步骤3，根据校正节点的数据处理得到无人机飞行网络图s，无人机在航迹上要求尽可能经过少的飞行误差校正节点，将无人机的航迹规划简化为一个多目标动态规划问题，即选择最优路线和最佳误差点数量和的问题，将航迹规划转化为了0-1整数规划问题；

步骤4，采用改进的dijkstra算法将设置的目标函数求解，得到的结果就是最优的航迹规划路径。

所述的步骤1，具体做法是：设智能无人机的出发点为a点，目的地为b点，其航行需要满足如下约束条件：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，设定航迹达标条件为到达终点时垂直误差和水平误差均应小于θ个单位，即当垂直误差和水平误差均小于θ个单位时，无人机仍能够按照规划路径飞行；

2)无人机在飞行过程中需要对定位误差进行校正，飞行区域中存在一些校正点可用于误差校正，当无人机到达校正点即可根据该位置的误差校正类型进行误差校正，矫正点的位置可根据地形在航迹规划前确定；可校正的飞行区域分布位置依赖于地形，无统一规律，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

所述的步骤2，具体做法是：

首先，将无人机飞行区域中的a、b及校正点的坐标进行数据处理，构建无人机飞行邻接距离矩阵，邻接距离矩阵的基本原理是将图中n个顶点的数据存放在一组一维数组中，用一个n×n矩阵的形式来表示各个顶点间的邻接关系，并在邻接距离矩阵中剔除不满足条件的校正节点关系，构建无人机飞行网络图；设置一个无向图d，根据无向图d的各个顶点之间是否可以直接连接，编写一个矩阵，1表示可以直接连接，0表示不能直接连接，定义如下：

a＝(aij)n×n称作无向图d的邻接距离矩阵，其中n为校正节点的个数加2，其中：

因为任何顶点自身不能相连，所以在任何无向图的邻接矩阵中，主对角线一定为0，其余可以相连的顶点皆为1，由此可得，每个无向图的邻接矩阵皆为关于主对角线对称的n×n的矩阵，邻接矩阵中包含了图的一切性质，邻接矩阵与它所唯一对应的图成一一对应关系；

将无向图转化为无人机飞行网络图，即测量出各个可以相连的校正点之间的距离，将无向图邻接矩阵中的1用实际距离代替，并在邻接距离矩阵中剔除不满足条件的校正节点关系，构建出无人机飞行网络图，其中针对相连的各个顶点之间的距离不满足最大允许调整的水平和垂直误差，用∞表示，即将无向图邻接矩阵中的0用∞代替，得到s＝(sij)n×n是无人机的飞行网络图，其中n为校正节点个数加2，2个点分别为起点a和终点b，其中：

所述的步骤3，又包括如下步骤：

步骤3.1：先规划从起点a到终点b的最短路径，再找出满足约束条件的最优路径使得无人机在从起点a到终点b飞行过程中需要满足飞行路程最短的同时，尽可能经过少的飞行误差校正节点，

首先对所有校正点进行编号：

1)校正点集:c＝[c1,c2,c3…,ci,…cn],i＝1,2,3…,n，其中n为校正点个数；

2)水平校正点集:u＝[u1,u2,u3…ui…um],i＝1,2,3…,m，其中m为水平校正点个数；

3)垂直校正点集：v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中k为垂直校正点个数；

其中校正点集c、水平校正点集u和垂直校正点集v之间的关系为：u∪v＝c，将各校正点分别按照最初的设置加入到水平校正点集u或者垂直校正点集v中；

首先假设无人机从一个校正节点到另一个校正节点之间始终匀速直线运动，忽略转弯引起的轨迹变化，无人机从起点a到终点b的飞行过程中需要依次选择水平校正节点和垂直校正节点进行误差校正，以满足能够在误差允许的范围内到达各个水平或者垂直校正节点进行水平和垂直误差校正，其中需要注意的是无人机到达终点b允许误差和到达各校正节点的允许误差值是不一致的，这是一个多目标航迹规划问题，多目标规划的优化目标是获得在从起点a到终点b的飞行中，航迹路程最小的同时，经过的飞行误差调整点uiorvi,i＝1,2,3…,mork数量最少；

无人机经过水平或者垂直误差校正点的目标函数为：

式中，xij表示0-1变量：

无人机飞行路程目标函数为：

起点or校正点到终点or校正点的路程长度：

式中，xi为i点的横坐标，yi为i点的纵坐标，zi为i点的竖坐标；xj为j点的横坐标，yj为j点的纵坐标，zj为j点的竖坐标；

其中无人机的约束条件包括：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，则无人机从起点或者校正点ci到终点或者下一个校正点cj的误差增量pij为：

因为任意一个误差校正点ci,i＝1,2,3…n在误差允许范围内可调整水平或者垂直误差，则在任意一个校正点ci,i＝1,2,3…n的误差p为：

其中pju表示j到下一个校正点的水平增量，pjv表示j到下一个校正点的垂直增量，piu表示i到下一个校正点的水平增量，piv表示i到下一个校正点的垂直增量；xij表示i到j的0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位；vj表示第j个垂直校正点，uj表示第j个水平校正点，要求到达终点时垂直误差和水平误差均应小于θ个单位，即：

piu+xibsibδ≤θ且piv+xibsibδ≤θ

其中piu表示i到下一个校正点的水平增量，xib表示点i到终点b的0-1变量，sib表示点i到终点b的路程长度，δ为垂直误差和水平误差各增加的单位，θ表示到终点的误差阈值；piv表示i到下一个校正点的垂直增量；

步骤3.2，设无人机的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正，即：

piv+xijsijδ≤α1,vj∈v

piu+xijsijδ≤α2,vj∈v

其中piv表示i到下一个校正点的垂直增量，xij表示0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值，vj表示第j个垂直校正点；piu表示i到下一个校正点的水平增量，α2为水平误差的阈值；

当无人机的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正，即：

piv+xijsijδ≤β1,uj∈u

piu+xijsijδ≤β2,uj∈u

其中piv表示i到下一个校正点的垂直增量，xij表示0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值，uj表示第j个水平校正点；piu表示i到下一个校正点的水平增量，β2为水平误差的阈值；

无人机在飞行过程中会根据最优路径和节点数选择经过水平误差补偿和垂直误差补偿点，在实际计算中常常需要直接约束任意两个水平或者垂直误差的阈值，约束条件如下所示：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值，综上所述，无人机飞行航迹的多目标规划为：

无人机经过水平或者垂直误差校正点的目标函数为：

无人机飞行路程目标函数为：

所述的步骤4，具体做法是：

飞行网络图中有n个顶点，现需要求从校正节点a至校正节点b的最短路径，设s＝(sij)n×n为邻接距离矩阵，其分量为：

其中，ciorcj为a到b的某一校正点时，sij为实际距离；否则sij取∞，决策变量为xij，当xij＝1，说明ci，cj位于起点a至终点b的路上；否则xij＝0，其数学规划表达式为：

xij＝0or1

采用dijkstra算法和动态规划方法求解。

所述的dijkstra算法和动态规划方法求解，具体包括以下步骤：

step1:对ci≠c0，即无人机从起点出发以后，令

其中ci为校正点集中的第i个校正点，c0为起点，其实就是a点，在起点时即i＝0时，飞行网络图取0即s0＝0，当无人机出发以后，i≠0时，飞行网络图当前点即si＝{ci}；

step2:对每个ci∈c，把最小值的定点ci记为yi，令si+1＝si∪{yi+1}；

其中ci为校正点集中的第i个校正点，c为校正点集，yi为暂时存储ci的值，si+1为第i+1个校正节点的飞行网络图，si为第i个校正节点的飞行网络图，yi+1存储的是ci+1的值；

step3:若i＝n-1，停止计算；若i<n-1，用i+1替代i，转到step2；其中n为校正节点的个数加2；

step4:将得到的路径中的每一个ci，按水平或者垂直校正节点类型分别标记为水平校正点集u＝[u1,u2,u3…uk…um],i＝1,2,3…,m和垂直校正点集v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中m和k分别为路径中的水平校正点和垂直校正点个数；

其中ci为校正点集中的第i个校正点，u＝[u1,u2,u3…ui…um],i＝1,2,3…,m为水平校正点集，v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k为垂直校正点集；

step5:对路径中的水平或者垂直误差校正点进行约束优化，约束优化条件如下：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值；

step6:若满足约束条件，则跳转至step7，否则跳转至step2；

step7：结束。

本发明的有益效果是，

1)通过简化无人机飞行环境中的校正点为0-1模型，将无人机的飞行环境简化为一些可用和不可用的节点集合，简化了飞行环境。

2)将选取最优航迹规划的问题，简化为选择最优路线和最佳误差点数量和的问题，可以简化航迹规划的目标函数，便于求解。

附图说明

图1是无人机的飞行区域；

图2是无人机航迹规划流程图；

图3是改进dijkstra算法流程图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

多约束条件下无人机快速航迹规划方法，其特征在于，包括以下步骤：

步骤1，将无人机飞行环境中的各校正点简化为无向图中的0-1模型，简化无人机对校正点的选取；

步骤2，无人机航迹规划，设置无人机飞行网络的无向结构图，得到各校正点之间的距离；

步骤3，根据校正节点的数据处理得到无人机飞行网络图s，无人机在航迹上要求尽可能经过少的飞行误差校正节点，将无人机的航迹规划简化为一个多目标动态规划问题，即选择最优路线和最佳误差点数量和的问题，将航迹规划转化为了0-1整数规划问题；

步骤4，采用改进的dijkstra算法将设置的目标函数求解，得到的结果就是最优的航迹规划路径。

所述的步骤1，具体做法是：设智能无人机的出发点为a点，目的地为b点，其航行需要满足如下约束条件：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，设定航迹达标条件为到达终点时垂直误差和水平误差均应小于θ个单位，即当垂直误差和水平误差均小于θ个单位时，无人机仍能够按照规划路径飞行；

2)无人机在飞行过程中需要对定位误差进行校正，飞行区域中存在一些校正点可用于误差校正，当无人机到达校正点即可根据该位置的误差校正类型进行误差校正，矫正点的位置可根据地形在航迹规划前确定；可校正的飞行区域分布位置依赖于地形，无统一规律，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

所述的步骤2，具体做法是：

首先，将无人机飞行区域中的a、b及校正点的坐标进行数据处理，构建无人机飞行邻接距离矩阵，邻接距离矩阵的基本原理是将图中n个顶点的数据存放在一组一维数组中，用一个n×n矩阵的形式来表示各个顶点间的邻接关系，并在邻接距离矩阵中剔除不满足条件的校正节点关系，构建无人机飞行网络图；设置一个无向图d，根据无向图d的各个顶点之间是否可以直接连接，编写一个矩阵，1表示可以直接连接，0表示不能直接连接，定义如下：

a＝(aij)n×n称作无向图d的邻接距离矩阵，其中n为校正节点的个数加2，其中：

因为任何顶点自身不能相连，所以在任何无向图的邻接矩阵中，主对角线一定为0，其余可以相连的顶点皆为1，由此可得，每个无向图的邻接矩阵皆为关于主对角线对称的n×n的矩阵，邻接矩阵中包含了图的一切性质，邻接矩阵与它所唯一对应的图成一一对应关系；

将无向图转化为无人机飞行网络图，即测量出各个可以相连的校正点之间的距离，将无向图邻接矩阵中的1用实际距离代替，并在邻接距离矩阵中剔除不满足条件的校正节点关系，构建出无人机飞行网络图，其中针对相连的各个顶点之间的距离不满足最大允许调整的水平和垂直误差，用∞表示，即将无向图邻接矩阵中的0用∞代替，得到s＝(sij)n×n是无人机的飞行网络图，其中n为校正节点个数加2，2个点分别为起点a和终点b，其中：

所述的步骤3，又包括如下步骤：

步骤3.1：先规划从起点a到终点b的最短路径，再找出满足约束条件的最优路径使得无人机在从起点a到终点b飞行过程中需要满足飞行路程最短的同时，尽可能经过少的飞行误差校正节点，

首先对所有校正点进行编号：

1)校正点集:c＝[c1,c2,c3…,ci,…cn],i＝1,2,3…,n，其中n为校正点个数；

2)水平校正点集:u＝[u1,u2,u3…ui…um],i＝1,2,3…,m，其中m为水平校正点个数；

3)垂直校正点集：v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中k为垂直校正点个数；

其中校正点集c、水平校正点集u和垂直校正点集v之间的关系为：u∪v＝c，将各校正点分别按照最初的设置加入到水平校正点集u或者垂直校正点集v中；

首先假设无人机从一个校正节点到另一个校正节点之间始终匀速直线运动，忽略转弯引起的轨迹变化，无人机从起点a到终点b的飞行过程中需要依次选择水平校正节点和垂直校正节点进行误差校正，以满足能够在误差允许的范围内到达各个水平或者垂直校正节点进行水平和垂直误差校正，其中需要注意的是无人机到达终点b允许误差和到达各校正节点的允许误差值是不一致的，这是一个多目标航迹规划问题，多目标规划的优化目标是获得在从起点a到终点b的飞行中，航迹路程最小的同时，经过的飞行误差调整点uiorvi,i＝1,2,3…,mork数量最少；

无人机经过水平或者垂直误差校正点的目标函数为：

式中，xij表示0-1变量：

无人机飞行路程目标函数为：

起点or校正点到终点or校正点的路程长度：

式中，xi为i点的横坐标，yi为i点的纵坐标，zi为i点的竖坐标；xj为j点的横坐标，yj为j点的纵坐标，zj为j点的竖坐标；

其中无人机的约束条件包括：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，则无人机从起点或者校正点ci到终点或者下一个校正点cj的误差增量pij为：

因为任意一个误差校正点ci,i＝1,2,3…n在误差允许范围内可调整水平或者垂直误差，则在任意一个校正点ci,i＝1,2,3…n的误差p为：

其中pju表示j到下一个校正点的水平增量，pjv表示j到下一个校正点的垂直增量，piu表示i到下一个校正点的水平增量，piv表示i到下一个校正点的垂直增量；xij表示i到j的0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位；vj表示第j个垂直校正点，uj表示第j个水平校正点，要求到达终点时垂直误差和水平误差均应小于θ个单位，即：

piu+xibsibδ≤θ且piv+xibsibδ≤θ

其中piu表示i到下一个校正点的水平增量，xib表示点i到终点b的0-1变量，sib表示点i到终点b的路程长度，δ为垂直误差和水平误差各增加的单位，θ表示到终点的误差阈值；piv表示i到下一个校正点的垂直增量；

步骤3.2，设无人机的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正，即：

piv+xijsijδ≤α1,vj∈v

piu+xijsijδ≤α2,vj∈v

其中piv表示i到下一个校正点的垂直增量，xij表示0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值，vj表示第j个垂直校正点；piu表示i到下一个校正点的水平增量，α2为水平误差的阈值；

当无人机的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正，即：

piv+xijsijδ≤β1,uj∈u

piu+xijsijδ≤β2,uj∈u

其中piv表示i到下一个校正点的垂直增量，xij表示0-1变量，sij表示i到j的路程长度，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值，uj表示第j个水平校正点；piu表示i到下一个校正点的水平增量，β2为水平误差的阈值，

无人机在飞行过程中会根据最优路径和节点数选择经过水平误差补偿和垂直误差补偿点，在实际计算中常常需要直接约束任意两个水平或者垂直误差的阈值，约束条件如下所示：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值，综上所述，无人机飞行航迹的多目标规划为：

无人机经过水平或者垂直误差校正点的目标函数为：

无人机飞行路程目标函数为：

所述的步骤4，具体做法是：

飞行网络图中有n个顶点，现需要求从校正节点a至校正节点b的最短路径，设s＝(sij)n×n为邻接距离矩阵，其分量为：

其中，ciorcj为a到b的某一校正点时，sij为实际距离；否则sij取∞，决策变量为xij，当xij＝1，说明ci，cj位于起点a至终点b的路上；否则xij＝0，其数学规划表达式为：

xij＝0or1

采用dijkstra算法和动态规划方法求解。

所述的dijkstra算法和动态规划方法求解，具体包括以下步骤：

step1:对ci≠c0，即无人机从起点出发以后，令

其中ci为校正点集中的第i个校正点，c0为起点，其实就是a点，在起点时即i＝0时，飞行网络图取0即s0＝0，当无人机出发以后，i≠0时，飞行网络图当前点即si＝{ci}；

step2:对每个ci∈c，把最小值的定点ci记为yi，令si+1＝si∪{yi+1}；

其中ci为校正点集中的第i个校正点，c为校正点集，yi为暂时存储ci的值，si+1为第i+1个校正节点的飞行网络图，si为第i个校正节点的飞行网络图，yi+1存储的是ci+1的值；

step3:若i＝n-1，停止计算；若i<n-1，用i+1替代i，转到step2；其中n为校正节点的个数加2；

step4:将得到的路径中的每一个ci，按水平或者垂直校正节点类型分别标记为水平校正点集u＝[u1,u2,u3…uk…um],i＝1,2,3…,m和垂直校正点集v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中m和k分别为路径中的水平校正点和垂直校正点个数；

其中ci为校正点集中的第i个校正点，u＝[u1,u2,u3…ui…um],i＝1,2,3…,m为水平校正点集，v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k为垂直校正点集；

step5:对路径中的水平或者垂直误差校正点进行约束优化，约束优化条件如下：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值；

step6:若满足约束条件，则跳转至step7，否则跳转至step2；

step7：结束。

在复杂环境下，考虑无人机在自身条件的约束下对其飞行路径进行规划。首先将无人机飞行的飞行环境中的各校正点简化为无向图中的0-1模型，这样可以简化无人机对校正点的选取；然后将选取最优航迹规划的问题简化为选择最优路线和最佳误差点数量和的问题，可以将航迹规划的目标函数简化；最后采用改进的dijkstra算法将设置的目标函数求解，得到的结果就是最优的航迹规划路径。

多约束条件下无人机快速航迹规划方法，包括以下步骤：

步骤1，设智能无人机的出发点为a点，目的地为b点。其航行需要满足如下约束条件：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，设定航迹达标条件为到达终点时垂直误差和水平误差均应小于θ个单位，即当垂直误差和水平误差均小于θ个单位时，无人机仍能够按照规划路径飞行。

2)无人机在飞行过程中需要对定位误差进行校正。飞行区域中存在一些校正点可用于误差校正，当无人机到达校正点即可根据该位置的误差校正类型进行误差校正。矫正点的位置可根据地形在航迹规划前确定(如图1中黄色的点为水平误差校正点，蓝色的点为垂直误差校正点，黑色曲线代表规划的一条航迹)。可校正的飞行区域分布位置依赖于地形，无统一规律。若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

步骤2，图2为无人机航迹规划流程图。首先将无人机飞行区域中的a、b及校正点的坐标进行数据处理，构建无人机飞行邻接距离矩阵。邻接距离矩阵的基本原理是将图中n个顶点的数据存放在一组一维数组中，用一个n×n矩阵的形式来表示各个顶点间的邻接关系。并在邻接距离矩阵中剔除不满足条件的校正节点关系，从而构建无人机飞行网络图。设置一个无向图d，根据d的各个顶点之间是否可以直接连接，编写一个矩阵，1表示可以直接连接，0表示不能直接连接，定义如下：

a＝(aij)n×n称作无向图d的邻接距离矩阵，其中n为校正节点的个数加2，其中

因为任何顶点自身不能相连，所以在任何无向图的邻接矩阵中，主对角线一定为0，其余可以相连的顶点皆为1。由此可得，每个无向图的邻接矩阵皆为关于主对角线对称的n×n的矩阵。邻接矩阵中包含了图的一切性质，邻接矩阵与它所唯一对应的图成一一对应关系。

将无向图转化为无人机飞行网络图，即测量出各个可以相连的校正点之间的距离，将无向图邻接矩阵中的“1”用实际距离代替，并在邻接距离矩阵中剔除不满足条件的校正节点关系，构建出无人机飞行网络图。其中针对相连的各个顶点之间的距离不满足最大允许调整的水平和垂直误差，用“∞”表示，即将无向图邻接矩阵中的“0”用“∞”代替。得到s＝(sij)n×n是无人机的飞行网络图，其中n为校正节点个数加2，2个点分别为起点a和终点b，其中：

步骤3：根据校正节点的数据处理得到无人机飞行网络图s，无人机在航迹上要求尽可能经过少的飞行误差校正节点，将无人机的航迹规划简化为一个多目标动态规划问题，即选择最优路线和最佳误差点数量和的问题，无人机在飞行过程中经过校正节点ci到校正节点cj，则选中无人机飞行网络图中的边sij，加入最优飞行路径中，否则sij取零，因此将航迹规划转化为了0-1整数规划问题；

步骤3.1：先规划从起点a到终点b的最短路径，再找出满足约束条件的最优路径使得无人机在从起点a到终点b飞行过程中需要满足飞行路程最短的同时，尽可能经过少的飞行误差校正节点。

首先对所有校正点进行编号：

1)校正点集:c＝[c1,c2,c3…,ci,…cn],i＝1,2,3…,n，其中n为校正点个数；

2)水平校正点集:u＝[u1,u2,u3…ui…um],i＝1,2,3…,m，其中m为水平校正点个数；

3)垂直校正点集：v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中k为垂直校正点个数；

其中校正点集c、水平校正点集u和垂直校正点集v之间的关系为u∪v＝c，将各校正点分别按照最初的设置加入到水平校正点集u或者垂直校正点集v中；

首先假设无人机从一个校正节点到另一个校正节点之间始终匀速直线运动，忽略转弯引起的轨迹变化。无人机从起点a到终点b的飞行过程中需要依次选择水平校正节点和垂直校正节点进行误差校正，以满足能够在误差允许的范围内到达各个水平或者垂直校正节点进行水平和垂直误差校正；其中需要注意的是无人机到达终点b允许误差和到达各校正节点的允许误差值是不一致的，这是一个多目标航迹规划问题。多目标规划的优化目标是获得在从起点a到终点b的飞行中，航迹路程最小的同时，经过的飞行误差调整点uiorvi,i＝1,2,3…,morq数量最少；

无人机飞行路程目标函数为：

无人机经过水平或者垂直误差校正点的目标函数为：

式中，0-1变量：

起点or校正点到终点or校正点的路程长度：

其中无人机的约束条件包括：

1)无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。无人机每飞行1m，垂直误差和水平误差将各增加δ个单位，则无人机从起点或者校正点ci到终点或者下一个校正点cj的误差增量p为：

式中(a，b)为1×2维矩阵，因为任意一个误差校正点ci,i＝1,2,3…n在误差允许范围内可调整水平或者垂直误差。则在任意一个校正点ci,i＝1,2,3…n的剩余误差为：

其中pju表示校正点ci,i＝1,2,3…n的水平剩余误差；pjv表示垂直剩余误差。要求到达终点时垂直误差和水平误差均应小于个单位，即：

piu+xibsibδ≤θ且piv+xibsibδ≤θ

步骤3.2：设无人机的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正，即：

piv+xijsijδ≤α1,vj∈v

piu+xijsijδ≤α2,vj∈v

当无人机的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正，即：

piv+xijsijδ≤β1,uj∈u

piu+xijsijδ≤β2,uj∈u

无人机在飞行过程中会根据最优路径和节点数选择经过水平误差补偿和垂直误差补偿点，在实际计算中常常需要直接约束任意两个水平或者垂直误差的阈值，约束条件如下所示：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值。

综上所述，无人机飞行航迹的多目标规划为：

无人机经过水平或者垂直误差校正点的目标函数为：

无人机飞行路程目标函数为：

步骤4：通过改进dijkstra算法和动态规划方法对上述模型求解。图3为改进dijkstra算法流程图。

飞行网络图中有n个顶点，现需要求从校正节点a至校正节点b的最短路径。设s＝(sij)n×n为邻接距离矩阵，其分量为：

当xij＝1时，说明ci,cj位于校正节点a至校正节点b的路上；否则xij＝0。

采用dijkstra算法和动态规划方法求解，具体步骤如下：

step1:对ci≠c0，即无人机从起点出发以后，令

其中ci为校正点集中的第i个校正点，c0为起点，其实就是a点，在起点时即i＝0时，飞行网络图取0即s0＝0，当无人机出发以后，i≠0时，飞行网络图当前点即si＝{ci}；

step2:对每个ci∈c，把最小值的定点ci记为yi，令si+1＝si∪{yi+1}；

其中ci为校正点集中的第i个校正点，c为校正点集，yi为暂时存储ci的值，si+1为第i+1个校正节点的飞行网络图，si为第i个校正节点的飞行网络图，yi+1存储的是ci+1的值；

step3:若i＝n-1，停止计算；若i<n-1，用i+1替代i，转到step2；其中n为校正节点的个数加2；

step4:将得到的路径中的每一个ci，按水平或者垂直校正节点类型分别标记为水平校正点集u＝[u1,u2,u3…uk…um],i＝1,2,3…,m和垂直校正点集v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k，其中m和k分别为路径中的水平校正点和垂直校正点个数；

其中ci为校正点集中的第i个校正点，u＝[u1,u2,u3…ui…um],i＝1,2,3…,m为水平校正点集，v＝[v1,v2,v3…vi…vk],i＝1,2,3…,k为垂直校正点集；

step5:对路径中的水平或者垂直误差校正点进行约束优化，约束优化条件如下：

其中表示垂直校正点vk-1与vk间的垂直误差，δ为垂直误差和水平误差各增加的单位，α1为垂直误差的阈值；表示水平校正点vk-1与vk间的水平误差，δ为垂直误差和水平误差各增加的单位，β1为垂直误差的阈值；

step6:若满足约束条件，则跳转至step7，否则跳转至step2。

step7：结束。