楼宇之间穿行这件事我在之前的项目里吃过大亏。当时用的是二维平面路径规划无人机飞着飞着就撞上了高层建筑的侧立面——原因很简单二维路径只考虑水平面的躲避根本没算高楼在不同高度层的“投影范围”更没考虑爬升率、转弯半径这些真实飞行约束。后来我换成了三维航迹规划才算把问题真正按三维空间的逻辑去解。这次我要分享的是我在模拟城市地形上反复调试验证的一套算法部落竞争与成员合作算法CTCM配上Matlab完整实现思路专门用来解决复杂城市环境下无人机避障三维航迹规划这个具体问题。这篇文章不是上来就贴代码的教程我会把四个关键问题一次讲透城市环境建模里到底哪些约束在真正卡脖子、CTCM的“部落竞争”和“成员合作”各自在优化里扮演什么角色、代价函数怎么设计才不让无人机贴着楼飞以及Matlab实现中那些不跑仿真根本发现不了的坑。适合正在做无人机路径规划、元启发式算法应用或者被城市场景航迹规划折磨过的同学参考。完整可运行的代码框架我也会拆开讲解你自己在Matlab里按这个思路搭一套并不难。1. 城市峡谷里的三维航迹规划为什么二维思路直接失效1.1 城市环境与自然地形最大的区别高密度离散障碍做航迹规划先分清楚环境类型这事太重要了。自然地形里障碍物是连续变化的山体、丘陵是缓变曲面无人机绕山飞行本质上是在等高线之间找通道常规的势场法或者A*在栅格化后的数字高程图上都能工作。但城市环境完全不是一回事楼宇是离散的、垂直的、顶部齐平或错落的多面体地面俯视是一堆矩形投影换个高度层看投影位置不变但“有效障碍集”完全不同。我测试的场景是2000米×2000米×500米的城市区块里面随机布置了十几栋不同高度的楼宇外加两个禁飞区。第一次按传统二维思路把楼宇投影到水平面做避障规划出来一条“完美绕开所有投影”的路径结果一拉高到150米巡航高度路径直接从一栋120米高的楼顶正上方飞过——这不算撞楼但城市上空有大量不明气流和潜在风险贴顶飞行本来就是一种隐患。真正的三维规划必须同时考虑障碍物的水平位置和垂直高度把它当“空间中的立体块”而不是“平面上的圆”。这也直接决定了代价函数里必须包含高度维的惩罚项。1.2 三维航迹规划需要同时满足哪些约束城市无人机航迹不是一条好看的曲线就行它得同时满足四类约束我一条条列出来几何避障约束整条航迹上的每个点都不能进入障碍物膨胀后的包络体也不能穿越禁飞区。这里的关键是“膨胀”。城市里无人机飞行不可能精确贴着楼墙走定位误差、风扰、动态障碍物都要求安全间距我一般把楼宇外扩15米再参与碰撞检测。动力学约束无人机最大转弯角、最大爬升率/俯冲率、最小转弯半径都是硬限制。规划出来的相邻航迹点之间的切向夹角超出阈值飞控根本执行不了算法输出的只能是“纸上航线”。我测试时把最大转弯角设为45度最大爬升/俯冲角设为30度。高度约束城市环境里最低飞行高度不能太低否则撞上低矮建筑、路灯和行人的风险很高也不能太高城市上空300米以上往往是管制空域。我在这个场景里把可行高度带设在40米到260米之间。任务经济性约束航迹长度尽量短、转弯尽量少、能耗尽量低。这是做优化而不是仅求“可行解”的原因。1.3 传统元启发式算法在城市场景下的不足我一开始也试过粒子群、遗传算法这些经典方法。不能说完全不行但在高密度楼宇场景里它们暴露出一个共性问题容易陷入局部最优尤其是在楼宇形成的“U型”包围结构里。路径一旦钻进一个三面被楼围住的死胡同粒子群基本靠随机扰动逃不出来遗传算法靠交叉变异也很难跳出封闭区域。原因在于这些算法的搜索策略偏“均匀探索”没有形成有层次的竞争——一部分个体专心攻当前区域一部分个体必须去其他区域探路。这让我开始研究带“群体对抗”结构的算法。部落竞争与成员合作算法CTCM的思路恰好契合这个需求多个部落各占一块搜索区域部落之间存在竞争淘汰单个部落内部成员又互相合作交换信息既保证局部开发能力又通过部落整体层面的竞争维持全局探索动力。后面几节我详细拆解它的机制。2. CTCM机制拆解部落怎么分、成员怎么合作、部落怎么竞争2.1 部落划分与初始化策略CTCM的初始化不是单纯撒随机点而是把种群分成若干个部落每个部落内部再细化到成员个体。打个比方这不是一群散兵游勇在地图上乱走而是好几个部落各自圈定一片“领地”在领地里安排若干成员分头搜索部落之间因为领地资源问题天然形成了竞争关系。在实际实现里我把部落数量设置为6个每个部落包含12个成员。每个成员携带一条完整的候选航迹由20个三维航迹点组成所以整个种群就是72条候选航迹在并行进化。初始化时我让每个部落的航迹点在一定空间范围内聚拢但不同部落的中心位置彼此拉开距离。这样做的好处很明显前几代迭代时不同部落就自动覆盖了解空间的不同区域不会像传统粒子群那样所有个体一窝蜂聚在初始最优附近。这里有一个我在实验里反复验证过的规律如果初始化把所有部落中心设在同一区域那前30代基本都在做无用功——几乎所有成员都在同一片区域里找路竞争机制也起不到淘汰作用因为大家都差不多差。拉开幕布让每个部落从不同“入口”探索城市效果会好非常多。2.2 成员合作机制共享池与多方向搜索部落内部的合作是CTCM的核心特色之一。每个部落内部有一个信息共享池Shared Pool存放本部落当前适应度排名前三的航迹片段。更新某个成员时它并不仅仅朝自己部落的最优成员靠拢而是从共享池里随机选择一个片段作为引导——这一步我把它做成了两种更新策略学习策略成员向共享池中的优质航迹片段靠近模仿其经过的路径转折点。这种策略负责“开发”让新航迹继承前几代的优势路径结构。探索策略成员以较大步长向部落领地的边缘跳跃同时叠加一个Levy飞行的随机扰动。这种策略负责“探索”防止部落长时间困在同一片区域。在Matlab实现里这两种策略的切换由一个合作概率参数控制我一般设在0.4到0.5之间。低于0.3部落内部所有成员行为都偏探索收敛很慢看起来像在“瞎逛”高于0.7又变成全员抄作业多样性迅速崩溃。这个阈值值得多试几组。合作机制还有一个容易被人忽略的细节共享池更新要设置停滞期。如果连续5代部落最优没有变化强制把共享池里排名第三的片段换成随机新片段。这相当于给部落内部人为制造“新鲜血液”在城市这样障碍物高度不均匀的环境里非常有效能防止信息池过度同质化。2.3 部落竞争机制战力评估、吞并与领地再分配部落层面的竞争机制这是CTCM区别于其他群智能算法最明显的地方。每隔固定代数我在场景里用5代系统会对每个部落做一次综合战力评估战力值由三项组成部落成员的平均适应度反映整体水平部落最优适应度反映尖子成员的水平战力进步速度当前战力相对上次评估的提升幅度反映进化活力综合战力公式我设置为战力 0.4 × 平均适应度 0.3 × 最优适应度 0.3 × 进步速度每次评估后战力最弱的部落会被战力最强的部落吞并。吞并不是简单删除而是做领地再分配最弱部落的成员以50%概率发生较大幅度的变异后并入胜者部落胜者部落的领地范围扩张覆盖被吞并部落的原有区域同时为了维持种群规模稳定胜者部落随机丢弃一部分适应度较低的成员。这个机制解决问题的关键在哪它相当于在整个搜索过程中持续做了一次“资源再平衡”。某个部落如果长期困在死胡同区域它的平均适应度一定长期垫底进步速度也会放缓最终被淘汰。胜者部落带着更优秀的路径模式扩张领地本质上是在用“占领”的方式强制算法放弃那些没有希望的区域把计算资源集中到真正能出可行路径的方向上。编程层面要注意一个坑吞并操作不能太频繁。我在早期版本里把竞争周期设为1代结果部落结构刚建立优势就被吞来吞去整个种群陷入高强度的混乱状态收敛曲线像锯齿一样抖动。后来改成5代评估一次情况立刻好转算法能稳定向下收敛。这个周期和部落数量强相关部落多可以适当延长周期让它有足够时间在领地内形成局部开发优势。2.4 CTCM为什么能兼顾探索与开发群体智能算法一直有个核心矛盾探索和开发很难两全。探索过多收敛慢、计算浪费严重开发过多容易早熟陷进局部最优就出不来。CTCM的结构巧妙地把这对矛盾拆到了两个层面解决。在部落内部成员合作机制负责“开发”——共享池让优质路径信息在部落内快速传播成员们围绕当前较优路径做精细化搜索。在部落之间竞争淘汰机制负责“探索”——落后部落的领地会被胜者覆盖新变异成员相当于被强行扔到新区域胜者部落为了维持战力必须持续派出成员探测领地边缘。一个部落同时承担“守成”和“扩张”的双重压力算法整体就既能保持收敛速度又不至于一头扎进局部陷阱。在密集楼宇场景下这种机制的价值特别直观。我在某次实验里看到过这样的过程两个部落分别从南北两个方向尝试穿过同一片楼群北边部落找了一条需要连续急转弯的窄缝路径适应度很差但在缓慢上升南边部落找到了一条稍远但更平顺的绕行路径适应度明显更好。第40代评估时北边部落被吞并其成员变异后并入南边部落后续在绕行路径基础上通过共享池的引导不断优化高度和转弯位置最终收敛出一条城市里相对安全的走廊航线。这个过程中竞争机制果断放弃了高风险窄缝方案合作机制则持续打磨更优绕行方案——两者缺一不可。3. 复杂城市地形建模把楼宇和禁飞区变为数学约束3.1 环境栅格化与数字高程处理在Matlab里做城市地形建模我不用特别精细的网格跑来跑去的做法直接用三维栅格矩阵表达环境。整个2000米×2000米×500米的场景按10米分辨率划分成200×200×50的栅格数组每个栅格有三种状态0表示自由空间1表示障碍物占用2表示禁飞区。城市地形和自然地形最大的不同在于它没有连续的数字高程面可以插值楼宇是类似矩形柱体的垂直实体。所以我建地形时分成两层先完成地形底图也就是地面高度函数城市场景里地面基本平坦我设为20米到30米的微小起伏再叠加楼宇模型层按坐标、长宽和高度记录每一栋楼的参数之后逐点判断一个栅格是否落在某栋楼的外包长方体里。预处理阶段我会做一次快速碰撞检测把每栋楼的外包盒沿三个轴向各膨胀15米膨胀后的体素统一标记为1。注意这里的膨胀不是简单加几个栅格而是要考虑楼房的朝向和尺寸避免相邻楼宇膨胀后互相连通形成整片障碍墙——我在实际测试里遇到过这种误判两栋挨得近的楼膨胀后直接变成一堵墙把原本可通行的通道封死了。所以我会在膨胀后做一遍连通性检查确认没有被过度封锁的自由空间通道。3.2 楼宇障碍物与禁飞区的碰撞检测模型碰撞检测我用了两种表示。楼宇这类方正的建筑物最适合用OBB有向包围盒做碰撞检测判断航迹点是否落在某个轴对齐的长方体内部即可计算量小Matlab里几下就能跑完。禁飞区我用的是柱形区域因为城市里的禁飞区往往以某个地面参考点为中心向上延伸到一定高度形状适合用圆柱近似。等无人机航迹的每个关键节点出来后我会把相邻节点连成线段对每个线段做分段采样检测以1米为步长在线段中间插入插值点逐一判断是否进入障碍或禁飞区。这一步计算量虽然大但精度很关键。如果只检测航迹节点而不检测节点之间的连线很容易出现“节点都在自由空间但连线穿过楼体中段”的隐蔽碰撞仿真图上看着没问题实际飞机会一头撞上去。这种坑我在早期版本踩得很实在现在每一步都不敢省。3.3 综合代价函数设计与权重选择有了环境模型接下来就是优化算法里最核心的部分代价函数怎么定。我设计了一个四项加权代价总代价 w₁ × 航程代价 w₂ × 威胁代价 w₃ × 高度代价 w₄ × 动力学代价各项的具体定义和权重我在实际调试中反复调过总结如下航程代价整条航迹的总长度除以直线距离反映路径的绕行程度。城市环境下绕行难以避免但过度绕行意味着耗能和暴露风险增加这个系数我设为0.3。威胁代价航迹上所有采样点到障碍物表面的距离平方的倒数之和。距离越近惩罚越急剧增大就像在障碍物周围形成一圈“排斥场”。这个系数我设为0.35是权重最大的项。为什么不设得更大太大会让航迹为了远离所有障碍大幅绕行生成“极度保守但没实际意义”的路线太小又会让航迹贴着楼宇走违背安全初衷。0.35是我在多个场景中反复测试后比较平衡的值。高度代价航迹点在高度上相对参考巡航高度的偏差。参考高度我根据场景里楼高的中位数设定为120米太低容易撞低矮建筑太高又逼近城市空域上限。这个系数我设为0.2让算法尽量维持稳定的巡航高度避免频繁爬升下压。动力学代价对相邻航迹段之间的夹角和坡度变化做惩罚。相邻段夹角超过最大转弯角限制或坡度超过最大爬升角限制就按超限比例线性增加惩罚。这个系数我设为0.15。必须强调的是动力学代价虽然权重低但绝不能省。在贴楼飞行的实验里如果把威胁代价调到0.5以上算法生成的航迹离楼体会有40到50米的净空但同时航程变得很长如果把威胁代价降到0.2以下航迹会非常贴近楼体最近距离只有10来米。这个权衡值得你在自己的场景里按实际安全要求标定。4. Matlab实现细节从坐标编码到平滑轨迹输出4.1 航迹点编码方式在CTCM里每个成员对应的解是一整条三维航迹我用固定数量的航迹点来编码。在场景里我设定每条航迹由20个航迹点构成起点和终点固定中间18个点是待优化的变量。每个航迹点包含x、y、z三个坐标所以一个成员的搜索维度是18×354维。这样的编码方式直接决定了解空间的搜索效率。航迹点太少路径形状表达能力差复杂楼群里绕不过去航迹点太多维度上去的同时算法要协调的点也变多收敛难度呈指数上升。在城市场景里20个点是我试下来精度和计算量的平衡点。坐标编码时为了不让每个维度在毫无边界约束的空间里乱搜我为每个航迹点设置了可搜索范围。比如x和y方向限制在整个场景范围内z方向限制在40米到260米的可行高度带内。初始化和变异更新时所有坐标都必须落在这个范围内越界的直接截断回边界。这样能有效避免大量无效方向上浪费迭代资源。4.2 CTCM迭代主循环伪代码整个CTCM的核心流程我用伪代码表示是这样的初始化6个部落每部落12个成员 每个成员生成一条随机航迹20个航迹点 评估所有成员适应度初始化全局最优 for iter 1:200 for tribe 1:6 更新部落共享池取适应度前3航迹片段 for member 1:12 rand_value random() if rand_value 合作概率 从共享池选一个片段执行学习策略更新 else 执行Levy探索策略更新 end 越界修正 评估新航迹适应度 贪婪保留新航迹不优于旧航迹就保留旧值 end end if mod(iter, 5) 0 计算部落战力 0.4*平均适应度 0.3*最优适应度 0.3*进步速度 最弱部落成员变异后并入最强部落 最强部落丢弃部分低适应度成员扩张领地 end 更新全局最优记录 end这个主循环结构清晰Matlab实现难度不大。个人经验里最重要的一点是最后一步“贪婪保留”成员更新后如果新航迹的适应度没有优于旧航迹就保留旧值不替换。这个策略在元启发式算法中让收敛曲线更稳定否则噪声大的评估会让种群在后期反复横跳明明已经接近最优了还要拼命折腾。4.3 关键函数代码片段这部分我把几个最核心的Matlab代码段拆出来讲。首先是代价计算函数它的输入是一条航迹矩阵和场景结构体输出是总代价function cost calcCost(path, env) % path: 20x3矩阵, 每行是[x, y, z] % env: 场景结构体包含障碍物、禁飞区、参考高度等 % 1. 航程代价 dist_total sum(sqrt(sum(diff(path).^2, 2))); dist_line norm(path(end,:) - path(1,:)); cost_path dist_total / max(dist_line, 1e-6); % 2. 威胁代价 cost_threat 0; sample_pts sampleLine(path, 1); % 按1米步长采样 for i 1:size(sample_pts, 1) d minDistToObstacle(sample_pts(i,:), env.obstacles); if d 15 % 已经进入膨胀区 cost_threat cost_threat 1000; else cost_threat cost_threat 1 / (d^2); end end % 3. 高度代价 cost_height mean(abs(path(:,3) - env.ref_height)); % 4. 动力学代价 cost_dyn calcDynPenalty(path, env.max_turn_deg); cost 0.30 * cost_path 0.35 * cost_threat ... 0.20 * cost_height 0.15 * cost_dyn; end“采样点进入膨胀区直接给1000的硬惩罚”这个处理很关键。如果不给这种硬惩罚进入障碍物内部会导致后面1/d²那项直接变成无穷大或者千亿级的数值代价函数瞬间被单个点支配其他三项全失去意义。给一个有限的大惩罚能让算法既知道“这里绝对不能进”又不至于因为一个采样点就让整个解被判死刑、完全没法传递梯度信息。合作更新部分的核心代码我这样写function new_member cooperateUpdate(member, shared_pool, tribe_center, env) if rand() 0.5 % 学习策略向共享池随机优质片段靠拢 ref shared_pool{randi(length(shared_pool))}; alpha 0.6 * rand(); new_member member alpha * (ref - member); else % 探索策略Levy飞行跳向领地边缘 levy_step levyFlight(size(member,1), size(member,2)); direction tribe_center - member; new_member member 0.3 * direction 1.2 * levy_step; end % 越界修正 new_member clampPath(new_member, env.bounds); endLevy飞行的步长分布我用了重尾分布偶尔产生一个大幅跳跃这正是逃离局部死胡同的关键。城市楼群死胡同不像自然地形那样有一个连续缓变的上坡能“慢慢爬出去”它是个突变结构需要一次足够大的跳跃才能脱离。4.4 轨迹后处理与B样条平滑CTCM优化输出的是一条由20个离散航迹点组成的折线直接交给飞控肯定不行——转弯点太生硬无人机需要连续的曲率变化。所以我在仿真输出的最后一步加上了B样条平滑后处理。具体做法是把20个航迹点作为B样条的控制点用三阶B样条插值生成一条密化的光滑曲线然后在曲线上按等间距重新采样出用于显示的航迹。这一步的麻烦在于B样条曲线会偏移控制点平滑后可能从楼宇边缘“蹭”过去。为规避这个风险我在平滑后会重新做一遍碰撞检测如果发现平滑后的曲线进入了膨胀区就把该段对应的控制点做一次小幅度反向修正后重新平滑重复两三次基本能保证安全。平滑处理对代价函数的意义也不容忽视。初期版本里我在代价函数的动力学项中用相邻航迹点夹角做惩罚但用B样条平滑后发现真正影响飞行性能的其实是曲线曲率。所以优化阶段的动力学惩罚和平滑后的曲率检测我会各做一遍。优化阶段不做平滑因为迭代200次、72个成员都做平滑再评估计算量大约增加8倍会拖慢收敛速度只在最后对最优解做一次平滑验证性价比最高。5. 仿真验证与调参经验数据和坑一起说5.1 不同地形复杂度下的仿真结果我在Matlab里做了两组对比实验验证CTCM在简单和复杂两种场景下的表现。简单场景3栋楼、无禁飞区。CTCM大约在45代左右收敛到稳定解规划出的路径长约2800米绕行率1.4高度基本稳定在120米巡航带附近全程无碰撞风险。粒子群算法在相同场景下大约75代才收敛且有两三次实验陷入局部最优路径明显多绕了一个大弯。复杂场景12栋楼、2个禁飞区故意设置了一片“U型楼群”。CTCM平均在85代左右收敛到可行解避障成功率约96%粒子群在120代左右才收敛避障成功率明显低于CTCM且有多条路径从禁飞区边缘擦过遗传算法失败率更高经常只能给出“绕了极大一圈但成功避开”的低效路径。航迹典型性对比CTCM生成的路径能主动避开U型楼群的敞口侧从侧面绕行而非钻入开放区域再原路返回这种路径结构上的“决策”正是部落竞争机制发挥作用的表现——一旦有部落钻进了U型底部战力评估会快速暴露它的劣势最终在吞并中被淘汰。这些结果虽然来自我搭建的模拟场景不代表实际飞行数据但趋势已经足够说明CTCM在密集离散障碍环境下的优势。5.2 部落数量与合作概率等参数的影响参数对CTCM性能的影响我做了逐一扫描这里直接给结论部落数量6个部落表现综合最好。少于3个竞争机制形同虚设算法退化成普通群智能算法多于12个计算量急剧上升且弱部落太多导致淘汰频繁反而打乱收敛节奏。每部落成员数8到15之间效果最好。太少则共享池信息量不够合作机制难以发挥太多则每次迭代的计算开销过大。合作概率0.4到0.5是甜区。合作概率过高部落内行为趋同多样性崩塌过低则成员整天瞎窜收敛缓慢。竞争周期5代更新一次最稳。1到3代太频繁吞并带来的扰动远大于收益10代以上又会浪费计算资源在明显无望的部落实地上。5.3 实际调试中踩过的几个坑踩坑一权重失衡导致的贴楼飞行。一开始我把威胁代价权重调到0.5期望最大化安全间距结果路径为了离所有楼都远远的绕行了将近两倍的航程而且高度变化剧烈。后来改成0.35并给碰撞硬惩罚路径明显更合理安全间距保持在20米以上航程也短了30%。踩坑二初始化范围过窄导致前30代集体失效。早期版本部落中心位置是随机生成的但没有强制拉开距离结果有一半的部落中心集中到场景边缘区域那边根本没有障碍物成员们很快就收敛到无意义的高适应度平直路径战争竞争机制完全失效。修正方案是初始化时强制部落中心两两间距不小于300米。踩坑三碰撞检测只查节点不查连线。这是最危险的一类问题因为仿真图上看路径在障碍物之间穿梭看起来挺安全但实际上两个相邻航迹点间的连线穿过一栋楼的楼体中部曲线插值出来整条路径都要穿楼了。以后所有路径评估都必须做线段级采样检测。踩坑四B样条平滑后路径穿透障碍。平滑前安全、平滑后穿楼这个问题非常隐蔽。解决办法我已经在前面说过平滑后重新检测碰撞并对穿障控制点做迭代修正。5.4 对CTCM在真实场景落地的一些思考仿真和真实飞行之间还有一道巨大的转换鸿沟。雷达或视觉探测到的障碍物是动态的、有误差的不像仿真里那么干净气流扰动会让实际轨迹偏离规划轨迹。我理解的三维航迹规划本质上是给飞控一个“安全的参考走廊”而不是绝对精确的飞行轨迹。所以在实际工程里CTCM规划出的路径之后应该再叠加一个局部重规划层实时检测动态障碍物发现碰撞风险就局部调整上一段航迹而不是从头开始重新规划。CTCM这种族群的竞争架构用来做全局规划很有效局部实时响应还是要交给更轻量的方法。6. 结束语一些有价值的操作心得我使用的方法有一个很独特的点就是用部落冲突产生的压力来维持种群的多样性这个点在我做过的其他优化场景里也能借鉴——不只是航迹规划任何高维约束优化问题如果个体之间完全平等没有对抗关系很容易早熟一旦引入“群体层面的淘汰与合并”搜索节奏就会明显不一样。实战中如果真的要用CTCM我建议从这几个参数起手6个部落、每部落10到12个成员、合作概率0.45、竞争周期5代、最大迭代200次。先跑一遍简单场景确认算法逻辑没问题再逐步增加障碍物密度。调试时把每次迭代的部落最优、全局最优和部落领地中心位置都记录下来画出来看看不同部落是怎样探索和落败的这个过程会让你对算法机制的理解比读十篇论文都深刻。我刚才运行的几个场景里最让我印象深刻的不是最终航迹有多顺而是第38代那次关键吞并——一个一直试图从正南方向穿越楼群的部落注定失败在它被吞并之后算法在10代内就找到了绕行东侧的稳定路径。这种“算法主动做出方向性决策”的时刻正是这类带竞争淘汰机制的算法最迷人的地方。希望这篇分享能帮你少走弯路把CTCM真正用到你的无人机航迹规划项目里去。