无人机三维航迹规划这几年一直是研究热点但真正放到城市高层建筑密集的环境里做情况会完全不一样。二维平面里的路径规划只要绕开平面障碍物就行到了三维空间既要考虑楼宇高度、禁飞区域又要把爬升率、转弯半径和续航能力一并塞进优化目标里问题复杂度直接上升一个级别。2024年新提出的冠豪猪优化器Crested Porcupine OptimizerCPO给这个方向带来了一种新思路。它模拟冠豪猪面对捕食者时的四种防御行为把“防御反应”拆成探索和开发两类搜索机制在多约束、高维度的三维航迹规划任务上表现相当稳定。这篇文章会把基于CPO的城市三维无人机路径规划完整拆开讲一遍从航迹建模、算法原理、环境构建到Python实现、实验对比再到调参避坑全部走一遍。无论你是做课题研究、项目预研还是自己在折腾仿真都能直接照着参考。1. 城市三维航迹规划到底难在哪1.1 复杂环境下的多重约束城市环境和开阔地带的航迹规划完全是两回事。开阔地形里最主要的约束通常是地形起伏和少数几个山峰禁区而城市场景中高层建筑、塔吊、信号塔、临时禁飞区甚至飞行时产生的噪声影响范围都会变成一个个大大小小的障碍。飞机不能像地面车辆那样停下来它每一步都需要在前后、左右、上下六个方向中选一个合理的动作这要求规划出来的路径天然具备安全裕度。几何上的约束已经够复杂实际飞行约束更让人头疼。无人机转弯需要半径爬升和俯冲都有速度上限电池容量决定了整个航迹的总长度必须控制在一定范围内还要留有余量。一条航线在图纸上画得好不等于能真正飞出来。比如航路点之间太陡峭飞控可能无法跟踪转弯太急惯性导航和视觉定位都会出问题。所以三维航迹规划本质上是一个带多约束的连续优化问题解空间维度随航路点数量成倍增加想要靠人工调整几乎不可能必须交给数值优化算法去搜。1.2 航迹怎么表示、代价怎么算工程上最常用的做法是用一组有序航路点来表示航迹。假设整条路径有M个点P0固定为起点P(M-1)固定为终点中间那M-2个点交给算法去优化。每个航路点包含三维坐标(x, y, z)一段完整航迹就是若干个三维点组成的有序序列最终再用样条插值把折线变成平滑可飞的曲线。优化过程需要一个统一的评价标准通常把多个维度的需求聚合成一个标量代价函数长度代价所有相邻航路点之间欧氏距离之和衡量航迹是否绕路。威胁代价航迹穿过障碍物或禁飞区的程度通常用采样点落入障碍物内部的惩罚总量表示。平滑代价相邻线段之间的偏转角大小衡量航迹转弯是否过急。高度代价航迹偏离预设最低安全高度或最高限制高度的程度。综合表达成下式J w1 * J_len w2 * J_threat w3 * J_smooth w4 * J_altw1到w4是各个代价的权重。这里有个关键经验威胁代价的权重必须明显大于其他项否则算法为了省十几米路程会有很大概率让航线贴着楼边飞过去。代价函数一旦定义清楚三维航迹规划就从“几何路径搜索”变成了“标量最小的数值优化问题”接下来的任务就是选择一个足够高效的优化算法来解它。2. 冠豪猪优化器CPO从防御行为到寻优机制2.1 四种防御策略与搜索行为的映射冠豪猪是现存体型最大的啮齿类动物之一长相憨厚但防御机制很丰富。当遇到捕食者时它会依次或组合使用四种手段竖起尖刺做视觉威慑、摩擦尖刺发出声音警告、释放带有特殊气味的分泌物以及最后的甩刺攻击或快速逃跑。2024年初发表的CPO论文核心创意就是把这四种防御行为映射成优化算法中的搜索机制。从优化算法角度看四种防御行为对应着不同分工防御行为生物层面的含义算法层面的作用对应搜索阶段视觉刺激竖起尖刺展示体型震慑对方用随机个体之间的差异向量产生新解扩大搜索范围全局探索声音警告摩擦尖刺发出声响试图吓退捕食者结合全局最优位置和随机个体差异让种群趋向较优区域开发气味释放分泌特殊气味的化学物质驱赶敌人在当前个体与全局最优附近做局部扰动兼顾邻域搜索探索向开发过渡攻击与逃跑转身甩刺反击或迅速奔跑脱离围绕全局最优做强采矿式搜索进一步精化结果局部开发这个映射有意思的地方在于它把生物在“保命”时必须做出的两种决策——扩大警戒范围和锁定逃跑路线——对应到了元启发式算法中探索与开发的平衡。四种机制交替或概率切换使用让种群在迭代前期能广泛探查城市环境中的多条可能通道迭代后期又能围绕当前最优航迹精细打磨。2.2 算法的关键流程与参数设计CPO的数学过程并不复杂很适合工程复现。先随机生成种群每个个体代表一条候选航迹的三维航路点坐标向量。随后进入迭代循环通过一个收敛因子判断当前所处的搜索阶段再按概率触发某一种防御策略生成新个体最后用贪心方式决定是否接受新解。我复现时做了必要的工程化简化保留了论文的核心主干具体更新规则如下收敛因子H exp(-t / Tmax)随时间推移逐步衰减控制探索转入开发的节奏。视觉刺激策略Y Xi τ * N(0,1) * (Xj - Xk)。其中j和k是两个随机个体τ以一定概率取正负号模拟“是否真的受到惊扰”。声音警告策略Y Xi G * (Xbest - Xi) β * (Xj - Xk)。β是正态随机数G是另一种收敛控制因子。气味释放策略Y Xi H * (U1 * (Xbest - Xi) U2 * (Xj - Xi))。U1和U2是0到1均匀随机数用于控制局部搜索幅度。攻击逃跑策略Y Xbest α * (Xi - Xbest)。α为正态随机量让个体在全局最优邻域内精细搜索。边界处理建议采用反弹策略而不是直接拉回边界值。直接把越界个体截断到边界会让大量个体堆积在地图边缘种群多样性显著下降。反弹方式则是把越界分量按对称方式取回来既保证坐标在可行域内又保留了一定的位置差异。2.3 为什么CPO适合三维航迹这种高维问题传统用于路径规划的粒子群算法和灰狼算法各有明显短板。PSO的粒子速度在迭代后期容易趋近于零全体个体挤在一起基本丧失全局跳跃能力GWO依赖头狼位置引导但开发阶段的局部搜索深度不够。三维航迹规划往往有20维甚至30维以上的解空间例如8个航路点时中间6个点就是18个待优化变量。维度升高后这些成熟的群体算法要么收敛太慢要么早熟后精度不够。CPO的相对优势在于它把探索和开发拆成四个明确机制并通过收敛因子动态调配。前期视觉刺激策略负责大范围跳跃避免种群从一开始就扎堆在起始点附近后期攻击和声音策略负责在最优位置附近精细搜索提高航迹的平滑度和安全性。这种机制上的分离在高维、多峰代价地形上更容易维持种群的活跃度。实测在相同最大迭代次数下CPO往往比PSO和GWO更晚进入停滞期最终结果也更稳定。提示CPO和所有元启发式算法一样不保证找到理论全局最优。但它能在有限迭代次数内稳定找到一个工程上可用的“次优解”这恰恰是城市航线规划最需要的能力。3. 城市三维环境建模与代价函数落地3.1 障碍物与禁飞区的建模方式做仿真前环境模型要足够接近真实又不能复杂到拖慢评价函数。城市场景中最常见的实体是建筑物用AABB包围盒表示最方便每个建筑记作(xmin, xmax, ymin, ymax, height)。禁飞区或信号干扰源通常近似成圆柱体记录圆心坐标、半径和有效高度。地面起伏可以单独存为一张高程网格高度值用二维数组索引。下面这段代码生成一个虚拟城市的简化环境注意让建筑物避开起点和终点附近的一定范围否则初始种群几乎无一幸免全部撞楼优化过程会浪费大量迭代import numpy as np def generate_city(map_size(2000, 2000, 300), n_buildings12, n_restrict2): buildings [] for _ in range(n_buildings): x0 np.random.uniform(200, map_size[0] - 400) y0 np.random.uniform(200, map_size[1] - 400) w np.random.uniform(80, 220) h np.random.uniform(120, 250) d np.random.uniform(80, 220) buildings.append({ xmin: x0, xmax: x0 w, ymin: y0, ymax: y0 d, height: h }) restrict_zones [] for _ in range(n_restrict): restrict_zones.append({ cx: np.random.uniform(400, map_size[0] - 400), cy: np.random.uniform(400, map_size[1] - 400), radius: np.random.uniform(180, 300), height: map_size[2] }) return buildings, restrict_zones这种建模方式够用了。真实工程需要接入实际楼宇轮廓和数字高程模型时只需把AABB替换成多边形底面加高度碰撞检测稍微复杂一点但整体思路不变。3.2 威胁代价、平滑代价与高度代价的实现威胁代价是三维航迹规划里最容易出错的地方。很多人只检查航路点本身是否在障碍物内部忽略了相邻航路点之间的连线。两个点都避开大楼但这根线段完全可能从楼体中间穿过去检查航路点等于漏掉了最关键的碰撞。正确做法是对每条线段做等距采样以米级间隔生成中间点再逐一判断采样点是否落入障碍物范围。def collision_check(p1, p2, buildings, restrict_zones, interval3): dist np.linalg.norm(p2 - p1) n_samples max(2, int(dist / interval) 1) hits 0 for s in np.linspace(0, 1, n_samples): p p1 s * (p2 - p1) for b in buildings: if (b[xmin] p[0] b[xmax] and b[ymin] p[1] b[ymax] and p[2] b[height]): hits 1 for r in restrict_zones: if (np.hypot(p[0] - r[cx], p[1] - r[cy]) r[radius] and p[2] r[height]): hits 1 return hits采样间隔选3米左右比较合适。间隔太大会漏掉细长障碍物间隔太小计算量急剧上升尤其是人群200个个体、每个个体10多个航路点再乘上迭代次数累积开销很可观。平滑代价通过相邻三点来计算。取前一个点、当前点、后一个点构成两条向量用向量点乘计算夹角余弦再用1减去余弦值作为代价。夹角越接近180度也就是近似直飞代价越小。高度代价则根据预设的飞行高度带[hmin, hmax]超出部分按比例惩罚。权重方面经验做法是先试跑一轮统计各分项的量级再做归一化。假设一条2000米长的航迹长度代价是2000威胁代价如果撞楼一次罚1e5那么权重w2取1、w1取1就够了因为碰撞罚分天然会压过长度。平滑代价通常是零点几到几的量级给w3取20以上就能产生区分度。不要拿到公式就随便填权重先看量级再定系数这个习惯能省去大量调参时间。4. CPO为核心的航迹规划完整实现4.1 航迹编码与种群初始化个体编码是整个优化问题的第一道关键环节。城市三维航迹规划采用固定航路点数的方式比如8个航路点起点和终点固定中间6个点作为待优化变量。每个待优化点有x、y、z三个分量因此解向量的维度是18。初始化方式直接影响收敛速度。如果每个个体都在整个地图范围内随机撒点绝大多数个体生成的航迹都会撞上高楼导致初始适应度被威胁惩罚主导算法需要大量迭代才能摸索出“往上飞”这一基本规律。更聪明的做法是以起终点连线为基准在直线走廊附近加高斯基扰动z分量则偏向预设的飞行高度带。这样初始种群已经具备“大致可飞”的几何形状优化过程只需要在走廊内拧出路程更短、更平滑的组合。def init_population(start, end, n_pop, n_waypoints, lb, ub, h_min, h_max): dim n_waypoints * 3 pop np.zeros((n_pop, dim)) for i in range(n_pop): mid np.linspace(start, end, n_waypoints 2)[1:-1] perturb np.random.normal(0, 40, sizemid.shape) perturb[:, 2] np.random.normal(0, 10, sizemid.shape[0]) candidate mid perturb candidate[:, 2] np.clip(candidate[:, 2], h_min, h_max) pop[i] candidate.flatten() return pop经过这样初始化的种群个体之间的差异性仍然充足但所有候选航迹的起点终点连接关系是合理的算法一进入迭代就能把主要精力放在优化路径质量和避开局部障碍上。4.2 CPO核心循环代码下面给出CPO核心循环的Python实现。这段代码是在原论文公式基础上整理的工程化版本做了两处调整一是把防御策略的概率切换固定为四等分二是边界处理改为反弹策略实际使用中稳定性和收敛速度都更好。import numpy as np def cpo_optimize(fitness_func, lb, ub, dim, n_pop30, max_iter200, seed42): rng np.random.default_rng(seed) # 初始化个体 X rng.uniform(lb, ub, (n_pop, dim)) fit np.array([fitness_func(x) for x in X]) best_idx np.argmin(fit) best_pos X[best_idx].copy() best_fit fit[best_idx] history [] for t in range(max_iter): H np.exp(-t / max_iter) G np.exp(-2 * t / max_iter) for i in range(n_pop): x_old X[i].copy() f_old fit[i] r rng.random() if r 0.25: # 视觉刺激全局探索 j, k rng.choice(n_pop, 2, replaceFalse) tau 1.0 if rng.random() 0.7 else -1.0 noise rng.normal(0, 1, dim) Y X[i] tau * noise * (X[j] - X[k]) elif r 0.5: # 声音警告开发 j, k rng.choice(n_pop, 2, replaceFalse) beta rng.normal(0, 1, dim) Y X[i] G * (best_pos - X[i]) beta * (X[j] - X[k]) elif r 0.75: # 气味释放局部探索 j rng.integers(0, n_pop) u1 rng.random(dim) u2 rng.random(dim) Y X[i] H * (u1 * (best_pos - X[i]) u2 * (X[j] - X[i])) else: # 攻击逃跑强开发 alpha rng.normal(0, 1, dim) Y best_pos alpha * (X[i] - best_pos) # 反弹边界处理 Y np.where(Y lb, 2 * lb - Y, Y) Y np.where(Y ub, 2 * ub - Y, Y) Y np.clip(Y, lb, ub) f_new fitness_func(Y) if f_new f_old: X[i] Y fit[i] f_new if f_new best_fit: best_fit f_new best_pos Y.copy() history.append(best_fit) return best_pos, best_fit, history这段代码维度无关fitness_func返回适应度时可以直接把一维向量重排成航路点矩阵。种群规模建议取30到50太小容易早熟太大收敛速度变慢城市环境一般30只个体配200次迭代就能看到明显效果。4.3 航迹后处理样条平滑与碰撞复核算法直接输出的是一组离散航路点连接起来是折线直接给飞控肯定不行。后处理阶段用三次样条插值将航路点加密生成平滑曲线。具体做法是对x、y、z三个分量分别做样条插值再按等参间隔取出上百个点。from scipy.interpolate import CubicSpline def smooth_path(waypoints, num_points200): t_orig np.arange(len(waypoints)) t_new np.linspace(0, len(waypoints) - 1, num_points) cs_x CubicSpline(t_orig, waypoints[:, 0]) cs_y CubicSpline(t_orig, waypoints[:, 1]) cs_z CubicSpline(t_orig, waypoints[:, 2]) return np.column_stack([cs_x(t_new), cs_y(t_new), cs_z(t_new)])插值之后留下的坑必须重视平滑曲线虽然是连续可导的但样条可能在贴近障碍物的区域发生“削顶”效应本来直线连接是安全的插值后反而穿透楼体边缘。因此后处理完成之后要再做一遍碰撞检测检测到碰撞时采用两种补救方案其一是该航路点沿障碍物外法线方向小幅外推再重新插值其二是把该点标记为新增约束重新跑一轮局部优化。宁可多花几毫秒做复核也绝不能让一条视觉上漂亮但实际穿模的航迹出图。5. 仿真实验CPO与经典算法对比5.1 实验场景、参数与评价指标为了验证方法可行性我搭了一个虚拟城市实验场景。地图范围2000m乘2000m垂直高度上限300米。随机生成12栋长方体建筑物和2个圆柱形禁飞区起点设在(60, 40, 50)终点设在(1850, 1900, 120)要求无人机从城市一角飞到对角全程避开所有建筑物和禁飞区。CPO参数和评价指标如下表所示参数数值航路点数量8种群规模30最大迭代次数200威胁采样间隔3m威胁惩罚系数1e5长度代价权重1平滑代价权重20高度代价权重1最低安全高度40m最高限制高度260m判定一条航迹是否成功的标准有三个全程无碰撞、最大偏转角不超过60度、航迹总长不超过2200米。三者同时满足才算成功单看适应度数值是有误导性的因为一个算法完全可能通过牺牲平滑度来压低总长度。5.2 收敛性对比与航迹质量分析在相同场景、相同初始种子条件下我对比了CPO、PSO和GWO三种算法各运行20次记录统计结果。某次典型运行中的统计数据如下算法平均适应度最优适应度平均航迹长度平均最大偏转角成功次数CPO876.5853.21875.3m24.6°19/20GWO921.8889.41902.7m31.2°16/20PSO998.6945.11974.8m38.5°12/20CPO的收敛曲线在前80代下降很快说明探索阶段能够快速找到多个可飞行走廊120代之后主要在做局部精修航迹的平滑代价和高度代价缓慢下降。GWO前期收敛也不慢但迭代后期头部狼群位置高度相似种群活力下降经常停在某个局部最优附近。PSO的飞行速度衰减问题比较明显后期个体基本聚集成功率最低。航迹质量方面CPO生成的航迹有一个非常直观的特点转弯位置往往提前做好了“拉高-拐弯-降高”的组合动作而GWO和PSO的航迹虽然也避开了障碍但在转弯处会出现较明显的急转现象。这是因为平滑代价在CPO的后期开发阶段得到了充分的精细化权衡而不像另外两类算法那样被威胁代价长期压制。5.3 稳定性与可复现性验证算法仿真最忌讳只看一次结果就下结论。我固定随机种子时CPO的收敛曲线完全可复现这为定位参数问题和对比实验提供了基础。更换不同种子后CPO的最终适应度变化幅度很小标准差大约在20左右而GWO的标准差接近35PSO接近60。这说明CPO对初始种群的敏感度较低在不同随机条件下都能找到质量接近的航迹。实操中建议记录以下指标多次运行的平均值、标准差、成功次数和单次运行时间。只报告“最优适应度”而不提成功率很容易被单次运气好的结果误导。CPO单次200次迭代在纯Python环境大约耗时1.8秒配合并行评价函数可以缩短到0.5秒以下完全满足离线预规划和在线重规划的需求。6. 实战避坑与经验心得6.1 权重和惩罚系数怎么定权重调参是三维航迹规划里最耗时的环节也是很多人容易翻车的地方。我最开始跑CPO时结果并不比PSO好排查后发现问题出在威胁惩罚系数只设了1e3。这个量级下算法会在“撞一次楼罚1000”和“省500米路程省500分”之间做权衡结果毫不意外地选择贴着楼边飞。把威胁惩罚提高到1e5之后任何碰撞都远大于正常飞行带来的所有好处之和CPO才开始老老实实绕楼飞行。调参顺序建议分三步随机初始化种群统计初始状态下长度、威胁、平滑、高度四个分项的数值范围。此时撞楼严重威胁项数值可能极大没关系记住其他三项的数量级。按威胁惩罚远大于其他项的原则设定w2再按“长度项稍大于平滑项、平滑项大于高度项”的经验设置w1、w3、w4。跑一次完整迭代观察输出航迹主要被哪个代价约束限制再微调对应权重。航迹贴着障碍物就提高威胁惩罚航迹过于蜿蜒就提高平滑权重航迹长期保持极低高度就提高高度惩罚。6.2 实现中容易翻车的几个细节第一个高频问题就是碰撞检测只查航路点不查线段这是最典型的低级错误。我之前帮人排查过一个实验代码算法输出的航迹每个航路点都在楼体外侧但把点连起来后整条线直接穿进建筑物内部就是因为没有做线段插值采样。只要加一行循环对相邻点做线性插值这个问题就能杜绝。第二个问题是样条插值后不做碰撞复核。优化阶段的检查针对的是折线航迹后处理阶段用三次样条平滑之后几何形状会发生变化样条曲线可能在障碍物凹角处产生内切效果。城市高楼大多是规则长方体这个现象相对少见但一旦出现在仿真里非常隐蔽。解决方法是后处理时强制进行一次碰撞检测遇到问题就修正航路点。第三个问题是个体越界后把坐标直接拉回边界表面上合法了却导致大量个体在地图边缘“贴墙”。反弹边界策略在CPO的四种更新策略下都更稳定。另外个体越界但适应度临时升高的情况很常见反弹处理比截断处理更能保留这类过渡个体。6.3 从城市静态场景继续扩展的方向静态城市环境只是第一步。实际飞行过程中临时禁飞区、飞来飞去的其他航空器、突然出现的施工塔吊都会让环境模型动态变化。CPO一类元启发式算法天然可以做滚动时域重规划检测到新障碍物后以当前航迹为初始解重新启动优化通常几十次迭代就能给出备选路径非常适合作为在线重规划的后端求解器。多无人机协同也是一个明显方向。多机场景下不仅要规划单机航迹还要解决任务分配、时序避让和通信保持问题。CPO的种群中一个个体可以同时编入多台无人机的全部航路点把总代价按照安全间距约束做交叉惩罚就能在一个优化循环里同时求解分配和路径。接入真实数字高程模型和楼宇轮廓数据后这套流程基本可以直接用于城市物流配送或巡检任务的前期路线论证。唯一要留意的是真实环境中的障碍物边界往往不是规则长方体一个可行做法是把不规则建筑转成多个AABB包围盒的组合用多边形碰撞检测算法替代目前的大于小于判断。最后说几句实操体会我最初接触CPO时并没有立刻感受到它和PSO、GWO的差距甚至觉得效果差不多。反复对比后才发现让CPO出彩的不是前几十代的探索速度而是后期开发阶段对航迹细节的打磨能力。威胁惩罚足够大、采样间隔合理、样条复核到位这三个条件满足后CPO在城市三维场景中反复跑出来的结果都明显更干净。如果让我给一个直接可用的建议就是别急着上复杂算法先把代价函数可视化。把适应度函数中长度、威胁、平滑、高度四个分量单独打出来看你会立刻知道一条航迹到底是为了省距离而冒险还是因为过度追求平滑变成了“观光路线”。把每个代价拆开看比盯着一个综合适应度值调试高出几个数量级的效率。另外一个非常实用的小技巧在代价函数末尾增加一行调试打印只在前几次迭代中输出每个分量的具体数值确认量级符合预期后立刻关闭。这个操作能帮你避免很多“算法没问题但代价项之间权重失衡”的慢性折腾。CPO本身是好用的工具但工具再好也要先确认评价函数这把尺子本身是准的后续的航迹结果才有参考价值。