当前位置: 首页 > article >正文

基于雪雁算法(Snow Geese Algorithm,SGA)的多个无人机协同路径规划(可以自定义无人机数量及起始点),MATLAB代码

一、雪雁算法

雪雁算法(Snow Geese Algorithm,SGA)是2024年提出的一种新型元启发式算法,其灵感来源于雪雁的迁徙行为,特别是它们在迁徙过程中形成的独特“人字形”和“直线”飞行模式。该算法通过模拟雪雁的飞行行为,实现了在解空间中的高效搜索和优化。SGA算法主要分为三个阶段:初始化阶段、探索阶段和开发阶段。
在这里插入图片描述

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

参考文献:
[1]Bian, H., Li, C., Liu, Y. et al. Improved snow geese algorithm for engineering applications and clustering optimization. Sci Rep 15, 4506 (2025). https://doi.org/10.1038/s41598-025-88080-7
[2]Tian A Q , Liu F F , Lv H X .Snow Geese Algorithm: A novel migration-inspired meta-heuristic algorithm for constrained engineering optimization problems[J].Applied Mathematical Modelling, 2024, 126:327-347.DOI:10.1016/j.apm.2023.10.045.

2. 无人机路径规划数学模型

2.1 路径最优性

为了提高无人机的操作效率,规划的路径需要在特定的应用标准下达到最优。在我们的研究中,主要关注空中摄影、测绘和表面检查,因此选择最小化路径长度作为优化目标。由于无人机通过地面控制站(GCS)进行控制,飞行路径 X i X_i Xi 被表示为无人机需要飞越的一系列 n n n 个航路点的列表。每个航路点对应于搜索地图中的一个路径节点,其坐标为 P i j = ( x i j , y i j , z i j ) P_{ij} = (x_{ij}, y_{ij}, z_{ij}) Pij=(xij,yij,zij)。通过表示两个节点之间的欧几里得距离为 $| \overrightarrow{P_{ij}P_{i,j+1}} |,与路径长度相关的成本 F 1 F_1 F1 可以计算为:

F 1 ( X ) = ∑ j = 1 n − 1 ∥ P i j P i , j + 1 → ∥ F_1(X) = \sum_{j=1}^{n-1} \| \overrightarrow{P_{ij}P_{i,j+1}} \| F1(X)=j=1n1PijPi,j+1

2.2 安全性和可行性约束

除了最优性之外,规划的路径还需要确保无人机的安全操作,引导其避开操作空间中可能出现的威胁,这些威胁通常由障碍物引起。设 K K K 为所有威胁的集合,每个威胁被假设为一个圆柱体,其投影的中心坐标为 C k C_k Ck,半径为 R k R_k Rk,如下图 所示。
在这里插入图片描述

对于给定的路径段 ∥ P i j P i , j + 1 → ∥ \| \overrightarrow{P_{ij}P_{i,j+1}} \| PijPi,j+1 ,其相关的威胁成本与它到 C k C_k Ck 的距离 d k d_k dk 成正比。考虑到无人机的直径 D D D 和到碰撞区域的危险距离 S S S,威胁成本 F 2 F_2 F2 在障碍物集合 K K K 上计算如下:

F 2 ( X i ) = ∑ j = 1 n − 1 ∑ k = 1 K T k ( P i j P i , j + 1 → ) , F_2(X_i) = \sum_{j=1}^{n-1} \sum_{k=1}^K T_k(\overrightarrow{P_{ij}P_{i,j+1}}), F2(Xi)=j=1n1k=1KTk(PijPi,j+1 ),

其中

T k ( P i j P i , j + 1 → ) = { 0 , if  d k > S + D + R k ( S + D + R k ) − d k , if  D + R k < d k ≤ S + D + R k ∞ , if  d k ≤ D + R k T_k(\overrightarrow{P_{ij}P_{i,j+1}}) = \begin{cases} 0, & \text{if } d_k > S + D + R_k \\ (S + D + R_k) - d_k, & \text{if } D + R_k < d_k \leq S + D + R_k \\ \infty, & \text{if } d_k \leq D + R_k \end{cases} Tk(PijPi,j+1 )= 0,(S+D+Rk)dk,,if dk>S+D+Rkif D+Rk<dkS+D+Rkif dkD+Rk

在操作过程中,飞行高度通常被限制在给定的最小和最大高度之间,例如在调查和搜索应用中,需要相机以特定的分辨率和视场收集视觉数据,从而限制飞行高度。设最小和最大高度分别为 h min h_{\text{min}} hmin h max h_{\text{max}} hmax。与航路点 P i j P_{ij} Pij 相关的高度成本计算为:

H i j = { ∣ h i j − h max + h min 2 ∣ , if  h min ≤ h i j ≤ h max ∞ , otherwise H_{ij} = \begin{cases} |h_{ij} - \frac{h_{\text{max}} + h_{\text{min}}}{2}|, & \text{if } h_{\text{min}} \leq h_{ij} \leq h_{\text{max}} \\ \infty, & \text{otherwise} \end{cases} Hij={hij2hmax+hmin,,if hminhijhmaxotherwise

其中 h i j h_{ij} hij 表示相对于地面的飞行高度,如下图所示。
在这里插入图片描述

可以看出, H i j H_{ij} Hij 保持平均高度并惩罚超出范围的值。对所有航路点求和得到高度成本:

F 3 ( X ) = ∑ j = 1 n H i j F_3(X) = \sum_{j=1}^n H_{ij} F3(X)=j=1nHij

平滑成本评估转弯率和爬升率,这对于生成可行路径至关重要。如下图 所示。
在这里插入图片描述

转弯角 ϕ i j \phi_{ij} ϕij 是两个连续路径段 P i j ′ P i , j + 1 ′ → \overrightarrow{P'_{ij}P'_{i,j+1}} PijPi,j+1 P i , j + 1 ′ P i , j + 2 ′ → \overrightarrow{P'_{i,j+1}P'_{i,j+2}} Pi,j+1Pi,j+2 在水平面 Oxy 上的投影之间的角度。设 k → \overrightarrow{k} k 是 z 轴方向的单位向量,投影向量可以计算为:

P i j ′ P i , j + 1 ′ → = k → × ( P i j P i , j + 1 → × k → ) \overrightarrow{P'_{ij}P'_{i,j+1}} = \overrightarrow{k} \times (\overrightarrow{P_{ij}P_{i,j+1}} \times \overrightarrow{k}) PijPi,j+1 =k ×(PijPi,j+1 ×k )

因此,转弯角计算为:

ϕ i j = arctan ⁡ ( ∥ P i j ′ P i , j + 1 ′ → × P i , j + 1 ′ P i , j + 2 ′ → ∥ P i j P i , j + 1 ′ → ⋅ P i , j + 1 ′ P i , j + 2 ′ → ) \phi_{ij} = \arctan\left( \frac{\| \overrightarrow{P'_{ij}P'_{i,j+1}} \times \overrightarrow{P'_{i,j+1}P'_{i,j+2}} \|}{\overrightarrow{P_{ij}P'_{i,j+1}} \cdot \overrightarrow{P'_{i,j+1}P'_{i,j+2}}} \right) ϕij=arctan PijPi,j+1 Pi,j+1Pi,j+2 PijPi,j+1 ×Pi,j+1Pi,j+2

爬升角 ψ i j \psi_{ij} ψij 是路径段 P i j P i , j + 1 → \overrightarrow{P_{ij}P_{i,j+1}} PijPi,j+1 与其在水平面上的投影 P i j ′ P i , j + 1 ′ → \overrightarrow{P'_{ij}P'_{i,j+1}} PijPi,j+1 之间的角度,由下式给出:

ψ i j = arctan ⁡ ( z i , j + 1 − z i j ∥ P i j ′ P i , j + 1 ′ → ∥ ) \psi_{ij} = \arctan\left( \frac{z_{i,j+1} - z_{ij}}{\| \overrightarrow{P'_{ij}P'_{i,j+1}} \|} \right) ψij=arctan PijPi,j+1 zi,j+1zij

然后,平滑成本计算为:

F 4 ( X ) = a 1 ∑ j = 1 n − 2 ϕ i j + a 2 ∑ j = 1 n − 1 ∣ ψ i j − ψ j − 1 ∣ F_4(X) = a_1 \sum_{j=1}^{n-2} \phi_{ij} + a_2 \sum_{j=1}^{n-1} |\psi_{ij} - \psi_{j-1}| F4(X)=a1j=1n2ϕij+a2j=1n1ψijψj1

其中 a 1 a_1 a1 a 2 a_2 a2 分别是转弯角和爬升角的惩罚系数。

2.3 总体成本函数

2.3.1 单个无人成本计算

考虑到路径 X X X 的最优性、安全性和可行性约束, i i i 个无人机总体成本函数可以定义为以下形式:

f i ( X ) = ∑ k = 1 4 b k F k ( X i ) f_i(X) = \sum_{k=1}^4 b_k F_k(X_i) fi(X)=k=14bkFk(Xi)

其中 b k b_k bk 是权重系数, F 1 ( X i ) F_1(X_i) F1(Xi) F 4 ( X i ) F_4(X_i) F4(Xi) 分别是路径长度、威胁、平滑度和飞行高度相关的成本。决策变量是 X X X,包括 n n n 个航路点 P i j = ( x i j , y i j , z i j ) P_{ij} = (x_{ij}, y_{ij}, z_{ij}) Pij=(xij,yij,zij) 的列表,使得 P i j ∈ O P_{ij} \in O PijO,其中 O O O 是无人机的操作空间。根据这些定义,成本函数 F F F 是完全确定的,可以作为路径规划过程的输入。

2.3.2 多无人机总成本计算

若共有 m m m 个无人机,其总成本为单个无人机成本和,计算公式如下:
f i t n e s s ( X ) = ∑ i = 1 m f i ( X ) fitness(X) = \sum_{i=1}^mf_i(X) fitness(X)=i=1mfi(X)
参考文献:
[1] Phung M D , Ha Q P .Safety-enhanced UAV path planning with spherical vector-based particle swarm optimization[J].Applied Soft Computing, 2021(2):107376.DOI:10.1016/j.asoc.2021.107376.

三、部分代码及结果

close all
clear
clc
% dbstop if all error
pop=100;%种群大小(可以修改)
maxgen=200;%最大迭代(可以修改)

%% 模型建立
model=Create_Model();
UAVnum=4;%无人机数量(可以修改)  必须与无人机的起始点保持一致

%% 初始化每个无人机的模型
for i=1:UAVnum
    ModelUAV(i).model=model;
end

%% 第一个无人机 起始点
start_location = [120;200;100];
end_location = [800;800;150];
ModelUAV(1).model.start=start_location;
ModelUAV(1).model.end=end_location;
%% 第二个无人机 起始点
start_location = [400;100;100];
end_location = [900;600;150];
ModelUAV(2).model.start=start_location;
ModelUAV(2).model.end=end_location;
%% 第三个无人机 起始点
start_location = [200;150;150];
end_location =[850;750;150];
ModelUAV(3).model.start=start_location;
ModelUAV(3).model.end=end_location;
%% 第四个无人机 起始点
start_location = [100;100;150];
end_location = [800;730;150];
ModelUAV(4).model.start=start_location;
ModelUAV(4).model.end=end_location;
%% 第5个无人机 起始点
% start_location = [500;100;130];
% end_location = [850;650;150];
% ModelUAV(5).model.start=start_location;
% ModelUAV(5).model.end=end_location;
% %% 第6个无人机 起始点
% start_location = [100;100;150];
% end_location =   [800;800;150];
% ModelUAV(6).model.start=start_location;
% ModelUAV(6).model.end=end_location;

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

四、完整MATLAB代码见下方名片

在这里插入图片描述


http://www.kler.cn/a/586731.html

相关文章:

  • MongoDB集合(表)自动创建机制
  • ffmpeg基础整理
  • 《AI浪潮中的璀璨新星:Meta Llama、Ollama与DeepSeek的深度剖析》:此文为AI自动生成
  • 利用matlab编制的转子动力学
  • springboot树形结构 支持模糊查询,返回匹配节点和父节点,其他节点不返回
  • Android开源库——RxJava和RxAndroid
  • Training-free neural architecture search: A review
  • docker构建镜像时总会有部分文件没有被更新,解决办法
  • Android Framework 之了解系统启动流程二
  • 011【fate/extra link】【概率论与数理统计】大数定律与中心极限定理 ,数理统计的基本概念,常用的统计三大分布,正态总体的抽样分布定理
  • Python 实现大文件的高并发下载
  • jenkins通过ssh连接远程服务器出错解决方案(Algorithm negotiation fail)
  • MySQL(第四周)
  • 基于“动手学强化学习”的知识点(六):第 19 章 目标导向的强化学习(gym版本 >= 0.26)
  • C++之OOP
  • 蓝桥杯嵌入式赛道复习笔记1(led点亮)
  • 【Python机器学习】2.4. K均值聚类(KMeans Analysis)实战(进阶)
  • SOME/IP:用Python实现协议订阅、Offer、订阅ACK与报文接收
  • 【Unity网络同步框架 - Nakama研究(二)】
  • Spark 中创建 DataFrame 的2种方式对比