多无人机协同路径规划：基于Dubins路径与粒子群优化的Matlab实践

最近在调一个基于多段Dubins路径的多无人机协同路径规划仿真，整个过程相当有意思：一边是数学上严格的最短曲线，一边是工程上必须处理的威胁区、机间安全距离和同时到达约束。这篇文章就把这个Matlab项目的完整思路、核心代码逻辑和我在调试中踩过的坑一次说清楚，给同样在啃固定翼无人机路径规划的朋友一个可以直接参考的底稿。

复杂威胁环境下的多无人机协同路径规划，拆开来看其实有三个硬骨头：单机的曲率约束路径、多机之间的时空协调、以及威胁区建模与规避。我最后选择的方案，是用多段Dubins路径串起各机航路，再用带有时间可行域检验的种群优化算法做协同，整条链路在Matlab里实现加验证，效果很稳。

1. 问题拆解：多段Dubins路径到底解决了什么

1.1 固定翼无人机的“转身”难题：最小转弯半径

如果你只做过地面机器人或者四旋翼的路径规划，可能很难理解为什么路径规划里要专门搞一套Dubins理论。四旋翼可以原地转向，甚至在悬停状态下调整机头方向；固定翼不行，它必须靠倾斜机身产生向心力来完成转弯，而转弯半径跟速度和最大坡度直接相关，是硬约束。

比如一架以50m/s巡航、最大坡度40度的固定翼无人机，最小转弯半径通常就在几十米到上百米这个量级。你给它的航迹如果是一个直角折线，它根本飞不出来，飞到拐点也拉不起这个弯。所以路径规划的输出不能是“几何可行”，而必须是“动力学可飞”。这就把问题收敛到一条非常重要的数学分支：在平面内给定起点位置、起点航向、终点位置和终点航向，寻找满足最小转弯半径约束的最短路径，也就是Dubins路径。

Dubins的结果很有意思，最短路径不是随便一条曲线，而是六种标准形态之一：LSL、RSR、RSL、LSR、RLR、LRL。L代表左转圆弧，R代表右转圆弧，S代表直线。所有可行最短路径，归根到底都落在这六种形态里。

1.2 威胁环境怎么建模：雷达圈、禁飞圈、障碍圈

“复杂威胁环境”是个很宽泛的说法，落到仿真里你必须先把它数学化。我在这个项目里把威胁统一建模成平面上的圆：每个威胁有圆心坐标和覆盖半径，无人机航迹上任意一点进入圆内就算违规。

实际中这些圆代表的东西五花八门：雷达探测区、防空火力圈、禁飞区、危险气象区，甚至可能是另一架飞机的安全包络。把它们都抽象成圆，有一个非常直接的好处：圆与圆、圆与直线、圆与圆弧之间的几何关系有闭式解，判断路径是否穿过威胁区变得非常快。这一点在后面的优化循环里极其重要，因为代价函数要被调用几百上千次，每次都必须快速给出结果。

但要注意，真实威胁区几乎不可能是完美圆，有些是凸多边形，有些是嵌套结构。对于项目型代码来说，圆形是很好的第一版；如果后面想升级，可以把每个圆拆成多个小圆的组合，或者直接用多边形碰撞检测替换圆检测。我在这个项目里用的就是圆形威胁，加上一个固定的安全余量，相当于把圆半径往外扩了一圈。

1.3 单段Dubins不够用，多段拼接的思路

既然单架无人机、单条Dubins路径只能处理“从一个航向飞到另一个航向”的问题，那在威胁区之间穿行时，一条路径肯定不够。想象一下：起点和终点之间横着两个威胁圆，如果你直接连一条Dubins曲线，大概率会把其中一个圆切开，路径不可行。

解决办法就是引入中间航点，把一条长路径拆成多段，每一段都单独用Dubins算法生成。比如飞机先飞到威胁A边缘的某个切点，绕开A，再飞到威胁B外侧的某个点，绕过B，最后到达目标点。这样整条飞行轨迹就是由多条Dubins曲线首尾相接组成的“多段Dubins路径”。

这里有一个工程细节：中间航点不只是坐标，还必须带航向。因为每段Dubins路径的边界条件需要起点和终点的位置加航向，如果只知道位置、不知道航向，问题就退化成“两点之间任意航向的最短曲率约束路径”，复杂度其实更高。我做项目时会直接把中间航点的航向也作为优化变量来解；如果只是为了快速出结果，也可以把航向固定成下一段路径的方向，两者在仿真效果上有明显差异，前者更稳，代价是搜索维度变高。

注意：多段拼接后，路径整体上仍然满足最小转弯半径约束，但在段的连接点处会出现曲率不连续，这在实际飞行中是可接受的——自动驾驶仪可以在该点快速完成坡度切换，等效为一个瞬时控制指令变化。如果追求全程曲率连续，需要进一步做回旋曲线插值，那是更进阶的话题。

2. 协同设计：不只是各飞各的

2.1 协同任务设定：同时到达与安全间隔

单机路径规划做完之后，难点才真正出现：多架无人机怎么协同。我这里的任务设定是N架无人机从不同起降点出发，各自飞向指定的目标点，但它们必须在同一时间窗口内到达，并且飞行过程中任意两机距离不能小于某个安全值。

为什么要同时到达？很多真实任务里这是刚性需求，比如对目标实施饱和攻击、多机接力侦察、编队数据链同步。如果单纯各飞各的，可能快的先到，慢的后到，任务失败。而安全间隔则是防碰撞的底线，固定翼没法像多旋翼那样急停，机间距离一旦小于安全包络，就是空中相撞的风险。

所以协同路径规划输出的是“路径+时间表”：每一架无人机沿什么样的曲线飞，以什么速度飞，什么时候到达，都要排清楚。这比单机路径规划多了一个维度，难度也上去一个台阶。

2.2 从单机最优到集群折中：目标函数设计

多机协同的目标不是让某个单机路径最短，而是让整个集群的表现最好。常见的指标包括总航程最短、最大单机航程最短、任务总时间最短等。单机航程最短会导致一架飞机拼命绕路抄近道，而另一架路径特别长，最后协同时间被那架最长的拖死。所以我用的是“最大航程最小化”思路，即最小化所有飞机里最长的那个航程，同时给威胁违规、时间偏差、机间距离违规都加了惩罚项。

目标函数大致是：

J = max(L1, L2, ..., Ln) + alpha * P_threat + beta * P_timing + gamma * P_separation

其中P_threat是所有采样点穿入威胁圆的总深度，P_timing是各机到达时间与目标时刻的偏差总和，P_separation是任意两机间距小于安全值的压线惩罚。alpha、beta、gamma是惩罚权重，这三个权重怎么调，我后面在参数调整部分专门讲。

2.3 为什么用粒子群而不是暴力搜索

航点坐标和航向是连续变量，搜索空间大且高度非凸，暴力枚举完全不现实，梯度类方法在非凸地形下又容易卡进局部最优。我在这个项目里用粒子群优化来做协同搜索。

选粒子群有三个实际原因。一是它实现简单，整个核心逻辑不到一百行Matlab代码；二是它天然支持多目标加权目标函数，不需要求梯度，只要把每条候选路径的适应度算出来即可；三是Matlab的并行计算工具箱可以直接把种群粒子分配到多个worker上并行评估，速度提升很明显。

当然粒子群不保证全局最优，它只是用“群体智能”在可行域里找一个足够好的解。对路径规划这个场景来说，关键是给它一个合理的初始分布，后面我会讲到怎么用单段Dubins做初始种子，避免它一开始就在不可行区域瞎撞。

3. Matlab代码实现：从单机Dubins到多机协同

3.1 模块划分：别把主逻辑和细节搅在一起

这个项目我最初写成一个巨型脚本，两千行塞在一起，调参的时候想死的心都有。后来重构成了清晰的模块划分，合理化很多。完整代码不贴了，但核心结构值得参考：

• map_setup.m：设置威胁圆、无人机起点终点、速度范围和各类安全参数
• dubins_path.m：单段Dubins路径生成，输入起终点位置航向和转弯半径，输出采样轨迹和路径长度
• multi_seg_path.m：将多个航点序列转换成多段Dubins路径
• threat_check.m：对整条路径逐点检测威胁圆违规
• objective.m：把各机路径整合成代价函数
• pso_optimize.m：粒子群主循环
• main.m：把上面所有模块串起来跑仿真

每个函数只做一件事，参数通过结构体传递，这样后面调威胁惩罚权重、改速度区间、加动态威胁都很方便。别为了省事把所有东西堆在一起，这个项目后续一定会有改动，模块化是保命的。

3.2 Dubins单机路径生成核心函数

Dubins路径的经典实现思路是：根据起点和终点的航向，求出左右两个转弯圆的圆心，然后枚举六种候选路径，算出每一条的圆弧长度和直线长度总和，取最短的那条。

代码骨架如下：

function [path, cost] = dubins_path(P0, theta0, P1, theta1, R) % P0, P1: [x, y] 起点和终点 % theta0, theta1: 航向角，单位弧度 % R: 最小转弯半径 % path: N x 2 的采样点 % cost: 路径总长度 % 右转圆心: P + R * [sin(theta); -cos(theta)] % 左转圆心: P - R * [sin(theta); -cos(theta)] C0L = P0 + R * [-sin(theta0), cos(theta0)]; C0R = P0 + R * [ sin(theta0), -cos(theta0)]; C1L = P1 + R * [-sin(theta1), cos(theta1)]; C1R = P1 + R * [ sin(theta1), -cos(theta1)]; % 枚举六种组合，对每种组合计算 % 圆弧 + 公切线 + 圆弧 % 记录最短路径 % 最后沿最短路径按照步长采样 end

这段代码里最复杂的是求两个圆弧之间的公切线，尤其是RLR和LRL这种包含两段同向圆弧的形态，几何关系容易算错。我建议你先写一个只处理四种包含直线段的形态，LSL、RSR、RSL、LSR，很多场景已经够用；等确认无误后再补RLR和LRL。

采样密度很关键。我一般控制相邻采样点之间的距离不超过5米，这样既能保证威胁检测不漏检，也不会让数据量爆炸。如果飞机数量多、路径长，采样点太多会拖慢优化速度，这时可以先稀疏采样做优化，最后对最优解再加密采样做展示。

3.3 多段路径拼接与威胁校验

多段路径的输入是一串航点和一个航向列表，每两个相邻航点之间调用一次dubins_path，然后把结果拼接起来。这个过程看似简单，但有一个非常容易踩的坑：Dubins曲线默认路径是从起点航向开始、到终点航向结束，如果你的中间航点航向和段与段之间的连续方向不匹配，就会出现飞机在空中“甩头”的假象。

我在做法上倾向于把中间航点的航向一并交给粒子群优化，这样每一段Dubins都能精确满足边界条件。为了让优化更容易收敛，可以在初始化时把航向设成指向下一航点的方向，然后让粒子在这个附近扰动。

威胁校验函数必须对整条采样路径逐点检查：

function safe = threat_check(path, threats, margin) % threats: N x 3，每行是 [cx, cy, r] % margin: 安全余量 safe = true; for i = 1:size(path, 1) p = path(i, :); d2 = (p(1) - threats(:,1)).^2 + (p(2) - threats(:,2)).^2; if any(d2 < (threats(:,3) + margin).^2) safe = false; return; end end end

注意：不要只校验航点，要校验航点之间的整条路径。圆弧段中间很可能比两个端点更靠近威胁中心，只查端点会有漏网之鱼。我一开始就吃过这个亏，看起来路径绕开了威胁，实际上圆弧扫过了一片威胁边缘。

3.4 协同时机调度与种群优化主流程

主流程这部分是整个项目的灵魂。我的设计思路是：粒子群的每个粒子代表一套多机航点配置，评估该配置时先为每架无人机生成多段Dubins路径，然后做威胁检测和机间距离检测，最后做时间协调分析。

时间协调的核心逻辑很关键。假设第i架无人机的路径长度为Li，速度允许范围是[vmin, vmax]，那么它可达的到达时间区间就是[Li/vmax, Li/vmin]。如果所有无人机的区间存在交集，就取交集内的某一个共同到达时间T，再给每架飞机分配巡航速度Li/T。如果交集为空，说明某架飞机航程太短或者太长，无法和队友同时到达，此时要么惩罚这个粒子，要么允许在路径上增加一个半径等于最小转弯半径的盘旋圈，用Dubins的圆环段把航程补长。

我采用的是这两种方式的结合：先在目标函数里给时间不可行性加一个较大的惩罚，强制优化过程往可行方向走；等到粒子群迭代到后期，再检查最优解是否需要加盘旋圈，如果需要就直接在最优路径末尾拼接一整圈。

粒子群主循环的伪代码：

% 初始化种群位置与速度 for iter = 1:maxIter for each particle % 解码航点 % 生成多段Dubins路径 % 威胁检查 % 机间间距检查 % 时间可行域检查 % 计算适应度 end % 更新个体最优和全局最优 % 更新粒子速度与位置 end % 输出全局最优路径

在Matlab里建议把粒子评估写成独立函数，用parfor替换for做并行评估，速度会快非常多。我用4个worker跑三架飞机的场景，50次迭代、60个粒子，大概一分多钟就能跑完。

3.5 仿真结果可视化与指标输出

可视化看似锦上添花，实际上是排查问题的利器。我会把威胁圆、各机路径、航向箭头、目标点画在同一张图上，再用不同颜色区分飞机。每次迭代结束后，把当前最优路径更新到图上，观察路径的形态变化，比看数据直观得多。

指标方面我必看四个：每架无人机的路径长度、到达时间偏差、任意两机最小间距、路径违规深度。这四个指标写成一个结构体存下来，每次跑完实验直接统计对比。没有量化的“感觉还行”在项目里是行不通的。

4. 仿真结果分析与参数调整实录

4.1 一组典型场景的仿真参数与结果

我做过一组比较典型的验证场景，参数如下：

三架无人机从不同位置起飞，目标点各不相同但任务要求同时到达。跑完后的结果是：总航程大约5200m，三机到达时间误差控制在0.3秒以内，任意两机的最小间距大约45米，没有进入威胁区。这个结果在工程上完全可用，特别是时间协调效果让我比较满意。

4.2 协同效果怎么看：时间误差与最小间距

单看路径图，会感觉“绕得挺好看”，但协同效果必须用量化指标证明。我习惯输出两个核心数据：到达时间误差的绝对值和全程最小机间距离的分布曲线。

到达时间误差很好理解，就是各机实际到达时间减去设定共同到达时间后的差值。理想情况下这个误差应该是0，但因为速度分配是按路径长度反比算的，实际数值计算会引入微小误差，0.3秒以内完全能接受。如果你发现误差达到秒级以上，多半是粒子群搜索到的时间区间交集选择有问题，或者速度没有按整数分档。

机间距离是另一个重点。我会把三架飞机的两两距离在每一个时间步长上算出来，画成随时间变化的曲线。如果曲线上出现了低于安全距离的凹陷，说明两条路径在空间或者时间上太靠近了，需要增加分离惩罚权重，或者让粒子群的种群数量扩大一点。

4.3 参数微调经验：转弯半径、威胁半径、速度区间

参数这东西，不同场景要命程度完全不同。先说最小转弯半径：它基本由飞机动力学决定，不能随便改，但如果用这个仿真做原理验证，可以把它当作可调参数。半径越大，路径越平滑，但绕行距离越长；半径太小，曲线又容易贴着威胁区边缘。我实测下来，威胁区半径在100到200米的情况下，最小转弯半径设成60到80米是一个比较舒服的区间。

速度区间对协同可行性影响巨大。区间越宽，时间可行域越容易有交集，但低速度档会让总任务时间拉长；区间越窄，协同约束越难满足。我的建议是先从实际的飞机飞行包络取值，然后稍微放宽10%到20%作为优化余量。如果你发现算法经常因为无时间交集而给出超长绕路路径，那就是区间太窄了。

惩罚权重alpha、beta、gamma的调法也有规律。我习惯先用一组比较大的威胁惩罚把路径逼出威胁区，这个阶段路径往往特别长；等到威胁不再违规，再逐步加大时间惩罚和分离惩罚，把路径拉回合理长度。这个过程有点像是“先保安全，再优化规划”，比一开始就设置均衡权重好收敛得多。

5. 我踩过的坑：常见问题与排查技巧实录

5.1 路径穿威胁区，校验却说通过

这个坑我真的栽过两次。第一次是只检查了航点，没检查采样点；第二次是采样间隔太大，直线段和圆弧段中间恰好跨过了一个小威胁圆。解决方案很直接：先把采样步长调小，比如2米一采，再做逐点检查。另一个容易被忽略的问题是安全余量没加够，飞机路径离威胁边界太近，误差一累积就进去了。我后来在威胁半径上额外加了一个不小于5米的安全余量，同时把最小转弯半径的数值也考虑进去，因为转弯半径大的飞机，在切点附近会往外侧“扫”一块区域。

5.2 Dubins函数偶发返回NaN或路径角度跳变

这个问题的根源在于Dubins算法里的边界情况没有处理干净。比如起点和终点距离特别近，或者起点航向和终点航向几乎相同时，两圆之间的关系会退化成特殊情况，数学公式出现除零或者反三角函数越界。解决办法是在函数开头加防御逻辑：

if norm(P1 - P0) < eps path = [P0; P1]; cost = 0; return; end

另外，航向角要用atan2规范化到[-pi, pi]，避免角度差跨过2pi边界导致切线方向突变。这类问题属于“跑一万次偶尔崩一下”的类型，不处理的话优化循环跑到一半就会停，非常恶心。

5.3 粒子群不收敛或者收敛到明显绕远路的解

粒子群的问题多半出在初始种群上。如果你随机生成航点位置，很多粒子可能一开始就深陷威胁区内部，惩罚项巨大，梯度方向全是“往外逃”，根本找不到好路径。我的做法是先不管威胁，直接算一条从起点到终点的单段Dubins路径，把它当作一个初始种子放入第一代粒子；再在它周围生成若干扰动粒子。这样做之后，算法从“已经能飞”的基础上开始优化，收敛速度提升非常明显。

还有一个原因：搜索空间范围设置太大。中间航点不需要在整个地图上乱跑，把它限制在起点和终点连线附近的走廊区域即可，比如距离连线不超过几百米的矩形范围内。这样搜索效率高很多。

5.4 多段连接处出现掉头或者折返现象

当航点航向属于优化变量时，粒子群偶尔会给出一个“让飞机在航点原地掉头”的解。原因是为了匹配前后两段Dubins的边界条件，航向被推到了接近反向的位置。这在数学上是等价的，在物理上完全不可飞。

我的排查方法是画出航向变化曲线，看相邻两个航点的航向差是否超过某个阈值。为了避免这个问题，我在目标函数里加了一个航向变化惩罚：相邻航点航向差超过90度就扣分。这样即使某个局部解能通过掉头缩短航程，也会被代价函数压制下去。这个惩罚项的加入是我这个项目里体验最明显的改进之一。

6. 个人体会与后续扩展方向

这个项目做下来，我最大的一个体会是：多无人机协同路径规划的难点不在于单机Dubins，而在“协同”二字。单机路径再漂亮，放到多机时间轴上一切冲突都可能冒出来。所以一定要把时间约束和空间约束放在同一个目标函数里迭代优化，而不是先做几何路径、再做速度分配，那样容易出现“路径出来之后速度根本协调不了”的结果。

代码层面我还想提醒一句：这个框架里真正值得花时间调试的是惩罚权重和初始种群，而不是Dubins函数本身。Dubins是成熟标准工具，网上有大量开源实现；但怎么把威胁检测、机间分离、时间协调编织成一个可控的优化问题，才是项目成败的核心。

后续我打算在这个框架上继续加两个方向。第一个是动态威胁，也就是威胁圆的位置随时间移动，这需要把时间维度真正引入路径规划，不能再用静态几何；第二个是曲率连续化，在多段Dubins连接点处用回旋曲线或B样条过渡，让路径更适合直接给自动驾驶仪执行。如果你也在做类似的事情，先从本文这套静态协同框架着手，把基础指标和调参手感打通，再往动态方向走，会顺很多。