做无人机协同编队或者集群任务的人，估计都绕不开“动态环境下多无人机系统的协同路径规划与防撞”这道坎。我一开始做单机路径规划的时候，以为避障做好就够了，直到把两架、三架、五架无人机放进同一个任务空间里，才发现这完全是个新问题——无人机彼此就是对方的动态障碍物，而且谁走快了、谁绕远了、谁先让谁后走，直接影响整个机群能不能按时飞到目标点。最近把一套基于Matlab实现的多无人机协同路径规划与防撞仿真代码重新整理了一遍，准备把核心思路、算法选型、代码架构和踩过的坑都梳理出来，给正在做相关仿真或者课程设计的同学一个参考。
这套代码做的事情简单说就是：在包含静态障碍物和动态移动障碍物的环境里，让多架无人机从各自起点出发，协同地规划出无碰撞路径，并实时进行防撞判断和速度调整。它解决的痛点很明确——单机路径规划不处理机间冲突，而多机协同必须把“其他飞机”当成移动威胁来处理。适合谁呢？适合正在做无人机集群仿真、智能体协同算法验证、毕业设计或者想要快速验证路径规划算法的同学，也对做调度、机器人集群方向的朋友有参考价值。
1. 核心问题拆解：所谓“协同”，到底难在哪里
1.1 从单机避障到多机协同的数量级变化
单机路径规划的核心是“在已知环境下找一条从起点到终点的可行路径”，障碍物是静态的，位置不变，算法只要保证路径绕开它们就行。但多无人机协同一上来就把这个问题复杂化了，因为每一架无人机本身就是一组移动障碍物，而且它的轨迹还能被其他无人机影响。
很多人第一次接触多机协同时会犯一个直觉性错误：以为单独给每架飞机跑一次A-star或者RRT，再把结果叠加起来就能用。我在项目初期就这么试过，结果一架飞机发现路径被另一架飞机的路径穿过，两者同时到达交叉点，直接撞上。原因很简单：多机系统的规划维度不是“N条独立路径”，而是“N条相互耦合的路径”。
这种耦合在数量上有直观体现。假设有N架无人机，两两之间的关系数量是C(N,2) = N*(N-1)/2。3架只有3对关系，5架是10对，10架就变成45对。每一对都是一组持续演化的相对运动，再加上环境中本来就存在的移动障碍物，整个系统的动态关系数量会飞速增长。这也是为什么多机协同不能靠“先规划、后飞”的离线方式解决，必须要有实时调整的防撞层。
1.2 动态环境到底“动态”在哪里
标题里反复强调“动态环境”，这次项目里的动态因素其实有三类。
第一类是环境中的移动障碍物，比如交通场景中的车辆、巡查任务中遇到的移动目标、仓库里的AGV小车。它们的轨迹可能是已知的匀速直线运动，也可能做圆周运动，甚至带有随机扰动。代码里我用的是已知运动轨迹的模型，这是一个非常合理的研究起点——先解决确定性问题，再扩展到不确定性的预测。
第二类是其他无人机，这是多机协同特有的动态因素。每架无人机的速度方向都在变化，所以对其他飞机来说这就是一个实时变化的动态危险源。
第三类是通信约束带来的“感知盲区”。当两架无人机距离超过通信半径时，它们无法交换位置和速度信息，相当于对方进入了雷达盲区。这就要求防撞策略不能依赖全局信息，必须有一定的局部自主决策能力。
这三类动态因素叠加以后，规划就不再是空间问题，而是时空问题：不仅要规划“走哪条路线”，还要规划“什么时间出现在什么位置”。时间和空间，缺一不可。
1.3 协同防撞的三个层次
在动手写代码之前，我先把防撞策略想清楚，这个想清楚比直接写代码重要得多。常见的协同防撞分三个层次：
第一个层次是“先规划、后消解”，也就是顺序规划。先给第一架无人机规划路径，然后把这个路径当作动态障碍物引入第二架无人机的规划约束中，依次类推。优点是思路简单，缺点是后者必须等待前者，而且如果顺序不合理，后续无人机的可行空间可能被严重压缩，甚至无解。
第二个层次是“速度障碍法”这种实时反应式防撞，每架无人机根据其他飞机当前的位置和速度，实时计算是否存在碰撞风险，有风险就调整自己的速度方向。优点是计算量小、实时性好，缺点是没有全局视野，可能走出“看起来安全但实际上绕了远路”的路径。
第三个层次是“全局协同优化”，把整个机群的路径和时间关系放到同一个优化问题里，用约束条件保证不冲突。优点是全局最优性好，缺点是求解复杂、对动态环境适应性差。
这套代码采用的是第二层次为主、第一层次为辅的方案：静态环境下用图搜索算法生成参考路径，飞行过程中用速度障碍法做实时防撞修正。这样的组合既保证了大方向不跑偏，又能应对动态干扰，是工程上非常务实的一种折中。
2. 方案设计与算法选型
2.1 主路径规划算法：为什么用A*而不是RRT
主路径规划我选了A算法，而且是在栅格地图上做的。现在RRT系列算法在各种机器人项目中很火，但在这个场景下A更合适，原因有三个。
首先是最优性保证。A在启发函数一致的情况下能保证找到最短路径，而基础RRT只能找到可行路径，路径质量没法保证。虽然RRT可以逐步收敛到最优，但收敛需要大量采样，在栅格地图上反而没有A*高效。
其次是实现与调试成本。A*在Matlab里的实现非常直观，open列表、close列表、父节点回溯，逻辑清楚，非常容易排查问题。而RRT涉及随机采样、树扩展、最近邻搜索，一旦出问题，你很难判断是采样空间设置错了还是碰撞检测写错了。
第三个原因是后续平滑处理方便。A*生成的折线路径可以直接通过样条插值得到光滑曲线，保留了任务期望的几何形态。RRT生成的路径是树状搜索出来的，转折奇多，后续平滑的工作量明显更大。
不过A也有一个明显局限：它是在栅格上搜索的，分辨率决定了路径精度。栅格太粗会生成穿缝而过的不安全路径，栅格太细会导致搜索时间爆炸。我最后用的栅格分辨率是0.5米，在100x100米的地图上就是200x200的搜索空间，A能在几十毫秒内完成计算，这个性价比是可以接受的。
2.2 防撞算法核心：速度障碍法的原理与计算
实时防撞部分，我选了速度障碍法。这套方法的思路特别像我们日常生活中判断会不会撞车——不是看对方在哪里，而是看对方以什么速度朝哪个方向移动。
具体原理用生活化的方式解释：假设你在高速上开车，旁边车道的前车突然向你并线，你会本能地看一眼后视镜，確認后车的速度和距离，然后决定是加速通过还是减速让行。速度障碍法做的就是这个判断：以本机为参考系，把其他无人机看作带有速度的点。如果本机相对其他无人机的速度方向指向对方位置，且距离小于安全阈值，说明按照当前速度继续飞下去会撞上——这个速度方向就落在了“碰撞锥”里面。
判据很简洁：把两机相对位置向量记为Pre，相对速度向量记为Vrel。如果Vrel与Pre的夹角小于一定角度，并且两机距离正在减小，那么维持当前相对速度必然导致碰撞。要消除碰撞，就需要调整本机速度，让相对速度Vrel偏离这个碰撞方向。
在实现时，我把这个判断拆成了三步：
第一步，计算相对位置：Pre = P_b - P_a 第二步，计算相对速度：Vrel = V_a - V_b 第三步，判断是否存在碰撞风险：用向量夹角和距离双重条件判断。
如果检测到风险，就把本机速度向量往碰撞锥外旋转一个安全角度，同时叠加一个减速量。这个“往外旋转”的动作就是防撞模块的核心。代码里我把这个逻辑写成了一个独立的函数，输入本机位置速度、障碍机位置速度和一条安全距离，输出调整后的速度向量。
提示：用相对速度而不是绝对速度做判断是关键。两架飞机绝对速度都很快，但如果方向和大小恰好一致，它们就是相对静止的，没有碰撞风险。只看绝对速度会误判很多情况，白白做无效避险操作。
2.3 中等规模机群的优先级仲裁策略
速度障碍法有个经典问题：如果两架无人机同时检测到对方，都主动往同一侧避让，结果可能反而撞得更快——这就是“互相谦让”造成的死锁困境。就像两个人迎面走，同时往左让，结果又挡在一起。
解决这个问题的思路也来自现实交通秩序：给每架无人机设定明确的让行优先级。代码里我用的是“按编号优先级”策略：编队中编号小的无人机拥有优先通行权，当两机存在碰撞风险时，编号大的那架做出避让动作，编号小的保持参考路径不变。这样做的好处是简单、可靠、不会死锁。
但这种固定优先级有一个缺点：编号小的无人机如果路径本身有问题，它不知道让，只会硬飞，而后面的飞机只能跟着绕。所以在项目后期我加了一个改进——基于任务紧急程度的动态优先级判断。如果某架飞机正在执行时间敏感任务（比如紧急投送），它的优先级会临时提高。不过这个改动涉及任务调度层，代码里保留了一个配置开关，没做得很深。
2.4 通信半径作为协同约束
防撞模块之所以要看通信半径，是因为我在仿真中假设了一个重要约束：无人机只能感知到通信半径内的其他无人机。距离超过通信半径，就认为它不存在，也不做避让处理。
这个约束大大增强了仿真的真实性。如果没有它，每架飞机都像开了上帝视角，能感知所有飞机的动向，这在实际系统中根本做不到。加了通信半径后，当两架飞机还远时它们互不干扰，直到进入通信范围才触发防撞逻辑。我在代码中把通信半径设为了30米，超过这个距离的飞机不参与本机的防撞计算，既可以模拟真实约束，也顺带减少了计算量。
3. Matlab代码架构与关键模块实现
3.1 工程文件结构与运行流程
整套代码的工程结构非常清晰，我按功能拆分成下面几个模块：
| 文件 | 功能 | 关键接口 |
|---|---|---|
| main.m | 主程序入口，初始化并驱动仿真循环 | 运行后自动出结果图 |
| initParams.m | 参数集中配置 | 修改无人机数量、速度、地图等 |
| buildEnvironment.m | 构建栅格地图与静态/动态障碍物 | 返回环境结构体 |
| planGlobalPath.m | A*全局路径规划 | 输入地图起点终点，输出路径点集 |
| velocityObstacleAvoid.m | 速度障碍防撞计算 | 输入本机与障碍机状态，输出调整后速度 |
| plotResults.m | 可视化多机轨迹与防撞过程 | 显示二维俯视图与轨迹 |
建议拿到代码后先按这个结构过一遍，懂了文件分工再动手跑仿真的话，后面改起来心里有底。
运行流程是：main.m 读取参数 → buildEnvironment 建地图 → 每架无人机用 planGlobalPath 生成参考路径 → 进入时间循环，每步先检查是否到达终点，没到就看附近有没有障碍机，有就调用 velocityObstacleAvoid 调整速度，再更新位置 → 最后把所有轨迹用 plotResults 画出来。
3.2 动态障碍物建模
动态障碍物的建模逻辑在这套代码里相当关键。我采用的方式是给每个动态障碍物预定义一条运动轨迹函数，在当前时刻直接用时间参数t算出位置。这样建模的好处是简单直观，不引入复杂的预测误差，适合先验证防撞算法本身的正确性。
代码里一个动态障碍物的建模类似这样：
% 动态障碍物建模：绕圆心做匀速圆周运动 function pos = getDynamicObsState(obs, t) omega = obs.omega; % 角速度 rad/s center = obs.center; % 圆心坐标 radius = obs.radius; % 轨道半径 pos = center + radius * [cos(omega * t), sin(omega * t)]; end这种建模范式的好处是很容易扩展。如果后续要模拟更复杂的动态场景，只要替换这个函数里的位置计算公式，比如改成沿折线运动或者带随机扰动，防撞模块完全不用动。
给动态障碍物选的参数也要注意：角速度不宜过大，否则障碍物一帧之间位移太大，与无人机的碰撞检测步长对不上，容易出现“上一帧没碰到、下一帧直接穿越”的问题。我用的角速度是0.3rad/s，线速度大约1.5m/s，这个速度与无人机的巡航速度在一个量级，碰撞风险是真实的但又不至于快得失控。
3.3 协同规划主循环的实现
主循环是整个仿真的调度中心。每一帧仿真中，程序要完成目标跟踪、防撞判断、状态更新三件事。核心逻辑的简洁伪代码：
% 多无人机协同路径规划主循环 for t = dt:dt:T_end for i = 1:N_drones % 1. 检查是否到达目标 if norm(pos(i,:) - goal(i,:)) < goal_threshold status(i) = reached; continue; end % 2. 计算参考速度（朝向全局路径的下一航点） ref_v = computeRefVelocity(pos(i,:), path{i}, v_max(i)); % 3. 检测并避让其他无人机 v_adj = ref_v; for j = 1:N_drones if j == i, continue; end if norm(pos(i,:) - pos(j,:)) < comm_radius v_adj = velocityObstacleAvoid(pos(i,:), v_adj, ... pos(j,:), vel(j,:), d_safe); end end % 4. 更新当前无人机状态 vel(i,:) = v_adj; pos(i,:) = pos(i,:) + vel(i,:) * dt; end end这里有一个非常重要的工程细节：防撞速度v_adj是在前一架飞机已经更新位置之前计算的，用的是上一时刻的状态。如果你在同一层循环内实时更新pos后再计算下一架飞机，就会引入时序耦合误差。所以循环内所有无人机先基于“当前状态快照”计算新速度，全部算完再统一更新位置，这样结果更稳定，也更容易复现。
3.4 防撞模块的核心计算
velocityObstacleAvoid函数是整个代码的灵魂，它检查的是“本机速度向量和相对位置向量的夹角是否处于危险区”。核心计算逻辑我用数学公式来展开：
令当前本机位置为Pa，速度为Va，威胁机位置为Pb，速度为Vb。
- 相对位置向量：Pre = Pb - Pa
- 相对速度向量：Vrel = Va - Vb
- 两向量夹角：theta = acos( Vrel·Pre / (||Vrel||·||Pre||) )
- 安全角：alpha = asin( d_safe / ||Pre|| )，前提是||Pre|| > d_safe
当theta < alpha时，说明相对速度正指向对方，继续飞下去会撞进安全距离内，触发避让。避让动作是旋转Vrel，让它与Pre的夹角大于alpha，再还原出本机速度：
% 速度障碍法核心计算 if theta < alpha % 旋转方向和大小由叉积符号决定：左转还是右转 sign_dir = sign(Pre(1)*Vrel(2) - Pre(2)*Vrel(1)); rot_angle = alpha - theta + margin_angle; % 构造旋转矩阵 R = [cos(sign_dir*rot_angle), -sin(sign_dir*rot_angle); sin(sign_dir*rot_angle), cos(sign_dir*rot_angle)]; Vrel_new = R * Vrel'; % 恢复本机速度 Va_new = Vrel_new' + Vb; end那个margin_angle是额外安全余量，稍微多偏转一点，防止数值误差导致下一帧又检测到碰撞风险。我测试下来余量设在0.05rad到0.1rad之间比较合适，太小容易震荡，太大会导致路径过度偏离参考线。
3.5 参数标定与敏感性说明
这套代码的可用性很大程度取决于参数怎么配。参数之间相互牵连，不能孤立地调。我整理了一张默认参数表，附上了调参时的注意事项：
| 参数 | 默认值 | 调参说明 |
|---|---|---|
| 无人机数量 N | 5 | 超过8架时计算量明显增加，建议留意通信半径 |
| 地图范围 | 100m x 100m | 栅格0.5m，规划时间随地图尺寸急剧上升 |
| 最大速度 v_max | 3 m/s | 与防撞距离联动，速度越快需要越大的防撞安全距离 |
| 防撞距离 d_safe | 3 m | 过大则路径绕行严重，过小则安全性不足 |
| 通信半径 comm_radius | 30 m | 与N联动，数量越多通信范围越大越容易触发协同防撞 |
| 仿真步长 dt | 0.1 s | 减小到0.05s更安全但更慢，具体取舍看5.3节 |
| 目标到达阈值 | 1 m | 太大会在距离目标较远处就停，太小则可能震荡 |
特别强调一下防撞距离d_safe的敏感性。d_safe设置的物理含义是“两架无人机质心之间必须保持的安全距离”。设得过大，比如10米，无人机在交叉通过时会规划出很大的绕行弧，路径严重偏离参考线，到达时间显著延长；设得过小，比如1米，虽然轨迹看起来很流畅，但一旦通信刷新率偏低或者速度调整稍有延迟，就很容易突破安全边界。我实际测试下来，对3m/s巡航速度、0.1s步长的场景，3米是一个性价比不错的折中值。
4. 实验复现与结果优化
4.1 从零跑通仿真流程
拿到代码后，建议按下面几步走，每一步都确认输出正常后再继续：
第一步，阅读README和文件结构说明，打开main.m过一眼整体结构。哪怕赶时间也要做这一步，后面调试能帮你省至少两个小时。
第二步，在initParams.m里把无人机数量N改为2，场景先跑最简单的双机对飞，手动观察两架无人机是否能在中途错开。双机场景如果都出问题，直接调多机只会更难排查。
第三步，把无人机数量改回你的目标数量，跑通完整场景，确认plotResults.m画的轨迹图没有异常跳出。
第四步，依次尝试添加不同数量的动态障碍物，观察无人机在面对移动障碍物时的绕行是否合理。
我项目第一次跑的时候在第二步就发现了一个坑：两架对飞的无人机在交汇点附近会频繁“互让”，导致它们走出了一个明显的S形绕行轨迹，这个S形对交叉通过问题来说是可接受的，但如果你预期的是“小角度偏移”，就需要把margin_angle调小或者把防撞触发条件加一个“距离趋势判断”——只有当两机距离正在减小时才触发防撞，距离在增大时即使角度很小也不需要避让。这个优化非常管用，加了这个判断后路径绕行幅度明显减小。
4.2 典型场景：双机交叉航线防撞
先说一个我常用的标准测试场景：两架无人机从地图左下角和右下角起飞，目标点分别设在右上角和左上角。这样它们必然会在地图中部交叉。若关闭防撞模块，两架飞机会在同一时刻汇聚在同一个点，也就是碰撞点。
开启防撞模块后，两架无人机会在接近交汇点时各向外侧偏移，形成一个自然的错位轨迹。我在10x10的栅格上跑这个双机场景，具体参数是：v_max=3m/s，d_safe=3m，dt=0.1s。实测效果是：两机在距离约4.5米处开始做避让动作，完成避让后各自恢复原方向飞行，最终到达时间比理想无避让轨迹多了约2.3秒，最小距离被控制在3.05米左右。这个数字说明防撞逻辑既保证了安全边界，又没有过度反应导致大面积绕路。
这个典型场景还可以用来验证“优先级仲裁”的效果：把两架无人机的编号互换，观察谁让谁。如果优先级机制生效，编号小的飞机会保持原轨迹，编号大的飞机绕行，这就说明优先级逻辑正常工作。我做完这个测试后对代码的置信度提升了不少。
4.3 性能评估的三个关键指标
评估多无人机协同路径规划算法好不好，只看“有没有撞上”是远远不够的。我在这套代码里实现了三个维度的量化评估，大家在其他算法对比时也可以沿用这套指标体系。
第一个指标是最小机间距。这个最直观，统计整个仿真过程中任意两架无人机的最短距离，看它是否大于安全距离，以及是否几乎贴着安全边界。真正稳健的算法，最小距离应该比安全距离大30%以上，否则说明算法“在悬崖边走路”，一旦有扰动就突破边界。
第二个指标是路径冗余率。计算公式是（实际飞行路径长度 - 理想直线距离）/ 理想直线距离，用百分比表示。这个指标衡量的是防撞带来的额外飞行代价。我的测试里，静态环境下路径冗余率在8%到15%之间是正常的。如果超过30%，说明防撞参数过保守或者路径规划本身有问题。
第三个指标是到达时间偏差。指各无人机实际到达时间与无防撞理想到达时间的差。这个指标衡量协同机制的负载均衡效果，如果某些无人机为了避让目标到了绕行很多而另一些准时到达，说明优先级安排不合理。
代码里我在仿真结束时会把这三个指标打印出来。跑完一遍仿真后先看这三个数字，再决定往哪个方向调参，比盯着轨迹图“凭感觉调”有效得多。
4.4 提高算法鲁棒性的几个工程细节
仿真做到后期，我开始意识到算法“能跑通”和“稳”是两码事。几个工程细节能显著提升代码的鲁棒性。
速度平滑：速度障碍法计算出的新速度可能与上一时刻的速度有较大跳跃，直接应用会导致无人机轨迹抖动。我在速度更新后加了一个限幅逻辑：单帧速度变化量不超过最大加速度与时步的乘积。简单理解就是给速度变化加一个“惯性约束”，这样轨迹会平顺很多。实现上就是一行代码：delta_v = clamp(new_v - old_v, -a_maxdt, a_maxdt)。这个a_max我设为2m/s²，效果不错。
重规划触发机制：当动态障碍物长期阻挡当前航线时，只靠速度障碍法绕行会让无人机严重偏离参考路径。我在代码里加了一个判断条件：如果无人机偏离参考路径超过某个阈值，比如8米，就触发一次全局A*重规划。这个机制避免了“航线被堵死但飞机还在原地兜圈子”的尴尬情况。
随机扰动测试：把仿真环境加一点随机扰动，比如动态障碍物的初始相位随机变化，跑10次蒙特卡洛仿真，取最小距离的均值与标准差。如果标准差很大，说明算法对初始条件过于敏感，这样的算法在真实环境中一定不可靠。
5. 常见问题与调试实录
5.1 无人机“穿模”：直接越过对方或者穿过障碍物
做仿真最容易遇到的、也是最让人头大的问题就是“穿模”——明明有防撞逻辑，两架无人机却重叠或者直接穿过对方。出现这种问题，大概率不是防撞算法没触发，而是触发之后速度没有算对或者没有跟上。
我自己排查时先检查三个地方。第一，看步长dt是不是太大了。速度3m/s、步长0.1s的情况下，每帧位移0.3米。如果防撞距离3米，有足够的反应空间。但如果你把速度调到了10m/s，步长还是0.1s，一帧就是1米，可能两帧之间就突破了安全距离。第二，查防撞触发条件里有没有加“距离趋势判断”。如果两架飞机已经在远离了，但你仍然按照“接近”状态做避让计算，计算出的速度可能反而让它们继续纠缠。第三，检查速度障碍法实现时有没有正确使用相对速度。用了绝对速度会导致避让方向完全算反，两架飞机越让越正对着飞。
5.2 两机“僵持”：原地互相绕圈半天走不出来
这个问题的特征是两架无人机到了交叉点附近都停着不动或者原地绕小圈，谁也不先走。原因通常是优先级机制没有生效，或者两架飞机同时在做避让并且避让的方向恰好对称，导致永远无法错开。
我的解决方案很直接：给优先级机制加一个死锁检测超时。当检测到某架无人机速度持续低于某个阈值超过2秒，强制让它偏离原方向飞行一个固定角度，给整个系统加一个“扰动”。这个办法在工程上非常常用，虽然不够优雅，但绝对有效。当然更优雅的做法是从优先级配置入手：确保任意两架无人机之间存在明确的优先级差，并且在状态更新时严格按优先级顺序执行。
如果“僵持”发生在三架以及更多无人机之间，情况会复杂很多，因为可能形成“连环让行”的局面：A让B、B让C、C让A。这种情况建议把全局重规划触发条件调低一点，让系统能更早重新规划路径，绕开局部死锁区。
5.3 时间步长到底怎么选才合适
时间步长是一个典型的“鱼与熊掌”问题。我测试过几种步长：
| 步长 | 优点 | 缺点 |
|---|---|---|
| 0.5s | 速度快，适合跑大批量蒙特卡洛实验 | 极容易穿模，几乎不可用于动态障碍场景 |
| 0.1s | 速度和精度比较均衡，日常推荐 | 动态密集场景下偶尔会有惊险时刻 |
| 0.02s | 安全性高，轨迹平滑 | 仿真耗时是0.1s步长的5倍 |
我的建议是日常实验用0.1s，但在最终展示或者需要严格验证安全性的场景，把步长改成0.05s跑一遍确认结论能复现。另外要提醒的是，防撞距离d_safe和步长dt是一对联动参数。把dt减小一半后，d_safe也可以适当减小，因为防撞模块有更多时机做修正。
5.4 参数调优速查表
为了节省大家调试时间，我把常见现象和对应的参数调法整理成了一张速查表：
| 现象表现 | 可能原因 | 推荐调参方向 |
|---|---|---|
| 轨迹抖动频繁、波动大 | 速度变化过快 | 减小最大加速度 a_max，增大速度平滑系数 |
| 绕行范围过大、路线明显偏 | 防撞距离过大 | 减小 d_safe，或减小 margin_angle |
| 局部僵持、互不让路 | 优先级不明确或死锁 | 检查优先级机制，增加死锁检测超时 |
| 偶发碰撞但概率低 | 步长过大或安全余量不足 | 减小 dt，增大 d_safe 或 margin_angle |
| 全局路径明显绕路 | A*启发函数或栅格分辨率问题 | 检查地图建模，缩小栅格分辨率 |
| 动态障碍物附近剧烈抖 | 障碍物速度过快或状态更新不同步 | 降低障碍物速度，减少 dt |
快速定位问题的时候，先对照这张表做第一次粗调，通常能解决八成问题。剩下两成需要回看代码逻辑，特别是你自己改过的地方。
6. 从仿真走向实飞必须绕开的三个坑
6.1 仿真与实飞之间的“理想化”差距
做完了协同防撞仿真，下一步自然想把它搬到真机上。这里必须先泼一盆冷水：Matlab仿真里那些看起来很完美的轨迹，搬到实飞现场大概率会出现各种意料之外的问题。差距主要来自三个地方。
第一，感知噪声。仿真里无人机能精确知道其他飞机的位置和速度，但实际系统要靠GPS、视觉或者UWB定位，位置误差少则几十厘米多则数米。速度也不是直接测量的，需要从位置差分得到，噪声会被放大。第二，通信延迟。仿真里防撞模块拿到的状态是瞬时的，实际通信链路一有延迟，意味着你拿到的对方状态是几百毫秒前的，速度障碍法对这种“过期状态”特别敏感。第三，动力学延迟。仿真里速度指令立刻生效，实飞时电机、螺旋桨、机体姿态都有响应时间，从指令下达到实际速度变化往往需要几百毫秒。
6.2 我的两个简易过渡方案
如果短期没法上真机，建议先做两个低成本验证。一是模型在环仿真，把无人机动力学模型换成带一阶惯性延迟的简化模型——更新位置时不是直接pos += v*dt，而是先让v向期望速度缓慢靠拢，再更新位置。加这样一个延迟，所有在Matlab里发现不了的问题都会浮现。二是硬件在环，把防撞算法部署到实际飞控开发板上，通过串口或者局域网与Matlab环境连接，用Simulink做半物理仿真。这样能验证算法在嵌入式平台上的计算耗时能不能满足实时性要求——如果算法在PC上就需要跑50毫秒，那在机载芯片上很可能直接超过步长限制。
我给一个具体建议：拿到仿真代码后，立刻把速度指令到实际速度之间加一个0.2秒的一阶延迟。如果加完延迟后算法表现仍然可接受，说明它具备基本的鲁棒性；如果出现震荡或者碰撞，恭喜你，提前发现了问题，而这恰恰是实飞最容易踩中的坑。
6.3 可以继续深挖的扩展方向
这套代码的基础框架搭建完成后，后续扩展至少有四个方向值得探索。
方向一是分布式协同架构。当前代码里速度障碍防撞本身就是分布式的，但全局路径规划还是集中式的。可以改成多无人机各自独立规划，再通过通信进行路径交换与协商，这样能提升系统规模扩展性。
方向二是与编队控制结合。把“安全防撞”升级为“编队保持”，在防撞的同时维持期望队形。这需要引入队形位置偏移量的概念，在速度障碍法基础上叠加一个编队保持力。
方向三是处理感知不确定性。给每架无人机的位置估计加一个协方差矩阵，在防撞判断时考虑对方的“位置不确定性范围”，这样更贴近真实的传感器模型。
方向四是引入学习算法，让各无人机从大量仿真中学习避让策略的决策边界，比如深度强化学习。但这要求先有一套稳定可靠的仿真环境作为训练数据来源——这套Matlab代码正是合格的基础设施。
我个人在实际调试中最深的一个体会是：多机协同的仿真项目里，算法原理占三成，工程调试占七成。你花了大把时间调出来的稳定参数、踩过的每一个坑、总结出来的那些判断准则，才是这套代码真正的价值所在。在项目提交报告或者展示代码的时候，一定要把这些调试经验和问题记录写进去，它们比代码本身更能体现你对问题的理解深度。
