先说明一下，这篇内容是我在做一个“动态环境下多无人机协同路径规划与防撞”仿真项目时的完整记录。项目本身是纯科研性质的仿真验证平台，用 Matlab 实现，主要解决这样几个问题：多架无人机在同一个空域内执行任务时，怎么协同规划出各自安全可行的路径；当环境中有动态障碍物（比如突然出现的风扰区域、移动的空中物体）时，路径怎么实时更新；多机之间怎么避免互相碰撞，同时又不会因为过度避让导致任务效率崩掉。如果你正在做毕业设计、无人机集群方向的课题，或者刚开始接触多智能体系统的路径规划算法，这篇文章应该能给你省下不少踩坑时间。
先说结论：动态环境下的多无人机协同路径规划，绝对不是简单地把单机路径规划算法复制几十遍。真正的难点在于“协同”两个字——多机之间共享空域时，每一架的决策都会影响其他所有飞机的可行空间，这种耦合关系才是问题的核心。我在仿真里用的思路，是把问题拆成两层：底层用改进后的路径规划算法给每个无人机生成参考路径，上层用协同避障策略处理多机之间的冲突和动态障碍。下面我把整个项目从设计思路、算法原理、Matlab 实现到调参经验，完整拆给你看。
1. 项目整体设计思路与问题拆解
1.1 动态环境下多无人机协同的核心矛盾
任何多无人机系统，只要在同一片空域活动，就必须回答三个问题：
- 每架无人机去哪里：任务分配和路径规划，这是“做什么”。
- 怎么走才安全：不仅要绕开静态障碍物，还要绕开动态障碍物和其他无人机，这是“怎么做”。
- 怎么保证效率：如果每架飞机遇到潜在碰撞就原地悬停或者绕大圈，安全是安全了，但任务完成不了，这是“怎么做得更好”。
这三个问题在动态环境下纠缠在一起，比静态环境的单机路径规划复杂很多。静态环境里，障碍物位置是已知的、固定的，规划一次路径可以用很久。但动态环境里，障碍物在动、其他飞机也在动，今天上午规划好的飞行航线，下午可能就不适用了。所以从设计层面讲，项目必须包含两块核心能力：一是感知与预测，要知道周围环境发生了什么变化、其他无人机的意图是什么；二是在线重规划，要在极短的时间内算出新的可行路径。
我在项目里做的一个重要简化是：把动态障碍物分为“运动的障碍物”和“其他无人机”两类。运动障碍物主要表现为位置的连续变化，其他无人机则不仅位置在变，它们的运动意图也会因为协同策略而变。这两类对象的处理方式不同，后面会详细讲。
1.2 为什么选择 Matlab 作为实现环境
选择 Matlab 做这个项目，主要是因为两个原因。第一是算法验证效率高，无论是路径规划算法（A*、RRT、人工势场法）还是避障策略（速度障碍、模型预测控制），Matlab 都有足够的工具箱支撑，特别是自动驾驶工具箱和机器人系统工具箱，很多基础函数可以直接复用。第二是可视化调试能力强，多无人机系统的调试非常依赖对空间关系的直观判断，Matlab 的图形界面能快速把每架无人机的轨迹、速度矢量、障碍物位置画在一张图上，调参的时候能节省大量精力。
不过 Matlab 也有它的局限性——实时性远不如 C++，所以这个项目定位是算法验证平台而非机载部署系统。真正上机部署时，同样的算法要用 C++ 或者 Python 重写，但算法逻辑本身完全可以在 Matlab 里验证。
1.3 项目模块划分
我把整个项目拆成四个模块，每个模块各司其职：
- 任务与场景模块：定义无人机数量、起降点、任务目标点、静态障碍物分布、动态障碍物运动轨迹。
- 路径规划模块：为每架无人机生成从起点到目标点的初始路径，并在环境变化时触发重规划。
- 协同避障模块：检测多机之间的冲突，以及无人机与动态障碍物之间的冲突，生成避让指令。
- 仿真控制模块：管理整个仿真循环，推进时间、更新状态、记录数据、绘制图像。
这个模块划分其实也对应了项目的代码结构。我见过有些同学把所有函数堆在一个脚本里，几百行代码没有函数封装，到最后连自己都看不懂。模块化设计的好处不仅是代码清晰，更重要的是——你可以单独测试每个模块，哪个环节出问题一眼就能定位。
1.4 坐标系与运动模型的选择
多无人机系统的仿真，坐标系和运动模型必须从一开始就定好，否则后面所有代码全废。我采用的是二维平面运动模型，因为很多协同路径规划的验证场景在二维空间里就足够了，包括编队飞行、多点巡查、区域搜索等典型任务。
每架无人机的状态定义为[x, y, vx, vy]，x 和 y 是位置坐标，vx 和 vy 是速度分量。运动学模型采用常见的内聚控制模型，也就是可以抽象为：
dx/dt = vx dy/dt = vy dvx/dt = ax dvy/dt = ay其中ax和ay是控制量输入，在实际系统中由飞行控制器提供。仿真中我们直接控制加速度量，然后通过积分得到速度和位置。
为什么要用加速度作为控制量而不是直接用速度？因为真实的无人机无法瞬时改变速度，任何飞行器都有动力学约束。如果直接用速度作为控制量，你规划出来的路径看上去很漂亮，但实际飞行器根本飞不出来，因为速度突变做不到。在仿真阶段就把动力学约束加进去，后续把算法迁移到真机时能少走很多弯路。
2. 核心算法原理与实现方案
2.1 路径规划的基线算法：改进的 A* 算法
项目里的路径规划基线算法用的是 A*，但直接拿教科书上的 A* 来用是不够的，原因有两点：第一，标准 A* 只适用于静态环境，环境一变就要全图重算，效率太低；第二，标准 A* 规划出的路径会有很多尖锐拐角，不符合无人机的运动学约束。
我做了两个关键改进：
第一，改成增量式搜索。当环境中某个区域发生变化时，只更新受影响节点的代价估值，而不是整张栅格地图全部重算。这样在动态环境下，大部分时间只需要局部调整路径，计算效率提升非常明显。
第二，路径平滑处理。A* 出来的路径是一系列栅格节点，直接用会产生折线轨迹。我用 B 样条曲线对路径进行平滑处理，把折线变成满足曲率约束的平滑曲线。这个处理对后面的轨迹跟踪很重要——如果你给跟踪控制器的是一条带尖角的路径，控制器会非常痛苦，不断过冲、振荡，最后轨迹质量很差。
代价函数的设计也需要仔细考量。常见的 A* 代价函数是：
f(n) = g(n) + h(n)g(n)是起始节点到当前节点的实际代价，h(n)是当前节点到目标节点的启发式估计。我在g(n)里加入了两个额外项：一个是对路径长度的惩罚，一个是对无人机转弯角度的惩罚。转弯惩罚非常关键——在动态环境里，频繁大角度转向不仅消耗能量，还会增加与其他无人机冲突的概率。
2.2 动态避障算法：速度障碍法
路径规划和避障在项目中是两层的概念。路径规划回答的是“这条路走不走得通”，避障回答的是“当前这个速度向量能不能继续保持”。动态环境下，避障算法我更倾向于用速度障碍法（Velocity Obstacle，简称 VO）。
速度障碍法的核心思想是：在当前时刻，如果你以某个速度向量运动，而另一个物体（可能是动态障碍物或者其他无人机）也以某个速度向量运动，那么在未来的某一时刻你们可能会进入同一个位置。把所有会导致碰撞的速度向量集合起来，就是“速度障碍区域”。避障算法的任务就是在速度障碍区域之外，寻找一个最优的可行速度向量。
Matlab 实现速度障碍法的步骤如下：
- 计算无人机当前位置到障碍物（或其他无人机）的相对位置矢量。
- 根据两者的速度矢量，计算相对速度。
- 判断相对速度是否指向以障碍物为中心、以安全距离为半径的圆内。
- 如果指向圆内，说明存在碰撞风险，则需要调整速度向量。
- 在避让速度集合中选一个既能远离速度障碍区域，又尽可能接近期望速度的向量。
实际仿真里还有一个非常重要的细节：必须给速度障碍区域留出“时间窗口”。也就是说，不是未来任何时刻可能撞上都要立刻避让，而是只考虑最近的一段时间（比如未来 3 秒）内可能发生的碰撞。这样做的原因是，如果对过于遥远的碰撞也做出反应，无人机会因为过度敏感而不断调整速度，整个系统会变得非常抖动。这个时间窗口参数需要根据无人机速度和任务类型来调，我后续会在调参部分详细讲。
2.3 速度障碍法的两个改进点
标准的速度障碍法有个毛病：它假设障碍物保持当前速度不变，这在实际动态环境中不成立。我做了两个改进。
改进一：加入速度不确定性。当预测障碍物位置时，不再假设它是一个确定的点，而是假设它存在一个位置不确定性范围。这个不确定性随预测时间增长而增大，时间越远，不确定性越大。反映到算法上，就是把速度障碍的“安全圆”半径扩大，扩张量与时间窗口成正比。
改进二：结合一致性避让规则。当两架无人机互相接近时，如果都同时往同一侧避让，反而更容易撞上。我引入了一个简单的规则：在仿真开始时给每架无人机编号，编号较小的无人机在冲突中保持当前速度或只做小幅调整，编号较大的无人机负责大幅避让。这个规则虽然简单，但在多机协同里极其有效，它避免了“两架飞机都让、结果让出二次冲突”的尴尬局面。
2.4 协同层的设计：集中式与分布式折中
多无人机协同的控制架构，学术上一般分为集中式、分布式和分层式三种。集中式由中央节点统一计算所有飞机的路径，好处是全局最优，坏处是计算量爆炸，而且中央节点一挂全部瘫痪。分布式每架飞机各自规划，好处是鲁棒性强，坏处是容易出现全局冲突。我的项目选择了分层分布式架构：无人机之间通过通信协议交换状态信息，但每架无人机本地运行自己的规划与避障算法。
这个架构的选择还基于一个现实考虑：真实的无人机集群通信带宽有限，不可能把所有飞机的全部状态都实时传递给中央节点。分层分布式只需交换关键状态信息（位置、速度、目标点），就能达到较好的协同效果。Matlab 里模拟通信交换很容易，直接定义全局共享变量或对象数组就行，但代码结构上我还是按“各自独立求解”的方式组织。
2.5 冲突检测与消解策略
冲突检测不是等到飞机碰上了才做，而是要在空间上和时间上提前预判。我在仿真里实现了一个两层冲突检测机制：
- 空间冲突检测：实时检查两架无人机之间的距离是否小于安全距离阈值。这是兜底机制，类似于紧急刹车。
- 时间冲突检测：根据当前速度和位置，预测未来一段时间内两架飞机的最小距离。如果预测的最小距离小于安全阈值，则提前触发避让规划。
时间冲突检测的算法很简单：假设无人机的速度在极短时间内保持不变，那么相对位置矢量与相对速度矢量的叉积可以算出二者之间的最短距离，最短距离对应的时刻也可以估算出来。具体公式：
相对位置: d = p_j - p_i 相对速度: v = v_j - v_i 最短时间: t_min = - (d · v) / |v|^2 预测最小距离: d_min = |d + v * t_min|如果d_min小于安全阈值且t_min小于设定的预测时间窗口，就触发避障逻辑。这个算法计算量极小，非常适合实时循环调用。
冲突消解策略采用“优先级 + 代价最小”的组合方式。优先级规则决定谁让谁，代价最小规则决定让到什么程度。具体实现时，把“偏离期望速度的平方和”作为代价函数，然后在可行速度集合中找一个代价最小的解。这样既能保证安全，又不会让无人机绕太远的路。
3. Matlab 代码实现与关键细节
3.1 代码整体结构与数据流
项目的代码目录结构建议按我下面的方式组织，这是我实验中试过比较好维护的方案：
drone_coop_sim/ |-- init_scenario.m // 场景初始化 |-- main_sim.m // 主仿真循环 |-- path_planning/ | |-- a_star_path.m // A*路径规划 | |-- bspline_smooth.m // 路径平滑 |-- collision_avoidance/ | |-- velocity_obstacle.m // 速度障碍计算 | |-- conflict_detect.m // 冲突检测 |-- control/ | |-- controller.m // 轨迹跟踪控制 |-- visualization/ | |-- plot_frame.m // 仿真画面绘制 | |-- record_trajectory.m // 轨迹记录主仿真循环的数据流是这样的：每次时间步长开始时，更新所有无人机的位置和速度状态，然后执行冲突检测，如果有冲突风险，调用速度障碍法计算避让速度，再结合控制器的期望速度一起生成加速度指令，最后更新状态并绘图。
这里我要特别提醒一点：避障模块和路径规划模块不要互相调用。我最初犯过一个错误，避障算法发现前方有障碍物时直接重新调用路径规划算法，这会带来两个问题：一是计算量大，A* 一次规划可能消耗几百毫秒，在多机场景下根本跑不动；二是导航路径的频繁变化会导致无人机飞行轨迹摇摆不定。正确的做法是：路径规划只在任务目标变化或者环境出现重大变化时触发，轻微的避让动作完全由避障模块完成。
3.2 场景初始化与无人机建模
init_scenario.m里面定义这些关键参数：
% 无人机数量 n_drones = 5; % 地图尺寸 map_width = 100; % x方向 m map_height = 100; % y方向 m % 起点与目标点 start_pos = [10, 10; 20, 80; 40, 30; 60, 70; 80, 20]; goal_pos = [90, 90; 80, 10; 60, 60; 30, 80; 15, 50]; % 无人机参数 uav_speed_max = 5; % 最大速度 m/s uav_accel_max = 2; % 最大加速度 m/s^2 safe_radius = 3; % 安全距离半径 m time_horizon = 3; % 冲突预测时间窗口 s % 静态障碍物 obs_static = [30, 30, 4; 50, 50, 3; 70, 20, 5; 25, 75, 4]; % 动态障碍物初始位置与速度 obs_dyn_init = [45, 55, -0.8, 0.6, 2]; obs_dyn_init = [obs_dyn_init; 55, 35, 0.5, 0.9, 1.5];注意安全距离半径的值。这个参数不是物理碰撞半径，而是希望两架无人机保持的最小间距。由于真实无人机有定位误差和控制误差，安全距离需要考虑误差余量。我一般设定为无人机体型的 3 到 5 倍，既保证安全又不会让空域太拥挤。如果安全距离设得过大，5 架无人机在 100x100 的地图里会频繁互相避让，效率很低。
3.3 A* 路径规划的核心函数
A* 函数的实现核心部分如下，我这里只写关键逻辑：
function path = a_star_path(grid_map, start_node, goal_node) % grid_map: 栅格地图，1表示障碍物，0表示自由空间 open_list = []; closed_set = []; g_score = inf(size(grid_map)); f_score = inf(size(grid_map)); open_list = start_node; g_score(start_node(1), start_node(2)) = 0; f_score(start_node(1), start_node(2)) = heuristic(start_node, goal_node); while ~isempty(open_list) % 从open_list中选f_score最小的节点 current = min_f_node(open_list, f_score); if current == goal_node path = reconstruct_path(current); return; end open_list = remove_node(open_list, current); closed_set = add_node(closed_set, current); neighbors = get_neighbors(current, grid_map); for i = 1:size(neighbors, 1) neighbor = neighbors(i, :); if is_in_set(neighbor, closed_set) continue; end tentative_g = g_score(current(1), current(2)) + cost(current, neighbor); if tentative_g < g_score(neighbor(1), neighbor(2)) came_from(neighbor(1), neighbor(2)) = current; g_score(neighbor(1), neighbor(2)) = tentative_g; f_score(neighbor(1), neighbor(2)) = tentative_g + heuristic(neighbor, goal_node); if ~is_in_set(neighbor, open_list) open_list = [open_list; neighbor]; end end end end % 找不到路径时返回空 path = []; end这个函数的性能瓶颈主要在min_f_node和get_neighbors上。如果你的地图分辨率很高（比如 500x500 的栅格），建议使用二叉堆来维护open_list，否则每次遍历整个列表找最小值会非常慢。我在实验中用 100x100 的栅格地图，线性查找勉强能实时跑，地图再大就顶不住了。
代价函数cost(current, neighbor)我是这样设计的：
function penalty = cost(current, neighbor) % 基础距离代价 penalty = norm(current - neighbor); % 转弯代价（如果存在上个节点，计算转弯角度） if current.parent exists vec1 = current - current.parent; vec2 = neighbor - current; angle = atan2(norm(cross([vec1,0],[vec2,0])), dot(vec1, vec2)); penalty = penalty + 0.3 * abs(angle); end end这个 0.3 的转弯惩罚系数是我反复试出来的。系数太小，路径仍然有急转弯；系数太大，无人机会为了少转弯而绕很远的路。建议自己做实验时从 0.1 开始，逐步增大，观察路径形状的变化。
3.4 速度障碍法与避障指令计算
速度障碍法的核心函数长这样：
function [safe_vel, conflict_flag] = velocity_obstacle(pos_i, vel_i, pos_j, vel_j, safe_radius, time_horizon) % 计算相对位置和相对速度 rel_pos = pos_j - pos_i; rel_vel = vel_j - vel_i; % 如果相对位置距离已经大于安全距离，且相对速度在远离方，无冲突 if norm(rel_pos) > safe_radius && dot(rel_pos, rel_vel) >= 0 safe_vel = vel_i; conflict_flag = false; return; end % 构建速度障碍区域 % 把障碍物抽象为圆，计算相对速度与圆的切线关系 theta = atan2(rel_pos(2), rel_pos(1)); alpha = asin(safe_radius / norm(rel_pos)); % 障碍区域的角度范围 theta_min = theta - alpha; theta_max = theta + alpha; % 判断当前相对速度是否落在障碍区域 rel_vel_angle = atan2(rel_vel(2), rel_vel(1)); if rel_vel_angle >= theta_min && rel_vel_angle <= theta_max conflict_flag = true; % 把相对速度旋转出障碍区域 if rel_vel_angle < theta + alpha/2 new_rel_vel_angle = theta_min - 0.05; else new_rel_vel_angle = theta_max + 0.05; end new_speed = norm(rel_vel); new_rel_vel = new_speed * [cos(new_rel_vel_angle), sin(new_rel_vel_angle)]; % 反算安全绝对速度 safe_vel = vel_j + new_rel_vel; else safe_vel = vel_i; conflict_flag = false; end end这个函数是简化版。实际项目中，还要把多个障碍物产生的多个速度障碍区域合并起来求交集，然后在交集之外找最优速度。当有多架无人机时，我采用循环迭代的方式：对每架无人机，依次处理所有其他无人机和动态障碍物的速度障碍，每次迭代都更新一次可行速度范围。迭代次数设 2 到 3 次就够，太多迭代会浪费时间。
另一个关键细节是：速度输出的限制。计算出的safe_vel必须经过速度上限和加速度上限的约束，否则控制器跟踪不了。我的做法是加一个限幅函数：
function vel_limited = limit_velocity(vel_desired, vel_current, uav_accel_max, dt, uav_speed_max) % 加速度约束 max_delta = uav_accel_max * dt; delta = vel_desired - vel_current; if norm(delta) > max_delta delta = delta / norm(delta) * max_delta; end vel_limited = vel_current + delta; % 速度约束 if norm(vel_limited) > uav_speed_max vel_limited = vel_limited / norm(vel_limited) * uav_speed_max; end end3.5 主仿真循环的框架
主仿真循环是整个项目的主心骨，代码如下：
function main_sim() % 加载参数 p = init_scenario(); % 初始化无人机状态 drones = struct('pos', [], 'vel', [], 'goal', [], 'path', [], 'track', []); for k = 1:p.n_drones drones(k).pos = p.start_pos(k, :); drones(k).vel = [0, 0]; drones(k).goal = p.goal_pos(k, :); drones(k).path = []; drones(k).track = p.start_pos(k, :); end % 仿真时间与步长 dt = 0.1; % 时间步长 s t_total = 60; % 总仿真时间 s n_steps = round(t_total / dt); % 动态障碍物初始化 dyn_obs = p.obs_dyn_init; for step = 1:n_steps t = step * dt; % 更新动态障碍物位置 for m = 1:size(dyn_obs, 1) dyn_obs(m, 1:2) = dyn_obs(m, 1:2) + dyn_obs(m, 3:4) * dt; end % 对每架无人机独立计算控制量 for k = 1:p.n_drones % 1. 从全局路径中提取局部目标点 local_goal = get_local_goal(drones(k), p); % 2. 计算期望速度（向目标点靠拢） desired_vel = calc_desired_velocity(drones(k).pos, local_goal, p.uav_speed_max); % 3. 协同避障：检查与其他无人机和动态障碍物的冲突 for j = 1:p.n_drones if j == k continue; end [desired_vel, cf] = velocity_obstacle(... drones(k).pos, desired_vel, ... drones(j).pos, drones(j).vel, ... p.safe_radius, p.time_horizon); end for m = 1:size(dyn_obs, 1) obs_pos = dyn_obs(m, 1:2); obs_vel = dyn_obs(m, 3:4); % 动态障碍物按固定速度运动 [desired_vel, cf] = velocity_obstacle(... drones(k).pos, desired_vel, ... obs_pos, obs_vel, ... dyn_obs(m, 5), p.time_horizon); end % 4. 加速度限幅 vel_limited = limit_velocity(desired_vel, drones(k).vel, p.uav_accel_max, dt, p.uav_speed_max); % 5. 更新状态 accel = (vel_limited - drones(k).vel) / dt; drones(k).vel = vel_limited; drones(k).pos = drones(k).pos + drones(k).vel * dt; drones(k).track = [drones(k).track; drones(k).pos]; end % 每若干步绘制一次 if mod(step, 5) == 0 plot_frame(drones, dyn_obs, p, t); end % 检查任务完成情况 if all_tasks_completed(drones, p) break; end end % 绘制最终轨迹 plot_trajectories(drones, p); end注意这里有一个所有的控制系统的通用问题：仿真循环里必须先计算出所有无人机的控制量，再统一更新所有无人机的状态。如果写成“先计算第一架的避障，立刻更新第一架位置，再计算第二架的避障”，那第一架的位置变化会影响第二架的判断，仿真结果就不是严格的离散时间系统了，会引入不公平的先后顺序，导致结果失真。
3.6 动态障碍物重规划触发机制
动态环境里，路径规划模块不是一直运行的，而是靠触发器唤起。我的触发逻辑是：
- 如果无人机当前路径前方出现了之前不在栅格地图里的新障碍物，则触发重规划。
- 如果无人机距离当前路径终点的距离小于某个阈值，则触发下一段路径的规划。
- 如果避障模块连续多个时间步都在大幅偏离参考路径，也触发一次重规划。
具体实现上，我在速度障碍法检测到冲突后，会同时检查冲突点的位置是否偏离当前路径上的对应点超过阈值。如果超过，说明当前路径已经被环境“堵死”，需要重新规划。这个阈值我设为 2 倍的网格分辨率，太小会导致频繁重规划，太大又会让无人机在死路上浪费太多时间。
4. 常见问题、调试技巧与调参经验
4.1 问题速查表
我列出在开发过程中踩过的最多的几个坑，很多问题你遇到了可能根本没有想到是这些原因。
| 现象 | 可能原因 | 排查与解决方法 |
|---|---|---|
| 无人机频繁抖动、轨迹呈锯齿状 | 避障灵敏度太高或时间窗口太短 | 增大 time_horizon，降低避障响应频率；给速度更新加低通滤波 |
| 两架无人机互相对撞，避让无效 | 两架无人机避让方向不一致，导致二次冲突 | 确认优先级规则是否生效；画出速度障碍区域观察 |
| 路径明显绕远路 | A* 启发函数使用不当，比如欧氏距离在多障碍环境下可能不够 | 尝试改用对角距离或加入地图密度惩罚 |
| 无人机无限徘徊无法到达目标点 | 局部最小值问题（人工势场法常见，A* 增量式搜索也可能出现） | 加入逃逸机制：如果检测到同一区域重复访问，触发全局重规划 |
| 仿真越来越慢，到后期明显卡顿 | 轨迹记录数组无限增长 | 记录轨迹时做降采样，每 3 步记录一次；或者定期清理旧帧数据 |
| 动态障碍物穿过无人机 | 时间戳更新顺序错误 | 检查主循环：所有状态更新必须在同一时刻完成，不能穿插 |
| 避障效果很好但任务效率极低 | 安全距离设太大 | 逐步减小 safe_radius，观察碰撞率与完成任务时间的平衡点 |
4.2 调参顺序与经验
这个项目的参数很多，我不建议一上来就凭感觉调。我自己的调参顺序是：
- 先不看避障，单测路径规划。把避障模块关掉，让每架无人机单纯沿着 A* 规划的路径飞行，确认路径本身质量过关。
- 再打开两机避障测试，只放两架无人机，路径设为交叉航线，验证速度障碍法能否让它们安全交错。这个阶段调
safe_radius和time_horizon。 - 增加第三架无人机，观察多机冲突消解策略是否有效。
- 最后加入动态障碍物，调动态障碍物的预测时间窗口和不确定性参数。
这个顺序保证每一步的问题范围可控。你要是直接一把梭把五架无人机和动态障碍物全部怼进去，出了问题根本不知道是路径规划的锅还是避障的锅。
4.3 一个让我印象深刻的 Bug
项目开发中有一个 Bug 我修了很久，值得单独说一下。现象是：两架无人机明明初始位置很远，却在仿真开始后几秒就撞上了。后来逐步排查发现，问题出在放开避障限制之后，两架无人机都飞向同一个目标点（测试时偷懒给两架设置了相同目标点），A* 规划出几乎一样的路径，然后程序里又没有对“目标点相同”做任务分配处理，导致它们一路重复冲突。
这个 bug 让我意识到一个设计原则：路径规划层和协同避障层之间有信息边界，但任务分配的信息必须提前给到路径规划层。如果任务分配阶段两架飞机拿到同一个目标点，无论底层避障算法多强，都是治标不治本。所以在后来的版本中，我在场景生成时增加了一个简单的任务分配模块，用贪心算法给每架无人机分配距离最近的目标点，从源头避免这类冲突。
4.4 可视化调试的技巧
多无人机系统的调试，可视化做得好，效率翻倍。我的可视化工具有三个层次：
- 宏观视图：显示整个地图、所有无人机的轨迹线、障碍物、目标点，用于观察全局行为是否合理。
- 微观视图：随机选择一架无人机，画出它的速度矢量、速度障碍区域、期望速度和实际速度，用于调试单个控制律。
- 数据曲线：记录所有无人机之间的最小距离随时间的变化曲线，如果任何时刻曲线低于安全阈值，立刻能找到是哪两架差点撞上。
尤其是第三点——最小距离曲线——是我调参时最依赖的工具。它把“安全”这个抽象概念变成了一个直观的数值曲线，一眼就能看出避障算法在哪一时刻、因为什么原因失效了。我把这段绘图代码放在record_trajectory.m里，每次仿真结束时自动输出距离曲线图。
4.5 关于参数敏感性的实测结论
我做了很多组实验对比不同参数组合的效果，这里分享几个实测结论：
time_horizon从 1 秒增加到 3 秒时，碰撞率明显下降，但完成任务时间增加了约 12%。从 3 秒增加到 5 秒时，碰撞率几乎不再下降，但任务时间增加了约 20%。所以 3 秒是一个比较好的平衡点。safe_radius从 2 米增加到 3 米时，碰撞率下降约 30%，任务时间增加约 8%；从 3 米增加到 4 米时，碰撞率下降约 5%，任务时间增加约 15%。说明安全距离不是越大越好，存在边际效应。- 多机数量超过 8 架时，分层分布式架构的性能开始明显下降，主要瓶颈是每架无人机要循环检测所有其他无人机，O(n^2) 的计算复杂度在这里开始显现。如果需要支持更大规模的机群，应该考虑把空间划分成网格，只检测相邻网格内的无人机，可以显著减少计算量。
5. 项目后续扩展方向
这个仿真平台搭好之后，后续扩展空间非常大，我这里简单说两个我觉得最有价值的方向。
第一个是引入更真实的动力学模型。当前用的是二维平面运动学模型，真实无人机还有高度通道、姿态动力学、电机响应延迟等。如果要往真机迁移，可以把运动模型替换为更完善的六自由度模型，或者至少升级为三维空间加上偏航角约束。路径规划算法层面，A* 和速度障碍法都需要相应调整到三维空间，避障计算也会从二维圆变成三维球体，整体复杂度会提高一个量级。
第二个是引入机间通信模型。现在的仿真默认所有无人机都能完美获取彼此的精确状态，这在实际系统中不成立。通信延迟、丢包、有限带宽都会对协同效果产生致命影响。你可以在地图更新和状态交换之间加入随机延迟和丢包，观察协同避障算法的退化情况，这会让研究更有深度。再进一步，还可以对比“完全通信”和“部分通信”条件下的路径规划效果差异。
如果做毕设的话，我建议可以往基于学习的避障策略方向延伸——把速度障碍法计算出的避让指令作为监督信号，训练一个神经网络控制器，让它学习在更复杂、非结构化环境下的避障能力。这样既有仿真数据，又有算法对比，工作量也很饱满。
6. 写在最后的实际体会
整个项目做下来，我最深的体会是：多无人机协同路径规划与防撞，难点从来不在某一个具体算法，而在“多个子系统如何在一个动态环境中有效耦合”。A* 单看很简单，速度障碍法单看也不复杂，但当你把路径规划、避障、轨迹跟踪、任务分配放在同一个仿真循环里时，各种问题就开始交织出现——避障动作引起路径跟踪误差，路径跟踪误差又触发新的避障动作，最后形成振荡。这种系统级的问题，只有通过在真实仿真环境里反复调试才能有直观感受。
如果你准备做类似的项目，我建议你从头到尾保持一个朴素的习惯：每改一个参数，只改一个维度，记录下来结果变化，再改下一个。这个项目里的参数相互作用太强了，如果同时调三四个参数，出了问题你根本不知道是谁的锅。对我来说，这种方式虽然速度慢一点，但每一步都是扎实的。
另外建议给代码写上详细的注释，特别是每个参数的单位和物理含义。仿真项目跨时间长了，你回头看代码，一定会庆幸当初把safe_radius写的是“米”而不是“网格数”。这些细节看着不起眼，实际项目中能帮你避免无数无谓的返工。
