目录

MATLAB实现基于GA-PSO 遗传算法（GA）结合粒子群优化算法（PSO）进行无人机三维路径规划的详细项目实例     2

项目背景介绍... 2

项目目标与意义... 3

提升三维复杂环境中的路径安全性... 3

提高路径规划效率与收敛稳定性... 4

构建可扩展的三维路径规划算法框架... 4

推动智能群体优化算法在工程中的落地... 5

项目挑战及解决方案... 5

三维环境建模复杂与障碍表示难题及应对方案... 5

GA-PSO混合结构设计与参数协调问题及策略... 6

多目标与多约束融合导致的适应度设计难度及解决路径... 6

项目模型架构... 7

三维环境与路径表示模型... 7

GA-PSO混合优化主框架... 7

遗传算法模块结构与原理... 8

粒子群优化模块结构与原理... 8

适应度评价与约束处理模块结构与原理... 8

项目模型描述及代码示例... 9

三维环境和障碍物建模示例... 9

路径编码与初始种群生成示例... 10

遗传算法操作示例：选择、交叉与变异... 11

粒子群速度与位置更新示例... 12

主迭代流程与最优路径可视化示例... 13

MATLAB实现基于GA-PSO 遗传算法（GA）结合粒子群优化算法（PSO）进行无人机三维路径规划的详细项目实例

请注意此篇内容只是一个项目介绍 更多详细内容可直接联系博主本人 

 或者访问对应标题的完整博客或者文档下载页面（含完整的程序，GUI设计和代码详解）

多旋翼无人机在复杂三维环境中的自主飞行任务不断增多，从早期的简单航点巡检发展到如今在城市峡谷、山地峡谷、森林甚至室内环境中执行侦察、运输和应急救援等多种任务。在这些任务场景中，路径规划已经不再是单纯的二维平面问题，而是需要充分考虑地形高程、建筑高度、禁飞区域、雷达和通信遮挡、风场干扰等多种因素的三维优化问题。现实任务中，飞行环境往往存在大量障碍物，例如高层建筑群、输电线路、通信塔、树木群以及复杂山地地形，这些障碍物在空间中呈现出高度不规则、难以用简单几何结构精确描述的特点。如果仍采用传统基于栅格或规则曲线的平面规划方法，很容易出现路径不可行、飞行高度不合理、转弯过于频繁或能耗过高等问题，从而直接影响任务安全性和执行效率。

从算法角度看，经典的最短路径算法如Dijkstra或A*在结构规则、障碍简化的环境中表现较好，但扩展到连续的三维空间时，需要进行高密度栅格离散或构图，计算量急剧增加，并且很难自适应地兼顾飞行安全裕度、能耗、飞行时间和航迹平滑度等多目标需求。随着任务复杂度提高，对路径规划算法提出了更高要求：既要能够在高维、非线性、多约束的连续空间内搜索，又要具备快速收敛、高鲁棒性和较强的全局寻优能力，并适应任务在线更新和环境不确定性的挑战。基于群体智能的优化算法在处理此类问题方面显示出良好潜力，其中遗传算法通过选择、交叉与变异机制在大范围内搜索具有较强的全局探索能力，而粒子群算法通过速度和位置更新规则在连续空间中进行高效迭代，具备良好的收敛效率和实现简单的优势。

在三维路径规划场景中，仅采用单一群体智能算法往往面临明显不足。基于遗传算法的路径规划虽然具备全局搜索能力较强、不容易陷入局部最优等优点，但在连续空间的编码和算子设计上需要较多调试，且在迭代后期收敛速度较慢，易出现震荡和冗余迭代。基于粒子群的路径规划具有参数少、实现简单、收敛快等特点，但在复杂多峰代价函数环境中，粒子群容易早熟收敛，粒子整体陷入次优区域而难以逃离，从而导致规划出的路径存在安全裕度不足或能耗偏大的隐患。为了有效融合两类算法的优势，越来越多的研究开始探索遗传算法与粒子群算法的混合优化思路，通过在编码方式、适应度设计、种群初始化、算子混合和参数自适应等方面进行有机整合，构建既具全局探索又具局部加速能力的混合智能算法。

在真实工程环境中，还必须考虑算法在实际平台上的可实现性和可移植性。MATLAB在算法原型设计、可视化与仿真方面具有成熟的工具链，提供丰富的数值计算函数和三维绘图能力，特别适合构建路径规划算法的快速验证环境。基于MATLAB可以较为直接地构建三维环境模型，使用体素或连续空间障碍函数描述建筑与地形，定义起降点、任务区域和安全通道范围，并通过数值积分和几何检测对候选路径进行碰撞判定和代价评价。在MATLAB R2025b版本下，核心数值计算功能、数组操作与绘图接口仍然稳定可靠，非常适合聚焦于算法本身的设计与实现。通过在这一环境中构建完整的GA-PSO混合算法，可以系统验证其在不同场景复杂度下的路径质量、收敛速度、稳定性和参数敏感性。

三维路径规划不仅仅是求一条从起点到终点的连通曲线，而是往往需要满足多种工程约束。例如飞行高度要符合法规要求，不能低于建筑安全高度，也不能超过管制空域上限；路径要尽量避开强电磁干扰或雷达覆盖区域，保证通信链路稳定；转弯半径需要满足平台气动和控制性能约束，避免过于急剧的机动导致姿态过载或控制困难；在多机协同时，还要考虑航线之间的间距和时间错峰，以避免潜在的空中相撞风险。所有这些因素都需要综合进入路径规划的代价函数和约束条件中，使最终得到的航迹在安全性、经济性与可控性之间达到合理平衡。

在这种背景下，基于GA-PSO混合算法的三维无人机路径规划项目，旨在结合遗传算法的强全局搜索能力和粒子群算法的快速局部收敛能力，在连续三维空间中构建适应三维环境与多种约束条件的路径编码形式，设计合理的适应度函数，将路径长度、飞行能耗、航迹平滑度、安全裕度等多种性能指标融入统一优化框架。同时在算法结构上，通过种群初始化、参数自适应调节和混合算子耦合，实现搜索能力与收敛速度之间的动态平衡，减少过早收敛与长时间震荡的现象。通过MATLAB环境中的可视化调试，可以直观展示路径在复杂障碍环境中的走向，分析节点分布、飞行高度变化和避障效果，为进一步在真实无人机平台上部署提供参考。项目不仅有助于推动三维路径规划算法朝更智能、更高效方向发展，也对智能交通、智慧城市空中物流和应急救援等应用场景具有现实工程意义。

项目目标与意义

提升三维复杂环境中的路径安全性

在多建筑、山地和多障碍物环境中，无人机路径规划面临最核心的问题就是安全性。环境的不确定性、障碍物的空间分布以及飞行平台性能限制，都可能导致路径规划结果与实际安全需求之间产生偏差。通过构建基于GA-PSO混合算法的三维路径规划模型，核心目标之一是显著提高规划路径在多种复杂环境下的安全裕度。路径安全性不仅包括避免与障碍物发生几何碰撞，还包含满足一定的安全距离要求，使无人机在受到阵风、传感器误差或短暂控制偏差时仍有可用的缓冲空间。通过适应度函数中的惩罚项设计，可以对距离障碍过近的路径进行强烈惩罚，鼓励算法搜索更安全、更稳健的飞行通道。此外，在三维空间中合理控制飞行高度、避免穿越低空敏感区域和高风险区域，也是一项重要任务。利用GA-PSO融合全局搜索和局部精细调整的优势，在搜索过程中动态调整路径节点位置和高度，确保路径在多个维度上满足安全要求。相比单一算法，混合算法可以在较少迭代次数内找到兼顾安全裕度和路径效率的综合方案，从工程角度有效降低飞行任务中潜在的风险事件发生概率。

提高路径规划效率与收敛稳定性

在任务执行现场，路径规划不仅要安全合理，还需要在有限时间内完成规划过程，特别是在任务区域或障碍分布发生变化时，规划过程往往需要重新进行。提升计算效率和收敛稳定性成为规划算法能否实际应用的关键因素之一。遗传算法经常在后期迭代阶段表现出收敛缓慢的特点，而粒子群则在初期阶段收敛速度快但容易陷入局部最优。通过将两种算法进行有机融合，本项目目标在于构建一个既保持全局探索能力，又显著提升收敛速度和稳定性的混合优化框架。GA部分负责多样化种群和全局搜索，PSO部分负责在当前优势解附近进行高效微调，使得路径规划在较少代数内便可收敛到高质量路径。同时，通过对参数如惯性权重、学习因子及遗传算子概率的动态调节，可以让算法在不同迭代阶段自动平衡探索与开发，避免早熟收敛和长期震荡。这样，在MATLAB仿真平台上不仅计算效率可以得到明显提升，还能在多次随机初始条件下保持较高的收敛成功率和较小的结果波动，从理论和工程角度提升算法的稳定性和可靠性。

构建可扩展的三维路径规划算法框架

未来无人机应用场景多样且不断演化，不同场景对路径规划的约束条件和优化指标差异明显。例如物流配送更关注能耗与时间，巡检任务更强调覆盖率和路径平滑度，应急救援更重视任务完成时间和危险区域绕行能力。单一固定结构的算法难以适配不断变化的任务需求。项目目标之一是构建一个可扩展、可配置的三维路径规划算法框架，使GA-PSO混合算法能够灵活嵌入不同的任务需求和环境模型。通过灵活的路径编码方式，可以方便修改节点数目、路径参数维度和空间边界；通过模块化设计适应度函数，可以将路径长度、飞行时间、能耗、安全裕度、雷达暴露时间等指标按照权重组合进行统一优化，并允许在运行前根据信息优先级进行重新配置。这样一个具备扩展性的框架，能够在MATLAB环境中快速接入不同场景的数据，例如数字高程模型、城市建筑模型和雷达覆盖图等，形成面向多领域、多任务类型的通用路径规划算法平台。这种可扩展框架为后续引入更多智能算法或多目标优化机制打下基础，体现出重要的技术储备意义。

推动智能群体优化算法在工程中的落地

群体智能算法在理论研究领域已相当活跃，但在工程实践中的落地往往受限于实现复杂度、参数敏感性和工程适配度。通过在三维无人机路径规划这样的典型工程问题中深入应用GA-PSO混合算法，项目旨在推动群体智能算法从理论走向工程实践。一方面，通过在MATLAB中实现完整的算法流程，包括种群初始化、遗传算子设计、粒子群速度位置更新、适应度评估和约束处理等，可以为相关工程人员提供清晰的参考实现，降低将算法迁移到其他平台（如嵌入式处理器或机载计算机）时的难度。另一方面，在实际路径规划问题中，需要将理论上理想的算法与现实约束结合，如飞行高度限制、机动性能限制、计算资源限制等，这一过程可以检验算法的鲁棒性和可用性，并促使对算法结构做出针对性的改进。例如通过参数自适应、变邻域搜索或多种混合算子组合等手段提升工程适应性。通过这一项目，有望形成一套系统而完整的工程应用经验，将GA-PSO混合算法推广到更多机器人导航、多传感器布设、资源调度等跨领域的实际场景中，对智能优化技术在工业和公共安全领域的应用推广具有实际意义。

项目挑战及解决方案

三维环境建模复杂与障碍表示难题及应对方案

三维路径规划首先面临环境建模的复杂性问题。在真实场景中，障碍物可能是形态各异的建筑集合、坡度复杂的山体以及高度不规则的树木群，将其精确建模成连续函数常常十分困难。简单的体素离散会带来巨大的存储和计算开销，高精度网格则会在路径碰撞检测时造成严重的计算瓶颈。建模不当可能导致路径规划算法在错误或粗糙的环境描述中工作，最终给出的路径虽然在模型中可行，却在真实世界中存在碰撞风险。为解决这一问题，在环境建模层面采用分层与简化并重的策略。在关键障碍区域采用较精细的规则几何近似（例如长方体、圆柱体和多面体），在相对空旷或远离飞行路径的区域采用较粗略的体素或高度场描述，以平衡建模精度与计算成本。基于MATLAB的三维绘图与数值计算能力，可以通过网格生成函数、逻辑运算与空间查询，对障碍区域进行统一管理。对于路径与障碍之间的碰撞检测，不必进行连续轨迹的逐点检查，而是通过离散路径节点与连续插值相结合的方法，利用矢量化计算迅速判断路径是否进入障碍体积或过于接近障碍表面。解决方案还包括对飞行高度边界、禁飞区域和缓冲区进行单独编码，通过布尔运算与距离场计算构建统一的障碍约束函数，使混合算法在适应度评估阶段即可快速给出路径可行性判断。通过这种建模方式，既可以在MATLAB环境中直观地展示三维环境，又能在计算层面保持较高效率，为上层GA-PSO优化提供可靠的约束基础。

GA-PSO混合结构设计与参数协调问题及策略

另一大挑战来自于遗传算法和粒子群算法的混合结构设计。在实际实现中，如果两类算法的交互方式设计不当，可能导致种群多样性快速丧失、计算冗余或参数难以调试等问题。单纯把遗传算法和粒子群算法串联或并联，很容易出现重复搜索或搜索方向相互干扰，使整体性能不升反降。为应对这一问题，本项目在算法架构层采用“GA负责全局探索、PSO负责局部加速”的整体思路，具体在每一代或若干代迭代中分阶段交叉使用两类算子。遗传阶段通过选择、交叉和变异操作维持种群多样性，避免个体过早集中到局部区域；粒子群阶段则在当前种群中选出较优个体作为粒子，同时继承其位置和性能信息，通过速度与位置更新规则对这批粒子进行局部区域的快速搜索，提升收敛速度。参数方面，则采用动态自适应调节方式进行协调控制。例如惯性权重在迭代早期保持较大数值以鼓励探索，在迭代后期逐渐减小以加强局部开发；学习因子c1、c2在不同阶段进行微调，以改变个体与群体经验的权重；遗传阶段的交叉概率与变异概率可根据种群适应度分布的方差进行动态调整，适应度趋于集中时提高变异概率以增加多样性。通过在MATLAB中进行多轮仿真，对不同参数配置下的收敛曲线进行对比分析，可以找到一组在多数环境场景中表现稳健的参数策略，从而降低在工程部署时的调参成本。此外，在混合结构中必须注意避免重复计算，例如将适应度评估封装成独立函数，供GA与PSO共用，减少函数调用与重计算次数，提高整体效率。

多目标与多约束融合导致的适应度设计难度及解决路径

三维无人机路径规划通常涉及多种目标和约束：路径长度要尽量短，转弯要平滑，能耗要低，飞行时间要短，同时还需满足障碍避让、飞行高度限制和安全距离要求。直接在优化过程中对这些目标分开处理，不仅增加算法实现复杂度，也会造成各目标之间难以协调。适应度函数设计成为关键难点之一：如何在一个标量评价函数中合理地反映多目标综合性能，并对约束违规路径施加适当惩罚，从而引导GA-PSO向综合性能更优的解集收敛。为解决这一问题，本项目采用加权和与约束罚函数相结合的适应度设计方式，将路径总长度、能耗估计、航迹曲率或折线角度、距离障碍的最小裕度等指标归一化后按权重进行加权求和。权重的选择可以基于任务需求进行预先设定，也可以在仿真阶段通过多组测试寻找合适的折中配置。对于违反硬约束的路径，例如节点落入障碍物内部、飞行高度超出安全范围或与禁飞区相交，则通过引入较大幅度的罚值将适应度显著降低，使这些路径很难在选择过程中保留下来。对于某些软约束，如距离障碍过近但仍未碰撞，则通过连续的距离惩罚函数进行处理，使算法更倾向于在安全裕度允许的范围内收缩路径。考虑到适应度函数的连续性和可导性虽然不直接影响群体智能算法本身的可行性，但连续且平滑的评价函数有利于PSO在局部区域进行更稳定的搜索。通过对路径节点插值和曲率计算进行适当平滑处理，可以在不过度增加计算负担的情况下，提高适应度函数的平滑性。最终，使GA-PSO混合算法在面对复杂多目标、多约束的三维路径规划任务时，能够在统一评价框架下收敛到符合任务需求的高质量航迹。

项目模型架构

三维环境与路径表示模型

模型架构的第一层是三维环境和路径的数学表示。环境空间在MATLAB中通过连续坐标系进行描述，定义一个三维空间边界，例如在x、y、z三个方向上分别设置最小和最大坐标，用于约束路径节点搜索范围。障碍物采用组合建模方式，将建筑物等结构表示为长方体或多面体，将塔、杆、电线杆等表示为圆柱或细长体，将地形高程通过高度场或规则曲面建模。每个障碍体都可以使用几何参数（位置、尺寸、朝向）进行定义，并在MATLAB中构造相应的坐标集合或逻辑函数用于碰撞检测。路径表示方面，采用固定数量的控制节点来表达从起点到终点的三维路径，将每条路径编码为一个实值向量，其元素依次为各中间节点的坐标值。整个路径则通过起点、若干中间节点和终点串联成折线，为了提升飞行平滑性，可以在评估时对折线路径采用样条插值，以产生更加平滑的轨迹用于最终代价计算。这样的编码方式既保留了连续空间的特性，又使GA和PSO可以直接对实值向量进行操作，避免传统二进制编码带来的编码长度膨胀和解码复杂度。路径节点的数量可以根据任务规模与环境复杂度进行设定，数量越多，路径可调节自由度越高，但优化维度也随之升高，因此需要在表达能力与计算成本之间找到平衡。通过这种环境与路径表示模型，算法可以在连续三维空间内搜索高质量的可行路径，并且便于进行可视化展示。

GA-PSO混合优化主框架

在环境与路径表示基础上，模型架构的第二层是GA-PSO混合优化主框架。主框架负责管理种群初始化、迭代循环、GA阶段和PSO阶段的切换以及最终路径选择。整体结构在MATLAB中以主函数形式实现，通过循环结构控制迭代次数，在每一代迭代中按预先设定的规则执行遗传操作与粒子群操作。例如可以采用周期交替方式，在前若干代主要使用遗传算子进行大范围探索，随后若干代主要使用粒子群算子进行局部加速；也可以在每一代中先执行遗传操作更新种群，再选取部分精英个体作为粒子进行PSO微调。主框架中维护当前种群的路径编码矩阵、适应度向量和粒子群附加状态信息（如速度矩阵、个体最优和全局最优记录）。在每次更新完成后，通过统一的适应度计算模块对整个种群进行评价，并根据适应度排序选出下一代的基础个体与粒子群的目标解。整个主框架需要具备高度模块化特性，使得GA阶段和PSO阶段可以单独调试、替换或扩展，例如在后续可以加入自适应参数更新模块、多种交叉与变异算子等扩展功能。在主框架中还应引入收敛判定机制，例如当若干代内最优适应度提升极小或保持不变时提前终止迭代，从而节省计算时间。同时，为了提高稳定性，主框架可以在每代记录若干最优路径及其适应度，便于迭代结束后进行结果对比与可视化展示。

遗传算法模块结构与原理

遗传算法模块负责在种群层面进行全局探索，其核心思想来源于生物进化中的自然选择和遗传机制。在三维路径规划中，种群中的每个个体代表一条完整路径，对应一个实值向量编码。遗传模块主要由选择、交叉和变异三类操作构成。选择操作根据个体适应度进行优胜劣汰，可以使用轮盘赌选择、锦标赛选择或排序选择等策略，使适应度高的个体有更大概率被选入下一代。交叉操作在两个父路径之间交换部分节点信息，例如采用单点交叉或多点交叉，在某个节点索引前后对父代编码进行分段重组，从而创造新的路径结构。变异操作则对个别节点的坐标进行随机扰动，在一定范围内改变节点位置，以引入新的基因信息增强种群多样性。遗传模块中需要合理设置交叉概率与变异概率：交叉概率决定了种群重组的程度，变异概率则控制随机探索的强度。为避免出现不合法路径，交叉和变异结果需通过边界截断和障碍修复策略确保节点坐标落在有效范围内。遗传算法的优势在于能够在大范围搜索空间内持续发掘新方案，特别是在迭代初期能快速扩展解空间覆盖面，避免种群快速聚集到局部区域。通过不断重组与筛选，遗传模块为后续的粒子群局部优化提供多样而高潜力的初始解，为整个混合算法提供坚实的全局探索基础。

粒子群优化模块结构与原理

粒子群优化模块主要负责在当前已知较优解附近进行高效局部搜索。每个粒子对应一条路径，其位置表示路径节点的坐标向量，速度表示在路径空间中的变化方向和步长。粒子群算法通过不断更新粒子速度和位置，使粒子在个体历史最优位置和整体群体历史最优位置之间权衡，逐步接近最优解。在三维路径规划场景中，粒子群模块使用与遗传模块相同的编码方式，使路径表示在两个模块之间无缝切换。速度更新公式中，惯性项控制粒子保持原有运动趋势的程度，个体认知项反映粒子向自身历史最优解靠拢的趋势，社会认知项反映粒子向群体最优解靠拢的趋势。通过调整惯性权重和学习因子，可以控制搜索行为偏向探索还是开发。在混合框架中，一种常用策略是让粒子群在遗传筛选出的高质量个体附近进行集中的多次更新，以快速细化节点位置，减少路径长度、改善平滑度，同时充分利用环境信息。为了防止粒子产生过大步长导致跳出可行区域，需要对速度设置上限，并在位置更新后对节点进行边界检查和障碍检测，对超界或碰撞路径进行修复或惩罚。粒子群模块的特点是实现简单、参数较少且对连续路径优化效果显著，配合遗传模块的多样性保持机制，可以有效避免局部最优陷阱，加速整个算法的收敛过程。

适应度评价与约束处理模块结构与原理

适应度评价与约束处理模块是整个模型架构的评价核心，负责将路径编码映射为一个综合性能指标。在三维路径规划问题中，适应度函数通常要考虑路径长度、飞行时间、能耗估计、航迹平滑度以及安全裕度等多个方面。路径长度可以通过计算相邻节点之间的欧氏距离并求和获得；能耗估计可用路径长度和高度变化的加权组合进行近似；航迹平滑度可以通过计算相邻线段之间的夹角、曲率或加速度变化进行评价；安全裕度则需要计算路径上各点到障碍物的最小距离，将距离过小的路径施加惩罚。在约束处理上，将不可行路径通过罚函数机制进行处理，所有违反硬约束的路径在适应度中加入一个较大的惩罚值，使其在选择阶段不易被保留。对于软约束则采用连续的惩罚函数，例如安全距离不足时按距离的倒数或指数函数增加罚值，引导路径向更加安全的方向微调。为了提高计算效率，模块内部采用矩阵和向量化运算处理整条路径的节点序列，并尽量避免冗余重复计算，例如为障碍物预先构建距离场或快速查询结构。在混合算法的每一次迭代中，适应度评价模块都需要被频繁调用，因此其结构设计必须兼顾准确性与效率。通过合理的评价与约束处理，该模块为GA-PSO提供清晰的优化方向，使整个算法能够朝着符合任务需求的路径解集收敛。

项目模型描述及代码示例

三维环境和障碍物建模示例
env.xMin = 0; % 定义环境X轴最小边界用于约束路径搜索范围
env.xMax = 100; % 定义环境X轴最大边界决定场景长度
env.yMin = 0; % 定义环境Y轴最小边界用于限制横向范围
env.yMax = 100; % 定义环境Y轴最大边界形成正方形区域
env.zMin = 0; % 定义环境Z轴最小高度通常对应地面高度
env.zMax = 50; % 定义环境Z轴最大高度代表可飞行上限
hold on; % 保持当前图像以便继续绘制障碍物和路径
[xb,yb,zb] = meshgrid(20:40,20:40,0:30); % 生成建筑体素网格用于近似方形建筑物
obs1 = struct('x',xb,'y',yb,'z',zb); % 将建筑物网格坐标存入结构体便于统一管理
plot3(xb(:),yb(:),zb(:),'.','Color',[0.5 0.5 0.5]); % 以灰色散点形式绘制建筑体素表示占据空间
[xb2,yb2,zb2] = meshgrid(60:80,60:80,0:40); % 生成第二个建筑物的三维网格坐标集合
obs2 = struct('x',xb2,'y',yb2,'z',zb2); % 封装第二个障碍物的坐标数据结构
plot3(xb2(:),yb2(:),zb2(:),'.','Color',[0.4 0.4 0.4]); % 绘制第二个建筑物点云以展示其位置和高度
env.obstacles = {obs1,obs2}; % 将所有障碍物放入元胞数组以供后续碰撞检测使用
startPoint = [5,5,5]; % 定义无人机起点坐标靠近区域一角
goalPoint = [95,95,10]; % 定义无人机终点坐标位于对角区域略高位置
plot3(startPoint(1),startPoint(2),startPoint(3),'go','MarkerSize',10,'MarkerFaceColor','g'); % 用绿色圆点标记起点位置
plot3(goalPoint(1),goalPoint(2),goalPoint(3),'ro','MarkerSize',10,'MarkerFaceColor','r'); % 用红色圆点标记终点位置
title('3D Environment with Obstacles for GA-PSO Path Planning'); % 设置图窗标题便于识别场景内容
路径编码与初始种群生成示例
numWaypoints = 8; % 设置路径中间控制节点数量决定路径自由度
dim = numWaypoints * 3; % 每个节点包含x,y,z三维坐标总维度为节点数乘以3
popSize = 40; % 设置初始种群规模控制遗传与粒子群个体数量
lb = [env.xMin env.yMin env.zMin]; % 定义单个节点坐标下界向量用于后续约束
ub = [env.xMax env.yMax env.zMax]; % 定义单个节点坐标上界向量限制搜索范围
function f = evaluatePathFitness(pathVec,env,startPoint,goalPoint) % 定义路径适应度计算函数用于GA和PSO共用
numWaypoints = numel(pathVec)/3; % 根据向量长度还原中间节点数量
wp = reshape(pathVec,3,numWaypoints)'; % 将路径向量重排成节点坐标矩阵
fullPath = [startPoint; wp; goalPoint]; % 将起点中间节点和终点拼接成完整路径
segLen = sqrt(sum(segmentDiff.^2,2)); % 通过欧氏距离公式计算每段路径长度  
totalLen = sum(segLen); % 将所有段长度相加得到路径总长度  

smoothPenalty = 0; % 初始化平滑度惩罚项默认值为零  
for k = 2:size(fullPath,1)-1 % 遍历路径中间节点计算转折角度  
    if norm(v1) > 0 && norm(v2) > 0 % 确保向量长度非零避免数值问题  
        angle = acos(cosTheta); % 求反余弦得到转角弧度大小  
        smoothPenalty = smoothPenalty + angle; % 将转角累加作为平滑惩罚指标  
end  

collisionPenalty = 0; % 初始化碰撞惩罚项值为零  
for s = 1:size(fullPath,1)-1 % 遍历每一段路径进行碰撞检测  
    p0 = fullPath(s,:); % 获取路径段起点坐标  
    p1 = fullPath(s+1,:); % 获取路径段终点坐标  
    t = linspace(0,1,numSamples); % 在线段上生成插值参数序列  
    pts = p0 + (p1 - p0).*t'; % 计算插值采样点坐标集合  
    for o = 1:numel(env.obstacles) % 遍历所有障碍物进行距离判断  
        obs = env.obstacles{o}; % 取出当前障碍物结构体数据  
        obsPts = [obs.x(:),obs.y(:),obs.z(:)]; % 将障碍点云展平为二维坐标数组  
            d = sqrt(sum((obsPts - pts(q,:)).^2,2)); % 计算采样点与障碍点集合的欧氏距离  
            safeDist = 2; % 设定安全距离阈值单位与坐标一致  
            if minD < safeDist % 若最小距离小于安全要求则认为存在潜在碰撞风险  
                collisionPenalty = collisionPenalty + 1e5*(safeDist - minD); % 按差值大小叠加巨大惩罚压制该路径  
        end  
    end  
end  

wLen = 1.0; % 为路径长度设置权重控制其在适应度中的重要程度  
f = wLen * totalLen + wSmooth * smoothPenalty + collisionPenalty; % 综合路径长度平滑度和碰撞惩罚构成最终适应度  
end
遗传算法操作示例：选择、交叉与变异
[fitnessSorted,idxSort] = sort(fitness); % 按适应度从小到大排序获取排序索引
pop = pop(idxSort,:); % 根据排序结果重排种群将优秀个体排在前面
numElite = 4; % 设置精英个体数量用于直接保留到下一代
newPop = zeros(size(pop)); % 预分配新种群矩阵用于存放下一代个体
newPop(1:numElite,:) = pop(1:numElite,:); % 将最优若干个个体直接复制到新种群的前几位
crossRate = 0.8; % 设置交叉概率控制父代重组频率
mutRate = 0.1; % 设置变异概率控制随机扰动强度
function [c1,c2] = singlePointCrossover(p1,p2) % 定义单点交叉函数在父代间交换片段
dim = numel(p1); % 获取个体编码长度以限制交叉点范围
cp = randi([1 dim-1]); % 在编码范围内随机选择一个交叉点索引
c1 = [p1(1:cp) p2(cp+1:end)]; % 构造第一个子代前半来自父1后半来自父2
c2 = [p2(1:cp) p1(cp+1:end)]; % 构造第二个子代前半来自父2后半来自父1
end
粒子群速度与位置更新示例
numParticles = popSize; % 将粒子数量设置为与种群规模一致方便统一管理
particles = pop; % 将当前种群编码视为粒子位置矩阵
vel = zeros(size(particles)); % 初始化粒子速度矩阵初始速度设为零
pBest = particles; % 将粒子个体历史最优位置初始设为当前位置
pBestFit = fitnessSorted; % 将个体历史最优适应度初始设为当前适应度
[gBestFit,gIdx] = min(pBestFit); % 计算全局最优适应度并记录对应索引
gBest = pBest(gIdx,:); % 提取全局最优路径编码作为粒子群共同目标
w = 0.8; % 设置粒子群惯性权重控制速度惯性影响
c1 = 1.5; % 设置个体学习因子权重控制粒子向历史最优靠拢程度
c2 = 1.5; % 设置群体学习因子权重控制粒子向全局最优靠拢程度
vMax = 0.2*(repmat(ub - lb,1,numWaypoints)); % 设置速度上限与坐标范围成正比限制最大步长
for iterPSO = 1:numPSOIter % 在当前代内进行多次粒子群迭代
for i = 1:numParticles % 遍历每个粒子更新速度和位置
r1 = rand(size(particles(i,:))); % 生成个体认知随机系数向量
r2 = rand(size(particles(i,:))); % 生成社会认知随机系数向量
cognitive = c1 * r1 .* (pBest(i,:) - particles(i,:)); % 计算粒子向个体最优位置的速度分量
social = c2 * r2 .* (gBest - particles(i,:)); % 计算粒子向全局最优位置的速度分量
vel(i,:) = w*vel(i,:) + cognitive + social; % 综合惯性个体和社会三部分得到新速度
vel(i,:) = max(min(vel(i,:),vMax),-vMax); % 对速度进行裁剪确保在正负最大速度范围内
    nodeCoords(:,1) = min(max(nodeCoords(:,1),env.xMin),env.xMax); % 将X坐标裁剪到环境X边界范围  
    particles(i,:) = reshape(nodeCoords',1,[]); % 将修正后的节点坐标重新展平为编码向量  
    fitNew = evaluatePathFitness(particles(i,:),env,startPoint,goalPoint); % 计算更新后粒子路径的适应度  
        pBest(i,:) = particles(i,:); % 更新该粒子的最佳位置记录  
        pBestFit(i) = fitNew; % 更新该粒子的最佳适应度值  
    end  
    if fitNew < gBestFit % 若新解优于当前全局最优则更新全局记录  
        gBest = particles(i,:); % 更新全局最优路径编码为当前粒子位置  
        gBestFit = fitNew; % 更新全局最优适应度值  
end  
end
pop = particles; % 将粒子群更新后的编码反写回种群矩阵作为下一代基础个体
fitness = pBestFit; % 将个体最优适应度作为当前种群适应度估计供后续遗传阶段使用
主迭代流程与最优路径可视化示例
for gen = 1:maxGen % 外层循环执行多代进化过程
fitness = zeros(popSize,1); % 每代开始重新分配适应度数组
for i = 1:popSize % 遍历当前种群计算适应度
fitness(i) = evaluatePathFitness(pop(i,:),env,startPoint,goalPoint); % 调用评价函数得到每条路径代价
end
[fitnessSorted,idxSort] = sort(fitness); % 对适应度进行排序便于选择精英个体
pop = pop(idxSort,:); % 按照适应度排序更新种群矩阵顺序
bestHistory(gen) = fitnessSorted(1); % 记录当前代最优适应度以跟踪收敛情况  
bestPathRecord{gen} = pop(1,:); % 保存当前代最优路径编码便于最终显示与分析  

numElite = 4; % 设置需要保留的精英个体数量  
selectionProb = selectionProb / sum(selectionProb); % 将选择概率归一化使总和为1  

crossRate = 0.8; % 设置当前代交叉概率  
mutRate = 0.1; % 设置当前代变异概率  

for i = numElite+1:2:popSize % 使用轮盘赌和交叉产生新个体直到填满新种群  
    parent1 = rouletteSelect(pop,selectionProb); % 选择第一个父代个体  
    parent2 = rouletteSelect(pop,selectionProb); % 选择第二个父代个体  
    if rand < crossRate % 按概率决定是否执行交叉操作  
        [child1,child2] = singlePointCrossover(parent1,parent2); % 使用单点交叉生成两个子代  
        child1 = parent1; % 若不交叉则第一个子代直接等于父代一  
        child2 = parent2; % 第二个子代等于父代二  
    end  
    child1 = gaussianMutation(child1,mutRate,lb,ub); % 对第一个子代应用高斯变异保持多样性  
    child2 = gaussianMutation(child2,mutRate,lb,ub); % 对第二个子代应用高斯变异  
    newPop(i,:) = child1; % 将子代1添加到新种群  
        newPop(i+1,:) = child2; % 将子代2放入新种群相邻位置  
    end  

pop = newPop; % 用新种群替换旧种群准备进行粒子群阶段  

pBestFit = zeros(popSize,1); % 为个体最优适应度分配存储空间  
for i = 1:popSize % 为每个粒子计算当前适应度作为初始最佳记录  
[gBestFit,gIdx] = min(pBestFit); % 获取当前粒子群全局最优适应度与索引  

w = 0.8; % 粒子群惯性权重初始值  
c1 = 1.5; % 粒子个体学习因子  
vMax = 0.2*(repmat(ub - lb,1,numWaypoints)); % 构建速度上界向量用于约束每个维度  

numPSOIter = 3; % 设置本代执行粒子群迭代轮数适中提高效率  

for iterPSO = 1:numPSOIter % 进行多轮粒子群位置更新  
    for i = 1:popSize % 遍历所有粒子进行更新  
        r1 = rand(size(particles(i,:))); % 生成个体认知随机系数  
        cognitive = c1 * r1 .* (pBest(i,:) - particles(i,:)); % 计算个体认知速度分量  
        social = c2 * r2 .* (gBest - particles(i,:)); % 计算社会认知速度分量  
        vel(i,:) = max(min(vel(i,:),vMax),-vMax); % 裁剪速度防止过大步长导致不稳定  

        nodeCoords = reshape(particles(i,:),3,numWaypoints)'; % 将编码解读为节点坐标数组  
        nodeCoords(:,1) = min(max(nodeCoords(:,1),env.xMin),env.xMax); % 裁剪X坐标保持在环境边界之内  
        nodeCoords(:,3) = min(max(nodeCoords(:,3),env.zMin),env.zMax); % 裁剪Z坐标保持在环境高度范围内  
        particles(i,:) = reshape(nodeCoords',1,[]); % 将修正后的节点坐标重新转换成路径编码向量  
        fitNew = evaluatePathFitness(particles(i,:),env,startPoint,goalPoint); % 评估新位置路径的适应度  
        if fitNew < pBestFit(i) % 若新适应度优于历史记录则更新个体最优  
            pBest(i,:) = particles(i,:); % 更新该粒子最优位置  
            pBestFit(i) = fitNew; % 更新该粒子最优适应度  
        end  
            gBest = particles(i,:); % 更新全局最优位置为当前粒子路径  
            gBestFit = fitNew; % 更新全局最优适应度值  
        end  
    end  
    w = w * 0.95; % 在迭代过程中逐步减小惯性权重从探索过渡到开发  
end  
pop = particles; % 将粒子群最终位置作为本代结束后的种群状态  

figure(1); % 激活环境绘图窗口  
hold on; % 保持图形以叠加绘制路径  
plot3(fullPath(:,1),fullPath(:,2),fullPath(:,3),'b-','LineWidth',2); % 用蓝色实线绘制当前代最优路径  
drawnow; % 立即刷新图形窗口以动态显示路径演化过程  
end
[bestOverallFit,bestGen] = min(bestHistory); % 在所有代中寻找全局最优适应度及对应代数
bestOverallPath = bestPathRecord{bestGen}; % 获取全局最优路径编码
figure('Name','Best GA-PSO Path Result'); % 新建图窗展示最终最优路径结果
axis equal; % 设置坐标等比例显示
grid on; % 打开网格便于观察位置
xlabel('X'); % 标注X轴含义
ylabel('Y'); % 标注Y轴含义
zlabel('Z'); % 标注Z轴含义
view(3); % 采用三维视角展示最终路径
hold on; % 保持绘图以叠加障碍与路径
plot3(obs1.x(:),obs1.y(:),obs1.z(:),'.','Color',[0.5 0.5 0.5]); % 绘制第一个建筑障碍点云
plot3(obs2.x(:),obs2.y(:),obs2.z(:),'.','Color',[0.4 0.4 0.4]); % 绘制第二个建筑障碍点云
plot3(startPoint(1),startPoint(2),startPoint(3),'go','MarkerSize',10,'MarkerFaceColor','g'); % 以绿色圆点标注起点
plot3(goalPoint(1),goalPoint(2),goalPoint(3),'ro','MarkerSize',10,'MarkerFaceColor','r'); % 以红色圆点标注终点
wpBest = reshape(bestOverallPath,3,numWaypoints)'; % 将全局最优路径编码转为节点矩阵
fullPathBest = [startPoint; wpBest; goalPoint]; % 构造成完整路径坐标序列
plot3(fullPathBest(:,1),fullPathBest(:,2),fullPathBest(:,3),'m-','LineWidth',3); % 用洋红色粗线绘制全局最优路径
title('Optimal 3D UAV Path using GA-PSO Hybrid Algorithm'); % 设置图窗标题说明当前显示为GA-PSO优化结果
figure('Name','Fitness Convergence Curve'); % 新建图窗绘制适应度收敛曲线
plot(1:maxGen,bestHistory,'LineWidth',2); % 绘制每代最优适应度随代数变化的曲线
xlabel('Generation'); % 标注横轴为遗传代数
ylabel('Best Fitness'); % 标注纵轴为当前代最优适应度值
grid on; % 打开网格便于观察曲线趋势
title('GA-PSO Fitness Convergence History'); % 设置图窗标题说明曲线含义

三维环境和障碍物建模示例

env.xMin = 0; % 定义环境X轴最小边界用于约束路径搜索范围
env.xMax = 100; % 定义环境X轴最大边界决定场景长度
env.yMin = 0; % 定义环境Y轴最小边界用于限制横向范围
env.yMax = 100; % 定义环境Y轴最大边界形成正方形区域
env.zMin = 0; % 定义环境Z轴最小高度通常对应地面高度
env.zMax = 50; % 定义环境Z轴最大高度代表可飞行上限

hold on; % 保持当前图像以便继续绘制障碍物和路径
[xb,yb,zb] = meshgrid(20:40,20:40,0:30); % 生成建筑体素网格用于近似方形建筑物
obs1 = struct('x',xb,'y',yb,'z',zb); % 将建筑物网格坐标存入结构体便于统一管理
plot3(xb(:),yb(:),zb(:),'.','Color',[0.5 0.5 0.5]); % 以灰色散点形式绘制建筑体素表示占据空间

[xb2,yb2,zb2] = meshgrid(60:80,60:80,0:40); % 生成第二个建筑物的三维网格坐标集合
obs2 = struct('x',xb2,'y',yb2,'z',zb2); % 封装第二个障碍物的坐标数据结构
plot3(xb2(:),yb2(:),zb2(:),'.','Color',[0.4 0.4 0.4]); % 绘制第二个建筑物点云以展示其位置和高度

env.obstacles = {obs1,obs2}; % 将所有障碍物放入元胞数组以供后续碰撞检测使用

startPoint = [5,5,5]; % 定义无人机起点坐标靠近区域一角
goalPoint = [95,95,10]; % 定义无人机终点坐标位于对角区域略高位置
plot3(startPoint(1),startPoint(2),startPoint(3),'go','MarkerSize',10,'MarkerFaceColor','g'); % 用绿色圆点标记起点位置
plot3(goalPoint(1),goalPoint(2),goalPoint(3),'ro','MarkerSize',10,'MarkerFaceColor','r'); % 用红色圆点标记终点位置

title('3D Environment with Obstacles for GA-PSO Path Planning'); % 设置图窗标题便于识别场景内容

路径编码与初始种群生成示例

numWaypoints = 8; % 设置路径中间控制节点数量决定路径自由度
dim = numWaypoints * 3; % 每个节点包含x,y,z三维坐标总维度为节点数乘以3
popSize = 40; % 设置初始种群规模控制遗传与粒子群个体数量
lb = [env.xMin env.yMin env.zMin]; % 定义单个节点坐标下界向量用于后续约束
ub = [env.xMax env.yMax env.zMax]; % 定义单个节点坐标上界向量限制搜索范围

function f = evaluatePathFitness(pathVec,env,startPoint,goalPoint) % 定义路径适应度计算函数用于GA和PSO共用
numWaypoints = numel(pathVec)/3; % 根据向量长度还原中间节点数量
wp = reshape(pathVec,3,numWaypoints)'; % 将路径向量重排成节点坐标矩阵
fullPath = [startPoint; wp; goalPoint]; % 将起点中间节点和终点拼接成完整路径

segLen = sqrt(sum(segmentDiff.^2,2)); % 通过欧氏距离公式计算每段路径长度  

totalLen = sum(segLen); % 将所有段长度相加得到路径总长度  

smoothPenalty = 0; % 初始化平滑度惩罚项默认值为零  

for k = 2:size(fullPath,1)-1 % 遍历路径中间节点计算转折角度  

    if norm(v1) > 0 && norm(v2) > 0 % 确保向量长度非零避免数值问题  

        angle = acos(cosTheta); % 求反余弦得到转角弧度大小  

        smoothPenalty = smoothPenalty + angle; % 将转角累加作为平滑惩罚指标  

end  

collisionPenalty = 0; % 初始化碰撞惩罚项值为零  

for s = 1:size(fullPath,1)-1 % 遍历每一段路径进行碰撞检测  

    p0 = fullPath(s,:); % 获取路径段起点坐标  

    p1 = fullPath(s+1,:); % 获取路径段终点坐标  

    t = linspace(0,1,numSamples); % 在线段上生成插值参数序列  

    pts = p0 + (p1 - p0).*t'; % 计算插值采样点坐标集合  

    for o = 1:numel(env.obstacles) % 遍历所有障碍物进行距离判断  

        obs = env.obstacles{o}; % 取出当前障碍物结构体数据  

        obsPts = [obs.x(:),obs.y(:),obs.z(:)]; % 将障碍点云展平为二维坐标数组  

            d = sqrt(sum((obsPts - pts(q,:)).^2,2)); % 计算采样点与障碍点集合的欧氏距离  

            safeDist = 2; % 设定安全距离阈值单位与坐标一致  

            if minD < safeDist % 若最小距离小于安全要求则认为存在潜在碰撞风险  

                collisionPenalty = collisionPenalty + 1e5*(safeDist - minD); % 按差值大小叠加巨大惩罚压制该路径  

        end  

    end  

end  

wLen = 1.0; % 为路径长度设置权重控制其在适应度中的重要程度  

f = wLen * totalLen + wSmooth * smoothPenalty + collisionPenalty; % 综合路径长度平滑度和碰撞惩罚构成最终适应度  

end

遗传算法操作示例：选择、交叉与变异

[fitnessSorted,idxSort] = sort(fitness); % 按适应度从小到大排序获取排序索引
pop = pop(idxSort,:); % 根据排序结果重排种群将优秀个体排在前面

numElite = 4; % 设置精英个体数量用于直接保留到下一代
newPop = zeros(size(pop)); % 预分配新种群矩阵用于存放下一代个体
newPop(1:numElite,:) = pop(1:numElite,:); % 将最优若干个个体直接复制到新种群的前几位

crossRate = 0.8; % 设置交叉概率控制父代重组频率
mutRate = 0.1; % 设置变异概率控制随机扰动强度

function [c1,c2] = singlePointCrossover(p1,p2) % 定义单点交叉函数在父代间交换片段
dim = numel(p1); % 获取个体编码长度以限制交叉点范围
cp = randi([1 dim-1]); % 在编码范围内随机选择一个交叉点索引
c1 = [p1(1:cp) p2(cp+1:end)]; % 构造第一个子代前半来自父1后半来自父2
c2 = [p2(1:cp) p1(cp+1:end)]; % 构造第二个子代前半来自父2后半来自父1
end

粒子群速度与位置更新示例

numParticles = popSize; % 将粒子数量设置为与种群规模一致方便统一管理
particles = pop; % 将当前种群编码视为粒子位置矩阵
vel = zeros(size(particles)); % 初始化粒子速度矩阵初始速度设为零
pBest = particles; % 将粒子个体历史最优位置初始设为当前位置
pBestFit = fitnessSorted; % 将个体历史最优适应度初始设为当前适应度
[gBestFit,gIdx] = min(pBestFit); % 计算全局最优适应度并记录对应索引
gBest = pBest(gIdx,:); % 提取全局最优路径编码作为粒子群共同目标

w = 0.8; % 设置粒子群惯性权重控制速度惯性影响
c1 = 1.5; % 设置个体学习因子权重控制粒子向历史最优靠拢程度
c2 = 1.5; % 设置群体学习因子权重控制粒子向全局最优靠拢程度
vMax = 0.2*(repmat(ub - lb,1,numWaypoints)); % 设置速度上限与坐标范围成正比限制最大步长

for iterPSO = 1:numPSOIter % 在当前代内进行多次粒子群迭代
for i = 1:numParticles % 遍历每个粒子更新速度和位置
r1 = rand(size(particles(i,:))); % 生成个体认知随机系数向量
r2 = rand(size(particles(i,:))); % 生成社会认知随机系数向量
cognitive = c1 * r1 .* (pBest(i,:) - particles(i,:)); % 计算粒子向个体最优位置的速度分量
social = c2 * r2 .* (gBest - particles(i,:)); % 计算粒子向全局最优位置的速度分量
vel(i,:) = w*vel(i,:) + cognitive + social; % 综合惯性个体和社会三部分得到新速度
vel(i,:) = max(min(vel(i,:),vMax),-vMax); % 对速度进行裁剪确保在正负最大速度范围内

    nodeCoords(:,1) = min(max(nodeCoords(:,1),env.xMin),env.xMax); % 将X坐标裁剪到环境X边界范围  

    particles(i,:) = reshape(nodeCoords',1,[]); % 将修正后的节点坐标重新展平为编码向量  

    fitNew = evaluatePathFitness(particles(i,:),env,startPoint,goalPoint); % 计算更新后粒子路径的适应度  

        pBest(i,:) = particles(i,:); % 更新该粒子的最佳位置记录  

        pBestFit(i) = fitNew; % 更新该粒子的最佳适应度值  

    end  

    if fitNew < gBestFit % 若新解优于当前全局最优则更新全局记录  

        gBest = particles(i,:); % 更新全局最优路径编码为当前粒子位置  

        gBestFit = fitNew; % 更新全局最优适应度值  

end  

end

pop = particles; % 将粒子群更新后的编码反写回种群矩阵作为下一代基础个体
fitness = pBestFit; % 将个体最优适应度作为当前种群适应度估计供后续遗传阶段使用

主迭代流程与最优路径可视化示例

for gen = 1:maxGen % 外层循环执行多代进化过程
fitness = zeros(popSize,1); % 每代开始重新分配适应度数组
for i = 1:popSize % 遍历当前种群计算适应度
fitness(i) = evaluatePathFitness(pop(i,:),env,startPoint,goalPoint); % 调用评价函数得到每条路径代价
end
[fitnessSorted,idxSort] = sort(fitness); % 对适应度进行排序便于选择精英个体
pop = pop(idxSort,:); % 按照适应度排序更新种群矩阵顺序

bestHistory(gen) = fitnessSorted(1); % 记录当前代最优适应度以跟踪收敛情况  

bestPathRecord{gen} = pop(1,:); % 保存当前代最优路径编码便于最终显示与分析  

numElite = 4; % 设置需要保留的精英个体数量  

selectionProb = selectionProb / sum(selectionProb); % 将选择概率归一化使总和为1  

crossRate = 0.8; % 设置当前代交叉概率  

mutRate = 0.1; % 设置当前代变异概率  

for i = numElite+1:2:popSize % 使用轮盘赌和交叉产生新个体直到填满新种群  

    parent1 = rouletteSelect(pop,selectionProb); % 选择第一个父代个体  

    parent2 = rouletteSelect(pop,selectionProb); % 选择第二个父代个体  

    if rand < crossRate % 按概率决定是否执行交叉操作  

        [child1,child2] = singlePointCrossover(parent1,parent2); % 使用单点交叉生成两个子代  

        child1 = parent1; % 若不交叉则第一个子代直接等于父代一  

        child2 = parent2; % 第二个子代等于父代二  

    end  

    child1 = gaussianMutation(child1,mutRate,lb,ub); % 对第一个子代应用高斯变异保持多样性  

    child2 = gaussianMutation(child2,mutRate,lb,ub); % 对第二个子代应用高斯变异  

    newPop(i,:) = child1; % 将子代1添加到新种群  

        newPop(i+1,:) = child2; % 将子代2放入新种群相邻位置  

    end  

pop = newPop; % 用新种群替换旧种群准备进行粒子群阶段  

pBestFit = zeros(popSize,1); % 为个体最优适应度分配存储空间  

for i = 1:popSize % 为每个粒子计算当前适应度作为初始最佳记录  

[gBestFit,gIdx] = min(pBestFit); % 获取当前粒子群全局最优适应度与索引  

w = 0.8; % 粒子群惯性权重初始值  

c1 = 1.5; % 粒子个体学习因子  

vMax = 0.2*(repmat(ub - lb,1,numWaypoints)); % 构建速度上界向量用于约束每个维度  

numPSOIter = 3; % 设置本代执行粒子群迭代轮数适中提高效率  

for iterPSO = 1:numPSOIter % 进行多轮粒子群位置更新  

    for i = 1:popSize % 遍历所有粒子进行更新  

        r1 = rand(size(particles(i,:))); % 生成个体认知随机系数  

        cognitive = c1 * r1 .* (pBest(i,:) - particles(i,:)); % 计算个体认知速度分量  

        social = c2 * r2 .* (gBest - particles(i,:)); % 计算社会认知速度分量  

        vel(i,:) = max(min(vel(i,:),vMax),-vMax); % 裁剪速度防止过大步长导致不稳定  

        nodeCoords = reshape(particles(i,:),3,numWaypoints)'; % 将编码解读为节点坐标数组  

        nodeCoords(:,1) = min(max(nodeCoords(:,1),env.xMin),env.xMax); % 裁剪X坐标保持在环境边界之内  

        nodeCoords(:,3) = min(max(nodeCoords(:,3),env.zMin),env.zMax); % 裁剪Z坐标保持在环境高度范围内  

        particles(i,:) = reshape(nodeCoords',1,[]); % 将修正后的节点坐标重新转换成路径编码向量  

        fitNew = evaluatePathFitness(particles(i,:),env,startPoint,goalPoint); % 评估新位置路径的适应度  

        if fitNew < pBestFit(i) % 若新适应度优于历史记录则更新个体最优  

            pBest(i,:) = particles(i,:); % 更新该粒子最优位置  

            pBestFit(i) = fitNew; % 更新该粒子最优适应度  

        end  

            gBest = particles(i,:); % 更新全局最优位置为当前粒子路径  

            gBestFit = fitNew; % 更新全局最优适应度值  

        end  

    end  

    w = w * 0.95; % 在迭代过程中逐步减小惯性权重从探索过渡到开发  

end  

pop = particles; % 将粒子群最终位置作为本代结束后的种群状态  

figure(1); % 激活环境绘图窗口  

hold on; % 保持图形以叠加绘制路径  

plot3(fullPath(:,1),fullPath(:,2),fullPath(:,3),'b-','LineWidth',2); % 用蓝色实线绘制当前代最优路径  

drawnow; % 立即刷新图形窗口以动态显示路径演化过程  

end

[bestOverallFit,bestGen] = min(bestHistory); % 在所有代中寻找全局最优适应度及对应代数
bestOverallPath = bestPathRecord{bestGen}; % 获取全局最优路径编码

figure('Name','Best GA-PSO Path Result'); % 新建图窗展示最终最优路径结果
axis equal; % 设置坐标等比例显示
grid on; % 打开网格便于观察位置
xlabel('X'); % 标注X轴含义
ylabel('Y'); % 标注Y轴含义
zlabel('Z'); % 标注Z轴含义
view(3); % 采用三维视角展示最终路径

hold on; % 保持绘图以叠加障碍与路径
plot3(obs1.x(:),obs1.y(:),obs1.z(:),'.','Color',[0.5 0.5 0.5]); % 绘制第一个建筑障碍点云
plot3(obs2.x(:),obs2.y(:),obs2.z(:),'.','Color',[0.4 0.4 0.4]); % 绘制第二个建筑障碍点云

plot3(startPoint(1),startPoint(2),startPoint(3),'go','MarkerSize',10,'MarkerFaceColor','g'); % 以绿色圆点标注起点
plot3(goalPoint(1),goalPoint(2),goalPoint(3),'ro','MarkerSize',10,'MarkerFaceColor','r'); % 以红色圆点标注终点

wpBest = reshape(bestOverallPath,3,numWaypoints)'; % 将全局最优路径编码转为节点矩阵
fullPathBest = [startPoint; wpBest; goalPoint]; % 构造成完整路径坐标序列
plot3(fullPathBest(:,1),fullPathBest(:,2),fullPathBest(:,3),'m-','LineWidth',3); % 用洋红色粗线绘制全局最优路径

title('Optimal 3D UAV Path using GA-PSO Hybrid Algorithm'); % 设置图窗标题说明当前显示为GA-PSO优化结果

figure('Name','Fitness Convergence Curve'); % 新建图窗绘制适应度收敛曲线
plot(1:maxGen,bestHistory,'LineWidth',2); % 绘制每代最优适应度随代数变化的曲线
xlabel('Generation'); % 标注横轴为遗传代数
ylabel('Best Fitness'); % 标注纵轴为当前代最优适应度值
grid on; % 打开网格便于观察曲线趋势
title('GA-PSO Fitness Convergence History'); % 设置图窗标题说明曲线含义

更多详细内容请访问

http://【无人机路径规划】MATLAB实现基于GA-PSO遗传算法（GA）结合粒子群优化算法（PSO）进行无人机三维路径规划的详细项目实例（含完整的程序，GUI设计和代码详解）资源-CSDN下载 https://download.csdn.net/download/xiaoxingkongyuxi/92782359

https://download.csdn.net/download/xiaoxingkongyuxi/92782359

https://download.csdn.net/download/xiaoxingkongyuxi/92782359