多无人机动态避障路径规划研究：基于粒子群优化算法PSO的多无人机动态避障路径规划研究（可以自定义无人机数量及起始点），MATLAB代码

一、粒子群优化算法

粒子群优化算法（Particle Swarm Optimization，PSO）是一种基于群体智能的随机搜索算法，最早由 Kennedy 与 Eberhart 于 1995 年提出，灵感来源于鸟群或鱼群的觅食行为。它通过模拟粒子在解空间中的"飞行"来寻找最优解，具有实现简单、参数少、收敛快等特点，广泛应用于函数优化、神经网络训练、特征选择、路径规划等领域。

二、无人机（UAV）三维路径规划

单个无人机三维路径规划数学模型参考如下文献：

Phung M D , Ha Q P . Safety-enhanced UAV Path Planning with Spherical Vector-based Particle Swarm Optimization $J$ . arXiv e-prints, 2021.

每个无人机的目标函数由路径长度成本，安全性与可行性成本、飞行高度成本和路径平滑成本共同组成：

2.1路径长度成本

路径长度成本由相邻两个节点之间的欧氏距离和构成，其计算公式如下：

2.2路径安全性与可行性成本

路径安全性与可行性成本通过下式计算：

2.3路径飞行高度成本

飞行高度成本通过如下公式计算所得：

2.4路径平滑成本

投影向量通过如下公式计算：

转弯角度的计算公式为：

爬坡角度的计算公式为：

平滑成本的计算公式为：

2.5总成本（目标函数）

总成本由最优路径成本，安全性与可行性成本、飞行高度成本和路径平滑成本的线性加权所得。其中，b为加权系数。

2.6 动态窗口法原理

动态窗口法的核心思想是在速度空间中搜索最优速度向量，用于无人机动态避障其核心步骤包括：

速度窗口采样 ：根据无人机运动学约束，生成可行速度集 V s a m p l e V_{sample} Vsample；
轨迹预测：对每个采样速度，预测未来一段时间内的无人机轨迹；
评价函数计算：通过评价函数评估每条预测轨迹的优劣，筛选出最优轨迹对应的速度；
速度更新：将最优速度作为无人机的当前运动速度，完成局部路径规划。

评价函数包含目标趋近度 、避障安全性 和速度平滑性 三个指标，其表达式为：
G ( v , ω ) = α ⋅ h e a d i n g ( v , ω ) + β ⋅ d i s t ( v , ω ) + γ ⋅ v e l o c i t y ( v , ω ) G(v,\omega)=\alpha \cdot heading(v,\omega)+\beta \cdot dist(v,\omega)+\gamma \cdot velocity(v,\omega) G(v,ω)=α⋅heading(v,ω)+β⋅dist(v,ω)+γ⋅velocity(v,ω)

其中， α , β , γ \alpha,\beta,\gamma α,β,γ 为权重系数， h e a d i n g heading heading 表示轨迹与目标方向的夹角， d i s t dist dist 表示轨迹与障碍物的最小距离， v e l o c i t y velocity velocity 表示轨迹的速度大小。

三、实验结果

点击查看多无人机动态避障路径规划代码及效果演示

在三维无人机路径规划中，无人机的路径由起点，终点以及起始点间的点共同连接而成。因此，自变量为无人机起始点间的各点坐标，每个无人机的目标函数为总成本（公式9）。本文研究3个无人机协同路径规划，总的目标函数为3个无人机的总成本之和。

bash 复制代码

%% 第一个无人机 起始点
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;

figure
plot(Convergence_curve,'LineWidth',2)
xlabel('Iteration');
ylabel('Best Cost');
grid on;

点击查看多无人机动态避障路径规划代码演示

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

点击查看多无人机动态避障路径规划代码及效果演示