基于粒子群算法与动态窗口混合的无人机三维动态避障路径规划研究，MATLAB代码

针对无人机在三维动态环境下路径规划存在的实时性差、避障精度低、路径平滑性不足等问题，提出一种粒子群算法（Particle Swarm Optimization, PSO）与动态窗口法（Dynamic Window Approach, DWA）混合的路径规划方法。首先，利用粒子群算法完成全局路径的离线规划；其次，结合动态窗口法进行局部动态避障的在线优化，利用无人机运动学约束筛选速度窗口，保证路径的实时性和安全性；最后，通过三维动态仿真环境验证算法性能。实验结果表明，混合算法在动态障碍物场景下，具备更优的路径长度、避障成功率和实时响应速度，满足无人机三维动态环境下的路径规划需求。

一、引言

无人机在物流配送、电力巡检、应急救援等领域的应用日益广泛，其作业环境逐渐从结构化静态场景向非结构化动态场景拓展。三维动态避障路径规划作为无人机自主导航的核心技术，需要同时满足全局最优性 、局部实时性 和运动安全性三大要求。

传统路径规划方法可分为全局规划和局部规划两类。全局规划方法如A*算法、Dijkstra算法、粒子群算法等，能够生成全局最优路径，但对动态障碍物的响应能力不足；局部规划方法如动态窗口法、人工势场法等，具备较强的实时避障能力，但易出现局部最优或路径震荡问题。因此，将全局规划与局部规划相结合，成为解决三维动态避障问题的主流思路。

粒子群算法因原理简单、收敛速度快、参数易调整等优点，被广泛应用于无人机全局路径规划，但传统PSO存在后期收敛速度慢、易陷入局部最优的缺陷。动态窗口法基于无人机运动学模型筛选可行速度集，能够快速响应动态障碍物，但缺乏全局路径引导，易导致路径偏离目标。基于此，本文提出PSO-DWA混合算法，通过全局路径引导局部避障，实现无人机在三维动态环境下的高效路径规划。

二、相关基础理论

2.1 无人机三维运动学模型

为简化分析，将无人机视为质点，其在三维空间中的运动状态可由位置坐标 (x,y,z)(x,y,z)(x,y,z)

2.2 粒子群算法原理

粒子群算法模拟鸟群觅食行为，将每个可行路径视为一个粒子，粒子的位置对应路径的节点坐标，粒子的适应度值由路径长度、安全性等指标决定。

粒子在搜索空间中的位置更新公式为：
xik+1=xik+vik+1x_{i}^{k+1}=x_{i}^{k}+v_{i}^{k+1}xik+1=xik+vik+1
vik+1=ωvik+c1r1(pbest,ik−xik)+c2r2(gbestk−xik)v_{i}^{k+1}=\omega v_{i}^{k}+c_1r_1(p_{best,i}^{k}-x_{i}^{k})+c_2r_2(g_{best}^{k}-x_{i}^{k})vik+1=ωvik+c1r1(pbest,ik−xik)+c2r2(gbestk−xik)

其中：

xikx_{i}^{k}xik 为第 iii 个粒子在第 kkk 代的位置；
vikv_{i}^{k}vik 为第 iii 个粒子在第 kkk 代的速度；
ω\omegaω 为惯性权重，决定粒子继承历史速度的程度；
c1c_1c1、c2c_2c2 为学习因子，分别表示粒子向个体最优和全局最优学习的权重；
r1r_1r1、r2r_2r2 为 [0,1][0,1][0,1] 范围内的随机数；
pbest,ikp_{best,i}^{k}pbest,ik 为第 iii 个粒子的个体最优位置；
gbestkg_{best}^{k}gbestk 为种群的全局最优位置。

传统PSO的惯性权重 ω\omegaω 通常为固定值，导致算法前期探索能力不足或后期收敛速度慢，因此需要对其进行改进。

2.3 动态窗口法原理

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

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

评价函数包含目标趋近度 、避障安全性 和速度平滑性 三个指标，其表达式为：
G(v,ω)=α⋅heading(v,ω)+β⋅dist(v,ω)+γ⋅velocity(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α,β,γ 为权重系数，headingheadingheading 表示轨迹与目标方向的夹角，distdistdist 表示轨迹与障碍物的最小距离，velocityvelocityvelocity 表示轨迹的速度大小。

2.4 混合算法的整体流程

改进PSO-DWA混合算法的整体流程如下：

环境建模：构建三维空间环境模型，输入无人机起点、终点坐标，以及静态障碍物和动态障碍物的初始位置与运动参数；
全局路径规划 ：利用粒子群算法生成避开静态障碍物的全局路径，并将路径节点存储为引导点序列；
无人机状态感知：无人机通过传感器实时获取自身位置、速度信息，以及动态障碍物的实时位置；
局部动态避障 ：以全局路径为引导，采用动态窗口法生成局部避障路径，驱动无人机运动；
路径更新与终止判断：无人机每运动一个步长，重复步骤3-4，直至到达终点。

三、仿真实验与MATLAB代码

bash 复制代码

clc;
clear;
close all;
global model
% Number of runs
model = CreateModel(); % Create search map and parameters
CostFunction=@(x) MyCost(x);    % Cost Function
nVar=model.n;       % Number of Decision Variables = searching dimension of ACO = number of path nodes
VarSize=[1 nVar];   % Size of Decision Variables Matrix
%% 算法求解
[Fgbest,Xgbest,BestCost,Kk] = PSO(pop,M,lb,ub,dim,CostFunction);

%% Plot results
% Best solution
BestPosition = SphericalToCart1(Xgbest);
disp("Best solution...");
BestPosition;
smooth = 0.95;
PlotSolution(BestPosition,model,smooth);

% Best cost
figure;
plot(BestCost,'LineWidth',2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;
save data.mat