1. 项目背景与算法选型思考无人机路径规划这几年在巡检、测绘、物流配送、应急救援这些场景里被反复提起做这类项目的人也越来越多了。我最初接触这个方向是在做一个模拟任务给定一片带威胁源的空域要求无人机从指定起点飞到终点在保证不撞上障碍、不被威胁源覆盖的前提下尽量飞得短、飞得稳。听起来不复杂真正落地时会发现路径规划根本不是找一条最短线那么简单它是一个带多个约束条件的优化问题。麻雀搜索算法Sparrow Search AlgorithmSSA是近些年提出的群体智能算法核心是模拟麻雀觅食时的发现者-加入者-侦察者分工机制。用这个算法配合Matlab做无人机路径规划实现成本低、收敛快、易调试非常适合工程验证和教学演示。这篇内容我会从算法原理、路径建模、代码实现到调参踩坑一条线讲下来适合正在做无人机路径规划相关实验的开发者也适合需要快速完成一个仿真对比系统的同学参考。1.1 无人机路径规划到底在解决什么问题往数学上靠路径规划是在一个连续或离散空间里找到一条从起点到终点的可行曲线。如果这个空间是二维的要满足的条件可能只是避开障碍物但真实无人机任务往往是三维空间要考虑的因素一下子多起来几何约束飞行航迹要避开山体、建筑物、禁飞区。威胁约束战场上可能存在的雷达探测区、防空威胁区路径越深入威胁区越危险。飞行性能约束无人机有最大转弯角限制不能出现急转弯否则姿态调不过来。任务意图约束有些任务要尽量贴地飞行有些要保持在某一巡航高度这时路径的高度变化不能太剧烈。如果把这些问题全部塞进一个目标函数里传统路径规划算法就有点吃力了。比如A*这类基于网格搜索的方法一旦地图分辨率提高搜索空间爆炸内存和耗时根本扛不住而RRT这类随机采样算法虽然能在连续空间工作但生成的轨迹往往不够平滑后处理又得加一堆东西。群智能算法的优势在于它不关心你的目标函数长什么样——是不是连续、是不是线性、是否有解析梯度——只要能写出给一个解算出它的代价的函数就能在上面迭代搜索。这对路径规划非常友好因为我们的约束条件本来就五花八门用统一的适应度函数表达起来非常自然。1.2 为什么是麻雀搜索算法而不是粒子群或遗传算法很多第一次接触这个项目的人会问路径规划用粒子群PSO、遗传算法GA不也可以吗确实可以这类算法本质都属于群体智能核心都是生成一群候选解根据适应度反复迭代更新。我选择SSA主要有三个考虑。第一是收敛速度。SSA借鉴了麻雀觅食中的两种行为——发现者快速前往食物丰富区域加入者根据发现者位置进行跟随或绕行搜索。这种机制让算法在前中期能快速收缩搜索范围相比PSO和GA在同等迭代次数下更容易找到较优的路径。第二是参数少。PSO需要调惯性权重、两个加速因子GA需要调交叉概率、变异概率、选择策略。SSA的核心参数只有发现者数量比例、侦察者数量比例、预警阈值这几个并且默认值在大多数场景下都能跑出不错的结果这对不熟悉调参的人来说非常友好。第三是种群结构的差异化。SSA里同时存在倾向全局探索的发现者和倾向局部开发的加入者角色互补在一定程度上缓解了标准PSO容易早熟、陷入局部最优的问题。当然这也不是绝对的后面调参经验部分我会细说哪些情况会让SSA也陷入局部最优。下面这个表格是我在选型时做的一张对比直接抄作业即可算法参数数量收敛速度局部最优风险实现难度路径规划适配度SSA麻雀搜索少快中等低高PSO粒子群中中较高很低中GA遗传算法多中慢中中中GWO灰狼少快中低低中高2. 麻雀搜索算法的核心机制拆解想用好一个算法光会调用代码是不行的至少要知道它内部在干什么。SSA的设计灵感来自麻雀群体的日常行为你可以把它理解成一个微型社会一部分麻雀负责探路找食物一部分麻雀跟着大家蹭食物还有一小部分麻雀待在边缘负责放哨。这三种角色在算法里分别对应发现者、加入者、侦察者。2.1 三种角色的行为分工发现者的任务是开拓新区域。在每次迭代中它会根据自己上一轮的位置向食物更充足的方向移动搜索步长偏大保证群体能探索到更广的空间。加入者的策略比较简单在发现者已经找到的好位置附近继续搜索相当于在局部区域做精细搜索。侦察者的角色最特殊——随机数量的麻雀如果感知到危险会向安全区域转移这相当于人为给种群注入随机扰动避免所有个体都堆在同一个位置。这种分工在路径规划里很有用。路径规划的解空间往往有大量平坦区域多段路径代价差不多传统算法容易困在一个还行但不够好的区域而麻雀搜索算法中的侦察者机制会周期性破坏当前种群的一致性逼着一部分个体跳出原地重新搜索其他区域。实操中我发现适当提高侦察者比例确实能降低陷入局部最优的概率但也会让收敛速度变慢需要平衡。2.2 位置更新公式怎么读麻雀位置更新的核心公式分三部分。发现者的位置更新如下当预警值R2小于安全阈值ST时说明环境安全发现者可以进行大范围搜索。此时第i个麻雀在第j维上的新位置是x_new x_old * exp(-i / (alpha * max_iter));这个公式的含义是迭代前期i值小指数项接近1位移量大随着迭代次数增加移动步长逐渐缩小算法从粗探索转向精细收敛。这里alpha是一个介于0到1之间的随机数。如果预警值R2大于等于ST说明发现了危险这时候所有发现者需要飞到安全位置x_new x_old Q * randn();Q是服从正态分布的随机数相当于给位置加一个高斯扰动。加入者的更新逻辑是如果该加入者排名靠前就向当前最优发现者位置靠拢如果排名靠后说明它离最优解太远直接重新随机初始化。if i N/2 x_new rand() * (x_ub - x_lb) x_lb; % 重新随机初始化 else x_new x_best abs(x_old - x_best) * A_plus * ones(1, dim); end侦察者更新则会让位置最好的麻雀向附近飞离位置最差的麻雀向种群中心靠拢保证整个种群不会散得太开。说实话这些公式单独看有点抽象我第一次接触时也觉得干巴巴的。但把它放到路径规划语境下就清楚多了每一个麻雀个体其实就是一条完整的路径所有路径在解空间里飞行最优路径会把其他路径吸引过来逐步形成一条收敛的、代价最小的航迹。2.3 一次完整的迭代做了什么我用大白话总结一次迭代流程初始化N个麻雀个体每个个体各维坐标随机分布在定义域内。计算每个个体的适应度值也就是路径总代价。按适应度排序前PD%作为发现者其余作为加入者。根据预警值R2和阈值ST更新发现者位置。更新加入者位置跟随最优个体或重新随机初始化。随机选取SD%个体作为侦察者更新位置。处理越界重新计算适应度记录全局最优位置。迭代次数加1如果没达到最大迭代次数则回到第3步。这套流程和粒子群的整体框架相似但内在的行为机制更丰富。我在实际项目里最深的感受是SSA的发现者-加入者结构让N个麻雀个体能自然地分化为不同搜索策略而不是所有人都往同一个方向飞这比PSO所有粒子都被全局最优和个体最优牵引的机制更灵活。3. 无人机路径规划建模与适应度函数设计算法选好了接下来要考虑的是怎么把路径规划问题变成算法能解的问题。这是整个项目中最关键的一步因为不管算法多强如果你的建模不对最后飞出来的路径照样没法用。3.1 环境与威胁建模我做这个项目时用了一种很常见的三维环境规划空间是100m x 100m x 80m的立方体区域起点设置在(10, 10, 30)终点设置在(90, 90, 40)空间中散布几个球形威胁源每个威胁源用一个中心点坐标和影响半径表示。在实际代码里我把威胁源定义成向量形式threat_center [30, 20, 30; 55, 60, 30; 70, 30, 45]; threat_radius [15, 12, 10];威胁源的物理含义可以理解成雷达探测范围、防空火力区或者是禁飞球体。路径经过这些区域时会产生代价代价大小根据路径点到威胁源中心的距离计算。距离越近危险性越大。我用高斯型的威胁代价函数for i 1:num_threat d sqrt((x - threat_center(i,1)).^2 ... (y - threat_center(i,2)).^2 ... (z - threat_center(i,3)).^2); threatCost threatCost exp(-((d - threat_radius(i)).^2) / (2 * sigma^2)); end这个函数的优点是路径点刚进入威胁源半径附近时代价平滑上升进入核心区域时代价急剧增大算法为了最小化适应度会自然而然绕开威胁源。sigma控制代价衰减速度我一般取5~8太大则威胁影响范围过于模糊太小则惩罚过于生硬梯度信息不明显。这里顺便提一句边界处理因为无人机搜索空间是有限的如果麻雀个体某维坐标更新后越界最简单的处理是随机拉回边界内或者映射到边界值。我推荐映射到边界值原因是更稳定不会出现个体频繁越界又随机跳回导致的收敛抖动。3.2 路径编码方式是项目的关键设计点路径怎么用解来表示这是每个做这类项目的人必须自己回答的问题。我采用的做法是固定起点和终点在两者之间均匀插入K个中间航点每个航点有三维坐标那么一条路径就是一个3K维的向量。举个例子K5时路径为起点 → 航点1 → 航点2 → 航点3 → 航点4 → 航点5 → 终点。这个路径向量就是path [x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4, x5, y5, z5];麻雀种群中每一个个体就相当于这样一组航点坐标。解码后把所有点连起来就是一条三维轨迹。这个编码方式的优点是直观、维度可控缺点是航点数量K太大时维度会变高搜索空间也会指数膨胀所以K一般取5~10就够了太多反而难以收敛。更进阶的做法是B样条曲线编码个体只表示控制点路径本身通过B样条生成。优点是路径天然平滑缺点是复杂度提升。第一次做项目建议先用简单的航点连线方式把整个框架跑通后面再换高级编码。3.3 多目标代价函数怎么设计路径规划的目标从来不是单目标至少包括安全性和路径质量两方面。我在实验中把适应度设计成四个代价的加权和F w1 * f_length w2 * f_threat w3 * f_height w4 * f_smooth其中每个代价的含义和计算方式如下路径长度代价把所有相邻航点间的欧氏距离累加起来。路径越长代价越高。这是最基础的代价确保算法不会为了躲避一个威胁而绕太大的弯。威胁代价对路径上的多个采样点逐一计算威胁值累加。注意这里不是只计算航点而是把每段路径等距采样5~10个点因为有可能两个航点本身安全但它们连成的直线恰好从威胁源中间穿过只看航点会漏检。高度代价无人机有一定的期望巡航高度比如期望保持在35m左右如果路径点高度偏离期望值就产生惩罚。这个代价可以让规划结果避免出现忽上忽下的剧烈起伏。平滑代价相邻三个航点组成两个向量计算两个向量的夹角如果夹角过小说明路径出现了急转弯要加惩罚。我用的实现是对余弦值做惩罚cosTheta dot(v1, v2) / (norm(v1) * norm(v2) eps); smoothCost smoothCost abs(cosTheta 1); % 角度越小cos越接近-1四个代价放一起需要归一化处理否则数值量纲不同会导致某项目标统治一切。比如长度代价可能到300威胁代价可能只有几十甚至两百多直接相加的话长度代价权重过大路径会为了缩短长度直接穿过威胁源。我的做法是先跑一次随机初始化统计各子目标的平均量级再用权重调整到差不多的数量级。这种方法虽然土但很好用。我实际实验中用的权重是w11w23w31w42。不同场景需要重调后面我会讲怎么看结果判断权重合不合理。4. Matlab代码实现与实操过程建模完成后就到了写代码的环节。我用Matlab实现了整套流程整体结构不复杂核心代码大约三百行左右。下面的内容会按主函数、种群初始化、位置更新、适应度计算四部分讲解最后给出参数设置参考。4.1 程序整体结构为了不让代码纠缠成一团我按功能拆分成几个函数% main_ssa_path_planning.m % 参数设置 clear; clc; close all; N 30; % 种群规模 dim 15; % 维度5个中间航点 * 3维坐标 max_iter 200; % 最大迭代次数 lb [10*ones(1,15)]; % 下界 ub [90*ones(1,15)]; % 上界 % 起点终点与威胁区定义 start_pos [10, 10, 30]; end_pos [90, 90, 40]; threat_center [30, 20, 30; 55, 60, 30; 70, 30, 45]; threat_radius [15, 12, 10]; % 初始化种群 pop init_population(N, dim, lb, ub); % 计算初始适应度 fitness zeros(N, 1); for i 1:N fitness(i) calc_fitness(pop(i,:), start_pos, end_pos, threat_center, threat_radius); end % 迭代主循环 best_fitness_history zeros(max_iter, 1); for iter 1:max_iter [pop, fitness] ssa_update(pop, fitness, N, dim, lb, ub, iter, max_iter); [best_val, best_idx] min(fitness); best_fitness_history(iter) best_val; best_path pop(best_idx, :); end % 结果可视化 plot_path(best_path, start_pos, end_pos, threat_center, threat_radius);主循环中的ssa_update函数内部会按照发现者、加入者、侦察者三种机制更新位置。这样拆分的好处是代码逻辑清晰想替换成PSO或者其他算法时只要改update函数就行主程序不需要大动。4.2 种群初始化与边界处理初始化看着简单其实有个小坑航点坐标必须保持起点到终点的大致趋势。如果你完全随机生成中间航点那么初始解集里全是乱七八糟的路径虽然算法可以靠迭代修正但前期收敛会明显变慢。更好的做法是为每个中间航点生成时加入一个指向终点的偏置让初始路径更合理。function pop init_population(N, dim, lb, ub) pop lb (ub - lb) .* rand(N, dim); % 把中间航点改为在起点和终点连线附近随机扰动 for i 1:N for k 1:dim/3 t k / (dim/3 1); % 0~1之间的插值比例 mid (1-t) * start_pos t * end_pos; % 连线上的基准点 dist 5; % 扰动幅度 pop(i, (k-1)*31:k*3) mid dist * randn(1,3); end end end我测试过完全随机初始化和连线偏置初始化两种方式后者的收敛曲线前期下降速度明显更快大概能少跑30%的迭代次数就找到同等级的最优路径。这个技巧对任何群智能算法都适用。边界处理在update函数末尾统一完成如果新位置某维坐标低于lb则取lb高于ub则取ub。这样能保证所有航点都落在规划空间内。4.3 SSA位置更新的核心代码实现发现者更新和加入者更新是SSA最核心的部分我用标准实现方式演示function [pop, fitness] ssa_update(pop, fitness, N, dim, lb, ub, iter, max_iter) % 找到当前最优 [~, best_idx] min(fitness); best_pos pop(best_idx, :); % 发现者数量 PNum侦察者数量 SDNum PNum round(N * 0.2); SDNum round(N * 0.2); R2 rand(); % 预警值 ST 0.8; % 安全阈值 % 按适应度排序 [fitness_sorted, index] sort(fitness); pop_sorted pop(index, :); % 1. 发现者更新 for i 1:PNum if R2 ST alpha rand(); pop_sorted(i, :) pop_sorted(i, :) .* exp(-i / (alpha * max_iter)); else Q randn(1, dim); pop_sorted(i, :) pop_sorted(i, :) Q; end % 边界处理 pop_sorted(i, :) max(pop_sorted(i, :), lb); pop_sorted(i, :) min(pop_sorted(i, :), ub); end % 2. 加入者更新 for i PNum1:N if i N/2 % 极度饥饿的麻雀重新初始化 pop_sorted(i, :) lb (ub - lb) .* rand(1, dim); else % 向最优位置靠拢 A ones(1, dim); if rand() 0.5 A -A; end A_plus A * inv(A * A) * A; pop_sorted(i, :) best_pos abs(pop_sorted(i, :) - best_pos) .* A_plus; end pop_sorted(i, :) max(pop_sorted(i, :), lb); pop_sorted(i, :) min(pop_sorted(i, :), ub); end % 3. 侦察者更新 for i 1:SDNum j randi(N); if pop_sorted(j, :) ~ best_pos pop_sorted(j, :) pop_sorted(j, :) 0.1 * randn(1, dim); else % 最优个体飞离当前位置 pop_sorted(j, :) pop_sorted(j, :) 0.2 * randn(1, dim); end pop_sorted(j, :) max(pop_sorted(j, :), lb); pop_sorted(j, :) min(pop_sorted(j, :), ub); end % 重新计算适应度 for i 1:N fitness_sorted(i) calc_fitness(pop_sorted(i,:), start_pos, end_pos, threat_center, threat_radius); end % 按新适应度重新排序并写回 [fitness, idx] sort(fitness_sorted); pop pop_sorted(idx, :); end这段代码里的加入者更新用到了矩阵A_plus公式可能有点绕。其实意图很简单加入者向全局最优位置靠近但不会完全复制最优位置而是保留自身和最优位置的差异以便产生一定程度的空间散布。如果这个差异过大说明这个加入者离大家太远干脆重新随机初始化保证种群的多样性。4.4 适应度计算函数完整实现适应度函数是连接算法和问题的桥梁我给出的实现如下function cost calc_fitness(path_vec, start_pos, end_pos, threat_center, threat_radius) % 解码路径点 K numel(path_vec) / 3; X [start_pos(1), path_vec(1:3:end), end_pos(1)]; Y [start_pos(2), path_vec(2:3:end), end_pos(2)]; Z [start_pos(3), path_vec(3:3:end), end_pos(3)]; % 1. 路径长度代价 len 0; for i 1:K1 len len sqrt((X(i1)-X(i))^2 (Y(i1)-Y(i))^2 (Z(i1)-Z(i))^2); end % 2. 威胁代价分段采样 threat 0; num_samples 10; for i 1:K1 for t 0:num_samples x X(i) (X(i1)-X(i)) * t / num_samples; y Y(i) (Y(i1)-Y(i)) * t / num_samples; z Z(i) (Z(i1)-Z(i)) * t / num_samples; for tc 1:size(threat_center,1) d sqrt((x-threat_center(tc,1))^2 (y-threat_center(tc,2))^2 (z-threat_center(tc,3))^2); if d threat_radius(tc) threat threat 100; % 进入威胁区加大惩罚 else threat threat exp(-((d - threat_radius(tc))^2)/50); end end end end % 3. 高度代价 height sum(abs(Z - 35)) / (K2); % 4. 平滑代价 smooth 0; for i 2:K1 v1 [X(i)-X(i-1), Y(i)-Y(i-1), Z(i)-Z(i-1)]; v2 [X(i1)-X(i), Y(i1)-Y(i), Z(i1)-Z(i)]; len1 norm(v1); len2 norm(v2); if len1 0 len2 0 cosTheta abs(dot(v1,v2)/(len1*len2)); smooth smooth cosTheta; % 越小越好同向为1 end end % 加权求和 cost len 3 * threat 1 * height 2 * smooth; end这里提醒一个常见错误威胁代价如果只在进入威胁区时加一个固定惩罚值算法很难通过梯度信息逐步绕开威胁。所以我在非威胁区也加了随距离衰减的高斯项这样麻雀越靠近威胁区适应度就越高算法才能感知到危险的方向。这个细节对收敛效果影响巨大。4.5 参数设置参考表根据我自己的多组实验下面这组参数在大多数仿真环境下都能得到不错的结果参数推荐值说明种群规模N30~50太小容易早熟太大增加计算量迭代次数max_iter150~300200次左右通常已收敛发现者比例0.2低于0.1探索能力不足高于0.4收敛变慢侦察者比例0.2可适当增大防早熟安全阈值ST0.8影响发现者是否大范围移动中间航点数K5~8太多维度爆炸太少路径不够灵活采样点数10每段路径采10个点做威胁检测威胁权重w22~4威胁源较多时建议加大到55. 常见问题排查与调参经验实录这部分应该是最有含金量的内容了。我在实际跑这个项目的过程中踩了不少坑下面把典型的几个问题整理出来带排查思路和解决办法。5.1 算法收敛过快路径直接穿过威胁区现象迭代不到50次适应度就不再下降最终路径直接横穿了一个威胁源。检查威胁代价也没问题但权重太低了。刚跑这个项目时我遇到过这种情况一开始还怀疑是代码逻辑写错了后来逐个打印四个子代价才发现威胁源数量不多路径没有经过威胁源威胁代价可能只有个位数而路径长度代价动辄两百多导致算法认为绕开威胁根本不划算。解决办法是调大威胁权重同时给进入威胁区的情况一个极大的惩罚值。我的经验是先把威胁代价权重调到路径长度代价的3~5倍等保证安全之后再用另一组实验逐步减小威胁权重、观察路径是否变短。这是典型的先保安全再求最优思路。还有一个小问题容易被忽略路径采样点太少。如果你每段只取两个端点做威胁检测两条航点之间的连线恰好穿过威胁源时根本检测不到。所以每段至少采样5~10个点代价是计算量稍微变大了但安全性是值得的。5.2 路径锯齿状严重飞起来会很难受现象最终路径虽然避开了所有威胁区但航点之间来回摆动每条线段方向差异很大无人机根本没法平稳飞行。根因是平滑代价权重太小或者权重设置时方向搞反了。我在代码里平滑代价用的是cos值向量方向一致时cos接近1方向相反时cos接近-1。如果你在代码里写的是求夹角并累加就要注意累加的是夹角度数还是夹角惩罚值。如果用夹角度数累加急转弯反而可能数值更大需要谨慎。解决办法有两个一是调大平滑代价w4到3~5强制算法给出更顺滑的路径二是减少中间航点数量。中间航点太多时每个航点之间的自由度太大很容易产生抖动。用5个航点加一条末端平滑插值视觉效果会好很多。5.3 迭代后期陷入局部最优无法跳出现象收敛曲线平滑下降到一定值后就一直水平换了多组随机种子结果基本一样。这说明算法多次运行都被困在同一个局部最优解附近。SSA中的侦察者机制本来可以防止这种问题但如果侦察者比例设置得太小或者侦察者的随机扰动幅度太弱这个机制就形同虚设。我一般把侦察者比例提高到0.25到0.3并把扰动幅度从0.1加大到0.3。另外一个暴力但有效的方法是在迭代后半段每隔一定代数对种群中适应度排名后30%的个体执行一次大规模随机重置等于让一部分麻雀换地方觅食。这个方法能明显提升跳出局部最优的概率但注意幅度别太大否则会破坏已收敛的有用信息。5.4 计算时间太长实验跑不完了三维路径规划的适应度函数涉及威胁检测最耗时的是威胁代价里的采样循环。如果种群规模50、迭代200次、每段采样10个点、威胁源5个总共要计算的采样点数大约为502006105300万次加一堆exp和sqrt运算Matlab跑起来确实要等一阵。优化手段有几个一是把向量化做好不要用三重for循环逐点计算威胁值改成用矩阵运算批量计算距离。二是提前减掉威胁源数量比如把相距很近的威胁源合并成一个等效威胁源。三是采样点不必太多如果威胁源半径大5个采样点也能检测到。5.5 常见问题速查表问题现象可能原因解决方案路径穿过威胁区威胁权重过低、采样点太少加大w2增加每段采样点数收敛太快陷入局部最优侦察者比例低、扰动幅度小提高侦察者比例加大随机扰动路径锯齿状平滑权重低、航点过多调大w4减少中间航点数收敛太慢种群规模过大、迭代数过多减小N到25迭代减到120反复报错维度不符中间航点数与dim设置不一致统一公式dim3*K起点终点路径不完整解码时航点顺序搞错检查路径拼接顺序写在最后一些个人的实操体会做这个项目的过程中我最大的体会是算法选型和建模的合理程度往往比后面拼命调参重要得多。麻雀搜索算法的代码实现不难真正需要下功夫的是把安全优先、长度其次、平滑兜底这件事写进适应度函数里。如果一开始就把威胁代价权重设定得足够高收敛曲线会稳定很多后面几乎不用太折腾。另外一个小技巧分享给大家跑完算法后不要只盯着最终路径看把收敛曲线和路径图一起输出每次实验都存图存档。你会发现很多时候路径看着挺漂亮的但收敛曲线持续在下降说明再多跑几十代还能找到更优解这时候就该加大迭代次数或者调低收敛容差。路径规划这种问题找到全局最优解很难说百分之百但至少可以用多次随机种子的实验结果来验证稳定性。这套SSA三维威胁环境多目标代价的框架掌握之后换场景非常方便。把威胁源换成禁飞区模型、把高度代价换成油耗模型或者把单机路径规划扩展成多机协同路径规划都是在现有代码基础上局部替换。后续我还在尝试把B样条编码和SSA结合起来让路径直接输出为可飞的光滑曲线等跑出结果再跟各位分享。