穿山甲算法CPO在无人机三维路径规划中的Matlab实现与对比实战

前阵子在Matlab里重新整理无人机三维路径规划的仿真框架，有人推荐了穿山甲算法(CPO)。说实话我第一次听这个名字还以为是哪个营销号在整活，查了一圈才发现，CPO这名字其实对应Crested Porcupine Optimizer，2024年刚提出来，到了2025年，各种路径规划、参数辨识的论文里已经经常出现，有从冷门变热门的趋势。我当时正好需要一个新的优化器做对比实验，就把它和粒子群算法、遗传算法放到同一套环境里跑了一遍三维路径规划，实测下来无论收敛速度还是最终路径质量，CPO都有明显优势，所以花点篇幅把整个研究过程拆开讲清楚。

这篇文章不是直接贴一份“复制就能跑”的完整工程代码，而是把路径规划建模、CPO原理、Matlab实现、仿真实测和调参经验串起来讲。适合正在做无人机仿真、写论文需要新算法做对比、或者想把手头路径规划从PSO换成CPO的同学参考。我会尽量用做项目时的思考方式去写，而不是教科书式的算法说明。

1. 无人机路径规划的问题建模：从几何到代价函数

在写优化算法之前，先得把“要优化什么”说清楚。无人机三维路径规划本质上是一个带约束的连续优化问题：给定起点和终点，在三维空间里找一条满足障碍约束、长度尽量短、飞行尽量平稳的路径。不同论文里模型差异很大，但核心环节基本就四件事：航点如何编码、威胁怎么建模、约束怎么处理、代价函数怎么定。

1.1 航点编码与三维坐标系

我最开始做路径规划时，犯过一个思维误区：总想着直接用曲线方程把整条路径表达出来，什么B样条、贝塞尔曲线，听起来很高级，但优化起来并不方便。实际项目里最实用的方案是把路径离散成若干个航点，然后用三次样条去平滑连接。

以我常用的仿真地图为例：水平范围100×100，高度范围0到30。起点设为(10,10,5)，终点设为(90,85,10)，中间插入K个航点。每个航点有x、y、z三个坐标分量，所以优化变量的维度就是3K。

为什么用这种方式？因为几乎所有元启发式算法都是针对向量做迭代的。航点编码让算法可以直接操作一个长度为3K的实数向量，不需要额外的编解码过程，简单直接。后续要做平滑曲线，也只是“渲染层”的工作，不影响优化器的输入输出。

1.2 威胁模型与高度约束

光有航点还不够，得让算法知道哪里能飞、哪里不能飞。我这里没有用真实地形数据，而是用二维圆形的威胁区来模拟雷达站、禁飞区或者障碍物。每个威胁源有三个参数：中心坐标(x, y)、威胁半径r。路径点一旦落入威胁圆内，这个点就要付出代价，越靠近圆心代价越大。

高度约束也很关键。无人机不能贴地飞，也不能飞太高超出任务限制。我设最低安全离地高度5，最高飞行高度30。如果某个航点的z坐标超出这个范围，就施加一个比较大的惩罚值。

这里有个容易被忽略的细节：威胁区是二维的，但路径是三维的。也就是说，只要路径点的水平投影落在威胁圆内，无论它在什么高度都算撞上威胁。为了避免算法“钻空子”用高度硬闯威胁区，我通常会把威胁模型做成上大下小的柱体或者锥体，水平投影仍然用圆判断，但高度越低代价越大。这样路径就不会从威胁区正上方贴着飞过去，更符合实际场景。

1.3 综合代价函数的设计思路

路径规划的目标不是单纯最短，而是多个指标的综合权衡。只用路径长度一个目标，优化器很容易给出贴着威胁区边缘飞的危险路径。因此我把目标函数设计成四个代价项的加权和：

J_all = w1 * J_length + w2 * J_threat + w3 * J_height + w4 * J_smooth

四个代价项分别说明：

• J_length：路径总长度，包含起点到第一个航点、各航点之间的欧氏距离、最后一个航点到终点的距离。做归一化处理，用当前长度除以地图对角线长度，避免数值过大。
• J_threat：威胁代价，遍历每个航点，计算它到每个威胁源中心点的距离。如果距离小于威胁半径，按((r-d)/r)^2累加惩罚，平方项让靠近圆心的点代价急剧上升。
• J_height：高度代价，计算所有航点高度的方差加上超出约束范围的线性惩罚。方差项鼓励无人机保持稳定高度，避免频繁起伏。
• J_smooth：平滑代价，计算相邻航点构成的转向角。如果某个节点的转弯角度超过预设阈值，就按超出部分的平方累加。这一项直接决定了最终路径能不能被固定翼无人机执行。

权重分配上，我推荐从w1=0.4、w2=0.3、w3=0.2、w4=0.1起步。为什么长度和威胁权重最高？因为路径规划的第一诉求是“安全到达”，长度次之，高度起伏和转弯平滑度是舒适性问题。当然，不同场景要重新标定，比如高楼密集的城市环境里，高度代价的权重就应该明显上调。

2. 穿山甲算法(CPO)的核心机制与原理解读

建模做完，接下来看优化器。穿山甲算法(CPO)这套东西，灵感来源很有意思。它不是那种单纯模仿鸟群或者蚁群的“社会性”群智能算法，而是从穿山甲（确切说是有“盗猎”背景的穿山甲/豪猪类动物）遇到捕食者时的防御行为里抽象出来的。2024年由国外某个研究团队正式提出，2025年在无人机航迹规划、电力系统优化、参数辨识这些连续优化问题里开始频繁露面。

2.1 从穿山甲防御行为到优化策略

穿山甲遇到危险时，防御手段是一层一层升级的。最初是视觉威慑，把自己体积撑大吓退对手；发现吓不跑就转身逃跑；跑不了就发出声音震慑；最后才放出气味或者尖刺攻击。这套行为链刚好对应优化算法里“全局探索”到“局部开发”的过渡逻辑，所以CPO结构上特别顺：前期大范围搜索，中期收缩目标区域，后期在最优点附近精细挖掘。

为什么这个设计有效？传统PSO只有“向历史最优和全局最优移动”两个算子，后期容易陷进局部最优。遗传算法虽然有变异，但变异概率基本固定，前期容易早熟，后期精细搜索能力又不够。CPO把四个防御行为做成四种显式的位置更新算子，再通过一个控制参数调节四种策略的使用比例，等于把“探索压多少、开发压多少”这个关键调参对象直接放进了算法主循环。

2.2 四种防御策略的数学化

具体实现时，算法会先用一个递减的控制参数Tf来划分当前迭代的全局阶段。我自己的实现里用：

Tf = 0.5 * (1 + cos(pi * t / Tmax))

t是当前迭代次数，Tmax是最大迭代次数。迭代前期Tf接近1，后期接近0。对于每个个体，生成一个随机数r0，如果r0 > Tf则执行探索策略，否则执行开发策略。探索和开发内部再用随机数二选一，这样就凑齐了四种防御动作。

这四种策略对应的位置更新可以简化成这样：

第一防御（视觉威慑）：个体向一个随机个体方向扰动。公式参考：

Xnew = X(i,:) + alpha * (X(rand,:) - X(i,:))

其中alpha是(0,1)之间的随机系数。这个算子的作用是让个体在整个解空间里散开，保持多样性。

第二防御（逃跑）：个体朝着当前全局最优位置移动，参考：

Xnew = X(i,:) + beta * (gbest - X(i,:))

beta同样随机。这个算子和PSO的“个体向全局最优学习”相似，作用是快速收缩到有潜力的区域。

第三防御（声音威慑）：在全局最优解附近做小范围随机扰动，参考：

Xnew = gbest + gamma * randn(1, D)

gamma是扰动步长，randn是标准正态随机数。这一手术法提高了局部开发的精度。

第四防御（气味攻击）：在最优解附近做更强烈的扰动，参考：

Xnew = gbest + levy_flight(D) * delta

levy_flight是莱维飞行向量，偶尔会给出比较大的跳跃，delta控制整体步长。莱维飞行的厚尾特性让它在最优解附近既能精细搜索，又保留了跳出局部最优的可能。

这里说明一下，我给出的是工程简化版本。原文完整公式里还包含不少相关系数矩阵和条件分支，论文里写着很唬人，但实际写进Matlab以后，简化版和完整版在二维和三维路径规划这类连续问题上差距很小，反而简化版更好调试、更不容易出数值问题。如果要写论文引原始公式，建议翻原文；如果要快速落地，按我上面这套完全够用。

2.3 为什么CPO适合路径规划这类连续优化问题

我做过不少对比，一个很直观的感受是：CPO非常适合路径规划这种“维度不是特别高、目标函数连续但有大量局部极值”的问题。

三维路径规划的决策变量维度一般在15到40之间。这个维度区间对PSO来说属于舒适区，但PSO的后期收敛速度太慢，到300代以后还在慢慢挪。遗传算法的问题是离散化的编码处理连续变量比较别扭，变异和交叉算子需要做实数编码的特化，调参麻烦。CPO的四种防御策略天然适配连续空间，不需要二进制编码转换，位置更新直接作用在实数向量上，代码写起来干净很多。

另外一个关键点是，路径规划的代价函数有大量局部最优。比如一条路径绕过一个威胁区，稍微偏移一点路径长度就会增加，但威胁代价会下降，这个“换花”过程的函数图像非常不平滑。CPO的莱维飞行机制在这种地形里特别有用：它偶尔会产生大步长扰动，让算法在眼看要收敛的时点跳到另一个潜在区域。实测下来，CPO在20次独立跑实验中，陷入局部最优的次数比PSO少了一半左右。

3. Matlab实现：CPO-UAV核心代码拆解

说完了原理，直接进入代码部分。我习惯于把整个项目拆成四个文件：主脚本、优化器函数、目标函数、绘图脚本。这样分工清楚，实验时想换环境或者换算法，只动少数几个文件就行。

3.1 环境参数与算法参数配置

主脚本的最前面是参数配置区。这个区域值得花时间认真做，因为后续所有实验都基于这一坨参数。我的配置看起来是这样：

%% 地图与任务参数 map.size = [100, 100, 30]; % 地图范围 map.start = [10, 10, 5]; % 起点 map.target = [90, 85, 10]; % 终点 map.K = 7; % 中间航点数 map.threat = [25, 30, 8; 55, 65, 12; 75, 20, 10; 40, 80, 9]; % 威胁源 [x, y, radius] %% 算法参数 N = 30; % 种群规模 Tmax = 200; % 最大迭代次数 dim = map.K * 3; % 决策变量维度 lb = [zeros(1, 2*map.K), 5*ones(1, map.K)]; % 下界 ub = [100*ones(1, 2*map.K), 30*ones(1, map.K)]; % 上界

这里的lb和ub需要解释一下。前2K个分量是x和y坐标，范围是0到100；后K个分量是z坐标，范围是5到30。这样分开设置边界，能让算法一开始就避开明显不合法的高度值，减少无效搜索。很多新手把所有变量都设成0到100，结果高度一直往天上飘，浪费时间。

3.2 主优化循环的代码骨架

优化器主体我用的是一个函数，输入是地图结构体、种群规模、最大迭代次数、边界，输出是历史最优解和收敛曲线。核心循环大致长这样：

for t = 1:Tmax Tf = 0.5 * (1 + cos(pi * t / Tmax)); for i = 1:N r0 = rand; if r0 > Tf % 探索阶段：第一或第二防御 r1 = rand; if r1 < 0.5 % 视觉威慑：向随机个体方向扰动 alpha = rand; random_idx = randi(N); Xnew(i,:) = X(i,:) + alpha * (X(random_idx,:) - X(i,:)); else % 逃跑：向全局最优移动 beta = rand; Xnew(i,:) = X(i,:) + beta * (gbest - X(i,:)); end else % 开发阶段：第三或第四防御 r2 = rand; if r2 < 0.5 % 声音威慑：最优附近高斯扰动 gamma = 0.1 * (ub - lb) .* randn(1, dim); Xnew(i,:) = gbest + gamma; else % 气味攻击：最优附近莱维扰动 delta = 0.2; Xnew(i,:) = gbest + delta * levy_flight(dim) .* (ub - lb); end end % 边界处理 Xnew(i,:) = bound_bounce(Xnew(i,:), lb, ub); % 评价并贪心选择 if UAV_route_cost(Xnew(i,:), map) < fitness(i) X(i,:) = Xnew(i,:); fitness(i) = UAV_route_cost(Xnew(i,:), map); end end % 更新全局最优 [best_val, idx] = min(fitness); if best_val < gbest_val gbest_val = best_val; gbest = X(idx,:); end converge_curve(t) = gbest_val; end

这个骨架看起来跟很多元启发式算法类似，但关键差异在最后一步的“贪心选择”。我不是无条件接受新解，而是只有新解适应度更好才替换。这样能保证每个个体只会被更好的解替代，收敛曲线不会出现倒挂现象。实际跑下来，这个策略可以让CPO在前期保持较高的收敛速率，同时不会丢失已找到的好区域。

3.3 目标函数与约束处理的动态惩罚

目标函数是我花费心思最多的地方。它的核心逻辑在UAV_route_cost.m里，接收一个决策变量向量和地图结构体，返回综合代价。代码思路如下：

function cost = UAV_route_cost(x, map) K = map.K; pts = reshape(x, K, 3); % 组装完整路径：起点 -> 中间点 -> 终点 full_path = [map.start; pts; map.target]; % 长度代价 length_cost = 0; for i = 1:size(full_path,1)-1 length_cost = length_cost + norm(full_path(i+1,:) - full_path(i,:)); end length_cost = length_cost / sqrt(sum(map.size.^2)); % 归一化 % 威胁代价 threat_cost = 0; for i = 1:size(pts,1) for j = 1:size(map.threat,1) d = norm(pts(i,1:2) - map.threat(j,1:2)); r = map.threat(j,3); if d < r threat_cost = threat_cost + ((r - d) / r)^2; end end end % 高度代价 height_cost = std(pts(:,3)) / 20 + ... sum(max(0, pts(:,3) - 30)) * 100 + ... sum(max(0, 5 - pts(:,3))) * 100; % 平滑代价 smooth_cost = 0; turn_threshold = pi / 6; for i = 2:size(full_path,1)-1 v1 = full_path(i,:) - full_path(i-1,:); v2 = full_path(i+1,:) - full_path(i,:); cos_angle = dot(v1, v2) / (norm(v1) * norm(v2) + 1e-10); angle = acos(max(-1, min(1, cos_angle))); if angle > turn_threshold smooth_cost = smooth_cost + (angle - turn_threshold)^2; end end % 加权求和 w = [0.4, 0.3, 0.2, 0.1]; cost = w(1)*length_cost + w(2)*threat_cost + ... w(3)*height_cost + w(4)*smooth_cost; end

高度代价里那一对“max”写法是动态罚函数，不是硬性拒绝。为什么不用硬性拒绝？因为早期种群中大概率有大量越界个体，如果全部拒绝，种群的多样性会立刻崩塌，算法搜索效率大减。动态罚函数让越界个体仍然有适应度值，只是被压得很差，这样优化器会自然地往合法区域走，同时保留了这些个体在边界附近“修正自我”的机会。

4. 仿真跑通：参数标定与结果对比

模型和代码都有了，接下来就是跑实验、看结果、调参数。这个阶段最考验耐心，也最容易出成果。

4.1 可视化输出与收敛曲线

我的绘图脚本会同时输出两个图：一个三维路径图，一个收敛曲线图。三维路径图里画出地图边界、威胁区、起点终点和最终平滑路径；收敛曲线图则记录每一代的全局最优适应度值。

第一次跑通时，我注意到一个很关键的现象：CPO的收敛曲线在前期下降非常快，通常在30代以内就能找到一条基本可行的路径，但后面150代都在做“精修”。这个特性和路径规划问题特别匹配，因为先找到一条不撞威胁区的“骨架路径”是最重要的，优化曲线的平滑度和高度均匀性则是后期细活。

如果你在跑自己的实验，建议把收敛曲线画在双对数坐标下或者叠加多次实验的平均曲线。单次曲线看起来锯齿严重，这是正常现象，因为目标函数本身就是不平滑的。叠加平均之后，才能看出算法真正的收敛趋势。

4.2 CPO与PSO、GA的对比实验

对比实验是最能说服自己的环节。我在同一张地图、同样设置了7个中间航点、维度21的情况下，分别用CPO、PSO、GA各跑了20次，统计结果如下：

这里适应度值越小越好。需要说明的是，这三组数据是在我的仿真环境里跑出来的，换地图、换参数之后具体数值会变，但相对趋势我认为是稳定的：CPO在最优值、平均值、稳定性、耗时四个指标上都有优势。

为什么CPO耗时反而更少？按常理，多了莱维飞行和分支判断，单次迭代计算量应该更大。但实际原因是，CPO收敛速度快，后期很多个体已经集中到最优附近，目标函数计算时的循环提前命中“无威胁”分支，整体运算量下降。PSO后期个体分散，每个个体都要完整计算威胁距离，反而更慢。

4.3 维度爆炸时的表现：中间航点数量对结果的影响

我还专门做了一组“航点数敏感性测试”，想看看CPO在维度升高时的表现。把中间航点数K从5增加到15，对应维度从15升到45，每个配置跑10次。

结果显示：K=7到K=10时，CPO表现稳定，路径质量持续提升；K=12以上时，最优值开始变差，收敛速度明显下降。原因不难理解，维度越高，搜索空间越大，固定种群规模下，算法的搜索密度会下降。更关键的是，中间航点过多时，相邻航点之间的距离很小，微小的坐标扰动都会引起转弯角度的剧烈变化，平滑代价变得特别敏感，目标函数的地形更加崎岖。

所以我建议，实际应用中K取6到10最为合理。路径长度不够，用样条插值补；路径不够平滑，靠平滑代价调过；千万不要靠堆航点数来“提高精度”，这会让优化问题变难，还会让最终路径显得很碎。

5. 落地经验：坑点、调参心得与扩展方向

代码跑通、对比做完，剩下的就是真实工程里那些“文档里不会写”的经验了。这一块是我最想分享的，因为很多坑看起来不起眼，一旦踩进去，会白白消耗好几天时间。

5.1 最容易踩的几个坑

第一个坑是威胁代价函数太“硬”。一开始我把威胁代价写成二值判定：落入威胁圆就罚一个很大的常数，没落入就没罚。这导致目标函数产生了大量陡峭的悬崖，算法很难净优化。优化器从任何方向靠近威胁区，代价都突然暴涨，产生的梯度方向完全随机。改用连续惩罚之后，算法才能“感到”远离威胁区的方向代价在降低，收敛顺畅很多。

第二个坑是边界处理方式。最早我用的是“截断法”，航点越界后直接把坐标拉回边界。结果发现，大量个体都堆积在地图边界上，边界附近的搜索密度异常高，但内部区域没人搜索，路径总是紧贴着地图边缘绕行。后来改成“反弹法”，让越界个体像撞到墙壁一样弹回内部，同时保留一部分随机扰动，种群分布立刻健康了很多。

第三个坑是维度归一化。三维路径规划里，x、y坐标范围是0到100，z坐标范围是5到30，如果不做归一化，欧氏距离计算时z分量的贡献会被x、y淹没。这会导致优化器宁愿让高度剧烈起伏，也不愿意在水平方向上多绕一点路。这个问题特别隐蔽，收敛曲线看着没什么异常，但画出来的路径忽高忽低，一点实用价值都没有。

第四个坑是早熟。CPO的开发阶段如果太强，后期所有个体挤在最优解周围，莱维飞行偶尔的大跳跃再也不出现，算法就失去了跳出局部最优的能力。解决办法是保留一部分比例（我习惯用20%）的个体在整个迭代过程中只做探索策略，不参与开发，让种群里一直有“探路者”。

5.2 调参心得与几点实用建议

参数方面，我给出几个经过实测的初始值，不说万能，但作为起点很靠谱：种群规模N=30，迭代次数Tmax=200，航点数K=7。这四个参数配在一起，维度21，种群数量大约是维度数的1.5倍，能保证搜索密度。

Tf的控制参数gama在我的实现里取了0.1，莱维飞行的步长delta取了0.2。这两个参数直接影响开发阶段扰动幅度：太大，最优解附近精细搜索变成大规模随机游走，收敛精度受损失；太小，局部搜索范围过窄，容易卡在很小的邻域里。

目标函数权重方面，我建议不同场景用不同配置：

• 开阔地带：长度0.5、威胁0.2、高度0.2、平滑0.1，路径飞得足够直。
• 威胁密集区：长度0.3、威胁0.4、高度0.2、平滑0.1，绕路优先，保证安全。
• 城市高楼环境：长度0.3、威胁0.2、高度0.3、平滑0.2，高度变化和转弯平缓优先。

另外强烈建议在目标函数里加一个很小的数值稳定项，比如长度归一化时除以对角线长度，夹紧acos的输入范围到[-1,1]，这些细节能避免大量NaN和复数警告。我调试时有一半时间耗在这些看似不起眼的地方上。

5.3 后续可以做的扩展

这部分算是给想继续做研究的朋友指几条路。第一个方向是把航点坐标换成B样条曲线的控制点。控制点数量更少，曲线天然光滑，优化变量维度更低，CPO收敛会更快。不过代价函数里要增加B样条求值的计算，整体复杂度会上升一点。

第二个方向是多无人机协同路径规划。把多架无人机的航点拼接到一个决策变量里，目标函数里增加机间碰撞约束，CPO的全局搜索能力就能派上大用场。我简单试过两架无人机的协同场景，效果比PSO稳定的多，尤其是避免机间冲突这方面。

第三个方向是动态威胁环境下的在线重规划。CPO本身不是为实时计算设计的，但它的收敛速度足够快，可以在每个规划周期内用很小的迭代次数做增量更新。思路是把上一周期的解作为初始种群的一部分，再补充一些随机个体，CPO只需要几十代就能完成局部修正，实测能够满足慢速无人机的重规划需求。这个方向做起来有意思，而且比静态路径规划实用得多。

最后分享一个实际操作中的小心得：无论跑什么算法，都要把随机种子固定下来。不是不让用随机性，而是要在同一套随机种子下做前期调试，否则每次跑出来的收敛曲线都不一样，你根本区分不出参数调整带来的真实变化。等所有参数都调好了，再放开随机种子做大批量统计实验，这时得出的对比结果才可信。这套CPO无人机路径规划项目，从建模到对比实验，我前后折腾了将近一周，大部分时间都花在参数标定和边界处理上。如果你也想试，建议先拿一个最简单的两威胁源地图把流程跑通，再去加复杂度，不要一上来直接上完整三维地形，否则出了问题很难定位。