当前位置：首页 > news >正文

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

news 2026/5/12 4:59:07

一、粒子群优化算法

粒子群优化算法（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 headingheading表示轨迹与目标方向的夹角，d i s t distdist表示轨迹与障碍物的最小距离，v e l o c i t y velocityvelocity表示轨迹的速度大小。

三、实验结果

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

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