Matlab实现动态环境下多无人机协同路径规划与避障仿真

做多无人机协同路径规划也有一段时间了，最直观的感受是：单机路径规划是门技术，多机协同路径规划是门玄学。静态地图里跑通一架飞机的避障，新手花一两天就能搞定；可一旦把场景切到动态环境，再把飞机数量从一架加到五架、十架，问题就完全变味了——规划出来的不是路径，是连环套：你想避障碍，它想避你，最后大家一起卡在原地。

这个项目做的是就是在Matlab里把动态环境下多无人机系统的协同路径规划与防撞逻辑完整搭出来，包含全局路径搜索、局部动态避障、机间协同防撞三个层面的算法实现。方案里没有特别玄乎的东西，用的都是公开成熟的算法：全局路径用改进的A*搜索，局部动态避障用人工势场加速度障碍的思路，协同层则靠优先级分配加分布式约束来协调多机行为。但它能帮你把"多机协同"这件事从概念变成可运行、可复现、可看到的仿真效果。

如果你正在做无人机集群、多智能体系统相关的研究，或者刚从单机避障往多机协同方向转，这套Matlab框架会是一个很合适的起点。文章后面我把自己实际调通的整体流程、核心代码结构、参数设置经验以及踩过的几个典型坑都整理出来了，希望能帮你少折腾几个晚上。

1. 动态环境下多无人机协同路径规划，难点到底在哪里

1.1 这不是单机规划的简单叠加

很多人第一次做多机系统时，最自然的想法是：把N架无人机分别规划路径，然后合并起来跑。这种思路在飞机数量少、地图空旷、任务不复杂时确实能凑合用，但一旦进入动态环境，问题就会集中暴露。

原因在于多机路径是强耦合的——A的路径在空间上和B的路径相互竞争，A的飞行路径本身会成为B的动态障碍。如果各自规划完再检查碰撞，几乎必然会在交叉路口、共享走廊、目标点附近互相堵死。这就是经典的组合爆炸加轨迹耦合问题：单机规划的解空间是三维位置加时间，多机协同的解空间还要叠加机间共享约束，维度直接爆炸。

我自己最早试过一个笨办法：先让所有飞机独立规划，再用一个脚本反复检测冲突、微调路径。结果Micro调速度和位置，牵一发而动全身，改一架飞机的航迹会让另外三架重新产生冲突，陷入"调整—冲突—再调整—再冲突"的死循环。后来才明白，协同规划必须在框架设计阶段就考虑进去，而不是规划完再修修补补。

1.2 动态环境里到底什么在"动"

所谓动态环境，不是一句"障碍物会移动"就能概括的。实际仿真和工程里，至少有三类动态因素需要区分：

第一类是环境中本身存在的动态障碍物，比如地面车辆、其他飞行器、飞行中的物体等。它们的运动轨迹要么是预先设定的，要么是实时感知到的，但又不受我们无人机系统控制。这类障碍的处理方式是"预测+规避"：在局部避障模块里通过感知信息推断它们未来一段时间的位置，然后调整速度方向。

第二类是无人机之间的相互动态性。每架无人机对其他飞机来说，都是一个位置实时变化、行为不完全确定的移动障碍。这层动态最难处理，因为它不是单向的——你避它，它也在避你，双方的行为互相影响。

第三类是任务层面的动态性，比如目标点移动、任务区域临时变更、编队队形切换等。这要求路径规划不能只做一次，而要在飞行过程中周期性重规划或者局部修复。

1.3 协同路径规划要满足的三类约束

把问题抽象成数学模型时，动态环境下多机协同路径规划其实是在三个约束族的交集里寻找可行路径：

安全约束是最基本的：每架飞机与静态障碍、动态障碍、其他飞机都要保持最小安全距离。这个约束在时间维度上连续存在，不是只在某个离散点检查，所以仿真步长和检测频率的设置很关键。

物理约束是欠考虑的一块：无人机有最大速度、最大加速度、最大转弯速率，不是说规划出一条几何路径就能飞出来的，还要看飞行器动力学上跟不跟得住。很多仿真里面路径画得很漂亮，导成控制指令后飞机根本飞不过去，就是因为把动力学约束忽略了。

任务约束包括目标点分配、预计到达时间、通信连通保持、载荷任务窗口等。在多机协同里，任务约束还会在机间产生关联，比如某些任务要求两架飞机同时到达目标区域，或者飞行过程中保持一定的编队间距。

2. 核心算法选型思路：什么样的组合适合动态环境

2.1 全局路径规划：A* 和 RRT* 怎么选

全局路径规划的任务是给每架无人机找出一条从起点到目标点的可行通路。常见选项有A*系列、RRT系、PRM等。我在这个项目里测试过几组方案，简单说说对比感受。

A适合栅格地图，实现简单、路径确定性高、并且天然支持在网格里表达障碍分布。缺点是路径容易贴墙走、折角多，需要额外做平滑处理。RRT适合连续空间和非结构化环境，能处理带方向约束的问题，但是路径随时间不确定，每跑一次结果都不一样，而且需要更多采样点才能保证质量。

动态环境下的全局层，不需要做到每条路径都全局最优，但要能快速重规划，并且对地图整体信息有把握。所以在中等规模的栅格地图上，我倾向于用A做全局搜索，然后在路径后处理阶段做拉直和B样条平滑。如果地图是大范围连续空间、障碍也偏非结构化，才会换RRT。

2.2 协同层架构：集中式还是分布式

协同策略的架构选择，决定了整个系统跑起来的形态。集中式规划由一个中心节点收集所有飞机的状态和目标，统一求解全部路径。这种方式理论上能得到全局较优解，但代价是状态空间爆炸——N架飞机，每架M个可选动作，排列一下就是M的N次方，地图稍微大点就算不动。

此外集中式对中心节点的依赖太重，仿真里看着没啥问题，一旦实际运行中通信延迟或者中心节点出故障，整个系统就会瘫掉。

这个项目采用的是分布式为主、集中式为辅的结构：目标分配用集中式做一次（因为目标点数量一般不多，全局最优分配的计算可接受），但飞行过程中的实时路径调整和避撞决策都交给每架飞机自己处理。每架飞机只跟通信范围内的邻居交换状态信息，然后基于局部信息做决策。这样扩展性好，飞机数量从几架加到几十架，只是通信计算量线性增长，不会指数爆炸。

2.3 局部避障与机间防撞的方案选型

局部避障层的选择，是所有方案里对动态环境友好度影响最大的。人工势场法最经典：目标点产生引力，障碍物产生斥力，合力方向就是期望运动方向。实现简单、计算量小，但问题也很出名——局部极小值陷阱和障碍物旁的震荡行为。

速度障碍法（VO）思路是另一条路线：在速度空间里找出所有会导致碰撞的速度集合，然后从当前速度附近挑选一个安全的替代速度。ORCA进一步优化，在多智能体之间互相协调时表现尤其好，它给每个智能体生成一个半平面速度约束，让参与避让的双方都承担一部分调整量。

结合起来，这个项目最终的局部层逻辑是：基础速度由全局路径跟踪生成，然后先用改进人工势场修正静态和动态障碍，最后用速度障碍思路处理机间避让。势场给出"应该往哪个方向偏"，速度障碍回答"偏多少不会撞"，两者结合比单用任何一种都稳定。

2.4 整体算法链路

把各层串起来看，整个系统的信息流是这样的：目标分配模块给出每架飞机的目标点，全局路径规划模块用A*给每架飞机算出静态参考路径，飞行过程中每个控制周期执行三步操作——第一步，根据当前路径跟踪控制，计算期望速度和航向；第二步，检查局部动态障碍，用改进势场产生修正速度；第三步，与邻居无人机交换位置和速度信息，检测机间冲突，如果有碰撞风险就通过速度障碍方法计算避让速度。最后把得到的期望速度生成控制指令，驱动运动学模型更新位置。

每层的更新频率可以不一样：路径规划层几秒重规划一次就行，避障层要高频运行，控制层频率最高。这种分层设计的优势是模块之间解耦，随便替换其中某一层的算法都不会影响其他模块，调试起来特别方便。

3. Matlab仿真框架设计与代码实现细节

3.1 项目文件结构规划

Matlab仿真项目最怕的就是把所有代码堆在一个脚本里。我建议按功能拆成模块，每个模块独立文件，方便单独测试和改动。这个项目的目录结构大致如下：

drone_coop_sim/ ├── main_drone_coop.m % 主程序入口，参数初始化 + 主循环 ├── config/ │ ├── sim_params.m % 仿真基础参数 │ └── drone_params.m % 无人机物理参数 ├── map/ │ ├── generate_map.m % 生成静态地图 │ └── load_dynamic_obstacles.m % 定义动态障碍物运动轨迹 ├── planner/ │ ├── global_astar.m % 全局路径搜索 │ ├── path_smoothing.m % 路径平滑处理 │ └── replan_check.m % 判断是否触发局部重规划 ├── avoidance/ │ ├── potential_field.m % 人工势场局部避障 │ ├── velocity_obstacle.m % 速度障碍法机间避让 │ └── priority_assignment.m % 避让优先级计算 ├── dynamics/ │ ├── drone_kinematics.m % 无人机运动学模型 │ └── controller.m % 路径跟踪控制器 ├── utils/ │ ├── plot_result.m % 结果可视化 │ └── collision_check.m % 碰撞检测和日志记录 └── run_experiment.m % 批量跑实验、统计指标

3.2 无人机运动学模型搭建

路径规划仿真里用的无人机模型不需要太复杂，三自由度质点模型基本够用：状态量包括位置(x,y,z)、速度大小v、航向角ψ、爬升角θ，输入量是速度指令、航向角速度指令和爬升率指令。

无人机运动学模型的Matlab函数可以写成如下形式：

function [state_next] = drone_kinematics(state, control, dt, params) % state: [x; y; z; v; heading; climb_angle] % control: [v_cmd; heading_rate_cmd; climb_rate_cmd] % params: 包含最大速度、最大加速度、最大角速度等限制 % 状态限制 v = min(max(control(1), params.v_min), params.v_max); heading_rate = min(max(control(2), -params.max_heading_rate), params.max_heading_rate); climb_rate = min(max(control(3), -params.max_climb_rate), params.max_climb_rate); % 更新速度、航向角 v_new = v; heading_new = state(5) + heading_rate * dt; climb_angle_new = state(6) + climb_rate * dt; % 更新位置 x_new = state(1) + v_new * cos(heading_new) * cos(climb_angle_new) * dt; y_new = state(2) + v_new * sin(heading_new) * cos(climb_angle_new) * dt; z_new = state(3) + v_new * sin(climb_angle_new) * dt; state_next = [x_new; y_new; z_new; v_new; heading_new; climb_angle_new]; end

这里有个细节容易忽略：航向角是要做归一化处理的，否则时间长了角度值会漂移，导致计算cos和sin时出现不预期的跳变。归一化到[-pi, pi]区间即可。

3.3 地图与动态障碍建模

静态地图用二维栅格表示，每个栅格标记空闲或障碍。做全局规划前，要对障碍区域做膨胀处理——把无人机自身的尺寸考虑进去，避免规划出来的路径擦着障碍边缘飞。膨胀半径一般取无人机半径加一个安全余量，安全余量我习惯设成无人机半径的1.5倍。

动态障碍物单独定义运动轨迹。仿真里定义一个structure数组，每个元素包含障碍物的位置、速度、运动方向、当前所处轨迹段等信息。用一个时间相关的函数计算障碍物当前的位置：

function obs_pos = get_dynamic_obstacle_pos(obstacle, t) switch obstacle.motion_type case 1 % 匀速直线运动 obs_pos = obstacle.position + obstacle.velocity * t; case 2 % 圆周运动 theta = obstacle.init_theta + obstacle.omega * t; obs_pos = obstacle.center + [obstacle.radius * cos(theta), obstacle.radius * sin(theta)]; case 3 % 折线轨迹，分段运动 % 查找当前时间段和对应的插值位置 idx = find(t >= obstacle.time_stamps, 1, 'last'); seg_ratio = (t - obstacle.time_stamps(idx)) / ... (obstacle.time_stamps(idx+1) - obstacle.time_stamps(idx)); obs_pos = obstacle.waypoints(idx,:) + ... seg_ratio * (obstacle.waypoints(idx+1,:) - obstacle.waypoints(idx,:)); end end

3.4 主循环与协同规划实现

主程序写的是仿真主循环。每帧执行以下流程：

%% 主循环 for t = 0:dt:T_total for i = 1:N_drones % ---- 第1步：目标点位置更新 ---- if tasks Assigned and current target reached assign next target % ---- 第2步：全局路径规划与重规划判断 ---- if isempty(global_path{i}) || need_replan(i) global_path{i} = global_astar(map, start_pos, target_pos); global_path{i} = path_smoothing(global_path{i}); end % ---- 第3步：路径跟踪，计算期望速度 ---- [desired_v, desired_heading] = path_tracking_controller(global_path{i}, state(:,i)); % ---- 第4步：局部避障，修正速度方向 ---- [v_avoid, heading_avoid] = potential_field(state(:,i), dynamic_obstacles, desired_v, desired_heading); % ---- 第5步：机间协同防撞 ---- [v_final, heading_final] = velocity_obstacle(v_avoid, heading_avoid, state, i, comm_range); % ---- 第6步：更新控制指令 ---- control = [v_final, clamp_heading_rate(heading_final - state(5,i)), 0]; state(:,i) = drone_kinematics(state(:,i), control, dt, params); % ---- 第7步：记录轨迹与碰撞信息 ---- trajectory{i}(end+1,:) = state(1:3,i)'; collision_log = check_collision(state, i); end % 可视化更新（每N帧刷新一次） if mod(t, plot_interval) < dt update_plot(); end end

3.5 机间防撞的具体实现

机间防撞是协同部分的核心，这里我用速度障碍法的简化实现。基本思想是：把邻居无人机的当前位置和速度考虑进来，在当前速度空间中找出会导致未来一段时间内碰撞的速度集合，然后避开这个集合。

核心函数实现逻辑如下：

function [safe_v, safe_heading] = velocity_obstacle(v_cmd, heading_cmd, state, idx, params) % 先计算名义速度矢量 velocity_cmd = v_cmd * [cos(heading_cmd); sin(heading_cmd)]; best_velocity = velocity_cmd; neighbor_agents = find_neighbors(state, idx, params.comm_range); for j = neighbor_agents % 相对位置和相对速度 rel_pos = state(1:2, j) - state(1:2, idx); rel_dist = norm(rel_pos); rel_vel = state(4,j) * [cos(state(5,j)); sin(state(5,j))] - ... state(4,idx) * [cos(state(5,idx)); sin(state(5,idx))]; % 如果距离大于安全范围，跳过 if rel_dist > params.avoidance_radius continue; end % 计算碰撞时间 time_to_collision = compute_tc(rel_pos, rel_vel, params.safe_distance); % 如果存在碰撞风险，计算并施加避让速度 if time_to_collision > 0 && time_to_collision < params.time_horizon % 速度障碍约束：把速度调整到安全半平面 safe_velocity = project_velocity_outside_cone(velocity_cmd, rel_pos, rel_vel, params); best_velocity = best_velocity + safe_velocity * 0.5; % 根据优先级加权 priority_i = params.priority(idx); priority_j = params.priority(j); if priority_i < priority_j % 优先级低的一方偏移多一点 offset_factor = 1.2; else offset_factor = 0.8; end best_velocity = ensure_safe(best_velocity, rel_pos, rel_vel, params, offset_factor); end end % 把速度向量转回极坐标表达 safe_v = norm(best_velocity); safe_heading = atan2(best_velocity(2), best_velocity(1)); end

细说一个关键点：速度障碍算法的核心是把"位置空间的碰撞检测"转化为"速度空间的可行域判断"。生活化点理解就是，两个人在走廊里迎面走，如果都不变方向肯定会撞上，速度障碍法会各自检查自己走过去会不会撞，然后调整步伐方向，让出一部分走廊空间。这个调整量双方分摊，不会出现一架飞机大幅绕路而另一架纹丝不动的情况。

3.6 参数配置参考表

仿真里参数设置对结果影响非常大，下面这张表是我调通的典型值，可以直接作为初始配置跑起来，然后根据实际效果微调：

4. 调试过程中的典型问题与排查心得

4.1 多机同时避让导致的航迹震荡

第一次把5架飞机放进地图跑仿真时，很快发现一个让人头疼的现象：编队穿越一个狭窄通道后，几乎所有飞机都在通道出口附近来回摆动，想走走不动，队形也散了。后来跟踪日志发现，这是"避让反馈振荡"。

原因很简单：A为避让B向左偏，B为了避让A也向左偏，结果两人新航向冲突更严重，又同时向右偏，如此反复。这不是单一算法的问题，而是整个避让机制缺少滞回处理。

解决办法有两个：第一，避让触发半径设置得比安全距离大一截，让进入避让状态和退出避让状态用不同阈值，形成滞回区间，减小频繁切换的概率；第二，引入优先级机制，避让时低优先级飞机承担主要调整量，高优先级飞机尽量保持原路径。修改之后震荡基本消失，路径跟随也平滑多了。

4.2 分布式规划下的飞机死锁问题

分布式规划还有一个典型的"死锁"场景：两架飞机在大约60米距离上迎面相遇，都在互相避让，结果双方各自原地兜圈子，谁也不往前飞。用技术术语说，这叫做"互锁冲突"——每架飞机都在等待对方做出决定。

这个问题的本质是局部信息下双方的决策相互依赖。我在框架里用两种方式处理：一是在优先级分配中，一旦检测到两机持续相持超过一定时间，强制让优先级低的那架飞机绕行到等待点，给高优先级飞机让出通道；二是加入一个随机扰动项，当检测到避让速度接近零且持续了较长时间，给航向角加上一个小幅随机偏移，打破对称僵局。

这个经验在真实飞行中尤其重要——很多集群飞行任务出问题并非因为算法算不出路径，而是分布式决策下缺少一个"打破死锁"的机制。

4.3 人工势场局部极小值导致路径卡死

人工势场法在U型障碍物场景里会碰到特别尴尬的情况：无人机被斥力包围在局部极小点附近，目标点明明就在障碍物另一侧，却怎么都走不过去。我的做法是在势场基础上加判断逻辑：如果在连续时间内飞机的航向角变化超过一定累计值且位移很小，判定进入局部极小，触发"临时逃离"策略——沿垂直方向爬升绕行，绕过障碍后再回到全局路径上。

逃离策略需要对飞行高度留余量，所以地图的垂直方向上要预留足够空间。这也是为什么仿真区域高度设定为100m，不只是为了好看，是给无人机留出绕行和机动的操作空间。

4.4 Matlab仿真性能优化

多机协同仿真对Matlab性能影响很大，跑起来卡的话排查效率极低。几个实用的优化思路可以先记着：

第一，主循环里用向量化操作，避免在每架飞机内部用子循环处理所有邻居的判断条件；第二，静态地图的代价地图和距离场可以预先计算好，不需要每一帧重新生成；第三，可视化绘图的刷新频率降低，不需要每一帧都重绘，10Hz控制频率下绘图可以降到2～3Hz刷新；第四，用profiler工具跑一遍，找到真正的瓶颈再优化，不要盲目改写代码结构。

我实际优化以后，5架飞机仿真从龟速爬行跑到了实时速度的两倍以上，调参体验提升非常明显。

5. 从仿真到实际工程的几个扩展方向

5.1 把算法迁移到真实飞行平台

仿真跑通后，如果要往实际飞行平台上迁移，有几个差异需要提前心里有数：仿真是理想传感器，真实环境下位置和速度有噪声，需要加滤波处理；通信带宽有限，邻居状态信息达不到仿真里那么完整和实时；还有控制指令的底层执行延迟，仿真中默认指令立即生效，真实飞控还有响应时间。

建议迁移时保留分层架构不动，只替换底层模型和通信层，把仿真中的理想信息替换成带噪声的感知信息，然后先在单机避障中验证局部层的实际表现，再逐步增加协同飞机数量。

5.2 算法层面的可扩展方向

这套框架本身模块化程度高，扩展方向也比较多。比如想引入编队保持约束，可以在协同层加入一致性协议，让所有飞机朝目标点的同时维持相对位置关系；想进一步适应更复杂的动态环境，局部避障层可以接入强化学习模型，用训练好的策略替换人工势场；如果想做更细粒度的路径优化，可以在A*路径基础上用贝塞尔曲线做时间参数化，把路径带时间维度变成真正可以执行的轨迹。

从我自己的使用体会来说，调试多机协同仿真最重要的是别急着一次搞定所有功能。先把单机路径规划跑稳，再加入局部避障，然后扩展到两架飞机的协同避碰，验证逻辑没问题后再往上加飞机数量。每加一架飞机都会暴露新的交互问题，但如果基础框架的边界条件设计得干净，大部分问题的定位时间都不会太长。这套Matlab实现的价值也正在这里——让多机协同不再只是一个理论概念，而是能亲手调、能看见、能复现的仿真系统。后面如果再有人问多机协同从哪里开始，我会直接建议先搭一个这样的Matlab仿真框架，把参数调一遍，把坑踩一遍，比看多少论文都管用。