这个项目的起因其实很普通：课题组接了一个多无人机协同演示任务，要求无人机在模拟厂区上空按给定任务点飞行，环境里除了固定建筑，还有几辆移动的作业车辆，无人机之间还不能靠得太近。最开始用成熟的单机路径规划算法挨个给每架飞机算轨迹，地面站看着没问题，一旦放到同一个动态地图里跑，问题立刻暴露：要么两架无人机争抢同一片空域，要么被移动车辆挡住之后在原地绕圈。
后面整个方案推倒重做，花了几周时间把“动态环境下多无人机系统的协同路径规划与防撞”这套问题从建模、算法选型到Matlab代码实现完整跑通。这个项目的仿真框架后来成了我们做集群算法的公共测试平台，所以我想把整个过程中比较关键的设计思路、代码细节和踩坑经验完整记录下来。无论你是刚开始入门无人机集群，还是已经在做路径规划但被动态障碍和机间防撞搞得头疼，这篇内容都能给你一份可以直接参考的路线图。
1. 动态环境的“动态”到底指什么：先拆问题再写代码
做这类课题最忌讳一上来就写代码。你得先把“动态环境”这四个字拆开，弄清楚自己到底在解决哪几个子问题。我一般是把动态拆成三个层次去分析，分层之后很多看似复杂的现象其实都能归位。
1.1 三个层面的动态：外部障碍、机间相对运动、任务级时变
第一个层面是外部环境里的移动障碍物。模拟厂区里的作业车辆、行人、临时起降的车辆，都属于这一类。它们有自己的运动轨迹，但和无人机集群不是同一个决策主体，我们只能通过感知去估计它们的速度和位置。
第二个层面是无人机之间的相对运动。这个最容易被忽略。哪怕所有无人机的目标点都是静止的，只要每架飞机都在按自己的规划飞行，任意两架之间的相对位置和相对速度就是动态变化的。多机协同里最难处理的“对称冲突”“死锁”，基本都出在这个层面。
第三个层面是任务级时变。比如目标点本身在移动，或者某些区域是时变禁飞区。这个层面在纯路径规划里往往被当成约束条件处理，但在协同框架里会直接影响任务分配的结果，需要单独设计。
把动态环境拆成这三层之后，整个项目的技术路线就清晰了：外部移动障碍用感知层加防撞层处理，机间相对运动用协同时空约束处理，任务级时变用滚动更新处理。
1.2 协同路径规划和单机路径规划的本质区别
单机路径规划处理的是“在障碍物中找一条从起点到终点的可行轨迹”，障碍物只要被恰当建模，最短路径搜索或者采样类规划方法都能解决。多机协同多出来的核心问题是：每架无人机的决策会改变其他无人机的环境。
这种耦合关系也常叫做无人机状态的一致性与冲突消解——大家既希望在时间或空间上保持一致（比如同一时刻到达、编队队形保持），又不能为了全局最优牺牲单机的安全和效率。一个非常直观的区分：单机规划生成的轨迹只需要满足自身的运动学约束；多机协同生成的轨迹，必须同时满足自身的动力学约束、外部动态障碍的避让约束，以及“同一时刻与其他所有邻居的距离都大于安全半径”的耦合约束。
这也是为什么很多人第一步就把集中式规划排除掉的原因：把所有无人机的轨迹放进一个超大优化问题里求解，理论上能给出全局最优，但一旦规模超过八到十架，单步求解时间就会指数增长，而且整个系统对中央节点的通信可靠性要求极高，动态环境里根本跑不起来。
1.3 项目最终的能力边界
这套Matlab仿真最终实现了以下能力：在三维或二维平面地图中，任意给定一组分布不定的无人机初始位置和任务目标点，系统能够通过分布式滚动优化生成各自的平滑轨迹；环境中可加入多个移动障碍物，无人机能实时估计其运动趋势并主动避让；机群内部通过局部通信实现互相感知，保证整个飞行过程中任意两架无人机的最小距离不低于设定的安全半径。
需要说明的是，这只是一个算法验证级别的仿真平台，没有接入真实飞控硬件，也没有做传感器噪声模型。它解决的问题是“控制算法层面是否能够协同”，而不是“工程实现层面是否能落地”。后面如果要做真机实验，还需要加上惯性导航、视觉感知、通信延迟补偿等模块。
2. 算法选型：为什么最终选了“分布式MPC+速度障碍法”的组合
整个项目最花时间的不是写代码，而是定方案。最初我也踩过不少弯路，比如一开始想用人工势场法做防撞，后来发现动态环境下的局部极小值问题根本绕不开；后来又试过纯几何的速度障碍法，单次避撞效果不错，但没法处理“要多机同时往一个汇合点飞”这种协同任务。最后确定下来的框架是：上层任务分配用匈牙利算法，中层轨迹规划用分布式模型预测控制，底层防撞用速度障碍法做约束修正。
2.1 三条主流路线的横向对比
把集中式规划、分布式模型预测控制、纯反应式避碰放在一张表里看，优缺点非常明显：
| 路线 | 计算负载 | 通信依赖 | 协同一致性 | 动态环境适应性 | 实现难度 |
|---|---|---|---|---|---|
| 集中式规划 | 随无人机数量指数增长 | 严重依赖中央节点 | 天然全局最优 | 需重新求解，响应慢 | 中等 |
| 分布式MPC | 每架无人机固定规模 | 依赖局部邻居通信 | 可通过滚动交换轨迹实现局部一致 | 滚动在线重规划，适应性强 | 高 |
| 纯反应式避碰 | 最低 | 基本不依赖 | 无法保证编队或汇合协同 | 实时性好，但容易陷入局部 | 低 |
我的结论是：纯反应式避让只在“求生”层面有效，做不了协同任务；集中式规划在静态环境里很好用，动态环境一上来就会因为求解时间超标而失效。真正适合动态多机场景的是分布式MPC——每架无人机在本地求解一个有限时域优化问题，只跟邻居交换少量预测信息，整体上等效于一群决策者边飞边商量，既保留了协同能力，又把计算量控制在单机可接受范围内。
2.2 速度障碍法为什么适合当“防撞外挂”
速度障碍法的核心思想很直观：给定自身位置和速度、障碍物位置和速度，把所有会导致碰撞的相对速度集合画成一个“锥形”区域，只要当前相对速度不在这个锥内，就不会碰撞。这个方法最大的优点是计算快、几何意义清晰，能够直接输出一个安全的避让方向。
但在实际使用中它也有两个明显短板：第一，它本身不生成路径，只做速度修改，如果完全依赖它，飞机容易在一个小范围来回绕圈；第二，它没有预测能力，只对当前时刻的相对状态做出反应，无法把“未来几秒内的队形保持”纳入考虑。
所以我的设计思路是把速度障碍法嵌进MPC的约束层。平时无人机按MPC算出来的最优轨迹飞行，一旦检测到与某个邻居或移动障碍物的相对关系进入碰撞锥，就在当前控制量上叠加一个由速度障碍法计算出的避让修正量。修正只影响当前这一步，MPC在下一个周期会基于最新状态重新规划，相当于整个系统既有长远的规划能力，又有快速的应激反应。
2.3 混合框架的整体工作流程
整套系统按仿真时间步循环，每一步主要经历四个阶段：感知与通信、任务判断、轨迹规划、防撞修正。感知层维护本机位置和速度，同时接收邻居广播的状态信息；任务层在目标点变化或已有目标点被障碍物遮挡时触发重新分配；规划层运行分布式MPC，在预测时域内求解最优控制序列；防撞层在每个控制周期内检查碰撞锥，若触发则调整输出。
这样分层的最大好处是模块独立、便于调试。我在一开始就把单机运动学、MPC求解器、速度障碍判定写成三个独立的Matlab函数，后面换任何一块都不会牵连其他部分。这也是我强烈建议你在这个课题上保持的代码结构，别把所有逻辑堆在一个脚本里。
3. 仿真环境是怎么搭的：从无人机运动学模型到动态障碍物
有了算法框架，接下来就是搭仿真环境。这部分偏工程，但恰恰是后续所有算法调试的基础。如果运动学模型和障碍物模型搭得不够真实，后面再漂亮的算法也等于白做。
3.1 无人机运动学模型与约束的离散化
建模时我选的是二维平面内的三自由度运动学模型，状态向量包括位置、速度，控制量取加速度。这个模型简单但不失真，能够覆盖路径规划关心的核心动力学约束。
状态方程为：
p(k+1) = p(k) + v(k)dt + 0.5a(k)*dt^2
v(k+1) = v(k) + a(k)*dt
同时需要设置速度限幅和加速度限幅。用Matlab写一个单步模型更新函数：
function [p_next, v_next] = quadModel(p, v, u, dt) % 状态量：p为2x1位置，v为2x1速度，u为2x1加速度指令 a_max = 2.0; % 最大加速度，单位m/s^2 v_max = 5.0; % 最大速度，单位m/s a = u(:); a = max(min(a, a_max), -a_max); v_next = v + a * dt; % 速度限幅：超过最大速度时按比例缩放 speed = norm(v_next); if speed > v_max v_next = v_next / speed * v_max; end p_next = p + (v + v_next) / 2 * dt; end这种梯形积分比简单的欧拉积分更稳定，尤其是在速度变化剧烈的时候不容易漂移。另一个需要留意的地方是控制量限幅应该在速度限幅之前做，否则可能出现在加速度边界内但速度仍然越界的情况。
3.2 动态障碍物的生成与速度估计
对于外部移动障碍物，我在项目里分别做了两种建模方式。一种是“已知理想轨迹”模式，障碍物沿设定轨迹匀速运动，用于算法原理验证；另一种是“未知轨迹+速度估计”模式，障碍物以某种随机方式运动，无人机需要实时估计它的速度，更接近真实感知场景。
速度估计我采用最简单的差分法加低通滤波。具体做法是维护障碍物最近三帧的位置，用后一帧减前一帧除以时间间隔得到瞬时速度，再做指数滑动平均去除噪声：
obsV_now = (obsP_hist(:,end) - obsP_hist(:,end-1)) / dt; obsV_est = 0.7 * obsV_est + 0.3 * obsV_now;如果你希望速度估计更平滑，可以换成卡尔曼滤波。但我在实际调试中发现，基于匀速假设的卡尔曼滤波在障碍物突然转向时反而容易因为模型失配而给出错误的估计结果。差分加滑动平均虽然简单，但响应速度快、不容易出现滤波发散的问题，对防撞来说反而更可靠。
3.3 邻居关系与通信拓扑的模拟
多机协同的前提是每架无人机能获取周围邻居的信息。我在仿真里用通信半径加距离判断来模拟局部通信拓扑，只有距离小于通信半径的无人机才互称邻居，也才会在图论意义上进行信息交换。
neighbors = []; for j = 1:N if j ~= i && norm(pos_i - pos_j) < commRange neighbors = [neighbors, j]; end end一开始我用了全连通拓扑，也就是所有无人机之间都能通信。这个设定导致的一个结果是：当飞机数量超过六架时，每架飞机都要处理来自所有其他飞机的耦合约束，MPC求解时间迅速上升。改成局部通信拓扑之后，每架飞机通常只需要处理两到三个邻居的约束，计算量立刻降下来，而且更符合实际应用场景。
4. 协同层的实现：任务分配、滚动预测与分布式MPC
协同路径规划的“协同”两字，最终要落到两个具体机制上：一是任务分配，让每架无人机知道自己的目标是谁；二是滚动预测，让每架无人机不仅要考虑自己飞哪儿，还要预测邻居接下来几秒往哪儿飞，从而避免未来的冲突。
4.1 任务分配：用匈牙利算法解决“谁去哪个目标点”
动态环境下如果目标点固定，任务分配只需要做一次。但更常见的情况是：某个目标点进入了移动障碍物的覆盖范围，或者目标点本身在移动，这时候就需要在线重新分配。
我采用的是经典的匈牙利算法。代价矩阵里的每个元素计算的是“第i架无人机飞到第j个目标点的大致航程”，航程用两者之间的直线距离加上少量环境风险代价近似。分配完成后，每架无人机锁定自己的目标点，之后进入MPC滚动规划。
这里有一个需要注意的细节：任务分配频率不能太高。无人机实际飞行中，如果每一帧都重新分配，很容易出现目标点来回摇摆的现象。我设置的策略是：每隔五秒或者在目标点状态发生重大变化时才触发重新分配，其余时间沿用上一轮的分配结果。
4.2 代价函数的设计：距离目标、控制能耗、邻域一致性
分布式MPC的核心是本机求解一个有限时域最优控制问题。每一架无人机在每一个控制周期，需要预测未来N步的状态和控制量，然后选择一组控制序列，使代价函数最小。
整个代价函数由三个部分构成：
- 目标到达代价：预测轨迹的末端点与目标点的距离平方，加权系数Q；
- 控制能耗代价：预测时域内所有控制量的平方和，加权系数R；
- 邻域一致性代价：预测轨迹和邻居预测轨迹之间的位置偏差平方和，加权系数P。
其中第三项正是协同的关键。每架无人机通过通信收到邻居最近一次发布的预测轨迹后，会把自己预测轨迹的对应节点向邻居轨迹节点靠拢，从而形成一种“心照不宣”的编队保持效果。
4.3 求解器与初值策略：fmincon的工程化用法
代价函数确定之后，求解器我选用Matlab自带的fmincon，配上SQP算法。优化变量是所有预测时域内的控制序列。如果防撞约束是线性的，可以用quadprog快速求解；但如果加入了速度障碍法的非线性约束，就必须交给fmincon。
优化问题可以写成如下形式：
options = optimoptions('fmincon', 'Algorithm', 'sqp', 'Display', 'off', ... 'MaxIterations', 50, 'OptimalityTolerance', 1e-4); [uOpt, JOpt] = fmincon(@(u) costFunction(u, state, goal, neighborTraj), ... u0, A, b, Aeq, beq, lb, ub, @(u) nonlcon(u, state, neighbors, obstacles), options);这里最影响求解速度的是初值u0。我踩过一个大坑：每次都用零向量作为初值，导致fmincon在部分情况下找不到可行解或者求解时间很长。后来改成“热启动”策略，也就是把上一周期求解出来的控制序列整体平移一位，末尾补零，作为下一周期的初值。这个改动让平均求解时间直接下降约40%，而且稳定性也明显提升。
4.4 主循环数据流：感知、规划、执行、记录
整套仿真主循环的伪代码如下，方便你把模块串起来看：
for k = 1:simSteps % 第1步：状态更新和邻居发现 pos = updatePosition(pos, vel, u_actual, dt); % 第2步：接收邻居广播的预测轨迹 neighborTraj = receiveNeighborTrajectory(pos, neighborsList); % 第3步：滚动优化求解本机控制序列 u_seq = DMPCPlanner(pos, vel, goal, neighborTraj, obstacleList); % 第4步：防撞修正（速度障碍法） u_safe = collisionAvoidance(u_seq, pos, vel, obstacleList, neighborPos); % 第5步：执行第一个控制量并记录轨迹 u_actual = u_safe(:, 1); trajectory(k, :) = pos'; end这里有一个容易被忽略的设计点：MPC规划器输出的是一整段控制序列，但真正执行时只执行第一个控制量，然后进入下一轮重新规划。这种“滚动优化”机制保证了系统对动态变化有实时响应能力，也是分布式MPC区别于传统开环最优控制的关键。
5. 防撞策略的代码级拆解：速度障碍判定与避让仲裁
防撞是整个项目里最需要抠细节的部分。多机环境中，防撞约束和其他性能指标容易形成矛盾，处理不好就会出现“为了避让一架飞机，结果撞上了另一架飞机”的情况。
5.1 碰撞锥的几何判定：用最接近距离判断风险
相对两个运动物体，能否碰撞的本质是：它们之间的相对位置向量和相对速度向量能否在某一时刻让两物体中心距离小于安全半径。定义相对位置p_rel为障碍物位置减自身位置，相对速度v_rel为自身速度减障碍物速度。若相对速度指向障碍物，则存在碰撞趋势。
具体判定我用“最接近距离”来实现。先计算如果保持当前相对速度，两物体最接近的时间t_closest，再计算该时刻距离是否小于安全半径：
function [ttc, hasRisk] = collisionRisk(p_rel, v_rel, R) vr2 = v_rel' * v_rel; if vr2 < 1e-9 hasRisk = norm(p_rel) < R; ttc = inf; return; end ttc = - (p_rel' * v_rel) / vr2; if ttc < 0 dMin = norm(p_rel); else dClosest = norm(p_rel + v_rel * ttc); dMin = dClosest; end hasRisk = dMin < R; end这种方法比直接计算碰撞锥夹角要直观得多，而且天然能处理非球形障碍物膨胀后的情况。只要把安全半径R设置成无人机半径、障碍物半径和安全裕量之和，一切就都统一了。
5.2 避让方向选择：在期望速度附近寻找安全速度
一旦判定存在碰撞风险，就需要生成一个修正方向。最自然的思路是：找一个满足安全条件、同时又尽可能接近期望速度的速度向量。工程上可以简化为：在当前期望速度方向的基础上，向碰撞锥外侧旋转一个偏置角。
具体做法是：计算期望速度方向与相对速度方向的夹角，判断需要向左还是向右偏转，然后施加一个避让角速度：
alphaDes = atan2(vDes(2), vDes(1)); alphaRel = atan2(v_rel(2), v_rel(1)); turnAngle = 0.35; % 弧度，约20度 if mod(alphaRel - alphaDes, 2*pi) < pi alphaNew = alphaDes + turnAngle; else alphaNew = alphaDes - turnAngle; end vAvoid = speed * [cos(alphaNew); sin(alphaNew)];这个方法在只有单个威胁时非常好用，计算量也小。但在多机紧耦合场景下，两架无人机迎面飞行时各自选一边，可能反而共同选错方向，这就是“对称避让死锁”。解决方式我在下一节展开。
5.3 优先级仲裁与松驰变量：防止死锁和不可行解
对称避让死锁的解决方法是引入优先级。给每架无人机分配一个全局唯一的ID，冲突发生时编号小的保持当前速度不变，编号大的把对方的预测轨迹当作移动障碍物，调用速度障碍法重新规划。这种方案等效于给每架无人机定义了“路权”，规则清晰、实现简单。
另外，MPC求解时防撞约束可能过强导致优化问题不可行。为了避免算法崩溃，我引入一个非负松驰变量epsilon，将防撞约束放松为：
|| p_i - p_j || >= R_safe - epsilon
同时在代价函数里加入对epsilon的强惩罚项。这样在极端情况下，求解器可以通过略微突破安全距离来保证优化问题有解，而代价函数会引导它在下一周期尽快恢复正常避让距离。
不过提醒一句：松驰变量不是用来掩盖碰撞风险的。我在调试时观察到，当松驰变量频繁大于零时，说明这个场景本身就过于拥挤或安全半径设置偏大，应该去调整R_safe，而不是继续依赖松驰变量硬撑。
6. 仿真验证与参数调试：两架到十二架的实际表现
算法实现完成后，剩下的事就是让它跑起来，用数据说话。这一章分享的是一组仿真实验的设计思路、结果对比，以及几个关键参数对系统性能的影响。
6.1 实验设置与评价指标
仿真地图为100m×100m的平面区域。无人机数量分别取2、4、8、12架，起点随机分布在地图一侧，目标点由任务分配算法分配给各自。环境里布置2个移动障碍物，沿给定轨迹循环运动。
单机参数设置为：最大速度5m/s，最大加速度2m/s²，安全半径3m，通信半径30m。MPC预测时域N取12，控制周期dt取0.5s。
评价指标我重点看四个：最小机间距离、碰撞次数、平均到达时间偏差、单步求解耗时。
6.2 不同规模编队的实测结果对比
经过多轮MonteCarlo仿真，结果取100次随机初始状态的均值：
| 无人机数量 | 最小机间距离(m) | 碰撞次数 | 平均到达时间偏差(s) | 单步求解耗时(ms) |
|---|---|---|---|---|
| 2架 | 5.4 | 0 | 0.6 | 11 |
| 4架 | 3.8 | 0 | 0.9 | 16 |
| 8架 | 2.9 | 0 | 1.8 | 39 |
| 12架 | 1.6 | 0 | 3.2 | 86 |
12架时单步求解耗时已经接近0.1秒，相当于控制周期0.5秒的20%。这个数据说明，在我们设定的预测时域和安全半径下，12架已经是这套Matlab框架能稳定支撑的上限。想继续扩大集群规模，需要减少预测时域或者用线性化约束替代非线性约束。
6.3 三个关键参数怎么调：时域、安全半径、惩罚权重
预测时域N如果设得太小，无人机会变得“短视”，大量冲突要靠最后的防撞层去救火，轨迹抖动明显；设得太大会导致单步求解时间成倍增加。经过反复对比，N取10到15之间最平衡。
安全半径R_safe更像一个系统级系数，直接影响机队对空域的利用率。R_safe从3m调到5m后，防撞成功率从97%提升到100%，但平均到达时间增加了约15%，因为无人机往往需要绕更远的路线才能满足宽松的间距约束。所以R_safe不是越大越好，要根据任务对时间和安全的相对重要程度来定。
代价函数里的权重系数同样需要反复试。我通常先固定Q和R，再慢慢调整P。P过小时无人机完全不考虑邻居位置，各自飞各自的，集群协同性差；P过大时无人机过度追求与邻居保持一致，反而降低了整体避让灵敏性。这个系数没有通用答案，只能结合具体地图密度和任务目标来标定。
7. 调试过程中踩过的坑与几个实用的提速技巧
最后这部分是我最想写的，因为很多坑只看论文和源码是遇不到的。如果这些经验能帮你少走点弯路，那这篇分享就值了。
7.1 对称死锁：两架无人机迎面相遇时互相“谦让”
第一次跑双机对头测试时，我眼看着两架飞机在航道中间卡了十几秒，左绕右绕就是过不去。原因就是第5章提到的：双方都检测到碰撞风险，同时向外侧转向，结果避让变成了一眼望去的“镜像动作”，两个期望速度始终对称，谁也避不开谁。
解决分两步：第一步，引入优先级规则，奇数ID避让偶数ID，数量翻倍时效率损失很小；第二步，在速度障碍避让方向的选择中加一个小随机偏置，打破几何上的完全对称。
7.2 障碍物速度估计滞后导致的“假避让”
动态障碍物的速度估计如果比真实速度慢半拍，无人机就会频繁出现“本来不必避让却提前转向”的现象。我一开始用卡尔曼滤波估计障碍物速度，理论效果很好，但仿真中发现移动车辆一旦转弯，滤波输出的速度变化存在明显滞后。
后面改用“差分+滑动平均”的组合后，拐弯场景的假避让问题好了不少。另外还有个经验：把障碍物速度估计的不确定度折算进安全半径，也就是说当速度估计置信度低时，稍稍扩大安全半径，用空间换时间。
7.3 从60秒到12秒：三个有效的仿真提速手段
这个项目的仿真一开始跑得非常痛苦，一组8架无人机的完整实验要跑60多秒。做了三件事之后，总耗时压到了12秒左右。
第一件是把整段仿真改成矢量化的距离矩阵计算。原来每架无人机去找邻居是逐个距离判断，改成用pdist2一次性计算所有无人机之间的成对距离之后，邻居发现这一步几乎不耗时了。
第二件是给MPC做“危险检测预判”。只有当预测轨迹在未来N步内存在与他人距离小于安全半径的趋势时，才启用非线性防撞约束；如果路径很顺畅，直接用线性约束甚至跳过防撞约束。这样fmincon的调用次数大幅减少。
第三件是热启动初值策略，前面已经提过，再把上一周期的最优控制序列通过平移作为下一周期的初始可行解，既加快了求解，也降低了无解概率。
7.4 最后一点经验谈
整个项目跑完，我最深的体会是：多无人机协同路径规划的问题不在算法复杂，而在“如何把规划、避碰、通信、分配这些模块按正确的优先级组合起来”。分布式MPC负责远见，速度障碍负责反应，优先级仲裁负责规则，三层缺一不可。
如果你也要做类似课题，我的建议是：先别急着堆高级算法，先把双机对头、双机交叉、三机汇合这几个最简单的场景跑通，确认防撞逻辑没有死锁，再去扩展集群规模。动力学模型、防撞判定、MPC求解这三个模块一定要做成独立函数，单元测试过了再拼系统。
Matlab虽然跑起来比C++和Python慢，但它调试优化问题太方便了，很多代数和矩阵操作不用自己手写。等算法成熟之后，再把性能瓶颈的模块移植到编译型语言即可。做这类仿真验证项目，Matlab依然是效率最合适的工具，前提是你别把代码写成几千行的单个脚本。
