Multi-UAV|多无人机、多场景路径规划MATLAB

无人机(Unmanned Aerial Vehicle，UAV) 是一种无需机载驾驶员的半自主飞行器，由于其灵活度高、机动性强等特点，目前已广泛应用于民用和军用领域，如救援、农业、
输电线路巡检等。但在实际应用中，单个UAV难以应对任务点分散、耗时长、或者在每个任务点的不一致的需求。
无人机机群由于具有较好的鲁棒性和较低的成本，常被用来通过协作来解决该问题。在无人机群协作中，路径规划是最关键的问题之一。无人机生成航迹质量受到安全性和能量消耗的双重影响，如何找到一条安全合理的从出发位置到目的位置的最短路径以避免潜在的威胁成为主要关注的问题。
因此本文KAU针对多无人机路径规划问题提出解决代码及理论，后续也会更新更为完善的多目标解决方案，欢迎关注~

00 文章目录

1 路径规划算法简述
2 航迹规划问题描述
3 代码目录
4 仿真实验
5 源码获取

01 路径规划算法简述

研究者们在过去几十年对路径规划问题进行了大量有价值的研究，目前已经出现了各式各样的算法：
 数学规划法，如混合整数线性规划、非线性规划和动态规划等[1-3]，这些算法适用于解决小规模的路径规划问题，若问题规模增大，则其计算时间将呈指数级增长。
 人工势场和DWA法等，这类算法实时性好，但其中，人工势场当目标产生的引力和障碍物产生的斥力在某些势点上相等时，这种方法容易陷入局部最优[4-5] ，其解的质量取决于势场的建立。而DWA法在障碍物密集复杂环境具有一定的局限性，容易出现避障过度或避障失败的问题。
 基于图论的算法，如A*、RRT算法等[6,7]。这类算法能有效地找到最优路径，而其性能取决于搜索空间的划分。因此，当搜索空间非常复杂时，这种方法在路径规划中总是需要很长的执行时间。
 智能算法，如GA、ACO和PSO等[8-10]，这类算法可以在有限的计算时间内为无人机规划满足指定要求的可飞行路径。由于计算智能算法具有简单、计算成本低、性能好等优点，近年来被广泛应用于复杂路径规划问题。但不同的优化算法其寻优性能不同，应用于不同的场景或约束时，其效果不尽相同，因此也激励着各类改进算法的出现。

02 航迹规划问题描述

我们假设这是一个已知的环境。环境的一个示例如下图所示。采用绝对笛卡尔坐标系，便于描述运动环境。除了单无人机需要考虑的威胁物、飞行转弯/高度/长度等成本代价，对于多无人机协同作战，如何避免相互碰撞也是重要问题。因此该问题的目标是在出发位置、目的位置以及环境因素和威胁都固定的情况下，引导多架无人机以最小的成本代价飞向不同的目的位置，同时避免碰撞和威胁。
起始点和目标点分别定义为S和G；威胁源为雷达、火炮和禁飞区。雷达和火炮分别以绿色和红色虚线圆圈的形式表示。这些虚线圆圈被定义为无人机有一定概率受到攻击的威胁区域，无人机飞出该区域时不受攻击。红色虚线矩形代表禁飞区，在禁飞区内禁止无人机飞行。无人机航迹优化除了最小的长度成本外，飞行路径还需要得到最小的高度成本和最小的来自雷达和火炮的威胁代价，满足偏航角、爬升角和禁飞区的约束函数。

1.1 路径表示及平滑
(1)路径表示
本文的研究中，KAU假设无人机飞行速度固定，威胁区域位置已知。我们为每架无人机设置了航迹控制点，每架无人机的航路由起始位置S、目的地位置G以及S和G之间的N-2个航路点组成，航路点均匀分布。无人机i的航路点j的坐标为：

其中，i={1,2,…,I}；j={1,2,…,N-2}。I为无人机总数。
由此可知，无人机i的完整路径可表示为：

(2)路径平滑
在规划过程中，无人机飞行路径由N-2个航路点组成。实际上，路径规划就是航路点位置的优化。但由于固定翼无人机的物理约束，规划的航迹不能满足可飞条件。因此，有必要引入一种可行的平滑策略。本文KAU引入B样条曲线方法应用于平滑最优路径，该方法被广泛地用于路径平滑问题。
假设某个无人机的待优化的某个航迹为Pi，则B样条曲线方法由Pi，B样条基函数为Ni,k(u)，以及结点向量U=[u0,u1,…,um]构成，曲线方程表示如下：

基函数Ni,k(u)由de Cox-Boor公式更新(k是阶)。

(3)算法编码
算法中的编码与KAU往期中的单无人机路径规划中的编码一致，链接如下：

1.2 无人机航迹规划问题模型
无人机航迹规划是一个多约束、多目标的复杂优化问题，包括威胁、地形障碍、无障碍区、无人机物理特性等。本文的建模如下：
1.2.1 成本
(1)威胁成本
在作战环境中，敌方威胁（包括雷达威胁、火炮威胁）会导致无人机使命失败，需要在路径规划过程中进行规避。
⚪雷达
雷达探测的概率主要取决于无人机航迹点与雷达之间的距离，其由以下表达式描述。

其中，FRadar为雷达威胁成本；dr是无人机与雷达威胁中心的距离；雷达中心为(xR,yR,zR)；Rt_r是雷达探测最大半径；k为雷达威胁程度。由计算式可看出，无人机与雷达中心越近，则被监测到的威胁越大。

⚪火炮
无人机若在火炮攻击范围内则处于危险之中。火炮杀伤概率模型可以表示为。

其中，Fartillery是火炮威胁成本；da是无人机与火炮中心的距离，Rt_a是火炮杀伤最大半径。同样，若无人机与火炮中心越近，则危险越大。

(2)长度成本
长度成本是无人机飞行路径的长度，在飞行速度一定的情况下，路径距离越短，无人机消耗的能量越少，因此优选较短的路径。比值FLength通常用于表示路径长度成本，相应的函数可以计算如下：

其中，li是从航迹点i到i+1的距离；LEC是从起始点S(xs,ys,zs)到目标点G(xg,yg,zg)之间的欧几里得距离。

(3)高度成本
为了保证无人机飞行更加安全，其飞行高度不能太高；同时无人机也不能太低，其通常处于两个给定的极值之间，即最大高度和最小高度。以地面测绘为例，无人机所搭载的摄像机需要满足特定的分辨率和视场，才能满足数据收集的要求，故而就需要对飞行高度进行限制。高度成本FHeight表示如下：

式中，hmin和hmax为最小飞行高度和最大飞行高度；hi是无人机在航迹点i的飞行高度。公式中通过对超出飞行高度上下极值的进行惩罚，继而使得得以保持平均高度。

多无人机协同航路一方面要满足单架无人机自身物理性能的约束，另一方面要符合多机间协同约束的要求，本文将采用惩罚函数的方式对无人机的各项约束进行处理，单机约束与多机协同约束如下：
1.2.2 单机约束处理
(1) 最大航偏角
无人机的最大偏航角是指无人机飞行航迹在水平面上的投影变化的最大角度无人机受自身性能和惯性的约束，如果在飞行过程中需要改变飞行方向，则只能在最大航偏角度范围内转弯，并不能以任意角度航行。

其中，αi为当前的航迹线段相对于上一条航迹片段的偏移角。
(2)最大俯仰角
无人机的最大俯仰角是指在上升或下降过程中无人机机轴与水平面间的最大角度。无人机受自身性能约束，以及为了安全和控制速度，在飞行过程中如果需要爬升或下降，角度必须小于最大俯仰角。

其中，βi即当前航线对应于上一条航线的俯仰角
(3)最小航迹段约束
无人机在飞行过程中从一个姿态转变为另一个姿态，存在一个稳定的变化过程，这一段稳定的航迹称为最小直线航迹距离。同时，除了无人机自身机能的限制，由于无人机与地面控制中心之间的通信存在延迟现象，在高速飞行时还要考虑到延迟时间段的直线飞行距离。因此，在无人机航迹规划的过程中保证最小航迹段是规划出合理航迹的重要前提。设由无人机机能限制以及最大延迟导致的最小航迹段的距离为Lmin，因此航迹中任意直线航迹的距离必须满足条件如下所示：

(4)禁飞区约束
无人机需要避免飞入明文规定的一些区域，比如机场和一些敏感地区，还要避开高压线、高楼以及人群密集的地方等。。禁飞区约束NFZ定义如下：

其中xNFZl和xNFZu是X坐标中NFZ的下限和上限，并且yNFZl和yNFZu是y坐标中的下限和上限

1.2.3 多机协同约束
在多无人机路径规划中，不仅需要考虑避障问题，而且需要对协调关系进行处理，包括空间协同和时间协同。
(1)时间协同约束
所谓时间协同是指多无人机系统在执行任务时，各机到达目标点的时间应该满足某种特定关系，本文主要研究各机同时到达目标点。在真实场景中，无人机速度在一定范围内变化，到达目标点的时间不可能完全相同，因此只要各机到达目标点的时间范围存在交叉，即可认为各机能同时到达目标点。
本文设定的时间协同如下：设UAV到达目标终点的指令时间为tc，通过指令时间约束保证UAV在时间上协同。由UAV的速度范围与航迹长度可求得UAV到达终点的时间范围[tmin,tmax]，即：

其中，timin和timax即为第i个UAV达到终点的时间范围；L和v分别是时间和速度。

即若timin＜tc<timax，则可视为达到编队协同性。（但该方法存在一定问题，比如时间跨度很大怎么办？因此还有优化的空间）

(2)空间协同约束
为了确保多架无人机间免受相互碰撞的危险，需要对多架无人机在沿着为其所规划的路径航行时是否存在过于接近的情况进行检验。为此，需要将多架无人机各自的规划路径上的节点坐标间进行相互比较，若同一时间无人机间的距离小于安全距离，则有剐蹭的风险。

其中，di为无人机的航迹点i与其他无人机在同一时间的距离；dsafe为无人机的安全距离。
本文采用线性加权和法将约束转化为目标进行路径规划（也可视为多目标优化问题），该方法通常用于处理优化的现实问题。整个目标函数被认为是候选路径的评价函数。

1.2.4 目标函数
本文的目标函数由威胁成本、长度成本、高度成本、最大偏航约束、最大俯仰角约束、最小航迹段约束、禁飞区约束、时间协同约束、空间协同约束组成。其中，约束条件以罚函数的形式加入目标函数中，目标函数数学表达式如下：

其中，w为权重矩阵。

1.5 环境建模
本文使用真实的数字高程模型DEM地图进行作为无人机路径规划问题中的环境。选择了2个DEM。对其增添不同的威胁物和禁飞区以对不同复杂度的地形进行模拟，几种地形环境情况如下：

MATLAB中的仿真场景如下：

03 代码目录

代码仅有MATLAB版本，包含乱码解决方案，若运行出现问题可联系我。
部分代码如下：

可以看到，作者已基本整理的和函数测试的代码格式一致，若需修改优化算法，仅需将优化算法的调用格式改成和例程代码一致，优化算法本身的代码无需进行任何修改！。另外，注意事项如下：

04 仿真实验

其中，绘图的结果和我在程序中设定的各成本函数的权值有关。这里展示两个场景的结果：

05 源码获取

公众号(KAU的云实验台)后台回复：MUAV

参考文献

[1]Y. Kuwata, J.P. How, Cooperative distributed robust trajectory optimization using receding horizon MILP, IEEE Trans. Control Syst. Technol. 19 (2011)423–431.
[2] M.G. Earl, R. D’andre, Iterative MILP methods for vehicle control problems,IEEE Trans. Robot. 21 (2005) 1158–1167.
[3] T.R. Jorris, R.G. Cobb, Three-dimensional trajectory optimization satisfying waypoint and no-fly zone constraints, J. Guid. Control Dyn. 32 (2009)551–572.
[4] U. Orozco-Rosas, O. Montiel, R. Sepulveda, Mobile robot path planning using membrane evolutionary artificial potential field, Appl. Soft Comput.77 (2019) 236–251.
[5] Y.B. Chen, G.C. Luo, Y.S. Mei, J.Q. Yu, X.L. Su, UAV path planning using artificial potential field method updated by optimal control theory, Internat.J. Systems Sci. 47 (2016) 1407–1420
[6] S. Bayili, F. Polat, Limited-Damage A*: A path search algorithm that considers damage as a feasibility criterion, Knowl.-Based Syst. 24 (2011)501–512.
[7] Z. Nilforoushan, A. Mohades, M.M. Rezaii, A. Laleh, 3D hyperbolic Voronoi diagrams, Comput. Aided Des. 42 (2010) 759–767.
[8] P. Yang, K. Tang, J.A. Lozano, X.B. Cao, Path planning for single unmanned aerial vehicle by separately evolving waypoints, IEEE Trans. Robot. 31(2015) 1130–1146.
[9] V. Roberge, M. Tarbouchi, G. Labonte, Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning,IEEE Trans. Ind. Inform. 9 (2013) 132–141.
[10] D. Pandit, L. Zhang, S. Chattopadhyay, C.P. Lim, C.Y. Liu, A scattering and repulsive swarm intelligence algorithm for solving global optimization problems, Knowl.-Based Syst. 156 (2018) 12–42.

另：如果有伙伴有待解决的优化问题（各种领域都可），可以发我，我会选择性的更新利用优化算法解决这些问题的文章。

如果这篇文章对你有帮助或启发，可以点击右下角的赞/再看 (ง •̀_•́)ง(不点也行)，若有定制需求，可私信作者。