无人机三维动态避障路径规划：CTCM与DWA分层融合实战-开发者社区

做无人机三维动态避障路径规划这个课题时，我踩过最大的坑是“算法各管一段”：纯全局规划算出的路径看着漂亮，遇到动态障碍物立刻失效；纯局部避障算法反应够快，却经常被地形困住，绕来绕去出不来。后来把部落竞争与成员合作算法（CTCM）和动态窗口法（DWA）按“慢全局、快局部”的思路叠在一起，三维动态避障才真正跑通。这篇文章把我从建模、融合到MATLAB调参的过程完整写出来，代码骨架也一并拆开，给正在做无人机路径规划、准备写论文复现实验的同学一个可直接参考的版本。

1. 三维场景下单一算法的“死穴”：为什么全局规划与局部避障必须联手

无人机三维避障和二维小车路径规划有本质区别。二维只需要考虑平面位置和朝向，三维多了一个高度维度后，障碍物不再只是一个个“圈”，可能是山体、楼宇、悬停的其他飞行器；同时无人机自身还有爬升率、偏航角速率、最小转弯半径等运动约束。任何一个环节没考虑清楚，仿真里飞得再好的路径，拿到实飞环境里都可能直接撞上去。

先看全局规划这头。无论是A*、RRT还是各种群智能优化算法，它们共同的特点是拿到完整环境信息后离线搜一条从起点到目标点的最优路径。这类算法做静态避障非常擅长，比如CTCM这类元启发式算法，可以在三维栅格地图上找到一条长度短、平滑度高、远离障碍物的全局路径。但问题也出在“全局”两个字上：地图里没有出现的动态障碍物，算法根本感知不到；就算感知到了，让全局规划算法在一个大范围地图里快速重算，计算代价也非常高，很难达到实时性要求。

再看局部避障这头。DWA这类算法不依赖全局地图，它只根据当前无人机速度、加速度和传感器探测范围内的障碍物信息，在速度空间里现场采样并选出一条无碰撞、符合运动学约束的轨迹。优点是响应快，动态障碍物逼近时能立刻减速绕开；缺点是没有全局视野，容易陷入局部极小区域——比如无人机被几个障碍物围在中间，DWA可能反复左右试探就是找不到出口，表现为原地打转或者速度趋近于零。

这就是单一算法的“死穴”。我最早只在全局层做避障，动态障碍物一动，路径就必然需要重算，一次重算几十秒，根本没法看。后来试着只用DWA局部避障，静态场景还能凑合，一旦地形复杂一点，无人机就被卡死在障碍物包围圈里。

所以最终采用的分层思路很简单：CTCM跑在慢时间尺度上，负责全局寻路，给无人机一条宏观的飞行走廊；DWA跑在快时间尺度上，负责局部跟踪和动态避障，实时响应突发威胁。两者通过一组航路点和局部虚拟目标点衔接。这个组合不是简单把两个算法串起来，而是让它们在“规划周期”和“决策空间”上互补。

2. CTCM算法的建模思路：部落竞争与成员合作如何同时兼顾探索与收敛

CTCM这个名字看起来陌生，但它本质上是一种受群体协作启发的元启发式优化算法。核心思想是把整个种群分成多个“部落”，部落之间通过竞争机制淘汰弱者，部落内部通过成员合作机制精细搜索。我用它做三维路径全局规划时，最重要的体会是：它比普通粒子群算法多了一个“部落级”中间结构，这个结构让算法在探索广度和收敛精度之间找到了不错的平衡。

2.1 部落竞争：种群多样性的保持机制

先定义基本数据结构。假设种群总规模为 N，分成 N_tribe 个部落，每个部落有 N_member 个成员，那么 N = N_tribe × N_member。每个成员在三维地图里的位置就是一条候选路径的一个节点序列，或者更直接一点，把一条完整路径编码成一串坐标点，每个坐标点作为解向量的一个分段。

适应度函数我这样设计：一是路径总长度，二是路径与障碍物的最小距离惩罚，三是路径平滑度（相邻线段夹角越小越好），四是高度约束（不能飞太高也不能贴地）。对三条待选路径排序后，每轮迭代里比较各部落最优成员的适应度，最差的那个部落会被“淘汰”——淘汰不是说把这个部落删除，而是解散它，让它的成员按一定比例分配到其他部落中去。这个过程很像自然选择里的迁移机制。

竞争强度是可以用参数控制的。我常用竞争周期 T_comp 表示每经过多少代做一次部落淘汰重组，一般设5到10代一次。如果淘汰太频繁，种群波动剧烈，容易丢失已找到的较优区域；淘汰太慢，竞争机制就形同虚设。实测下来，三维地图尺寸在100×100×50栅格规模时，T_comp取8代比较稳。

部落竞争的数学表达可以用一个简单的排序选择来实现。假设第 i 个部落的最优适应度为 f_i_best，在竞争回合中按适应度从好到差排列，前面一半部落获得吸收被淘汰成员的权利，后面一半部落进入淘汰候选池。被淘汰部落的成员在吸收部落里重新初始化时，不是完全随机撒点，而是以吸收部落最优解为中心加高斯扰动生成，这样既能带来新信息，又不会让整个种群突然散开。

2.2 成员合作：局部搜索精度的提升手段

部落内部合作是CTCM在局部精细搜索上的主要来源。每个部落都有一个“首领”，也就是当前部落最优成员。普通成员在每轮迭代中更新自己的位置时，参考信息有三个：部落首领的位置、全局最优成员的位置、自身历史最优位置。这一步和粒子群很接近，但关键区别在于，CTCM里的合作是发生在部落这个中间集合上的，不同部落之间的成员不能跨部落直接共享位置信息，只能通过竞争重组间接传播。

更新公式用标准位置更新骨架，但扰动幅度会随着迭代次数自适应减小。具体做法是：当前迭代次数为 iter，总迭代次数为 T_max，步长缩放因子 scale = exp(-3 × iter / T_max)。新位置 = 部落首领位置 + scale × (全局最优位置 - 部落首领位置) + (0.5 - rand) × scale × 搜索半径。这样迭代前期步长大，成员敢于在部落周围大范围搜索；后期步长缩小，所有成员都集中在首领附近做精细打磨。

除了这种连续更新，我还加了“成员两两交叉”的合作方式。在同一个部落内随机选两个成员，按路径节点交叉生成两个新解，保留其中更优的一个。这个交叉操作本质上是把路径的不同区段重组，对路径平滑性很有帮助——因为一条路径的某个区段可能在成员A身上已经优化得很好，通过交叉可以直接把这段“优秀基因”平移到成员B的路径里。

2.3 CTCM的算法流程与复杂度分析

完整的CTCM算法流程可以概括为：

初始化：随机生成 N 个成员，划分为 N_tribe 个部落，计算每个成员的适应度。
部落内更新：每个普通成员根据首领、全局最优和自身历史最优更新位置。
部落内交叉：随机选成员做路径节点交叉，保留较优解。
部落竞争：判断是否到达竞争周期，如果是，按部落最优适应度排序，淘汰最差部落并重组。
全局最优更新：如果当前全局最优连续多代没有改善，触发一次“重新初始化首领附近的成员”的操作。
判断终止条件：到达最大迭代次数或满足精度要求则输出全局最优路径。

从复杂度来看，CTCM每轮迭代主要计算量集中在适应度函数评估上。假设种群规模为 N，解维度为 dim（路径节点数量），障碍物数量为 O，单次适应度计算复杂度约为 O(dim × O)，那么一轮迭代的总复杂度是 O(N × dim × O)。这个量级和粒子群算法相当，没有额外引入明显瓶颈。我实测100个成员、每个成员路径20个节点、几十个障碍物的场景下，100代迭代在MATLAB里跑完大约需要几秒到十几秒，完全可以接受。

3. 把DWA搬进三维：速度窗口、轨迹推演与评价函数的三处关键改造

DWA在二维环境下已经非常成熟，网上能找到大量版本。但直接把它用到三维无人机上，会遇到三个绕不开的问题：速度空间变大了，轨迹推演变复杂了，评价函数不知道该把哪些项放进去。我一步步说清楚我是怎么改的。

3.1 从二维速度空间到三维运动空间

二维DWA里，速度窗口通常只采样两个量：线速度 v 和角速度 ω。采样范围由当前速度、最大加速度和仿真周期共同确定。三维场景里，如果严格按无人机的六自由度模型来采样，要考虑线速度三分量 (vx, vy, vz)、偏航角速率、俯仰角速率、滚转角速率，状态空间直接爆炸，实时性肯定完蛋。

我的做法是做一个“三维质点运动模型 + 偏航角”的简化。控制量为三个方向的线速度 (vx, vy, vz) 和偏航角速率 ω_yaw，无人机姿态角通过简化动力学映射到线速度变化上，不在底层采样里展开。这样做的好处是算法核心还是DWA那一套“窗口采样—轨迹推演—评价选择”的流程，不增加维度灾难。

动态窗口的具体生成逻辑如下：当前时刻无人机实际速度是 v_now = [vx_now, vy_now, vz_now]，三个方向的最大加速度是 a_max，控制周期是 dt，那么下一时刻每个方向可达的速度范围是 [v_now_i - a_max_i × dt, v_now_i + a_max_i × dt]。除此之外还要和无人机本身的物理极限速度求交集，比如 vx 和 vy 的合成水平速度不能超过 25 m/s，垂直速度不能超过 5 m/s，偏航角速率不能超过 0.8 rad/s。

采样分辨率我常用的是：水平速度步长 1 m/s，垂直速度步长 0.5 m/s，偏航角速率步长 0.2 rad/s。二维DWA采样几百个速度对已经很花时间了，三维如果直接穷举所有组合，很容易到几千甚至上万。所以我在实现里用了一个技巧：先分离水平和垂直采样，水平方向选出一批候选速度，垂直方向选出一批候选爬升率，再按一定比例组合成完整候选集。比如水平选30个、垂直选10个，组合后300个候选，控制在一个可接受的范围。

3.2 碰撞检测与轨迹推演的三维化处理

有了候选速度，下一步就是推演“如果无人机以这个速度飞行，接下来几秒会走出一条什么样的轨迹”。我采用的推演方式是离散积分：从当前状态出发，以 dt = 0.1 秒为步长，按动力学模型逐步积分出未来 T = 3 秒内的轨迹点序列。

这里有一个容易被忽略的细节：轨迹推演不是简单把速度乘以时间，而是要考虑偏航角速率对运动方向的影响。每一步的状态更新是：

先根据当前偏航角 θ_yaw 和候选线速度水平分量 v_horiz，算出水平方向上的速度分量 vx = v_horiz × cos(θ_yaw)，vy = v_horiz × sin(θ_yaw)。
然后偏航角累加：θ_yaw_new = θ_yaw + ω_yaw × dt。
垂直速度 vz 直接叠加到 z 坐标上。

碰撞检测环节，障碍物我会统一转换成球体、圆柱或长方体的组合。对轨迹上的每个点，计算它到每个障碍物表面的最近距离。如果这个距离小于无人机安全半径（无人机半径 + 安全余量），就标记为碰撞轨迹，直接淘汰。注意这里要同时考虑静态障碍物和动态障碍物：动态障碍物我会假设其在推演时间窗内保持匀速直线运动，推演时同步更新障碍物的预测位置。这个假设在实际场景里不算严格，但对于仿真验证和多数低空场景已经足够。

3.3 评价函数的重新设计：方位角、障碍物距离、高度差与全局路径跟随

传统DWA的评价函数通常由三项构成：目标方位角评价、障碍物距离评价、速度评价。三维版本里，我保留了这三项，又增加了两个新的评价项：高度差评价和全局路径跟随评价。

各项的具体定义如下：

目标方位角项 heading：当前推演轨迹末端点与局部目标点之间的水平夹角偏差，夹角越小评价越高。
障碍物距离项 dist：当前轨迹与最近障碍物的最小距离，距离越大评价越高。如果距离小于安全阈值，该项直接压到最低分。
速度项 vel：候选速度的绝对值大小，用来保证无人机尽量快速前进，避免因为过度避障而龟速爬行。
高度偏差项 height：轨迹末端高度与当前目标高度之间的偏差，偏差越小越好。这一步主要是为了防止无人机为了绕障而不断爬升或俯冲，导致高度失控。
全局路径跟随项 follow：当前推演轨迹与CTCM给出的全局参考路径之间的横向和垂直偏差，这个偏差越小，说明DWA当前选的轨迹越贴合全局规划的“大方向”。

最后的总评价值是五项加权和，权重分别为 α、β、γ、δ、ε。这里必须强调，五项值的量纲完全不同，不归一化就加权等于白加。我先把五项分别做归一化处理（每项除以当前候选集里的最大值），再乘权重相加。我跑下来一组比较稳的权重是 α = 0.15、β = 0.4、γ = 0.1、δ = 0.15、ε = 0.2。障碍物距离权重最大，因为安全永远排在第一位；速度权重最小，因为避障场景里不能为了快而牺牲安全性。

4. CTCM-DWA融合的切换策略与全局-局部协同机制

两个算法单独能跑通之后，真正的难点来了：怎么把它们接起来，才能既保证全局方向不偏，又能在动态障碍物逼近时快速响应。融合不好会出现两种常见问题：一是全局路径和局部避障“对着干”，无人机在原地左右摇摆；二是局部路径已经明显偏离全局走廊了，CTCM还浑然不觉。

4.1 全局航路点与局部目标点的动态映射

CTCM输出的全局路径是若干个离散航路点 P = [p0, p1, ..., pn]。如果直接把下一个航路点塞给DWA让它追，会导致无人机飞行轨迹非常生硬——每到一个点就猛转向下一个点。所以我设计了一个“局部虚拟目标点”的映射机制：每次DWA计算前，先找到无人机当前位置在全局路径上的最近投影点，然后沿着全局路径方向往前取一个前瞻距离 L_ahead，这个位置就作为当前DWA的局部目标点。

前瞻距离需要自适应变化。我发现如果 L_ahead 太大，DWA只盯着远方，遇到近处突发障碍物时反应迟钝；如果 L_ahead 太小，无人机又会被全局路径的每一个小拐点牵着走，飞行不流畅。我采取的方法是让 L_ahead 与当前剩余路径长度和障碍物密度相关：剩余路径长、障碍物稀疏时就取大一点；障碍物密集时就取小一点。基准值一般取 8~12 米。

这样做还有一个额外的好处：当动态障碍物迫使DWA横向绕开时，局部目标点不会跟着变，无人机绕过去之后会自动回到全局路径附近，而不是越绕越偏。这种“指南针”作用正是纯DWA不具备的。

4.2 动态障碍物威胁下的重规划触发条件

局部绕障能处理大部分动态威胁，但它有处理不了的情况——比如动态障碍物完全挡在全局路径必经的狭窄通道里，或者无人机被多个移动障碍物围堵。这时候必须触发一次全局重规划，也就是让CTCM在更新后的地图信息上重新跑一遍。

重规划不能乱触发。我最初的设计是“检测到全局路径被阻挡就立刻重算”，结果算法每两秒重算一次，路径还没飞完就又变了，无人机反而一直在原地急转弯。后来我改成了三重条件判断，只有满足其一才触发：

条件一：DWA连续 30 个控制周期内，评价函数最优值持续低于某个阈值，说明局部规划已经找不到像样的速度组合。
条件二：无人机实际速度连续 2 秒低于 2 m/s，且周围障碍物距离都在安全半径附近，说明大概率被卡死了。
条件三：全局路径上的某个航路点被新出现的静态障碍物覆盖，且无法通过局部绕行规避。

三个条件有一个命中，就把当前时刻的地图障碍物信息同步给CTCM，重新初始化种群并启动竞争合作优化。为了不让重规划影响飞行安全，重规划期间DWA继续以减速和保持当前轨迹为主，不执行激进避障动作。

整个控制架构是典型的双时间尺度：CTCM重规划周期在 3~5 秒这个量级，DWA控制周期在 0.1 秒量级。两者通过共享的“全局路径 + 局部目标点”数据结构对接，不需要频繁交互。

5. MATLAB代码实现的骨架拆解

这一节我把代码结构完整拆开，按CTCM部分、DWA部分和融合主循环三块来讲。代码不完全贴全文，但关键逻辑和核心函数会给出来，结构足够你照着搭。

5.1 CTCM部分的代码结构

CTCM部分我按函数文件拆分，调试起来方便。

CTCM_main.m：算法主入口，读地图、设参数、调用其他函数。
initialize_tribes.m：初始化部落及成员，返回结构体数组。
tribe_competition.m：部落竞争与淘汰重组。
member_cooperation.m：部落内成员位置更新与交叉。
fitness_function.m：计算路径适应度，内部调用碰撞检测和路径平滑度计算。

每个成员在MATLAB里我用一个结构体表示，关键字段是pos（路径节点坐标序列，大小为 dim×3），fitness（适应度值），is_leader（是否为首领）。部落结构体则包含members数组和best_fitness。

核心适应度函数大致是这样的逻辑：

function fit = fitness_function(path, obstacles, start_point, goal_point) % path: n*3 矩阵，n个路径点 % obstacles: 结构体数组，包含障碍物类型、中心坐标、尺寸 len = sum(sqrt(sum(diff(path).^2, 2))); % 路径长度 smooth = sum(angle_penalty(path)); % 平滑度惩罚 [min_dist, collided] = cal_min_distance(path, obstacles); height_penalty = sum(max(0, path(:,3) - max_height)) + ... sum(max(0, min_height - path(:,3))); if collided fit = 1e10; % 碰撞路径直接给极大惩罚 else fit = len + 0.8*smooth + 50/max(min_dist, 0.1) + height_penalty; end end

成员合作更新我用一个向量化的方法写，比逐个循环快很多。伪代码如下：

for iter = 1:T_max scale = exp(-3 * iter / T_max); for t = 1:N_tribe leader = tribes(t).best_pos; for m = 1:N_member new_pos = leader + scale * (global_best_pos - leader) ... + (0.5 - rand(size(leader))) .* scale .* search_radius; new_fit = fitness_function(new_pos, obstacles, start, goal); if new_fit < member.fitness member.pos = new_pos; member.fitness = new_fit; end end end if mod(iter, T_comp) == 0 tribes = tribe_competition(tribes, global_best_pos); end end

这里有个细节：0.5 - rand(size(leader))产生的扰动方向是随机的，保证成员不总是单向偏移。search_radius根据地图大小设定，在地图对角线长度的 1/10 左右比较合适。

5.2 DWA三维部分的代码结构

DWA部分我同样是拆函数写：

DWA_3D_core.m：DWA主逻辑，输入当前状态、局部目标、障碍物、全局路径，输出最优速度和偏航角速率。
generate_window.m：根据当前速度和加速度限制生成速度窗口。
predict_trajectory.m：对每个候选速度做轨迹推演。
evaluate_trajectory.m：对轨迹做五项评价，返回归一化评分。
collision_check.m：三维碰撞检测。

动态窗口生成核心代码片段：

% 当前状态 vx_now = state(4); vy_now = state(5); vz_now = state(6); yaw_rate_now = state(7); % 三个方向可达速度范围 vx_min = max(vx_now - ax_max * dt, -vx_limit); vx_max = min(vx_now + ax_max * dt, vx_limit); % vy、vz类似操作 % 偏航角速率范围限制 % 生成候选速度网格 vx_cand = vx_min:vx_res:vx_max; vy_cand = vy_min:vy_res:vy_max; vz_cand = vz_min:vz_res:vz_max; yaw_cand = yaw_min:yaw_res:yaw_max; % 组合采样（水平降维后组合） [VX, VY, YAW] = ndgrid(vx_cand, vy_cand, yaw_cand); cand_num = numel(VX); % 对每个组合对应一个VZ，这里可以按比例随机分配或按目标高度差选

轨迹推演和评价我写成对矩阵操作，尽量不用循环。轨迹推演的核心就是前面提到的动力学积分，每一步根据偏航角更新速度方向。

评价函数最后一步选择最优候选速度：

scores = alpha * norm_heading + beta * norm_dist + ... gamma * norm_vel + delta * norm_height + epsilon * norm_follow; [~, best_idx] = max(scores); best_vel = [VX(best_idx), VY(best_idx), VZ(best_idx), YAW(best_idx)];

5.3 融合主循环与数据结构设计

融合主循环是整个仿真的发动机。我把它放在fusion_simulation.m里，基本流程是：

初始化地图、无人机状态、CTCM参数、DWA参数。
调用CTCM算出初始全局路径。
进入主循环：
- 更新无人机状态和传感器信息（仿真里是获取障碍物真实位置）。
- 调用local_target_update.m计算当前局部虚拟目标点。
- 调用DWA_3D_core.m得到最优控制量。
- 更新无人机位置，记录轨迹。
- 检查是否满足重规划条件，如果是则重新调用CTCM更新全局路径。
- 判断是否到达目标点。
输出结果和绘图。

数据结构上，我习惯把所有状态打包成一个state结构体：位置、速度、偏航角、时间戳、已经过的路径长度。障碍物则是结构体数组，每个元素包含类型、位置、速度（动态障碍物用）、尺寸。CTCM返回的全局路径是global_path矩阵，DWA每一步都从这里取局部目标点。

运行效率方面，MATLAB里最容易拖垮速度的是嵌套循环。一个优化技巧是：碰撞检测用向量化方式计算轨迹点与障碍物中心距离，利用pdist2或者广播运算，一次算出所有轨迹点到所有障碍物的距离矩阵，再判断最小值是否小于安全半径。我们后面实测中，单个DWA控制周期在几百个候选速度下能压到 10 毫秒以内。

6. 仿真验证与调参实测：迭代次数、权重系数和动态窗口大小的影响

代码搭起来之后，最难的是调参。我把整个验证过程分成了静态场景和动态场景两个阶段，分别做了参数敏感性测试，踩了不少坑才摸出一套相对稳定的参数组合。

6.1 仿真场景与评价指标

我搭了两个验证场景。

场景一是静态多障碍物山谷地形的三维栅格地图，障碍物包括两个长方体山体、三个球形障碍物和一片圆柱形高层建筑群，地图范围 100×100×50 米。这个场景主要验证CTCM全局寻路能力和路径质量。

场景二是在场景一基础上加入三个动态障碍物，其中两个做匀速直线运动，一个做周期往返运动。主要验证DWA局部避障响应速度以及融合系统的整体成功率。

评价指标选了五个：路径总长度、平均飞行速度、最小障碍物净距、单次规划耗时、任务成功率（无人机从起点到终点不碰撞且不超时的概率）。成功率我在每个场景跑20次随机种子统计，避免一次实验的偶然性。

6.2 参数敏感性实测

CTCM部分的迭代次数，我分别试了50、100、200、500代。50代时路径长度明显偏大，路径节点之间还经常出现突兀拐角，说明收敛不充分；100代往后路径长度趋于稳定，优化曲线在60~80代左右基本平缓，提升空间只在最后10%以内。所以三维地图100×100×50规模下，CTCM迭代100~150代是性价比最高的区间，不用追求500代。

部落数量的影响更微妙。我固定总种群数40个成员，测试部落数分为4、5、8、10。部落数太少，成员合作过于集中，容易早熟；部落数太多，每个部落只有4~5个成员，竞争压力不足，搜索精度下降。测试下来部落数取5、每个部落8个成员时，平均路径长度最短且方差最小。

DWA权重里，β（障碍物距离权重）影响最大。β从0.2提到0.4，最小障碍物净距明显增大，但路径长度也跟着增加10%左右，因为无人机会更早绕开障碍物而不是贴着边界穿行。β超过0.5之后，无人机在开阔地图里也会频繁绕弯，速度指标下滑。最终β取0.4是安全和效率的折中点。

动态窗口大小的影响主要体现在实时性上。采样分辨率太高，候选速度数量爆炸，单个DWA周期经常超过50毫秒；采样分辨率太低，轨迹覆盖率不足，会出现“明明旁边有安全通道但窗口里没有对应速度”的问题。最后用水平速度步长1 m/s、垂直速度步长0.5 m/s、偏航角速率0.2 rad/s，单周期可以稳定在30毫秒内完成。

一组典型仿真结果如下表（20次随机种子的统计均值）：

场景	路径长度(m)	平均速度(m/s)	最小净距(m)	单次CTCM耗时(s)	DWA单周期耗时(ms)	成功率
静态场景	156.3	8.2	1.8	8.6	22	100%
动态场景	172.8	7.1	1.5	9.2	26	95%

6.3 需要避开的几个坑

第一个坑是三维速度采样组合爆炸。刚开始做的时候，我把vx、vy、vz、yaw四维直接ndgrid，候选速度数量一下到了几千个，每个候选还要推演20个轨迹点，MATLAB跑一个周期要好几百毫秒。后来改成水平垂直分步采样，再把候选数量压到300~400个，才勉强达到实时。

第二个坑是动态障碍物的碰撞检测必须“预测未来”。我第一版DWA只根据当前时刻的障碍物位置做碰撞检测，结果无人机速度高的时候总感觉反应慢半拍，后来才意识到，轨迹推演的是未来几秒的状态，碰撞检测也应该用障碍物在未来几秒的预测位置去匹配轨迹点。这个改动让动态场景成功率从75%提高到了95%。

第三个坑是评价函数不归一化等于白搭。我最初把五项原始值直接加权求和，结果速度项数值大，几乎主导了所有决策，无人机总是选最大速度，完全不顾障碍物距离。把所有项都先除以当前候选集的最大值，让评价量纲统一到0~1之后，各权重才能真正发挥调节作用。

第四个坑是全局CTCM和局部DWA的评价标准不统一。CTCM的适应度函数偏重路径长度和平滑度，对“能不能实时避开动态障碍物”完全不敏感；DWA又天然对全局路径抱有不信任感。两者场景差异大时，无人机整体路径虽然安全，但偶尔会出现“全局路径走左边、DWA却绕到右边”的不一致情况。后来我在CTCM适应度里也加了“到动态障碍物预测轨迹的最近距离”作为软约束，让全局路径天生避开高频活动区域，问题才明显缓解。