MATLAB 2024a

1、算法描述

摘要

三维路径规划是无人机、移动机器人和低空智能装备中的关键问题。路径规划不仅要求从起点到终点生成一条可达路径，还要同时满足避障、安全距离、路径长度、转角平滑和边界约束等条件。传统单一最短路方法在规则环境中效果较好，但在三维复杂障碍环境下，容易出现路径贴近障碍物、转角突变或局部绕行不合理等问题。针对这一问题，本文设计了一种基于蜘蛛猴优化算法的三维路径规划仿真方法。系统首先构建三维空间、起点、终点和长方体障碍物模型；然后利用可视图思想生成一条初始可行路径；再将中间航路点编码为连续优化变量，采用蜘蛛猴优化算法对路径进行迭代搜索。适应度函数综合考虑路径长度、碰撞惩罚、安全距离惩罚、转角平滑惩罚、边界约束和路径前进方向约束。最后，通过路径平滑方法删除冗余航路点，并输出收敛曲线、三维路径图、XY投影图、高度剖面图、安全间距变化图和转角变化图。仿真结果表明，该方法能够在复杂三维环境中生成满足避障要求的可行路径，并兼顾路径长度和航迹平滑性。该系统结构清晰，参数可调，适合用于三维路径规划算法学习、仿真分析和改进算法对比。

关键词：蜘蛛猴优化算法；三维路径规划；无人机避障；安全距离约束；路径平滑；MATLAB仿真

1 引言

随着无人机和移动机器人应用场景不断扩展，路径规划问题已经从简单的二维平面寻路逐渐发展为复杂三维空间中的多约束优化问题。无人机在执行巡检、运输、测绘和应急任务时，通常需要在有限空间中绕过障碍物，同时保持安全距离，减少路径长度，并避免过大的转向动作。近期无人机路径规划综述也指出，路径规划与避障方法需要同时考虑全局路径、局部避障、单机任务和多机协同等问题，且不同算法在适应性、计算效率和复杂环境鲁棒性方面存在差异。

路径规划的本质不是简单画一条线，而是把空间约束、运动约束和优化目标统一到一个数学模型中。对于三维环境而言，障碍物不仅占据平面区域，还具有高度信息。路径需要在 x、y、z 三个方向上同时满足边界限制和避障限制。因此，若仅依赖普通最短路径搜索，容易得到过于贴近障碍物或转向不平滑的路径。无人机路径规划研究通常会将传统算法、智能算法和混合算法进行分类，其中智能优化算法在复杂空间搜索中具有较强适应性。

本文围绕三维路径规划问题，建立了一个基于蜘蛛猴优化算法的 MATLAB 仿真系统。系统并不是单纯随机搜索路径，而是采用“可视图初始路径 + SMO连续优化 + 路径平滑”的结构。这样做的实际意义是：可视图方法先提供一条较稳定的初始路径，避免优化算法从完全随机状态开始；蜘蛛猴优化算法负责在连续空间中进一步调整中间航路点；路径平滑模块则用于删除不必要的转折点，使最终路径更简洁。

2 三维环境建模

本文的三维环境由起点、终点、空间边界和多个长方体障碍物组成。起点设为 S=[1,950,15]S=[1,950,15]S=[1,950,15]，终点设为 E=[950,1,1]E=[950,1,1]E=[950,1,1]。搜索空间范围为 x 方向 0 至 1000，y 方向 0 至 1000，z 方向 0 至 20。障碍物使用长方体表示，每个障碍物由六个参数描述，分别为起始坐标 x、y、z，以及长、宽、高。

这种建模方法的优点是结构直观，计算简单，便于进行碰撞检测。对于任意路径采样点，系统可以计算该点到所有障碍物的最近距离。如果采样点位于障碍物内部，则距离被视为负值，并产生碰撞惩罚。如果采样点虽然没有进入障碍物，但距离小于设定安全距离，也会产生安全距离惩罚。本文设定安全距离为 8，表示路径不应过度贴近障碍物。

路径本身由起点、中间航路点和终点组成。中间航路点数量设为 8，每个航路点包含 x、y、z 三个变量，因此优化变量维度为 24。蜘蛛猴优化算法搜索的对象并不是整条离散路径，而是这些中间航路点的位置。通过这种编码方式，可以把三维路径规划转化为连续变量优化问题。

3 蜘蛛猴优化算法原理

蜘蛛猴优化算法是一类群智能优化算法，其思想来源于蜘蛛猴群体的觅食行为和分裂—融合社会结构。算法中存在全局首领和局部首领。全局首领代表当前整体种群中最优个体，局部首领代表某个子群体中的最优个体。种群个体在迭代过程中会同时受到局部首领、全局首领和其他个体的影响，从而完成搜索空间探索和局部精细搜索。

本文代码中的 SMO 算法主要包含以下步骤。第一，初始化种群，并将可视图生成的初始路径编码后作为种子个体加入种群。第二，计算每个个体的适应度值。第三，在局部首领阶段，个体围绕局部最优解和随机伙伴进行位置更新。第四，在全局首领阶段，个体根据选择概率向全局最优解和局部最优解靠近。第五，当局部子群长期没有改进时，对局部个体进行扰动；当全局最优长期停滞时，系统会进行分组或重新合并，以提高搜索多样性。

这种机制的核心优势是兼顾全局搜索和局部开发。对于路径规划问题，如果算法只强调全局搜索，容易收敛慢；如果只强调局部优化，容易陷入局部最优。SMO 的分组机制可以在一定程度上改善这个问题，使算法在复杂障碍环境中保持搜索能力。

4 适应度函数设计

路径规划算法能否得到合理结果，关键不只在优化器本身，更在适应度函数。本文适应度函数由多个部分组成，包括路径长度、平滑性、碰撞惩罚、安全距离惩罚、硬安全距离惩罚、边界惩罚和前进方向惩罚。

路径长度用于约束总航程。路径越长，代价越大。平滑性由路径转角计算得到，转角越大，平滑惩罚越高。这样可以避免路径出现过多急转弯。碰撞惩罚用于强制排除穿越障碍物的路径，其权重设置为较大值。安全距离惩罚用于处理路径虽然没有碰撞但离障碍物过近的情况。边界惩罚用于防止航路点超出三维空间边界。前进方向惩罚用于减少路径沿起点到终点方向的反向回绕。

本文采用的总代价函数可表示为：

J=wlJl+wsJs+wcJc+wdJd+whJh+wbJb+wpJpJ=w_lJ_l+w_sJ_s+w_cJ_c+w_dJ_d+w_hJ_h+w_bJ_b+w_pJ_pJ=wlJl+wsJs+wcJc+wdJd+whJh+wbJb+wpJp

其中，JlJ_lJl 为路径长度项，JsJ_sJs 为平滑项，JcJ_cJc 为碰撞项，JdJ_dJd 为安全距离项，JhJ_hJh 为硬安全距离项，JbJ_bJb 为边界项，JpJ_pJp 为前进方向项。各权重用于调节不同目标的重要程度。在实际系统中，碰撞惩罚权重最大，因为可行性必须优先于路径长度。安全距离和硬安全距离用于进一步提高路径安全余量。

这种适应度函数符合工程思路。路径规划不能只追求最短。最短路径如果贴障碍太近，实际飞行风险高；路径如果过度平滑但绕行太远，效率又低。因此，多目标加权是较实用的处理方式。类似研究也常把路径规划转化为包含安全、可行性和长度等指标的优化问题。

5 系统实现流程

本文系统主程序为 main.m，函数文件统一放在 function 文件夹中。程序运行时，首先清空环境并添加工程路径，然后设置三维环境参数和障碍物信息。之后，程序创建 figure 文件夹，用于保存结果图像。

系统流程如下。第一步，设置起点、终点、边界、障碍物、安全距离和适应度权重。第二步，设置 SMO 参数，包括中间航路点数量、种群数量、最大迭代次数和变量维度。第三步，调用 visibilityPath 函数生成初始路径。该函数根据障碍物外扩点构造候选节点，并通过节点间安全直连关系寻找一条初始可行路径。第四步，调用 encodePath 将初始路径编码为优化变量。第五步，调用 SMO 函数执行迭代优化。第六步，调用 buildPath 将最优变量解码为路径，并判断是否接受平滑结果。第七步，输出优化指标和六张结果图。

函数设计较清晰。decodePath 用于把一维变量恢复为三维航路点，并按起点到终点方向排序。evaluatePath 用于计算路径总代价和详细指标。pointObstacleDistance 用于计算采样点到障碍物的距离。samplePath 用于沿路径采样。smoothPath 用于删除可直接连通的冗余航路点。segmentIsSafe 用于判断两点之间的线段是否满足安全条件。SMO 是核心优化器。

这种模块化结构有一个明显好处：后续如果要替换算法，例如改成粒子群、灰狼、麻雀搜索或遗传算法，只需要替换优化器接口即可。环境建模、适应度函数、路径采样和绘图部分可以继续保留。

6 仿真结果分析

本系统输出六张图像。第一张为蜘蛛猴算法收敛曲线，用于观察适应度值随迭代次数下降的过程。如果曲线前期下降明显，说明种群搜索有效；如果后期趋于平稳，说明算法逐渐收敛。第二张为三维最优路径图，展示障碍物、原始航路点、最终路径、起点和终点。该图可以直接判断路径是否绕开障碍物。第三张为 XY 投影图，用于从平面角度观察路径绕行关系。第四张为高度剖面图，用于分析路径在 z 方向上的变化。第五张为安全间距变化图，用于检查路径沿程与最近障碍物之间的距离是否满足安全阈值。第六张为转角变化图，用于评价路径平滑程度。

从系统设计看，安全间距变化图是最关键的结果之一。很多路径规划图看起来没有碰撞，但实际采样距离可能低于安全阈值。通过安全间距曲线，可以直接判断路径是否具有足够余量。转角变化图也很重要。无人机实际飞行时，过大转角会带来控制难度和能耗增加。近年来关于无人机路径规划的研究也越来越重视能耗、转向、爬升和动态约束等因素，而不是只看几何距离。

本文仿真系统通过“距离 + 碰撞 + 平滑 + 安全阈值”的评价方式，使路径结果不仅可达，而且更接近实际工程需要。虽然当前系统主要面向静态障碍环境，但其结构已经具备扩展基础。如果后续引入动态障碍物，可以在 evaluatePath 中增加时间维度和动态碰撞检测。如果考虑无人机动力学约束，可以进一步增加最大爬升角、最大转角和最小转弯半径约束。

7 系统特点

本文系统具有以下特点。第一，环境建模直观。长方体障碍物便于配置，也便于计算点到障碍物的距离。第二，路径编码方式简单。中间航路点直接作为连续变量，适合群智能算法搜索。第三，初始路径质量较好。系统先通过可视图方法生成初始可行路径，减少随机初始化导致的无效搜索。第四，适应度函数较完整。系统同时考虑路径长度、碰撞、安全距离、转角、边界和前进方向。第五，结果图像完整。六张图能够从收敛、空间路径、平面投影、高度变化、安全距离和转角变化多个角度评价结果。第六，代码结构清楚。主函数和本地函数分离，便于阅读和二次扩展。第七，参数可调。用户可以通过修改障碍物、安全距离、权重、种群规模和迭代次数来适配不同场景。

8 结论

本文完成了一种基于蜘蛛猴优化算法的三维路径规划仿真系统。系统将无人机路径规划问题转化为连续变量优化问题，通过可视图方法提供初始可行路径，再利用 SMO 算法优化中间航路点，并通过路径平滑方法删除冗余节点。适应度函数综合考虑路径长度、转角平滑、碰撞、安全距离、边界和前进方向等因素，能够较好地反映三维避障路径规划的核心需求。

从工程角度看，该系统的价值不在于简单调用某个优化算法，而在于完整建立了“环境建模—路径编码—适应度评价—群智能搜索—路径平滑—结果分析”的闭环流程。该流程适合用于三维路径规划算法学习和仿真实验，也适合作为后续算法改进的基础框架。后续可以在该系统上继续加入动态障碍物、多无人机协同、能耗模型、速度约束和真实地图建模，从而进一步提高系统的工程适用性。

参考文献

Debnath, Dipanjan, et al. “A Review of UAV Path-Planning Algorithms and Obstacle Avoidance Methods for Remote Sensing Applications.” Remote Sensing, vol. 16, no. 21, 2024.
Luo, Jun, et al. “Research on Unmanned Aerial Vehicle Path Planning.” Drones, vol. 8, no. 2, 2024.
Cheng, Qing, et al. “Research on Particle Swarm Optimization-Based UAV Path Planning.” Drones, vol. 8, no. 12, 2024.
Zhou, X., et al. “Improved Grey Wolf Algorithm: A Method for UAV Path Planning.” Drones, vol. 8, no. 11, 2024.
Phung, Manh Duong, and Quang Phuc Ha. “Safety-Enhanced UAV Path Planning with Spherical Vector-Based Particle Swarm Optimization.” Applied Soft Computing, vol. 107, 2021.
Qi, Yuanhang, et al. “Multi-UAV Path Planning Considering Multiple Energy Consumptions via an Improved Bee Foraging Learning Particle Swarm Optimization Algorithm.” Scientific Reports, vol. 15, 2025.
Deng, L., et al. “Three-Dimensional Path Planning of UAV Based on Improved Particle Swarm Optimization.” Mathematics, vol. 11, no. 9, 2023.
Meng, Zhipeng, et al. “Intelligent Scheduling Technology of Swarm Intelligence Algorithm for Drone Path Planning.” Drones, vol. 8, no. 4, 2024.
Rosas-Carrillo, A. S., et al. “UAV Path Planning Using an Adaptive Strategy for the Particle Swarm Optimization Algorithm.” Drones, vol. 9, no. 3, 2025.
Gupta, D., et al. “A Novel Spider Monkey Optimization for Reliable Data Transmission.” Sensors, vol. 24, no. 7, 2024.