无人机避障三维航迹规划这个方向圈内做的人应该都有同感二维路径规划成熟得一抓一大把但一加上高度维、再加上城市高楼群的遮挡约束问题的复杂度直接上升一个量级。我之前在做一个无人机城市配送仿真项目时用传统群智能算法规划三维航迹要么收敛慢得让人怀疑人生要么规划出来的路径贴着楼顶走根本没法用。后来接触到一种基于“部落竞争与成员合作”机制的优化算法CTCM把外部竞争和内部协作拆开设计意外地适配复杂地形下的三维航迹规划场景。这篇文章把我从算法原理理解、三维环境建模、代价函数设计到Matlab代码实现、参数整定、对比实验的完整过程都梳理出来。内容包括CTCM算法的核心机制拆解、城市障碍物场景下的航迹规划数学模型、可直接改用的Matlab框架代码以及我调试过程中踩过的一堆坑。想做课程设计、毕业设计或者在实际任务仿真里用群智能算法做航迹规划的同学都可以直接参考。1. 城市峡谷里的航迹规划问题比想象中难1.1 三维航迹与二维路径的本质差别很多人觉得三维航迹规划就是把二维路径规划加一个高度坐标实际上完全不是这么回事。二维路径是在一个平面上找一条从起点到终点、绕过障碍物的折线自由度低约束也简单。但三维航迹规划中飞行器在每一个航迹点都有三个空间自由度而且高度维的变化直接牵扯到飞行性能约束——爬升率、转弯半径、最大飞行高度、最小离地高度这些在二维问题里根本不需要考虑。更重要的是城市地形下的威胁源是立体的。一栋高楼在二维地图上只是一个多边形但在三维空间里它是一个竖向的柱状禁飞区。无人机不仅要绕开楼的平面投影还要考虑楼顶上方是否有足够的越障高度甚至要考虑建筑物之间的“峡谷效应”——两栋高楼之间的狭窄通道风速湍流复杂实际飞行中通常应该避免穿行。这些约束叠加在一起搜索空间变得极其不规则算法很容易陷入局部最优。1.2 传统算法在这个场景下的三个痛点先说粒子群算法PSO。它在二维路径规划里表现不错但三维航迹规划的解空间维度高每个航迹点有三个坐标一条路径十几二十个航迹点就是五六十维的优化问题。PSO在这种高维空间下粒子容易“扎堆”早熟收敛严重经常所有粒子都挤到同一个局部最优附近路径贴着障碍物边缘代价函数看着挺小实际根本不能飞。遗传算法GA的全局搜索能力好一些但城市地形下需要精细的局部搜索能力来微调航迹点位置GA的变异算子随机性太强局部精调能力偏弱。而且种群一大每一代的适应度评估都要重新计算整条路径的代价计算开销非常高。RRT这类采样算法倒是能处理高维空间但规划出来的路径频繁出现锯齿状折线平滑处理之后又可能撞上障碍物需要反复做后处理。做航迹规划的人应该都有体会RRT出来的东西“能用但不好用”。这三个痛点的根源在于算法要么只擅长全局探索要么只擅长局部开发很难兼顾。我拿到CTCM算法时第一感觉就是这个算法的设计思路恰恰是冲着这个矛盾去的。2. CTCM算法拆解部落竞争与成员合作到底在做什么2.1 从社会部落行为到优化算法的映射CTCM的全称是部落竞争与成员合作算法Tribal Competition and Member Cooperation。它的设计灵感来自人类社会部落的生存模式多个部落生活在同一片区域部落之间争夺领地资源弱小的部落会被压制甚至吞并而同一个部落内部的成员之间又会互相协作、共享信息共同提高整个部落的生存能力。映射到优化算法里这个机制非常清晰整个种群 若干个子群体部落每个部落独立维护一组解成员。部落间竞争 每隔一定迭代次数比较各部落的整体表现表现差的部落会被惩罚性重组或淘汰部分成员把搜索资源让给表现好的部落。成员合作 部落内部成员围绕本部落当前最优解部落领袖进行信息共享和位置更新相当于每个部落内部有一个独立的局部搜索小组。这种“部落之间竞争 部落内部合作”的双层结构本质上是一种带竞争机制的多种群协同进化。相比单一种群算法它的好处是不同部落天然分布在解空间的不同区域竞争机制会淘汰那些长期找不到好解的探索方向而内部合作又能保证每个区域都被充分挖掘。2.2 部落竞争机制保证全局探索部落竞争是整个算法里最有特色的部分。我在实现时采用的竞争策略是这样的每个部落每一轮迭代后统计本部落成员的适应度均值作为部落的“竞争力指标”。每隔固定代数我通常设为5代对所有部落按竞争力排序。排名靠后的部落不会直接被删除而是执行“重组操作”保留该部落中表现最好的前20%成员剩下的80%成员用向全局最优解附近收缩的方式重新生成同时给这个部落的搜索半径乘上一个惩罚系数。这样一来表现差的部落并没有完全消失而是被压缩到更有希望的区域继续搜索保留了种群的多样性避免所有个体都涌向同一个最优解。还有一个细节值得注意竞争不能太频繁。我在初期测试时把竞争代数设为2代结果算法收敛极快但最后得到的路径质量并不好——因为部落还没来得及在各自区域充分搜索就被迫重组探索深度不够。后来改为5代一次竞争效果明显改善这也符合直觉竞争的目的是引导搜索资源流向有希望的区域而不是扼杀探索过程。2.3 成员合作机制提升局部开发能力成员合作是CTCM的另一个核心。在每个部落内部成员更新借鉴了PSO的“向最优解学习”思想但做了简化部落内部每个成员更新时参考两个信息源——本部落的领袖部落内最优解和全体部落的全局最优解。具体更新公式是这样的成员的新位置 当前位置 惯性项 认知项 社会项。其中惯性项保持原有运动方向认知项把成员拉向部落领袖社会项把成员拉向全局最优。这和PSO的经典速度更新很像但有一个关键区别这里的“惯性项”不是速度继承而是成员自身在当前解附近做一次高斯扰动扰动幅度随迭代次数逐渐减小。这个设计的好处是部落内部的搜索行为兼具“向领袖靠拢”和“自身邻域探索”两个属性。前期扰动幅度大成员在部落领袖周围做广泛搜索后期扰动幅度小成员精确微调航迹点位置。我在实现中把高斯扰动的标准差设为初始搜索范围乘一个线性递减系数从0.5降到0.05实测下来局部精调的效果比PSO原版的速度更新更稳不容易出现粒子飞出去撞到障碍物的情况。3. 复杂城市地形的建模与代价函数设计3.1 城市障碍物建模的两种思路做航迹规划第一步是把城市地形描述成算法能理解的形式。我用的方案是障碍物栅格化 威胁场叠加的组合。具体来说先把规划区域划分成三维网格每个网格标记是否为障碍物然后对每个障碍物生成一个辐射状的威胁场距离越近威胁值越高。城市环境里的障碍物主要是建筑物。我在仿真中把建筑物简化为不同高度的长方体用数组存储每个障碍物的底面中心坐标、长、宽、高。这样的模型足够接近真实城区又不会让碰撞检测的计算量失控。障碍物附近设置安全缓冲距离——我设的是2米也就是航迹点到建筑物表面距离小于2米时威胁代价开始显著上升。还有一种更精细的做法是导入真实城区的数字高程模型DEM和建筑物轮廓矢量数据但如果做算法研究没必要一开始就把数据搞得那么复杂。先用规则几何体把算法验证通过再换真实地形数据这个顺序更合理。3.2 综合代价函数的各项组成与权重分配航迹规划本质上是一个多目标优化问题CTCM算法需要一个标量化的适应度函数来比较解的优劣。我设计的综合代价函数由四项构成燃油代价航迹总长度。路径越长飞行时间和能耗越高。按所有航迹段长度求和。威胁代价航迹点及航迹段与障碍物的接近程度。我按航迹段每间隔一定距离取采样点计算各采样点的威胁场值并累加。这样比只检查航迹点更可靠因为航迹段中点完全可能穿过障碍物——这是初学者最容易忽略的坑。高度代价与理想飞行高度的偏差。城市环境下无人机保持一个适中的飞行高度既安全又省电过高会增加能耗过低容易撞障碍物。我设的理想高度是50米高度代价取航迹各点实际高度与理想高度的偏差平方和。平滑代价相邻航迹段之间的转弯角。转弯角过大说明路径有急转弯实际飞行时无法完成或需要大幅减速。用航迹点前后两段向量的夹角余弦来度量夹角越大代价越高。总代价为四项的加权和。权重分配我经过多轮实验后固定在燃油代价0.4、威胁代价0.3、高度代价0.2、平滑代价0.1。威胁代价权重不能太低我试过调成0.15规划出来的路径虽然更短更平滑但好几条都擦着建筑物边沿安全冗余不足。3.3 约束条件的处理方式航迹规划的约束条件我用罚函数方式处理而不是硬性拒绝不可行解。具体包括三类约束第一是地形与障碍物约束。航迹任何一点不得进入障碍物栅格且离障碍物表面距离不小于安全缓冲距离。违反时在威胁代价上叠加一个很大的惩罚项。第二是飞行性能约束。包括最大转弯角约束我设为60度、最大爬升/下降角约束设为30度、最大和最小飞行高度约束。违反时直接在总代价上加一个数量级很大的惩罚值。第三是端点约束。航迹必须从指定起点出发终点落在指定目标点附近的一个小范围内。用罚函数而不是硬约束的原因在于如果算法生成的一条路径有微小违规直接丢弃会浪费很多有效的搜索信息罚函数会让算法知道“这个方向接近可行但还差一点”从而逐步修正搜索效率高得多。4. Matlab实现从伪代码到可运行程序4.1 航迹编码方式与种群初始化实现CTCM之前先要确定解的表达形式。我把一条三维航迹表示为固定数量航迹点的坐标序列每个个体是一个行向量长度为 3×NN为航迹点数量不含起点终点起点终点固定。% 航迹编码个体向量 [x1,y1,z1, x2,y2,z2, ..., xN,yN,zN] % N为中间航迹点数量起点和终点单独存储 numWaypoints 10; % 中间航迹点数量 dim 3 * numWaypoints; % 个体维度初始化时起点和终点之间的连线作为基准每个航迹点在此基准附近随机偏移。这个初始化策略很重要——如果航迹点完全随机散布在整个搜索空间生成的大部分个体都是严重穿越建筑物的无效解算法前期会被罚函数惩罚项主导收敛极慢。function pop initPopulation(popSize, startPos, endPos, bound, numWaypoints) pop zeros(popSize, 3 * numWaypoints); for i 1:popSize % 在起点终点连线上均匀取基准点加随机偏移 for j 1:numWaypoints t j / (numWaypoints 1); base startPos t * (endPos - startPos); offset (rand(1,3) - 0.5) * 2 * bound; pop(i, (j-1)*31 : j*3) base offset; end end end4.2 CTCM核心循环的Matlab实现主循环部分重点体现部落竞争和成员合作两个机制。我用元胞数组管理各个部落每个部落内部包含成员矩阵、部落最优解、部落竞争力指标。function [bestPath, bestCost, convergeCurve] ctcm_main(...) % 参数初始化 numTribes 6; % 部落数量 tribeSize 20; % 每个部落成员数 maxIter 200; competeGap 5; % 竞争间隔代数 % 初始化各部落 tribes cell(numTribes, 1); tribeBest zeros(numTribes, dim); tribeBestCost inf(numTribes, 1); for i 1:numTribes tribes{i} initPopulation(tribeSize, startPos, endPos, bound, numWaypoints); % 计算各成员代价更新部落最优 [tribes{i}, tribeBest(i,:), tribeBestCost(i)] evaluateTribes(...); end globalBest tribeBest(1,:); globalBestCost tribeBestCost(1); for iter 1:maxIter for i 1:numTribes % 成员合作更新 tribes{i} memberCooperation(tribes{i}, tribeBest(i,:), globalBest, iter, maxIter); % 重新评估 [tribes{i}, tribeBest(i,:), tribeBestCost(i)] evaluateTribes(...); end % 更新全局最优 [~, idx] min(tribeBestCost); if tribeBestCost(idx) globalBestCost globalBest tribeBest(idx,:); globalBestCost tribeBestCost(idx); end % 部落竞争重组 if mod(iter, competeGap) 0 [tribes, tribeBest, tribeBestCost] tribalCompetition(...); end convergeCurve(iter) globalBestCost; end end成员合作更新的核心代码如下。注意高斯扰动的标准差异随迭代递减这一点直接对应了“前期广搜、后期精调”的策略。function members memberCooperation(members, tribeLeader, globalBest, iter, maxIter) [tribeSize, dim] size(members); sigma 0.5 * (1 - iter / maxIter) 0.05; % 扰动标准差递减 for k 1:tribeSize % 惯性项自身邻域高斯扰动 inertia members(k,:) sigma * randn(1, dim); % 认知项向部落领袖靠拢 cognitive 0.6 * rand * (tribeLeader - members(k,:)); % 社会项向全局最优靠拢 social 0.4 * rand * (globalBest - members(k,:)); members(k,:) inertia cognitive social; % 边界处理 members(k,:) max(members(k,:), bound(:,1)); members(k,:) min(members(k,:), bound(:,2)); end end部落竞争重组的实现逻辑按部落平均适应度排序排名靠后的部落保留前20%优秀成员其余成员向全局最优区域重新初始化同时压缩该部落的搜索扰动幅度。4.3 代价函数计算的实现细节代价函数是算法里被调用最频繁的部分务必保证高效和正确。我的实现中威胁代价计算是整个代价函数最耗时的部分必须做距离预计算。function cost pathCost(path, obsList, startPos, endPos, params) % path 是航迹点序列含起点终点 % 1. 燃油代价航段长度之和 lenCost sum(sqrt(sum(diff(path).^2, 2))); % 2. 威胁代价航段采样点与障碍物的距离检查 threatCost 0; for seg 1:size(path,1)-1 p1 path(seg,:); p2 path(seg1,:); numSamples 5; for s 1:numSamples pt p1 (p2 - p1) * (s / (numSamples 1)); for o 1:size(obsList,1) dist pointToBoxDist(pt, obsList(o,:)); if dist params.safeDist threatCost threatCost params.threatPenalty * (params.safeDist - dist)^2; end end end end % 3. 高度代价 heightCost sum((path(:,3) - params.idealHeight).^2); % 4. 平滑代价计算各航迹点处转弯角 smoothCost 0; for j 2:size(path,1)-1 v1 path(j,:) - path(j-1,:); v2 path(j1,:) - path(j,:); cosAngle v1 * v2 / (norm(v1) * norm(v2) eps); if cosAngle cos(params.maxTurnAngle) smoothCost smoothCost (cos(params.maxTurnAngle) - cosAngle)^2; end end % 综合代价 cost params.w1 * lenCost params.w2 * threatCost ... params.w3 * heightCost params.w4 * smoothCost; endpointToBoxDist这个函数计算点到长方体障碍物的距离本质上是把点投影到长方体六个面取最近面的距离。如果点在长方体内部则返回负值直接触发大惩罚。实现时注意向量化避免循环里套循环导致运行时间爆炸。5. 仿真实验结果与算法对比5.1 实验场景设定我构建了两个测试场景。场景一是一个相对简单的低密度城区规划区域500米×500米×200米随机布置15栋高度在30到80米之间的建筑。场景二是高密度城区同样大小的区域布置40栋建筑并且部分建筑形成“林荫道”式狭长通道对航迹规划非常不友好。起点设在区域西南角终点设在东北角。飞行高度约束在20到150米理想高度50米。每个方案对比实验都独立运行20次统计最优代价的均值、标准差和平均运行时间避免单次运行的随机性误导结论。5.2 与其他群体算法的收敛速度对比我把CTCM和标准PSO、标准GA放在同样的环境和同样的初始种群规模下对比。PSO种群规模120GA种群规模120CTCM为6个部落×20个成员总种群规模同样是120保证对比公平。算法场景一最优代价均值场景一标准差场景二最优代价均值场景二标准差平均收敛代数PSO1.420.151.880.2495GA1.350.121.760.18128CTCM1.210.061.530.0876从结果看CTCM在两个场景下求得的路径代价都更低标准差明显小于另外两个算法说明算法稳定性更好。收敛速度方面CTCM大约在76代左右就达到了最终质量PSO虽然前期下降快但没有后续提升GA则到后期仍在缓慢爬坡。更直观的差异体现在规划出的航迹形态上PSO规划出的路径在障碍物稀疏区域会有不必要的绕行GA的路径有几处急转弯而CTCM规划出的路径整体平滑在穿越高密度建筑区时能自动选择较宽的间隙而不是贴着楼体强行穿过。5.3 多场景鲁棒性测试为了排除单一场景对结论的影响我接着做了随机化鲁棒性测试生成20个不同建筑布局的随机城区场景每个场景下三种算法各跑10次统计所有场景下的最差情况表现。结果是CTCM在所有20个场景中都能找到可行路径而PSO有3个场景在全部10次运行中都有至少一次陷入了代价极高的不可行解GA有2个场景出现类似问题。CTCM在“最差场景下的最差解”对应的代价也明显优于对比算法。这说明部落竞争机制确实有效避免了整个种群在困难场景下集体陷入局部最优的情况。6. 调参经验与踩坑记录6.1 部落数、成员数与迭代次数的匹配CTCM一共有三个层级的规模参数部落数、部落成员数、总迭代次数。我的经验是部落数不宜超过8个成员数不宜少于15个。部落太多会导致每个部落分到的成员太少部落内部合作形不成气候竞争重组又频繁打断搜索过程成员太少则部落内部的多样性不足领袖的引导作用会变成一种“近亲繁殖”过早失去探索能力。迭代次数和竞争间隔要联动调整。竞争间隔一般设为迭代次数的2%到3%比较合适200代对应5代一次竞争。如果总迭代增加到500代我建议竞争间隔放到10到15代之间。原则是竞争间隔不能小于成员完成一次有效局部搜索所需的最小代数否则竞争机制会退化成为纯粹的随机重启。6.2 竞争强度系数的调节技巧竞争强度系数控制的是排名靠后部落的重组力度。我最初设计了一个“重组比例”参数表示淘汰成员的比例取值范围0到1。实测下来0.8这个值表现最好——保留20%精英重组80%成员。重组比例太高部落等于每次竞争后都是新成员之前的搜索积累全部丢失重组比例太低惩罚力度不够差部落继续在低效区域浪费算力。还有一个容易被忽略的点被重组的部落其成员的初始扰动范围要缩小。原因很简单如果重组后的成员还是在全局范围随机撒点那和重新初始化没有区别竞争机制就失去了“引导资源流向有希望区域”的意义。我通常将重组后成员的扰动范围设置为全局范围的30%并且从下一轮开始让这个范围随迭代递减。6.3 Matlab实现中容易出错的三处细节第一处是航迹点顺序问题。很多人在解码个体向量时把航迹点坐标搞乱比如某条路径中间有两个点坐标完全相同的退化情况导致实际航迹长度变短代价函数给出虚低的数值。我在评估前加了一个去重检查如果相邻航迹点距离小于某个阈值就做一次微小平移保证航迹有效。第二处是威胁代价只检查航迹点不检查航段内部。这个坑我在前面提过但值得再次强调。航迹点之间是直线段算法完全可能在两个航迹点都避开障碍物的情况下让中间的航段从建筑内部穿过。必须在航段上加密采样点检查碰撞。第三处是边界处理与约束处理的顺序。更新航迹点坐标后要先做边界裁剪再做代价评估否则航迹点飞出规划区域后威胁代价计算会出现异常值。我就是因为先算代价后裁剪边界出现过代价函数输出NaN导致整个种群崩溃的问题排查了很久才发现是航迹点越过区域边界后点到障碍物距离计算出现了负值开根号。结尾实际把CTCM算法落地到三维航迹规划项目之后我最大的感受是这个算法的价值不在于某一项机制多新颖而在于“部落竞争”和“成员合作”这两个机制刚好补齐了群智能算法在探索和开发之间的断层。城市地形下的航迹规划问题障碍物分布极不均匀解空间形态复杂恰好需要一个能自动淘汰低效搜索方向、同时保留局部精调能力的框架。最后分享一个调试阶段的实用小技巧先用低分辨率的地形模型把算法跑通比如把建筑物数量减半、航迹点数量从10降到6确认收敛曲线正常之后再逐步加大场景复杂度。我在一开始就直接上40栋建筑的复杂场景每次运行要好几秒改一次参数看一次结果效率极低。换成简化场景后单次运行时间降到一秒以内调参节奏快了很多。这个习惯帮我省下了大量调试时间也推荐给准备上手这个方向的同学。