【无人机三维路径规划】基于A、JPS、Bezier、L-BSGF与HAOAROA五种算法实现无人机三维路径规划附Matlab代码-优快云博客

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍

一、引言：无人机三维路径规划的价值与算法适配性

无人机三维路径规划是无人机自主飞行的核心关键技术，其核心目标是在复杂三维环境（含地形起伏、障碍物、禁飞区等约束）中，为无人机规划一条从起点到终点的最优路径，同时满足航程、能耗、飞行平稳性、避障安全性等多维度指标。该技术广泛应用于物流配送、电力巡检、应急救援、地形测绘、军事侦察等领域——例如电力巡检中需规避输电线路与杆塔，应急救援中需快速穿越复杂地形抵达目标区域，均对路径规划的精度、效率与鲁棒性提出极高要求。

传统无人机路径规划多基于二维平面拓展，难以精准适配真实三维空域环境；而三维路径规划因环境建模复杂度高、约束条件多（三维空间障碍物规避、高度梯度限制等），对算法的寻优能力与计算效率挑战极大。当前主流的三维路径规划算法可分为传统路径搜索算法、曲线平滑算法与智能优化算法三类：A*算法与JPS（跳点搜索）算法属于传统启发式搜索算法，具备寻优逻辑清晰、实时性较强的优势；Bezier曲线算法专注于路径平滑优化，可提升飞行稳定性；L-BSGF（分层栅格扫描生成框架）通过环境建模优化提升规划效率；HAOAROA（混合自适应鸵鸟优化算法）则属于新型智能优化算法，具备强全局寻优能力，可适配复杂多约束场景。

本文将系统解析上述五种算法的核心原理，构建统一的无人机三维路径规划框架，详细阐述从环境建模、算法实现、路径平滑到性能验证的全流程，并通过对比分析五种算法的规划精度、计算效率、路径平滑度等指标，为不同应用场景下的算法选型提供参考。

二、基础原理：五种核心算法的核心机制与三维适配改进

无人机三维路径规划的核心逻辑是“环境建模-目标函数构建-约束条件界定-算法寻优-路径优化”，五种算法虽属于不同类型，但均围绕这一核心逻辑展开，其差异主要体现在寻优策略、适配场景与优化目标上。以下分别解析各算法的核心原理及针对三维场景的适配改进。

（一）A*算法：三维启发式搜索的基础框架

A*算法是一种基于启发函数的贪婪搜索算法，核心优势在于通过启发信息引导搜索方向，平衡寻优精度与计算效率，是三维路径规划的基础算法之一。

核心原理：A*算法通过评估函数f(n)=g(n)+h(n)引导搜索，其中g(n)为从起点到当前节点n的实际代价（如路径长度、能耗），h(n)为从当前节点n到终点的启发代价（需满足h(n)≤真实代价，保证算法完备性）。算法通过优先扩展f(n)值最小的节点，逐步逼近终点，最终形成最优路径。
三维适配改进：二维A*算法的节点仅含(x,y)坐标，三维场景需扩展为(x,y,z)三维坐标节点；同时启发函数需适配三维空间——常用三维欧氏距离作为启发函数：h(n)=√[(xₑ-xₙ)²+(yₑ-yₙ)²+(zₑ-zₙ)²]（xₑ,yₑ,zₑ为终点坐标，xₙ,yₙ,zₙ为当前节点坐标），确保启发信息能精准反映三维空间距离关系。此外，三维场景中节点邻域扩展需考虑6个方向（前后、左右、上下）或26个方向（含斜向），需根据环境复杂度选择邻域扩展策略（26向扩展精度更高但计算量更大，6向扩展效率更高但可能遗漏最优路径）。
优势与局限：优势是原理简单、实现难度低、寻优结果稳定；局限是在复杂三维环境（如密集障碍物、大尺度空域）中，搜索节点数量多，计算效率下降，且规划路径易出现“锯齿状”，需后续平滑优化。

（二）JPS算法：三维跳点搜索的效率提升机制

JPS算法是A*算法的改进版，核心创新是通过“跳点（Jump Point）”筛选减少搜索节点数量，大幅提升搜索效率，尤其适配大尺度三维空域规划。

核心原理：JPS算法通过定义“跳点”规则，仅保留对路径规划有决定性作用的节点（跳点），跳过冗余中间节点。跳点需满足两个条件：一是从当前节点到跳点的路径为最优；二是跳点存在至少一个邻域节点，其最优路径需经过该跳点。算法通过递归搜索跳点，大幅压缩搜索空间，提升计算效率。
三维适配改进：三维JPS的核心是构建三维跳点判断规则，需兼顾x、y、z三个维度的方向约束：① 强制跳点：当节点的某一方向邻域存在障碍物，或节点为终点时，判定为强制跳点；② 自然跳点：当节点在某一方向上的延伸路径中，存在邻域节点的最优路径需经过该节点时，判定为自然跳点。例如在三维空间中，若节点(n_x,n_y,n_z)沿x轴正方向延伸时，其(y,z)平面内的邻域节点需通过该节点到达终点，则该节点为自然跳点。同时，三维跳点的邻域扩展需同步考虑三个维度的方向组合，确保不遗漏关键路径。
优势与局限：优势是计算效率远高于A*算法，搜索节点数量大幅减少，适配大尺度三维场景；局限是跳点规则在不规则障碍物环境中易失效，可能出现路径断裂或寻优失败，需结合环境预处理优化跳点判断逻辑。

（三）Bezier曲线算法：三维路径平滑的核心工具

Bezier曲线算法并非独立的路径搜索算法，而是专注于路径平滑优化的辅助算法——其核心作用是将A*、JPS等算法规划的“锯齿状”离散路径，拟合为连续光滑的曲线，降低无人机飞行过程中的姿态调整频率，提升飞行稳定性与安全性。

核心原理：Bezier曲线通过控制点定义曲线形状，n次Bezier曲线的表达式为：B(t)=Σₖ=₀ⁿ C(n,k)tᵏ(1-t)ⁿ⁻ᵏPₖ（t∈[0,1]），其中C(n,k)为二项式系数，Pₖ为控制点。曲线具备“端点插值性”（曲线经过首末控制点）与“凸包性”（曲线完全处于控制点构成的凸包内），可通过调整控制点位置精准调控曲线形状。
三维适配改进：三维Bezier曲线将控制点扩展为三维坐标(Pₖₓ,Pₖᵧ,Pₖz)，通过对离散路径节点进行采样，选取关键节点作为初始控制点，再通过优化控制点位置（如最小化曲线曲率、规避障碍物），拟合生成三维光滑路径。例如，针对A*算法规划的三维离散节点序列，可每隔2-3个节点选取一个控制点，补充路径拐点作为强制控制点，确保平滑后的路径不偏离原始最优路径，同时规避三维障碍物。
优势与局限：优势是路径平滑度高，可直接适配无人机飞行控制需求；局限是无法独立完成路径搜索，需依赖前置路径搜索算法提供初始路径，且控制点数量过多时计算复杂度上升。

（四）L-BSGF算法：分层栅格建模的效率优化框架

L-BSGF（分层栅格扫描生成框架）核心优势在于通过分层栅格化建模优化三维环境描述，减少冗余环境数据，提升路径规划的效率与精度，尤其适配地形起伏较大、障碍物分布复杂的三维场景。

核心原理：L-BSGF将三维环境按高度梯度划分为多个二维分层栅格，通过“全局粗栅格建模-局部细栅格优化”的双层架构处理环境数据：全局层采用大栅格尺寸快速构建环境轮廓，筛选出无障碍物的可行区域；局部层在可行区域内采用小栅格尺寸精准描述障碍物细节，实现“全局高效搜索-局部精准避障”的平衡。同时，通过栅格扫描算法快速标记障碍物区域，生成环境占用图，为后续路径搜索提供精准的环境信息。
三维适配改进：三维L-BSGF的核心是合理划分高度分层：需根据无人机飞行高度范围与地形起伏幅度，确定分层间距（如地形起伏50m范围内划分为10层，每层高度间隔5m），确保各分层能完整覆盖三维环境；同时，各分层栅格需通过高度坐标关联，形成完整的三维环境占用图。在路径规划阶段，算法先在全局粗栅格中快速搜索可行路径框架，再在局部细栅格中优化路径细节，规避微小障碍物。
优势与局限：优势是环境建模精度与效率平衡较好，适配复杂三维地形环境；局限是分层间距的选择对规划结果影响较大，分层过粗易遗漏障碍物，分层过细则计算量上升。

（五）HAOAROA算法：混合自适应智能优化的全局寻优机制

HAOAROA（混合自适应鸵鸟优化算法）是基于鸵鸟觅食行为改进的新型智能优化算法，通过模拟鸵鸟“全局探索-局部开发”的觅食策略，结合自适应参数调整机制，具备强全局寻优能力，可适配多约束、非线性的复杂三维路径规划场景。

核心原理：原始鸵鸟优化算法（AROA）通过“探索阶段”（鸵鸟大范围搜索食物，对应算法全局寻优）与“开发阶段”（鸵鸟在局部区域精准觅食，对应算法局部优化）实现寻优；HAOAROA在其基础上引入混合策略：① 自适应权重调整：根据寻优进程动态调整全局探索与局部开发的权重，避免过早收敛到局部最优；② 交叉变异机制：引入遗传算法的交叉变异操作，增强种群多样性；③ 约束处理机制：针对无人机飞行约束（如最大爬升角、最大航程）设计惩罚函数，确保寻优结果满足实际飞行需求。算法将三维路径规划问题转化为多目标优化问题，通过种群迭代寻优生成最优路径。
三维适配改进：将三维路径编码为种群个体，每个个体对应一条三维路径（由多个(x,y,z)节点组成）；构建多目标适应度函数：f=ω₁L+ω₂E+ω₃S（其中L为路径长度，E为飞行能耗，S为路径平滑度，ω₁、ω₂、ω₃为权重系数）；引入约束条件：路径节点需规避障碍物、高度梯度需小于无人机最大爬升角、路径总长度需小于最大航程等，通过惩罚函数将约束条件融入适应度函数（如违反约束则增加适应度值，降低个体竞争力）。
优势与局限：优势是全局寻优能力强，可处理多约束复杂场景，规划路径综合性能优；局限是计算复杂度较高，实时性略差于A*、JPS等传统算法，适用于离线规划或准实时规划场景

⛳️ 运行结果

📣 部分代码

% Benchmark Functions for HAOAROA Algorithm

clc;

clear;

close all;

n = 2; % Dimensionality for plotting in 2D or 3D

x = linspace(-5, 5, 100);

y = linspace(-5, 5, 100);

[X, Y] = meshgrid(x, y);

% Create a figure for all subplots

figure;

% Sphere Function

sphereFunction = @(X, Y) X.^2 + Y.^2;

subplot(2,3,1); % Create a subplot in a 2x3 grid, position 1

mesh(X, Y, sphereFunction(X, Y));

title('Sphere Function');

xlabel('X');

ylabel('Y');

zlabel('f(X,Y)');

grid on;

% Rastrigin Function

rastriginFunction = @(X, Y) 20 + (X.^2 - 10 * cos(2 * pi * X)) + (Y.^2 - 10 * cos(2 * pi * Y));

subplot(2,3,2); % Create a subplot in a 2x3 grid, position 2

mesh(X, Y, rastriginFunction(X, Y));

title('Rastrigin Function');

xlabel('X');

ylabel('Y');

zlabel('f(X,Y)');

grid on;

% Rosenbrock Function (Valley or Banana function)

rosenbrockFunction = @(X, Y) 100 * (Y - X.^2).^2 + (X - 1).^2;

subplot(2,3,3); % Create a subplot in a 2x3 grid, position 3

mesh(X, Y, rosenbrockFunction(X, Y));

title('Rosenbrock Function');

xlabel('X');

ylabel('Y');

zlabel('f(X,Y)');

grid on;

% Ackley Function

ackleyFunction = @(X, Y) -20 * exp(-0.2 * sqrt(0.5 * (X.^2 + Y.^2))) ...

- exp(0.5 * (cos(2 * pi * X) + cos(2 * pi * Y))) + 20 + exp(1);

subplot(2,3,4); % Create a subplot in a 2x3 grid, position 4

mesh(X, Y, ackleyFunction(X, Y));

title('Ackley Function');

xlabel('X');

ylabel('Y');

zlabel('f(X,Y)');

grid on;

% Griewank Function

griewankFunction = @(X, Y) 1 + (X.^2 + Y.^2) / 4000 - cos(X) .* cos(Y / sqrt(2));

subplot(2,3,5); % Create a subplot in a 2x3 grid, position 5

mesh(X, Y, griewankFunction(X, Y));

title('Griewank Function');

xlabel('X');

ylabel('Y');

zlabel('f(X,Y)');

grid on;

% Adjust layout

sgtitle('Benchmark Functions for HAOAROA Algorithm'); % Super title for the figure

🔗 参考文献

N. Basil et al., “Performance analysis of hybrid optimization approach for UAV path planning control using FOPID-TID controller and HAOAROA algorithm,” Sci. Rep., vol. 15, no. 1, p. 4840, 2025.

🎈 部分理论引用网络文献，若有侵权联系博主删除

👇 关注我领取海量matlab电子书和数学建模资料

🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：

🌟 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、风电场布局、时隙分配优化、最佳分布式发电单元分配、多阶段管道维修、工厂-中心-需求点三级选址问题、应急生活物质配送中心选址、基站选址、道路灯柱布置、枢纽节点部署、输电线路台风监测装置、集装箱调度、机组优化、投资优化组合、云服务器组合优化、天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、订单拆分调度问题、公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位、冷链、时间窗、多车场等、选址优化、港口岸桥调度优化、交通阻抗、重分配、停机位分配、机场航班调度、通信上传下载分配优化

🌟 机器学习和深度学习时序、回归、分类、聚类和降维

2.1 bp时序、回归预测和分类

2.2 ENS声神经网络时序、回归预测和分类

2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类

2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类

2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类

2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类

2.7 ELMAN递归神经网络时序、回归\预测和分类

2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类

2.9 RBF径向基神经网络时序、回归预测和分类

2.10 DBN深度置信网络时序、回归预测和分类

2.11 FNN模糊神经网络时序、回归预测

2.12 RF随机森林时序、回归预测和分类

2.13 BLS宽度学习时序、回归预测和分类

2.14 PNN脉冲神经网络分类

2.15 模糊小波神经网络预测和分类

2.16 时序、回归预测和分类

2.17 时序、回归预测预测和分类

2.18 XGBOOST集成学习时序、回归预测预测和分类

2.19 Transform各类组合时序、回归预测预测和分类

方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

🌟图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

🌟 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、充电车辆路径规划（EVRP）、双层车辆路径规划（2E-VRP）、油电混合车辆路径规划、船舶航迹规划、全路径规划规划、仓储巡逻、公交车时间调度、水库调度优化、多式联运优化

🌟 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划、

🌟 通信方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配

🌟 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测

🌟电力系统方面

微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电、电/冷/热负荷预测、电力设备故障诊断、电池管理系统（BMS）SOC/SOH估算（粒子滤波/卡尔曼滤波）、多目标优化在电力系统调度中的应用、光伏MPPT控制算法改进（扰动观察法/电导增量法）、电动汽车充放电优化、微电网日前日内优化、储能优化、家庭用电优化、供应链优化\智能电网分布式能源经济优化调度，虚拟电厂，能源消纳，风光出力，控制策略，多目标优化，博弈能源调度，鲁棒优化

电力系统核心问题经济调度：机组组合、最优潮流、安全约束优化。新能源消纳：风光储协同规划、弃风弃光率量化、爬坡速率约束建模多能耦合系统：电-气-热联合调度、P2G与储能容量配置新型电力系统关键技术灵活性资源：虚拟电厂、需求响应、V2G车网互动、分布式储能优化稳定与控制：惯量支撑策略、低频振荡抑制、黑启动预案设计低碳转型：碳捕集电厂建模、绿氢制备经济性分析、LCOE度电成本核算风光出力预测：LSTM/Transformer时序预测、预测误差场景生成（GAN/蒙特卡洛）不确定性优化：鲁棒优化、随机规划、机会约束建模能源流分析、PSASP复杂电网建模，经济调度，算法优化改进，模型优化，潮流分析，鲁棒优化，创新点，文献复现微电网配电网规划，运行调度，综合能源，混合储能容量配置，平抑风电波动，多目标优化，静态交通流量分配，阶梯碳交易，分段线性化，光伏混合储能VSG并网运行，构网型变流器，虚拟同步机等包括混合储能HESS：蓄电池+超级电容器，电压补偿,削峰填谷，一次调频，功率指令跟随，光伏储能参与一次调频，功率平抑，直流母线电压控制；MPPT最大功率跟踪控制，构网型储能，光伏，微电网调度优化，新能源，虚拟同同步机，VSG并网，小信号模型