多无人机协同路径规划，在最近几年的仿真和工程需求里出现频率非常高。我最近在做一个仿真项目，需要用Matlab实现一套能够应对复杂威胁环境的多机路径规划算法，最终方案选的是“多段Dubins路径+协同策略”。简单说，Dubins路径就是在固定转弯半径约束下，让无人机从起点到终点走一条满足曲率限制的最短路径；而“多段”的意思是，不指望一条Dubins曲线直接穿过所有威胁区，而是把整条飞行路线拆分成多段，段与段之间用直线或圆弧衔接，配合威胁区建模和机间协同，让每架无人机都能安全、同步地到达目标点。这篇文章把整个项目从设计思路、核心原理、Matlab实现到调试排坑的过程完整复盘一遍，适合正在做无人机路径规划仿真、Dubins曲线研究，或者需要快速搭一套多机协同规划Demo的同学参考。
1. 项目整体设计与思路拆解
1.1 问题定位：复杂威胁环境到底“复杂”在哪
很多人一上来就开始写路径搜索算法，但忽略了问题定义。这个项目里的“复杂威胁环境”不是简单几个障碍物，而是包含了多种约束叠加的场景。
威胁环境通常分几类：一是禁飞区，可以建模成多边形或圆形区域，无人机绝对不能进入；二是探测区，比如雷达覆盖范围，虽然可以进入，但会增大被发现概率，所以路径规划时要尽量缩短穿过时间或完全避开；三是地形障碍，直接影响飞行高度和转弯能力；四是动态威胁，比如移动的禁飞区或临时出现的危险区域。对于固定翼无人机来说，它的运动学约束比四旋翼严格得多，不能悬停，不能原地转向，所有转弯都受最小转弯半径限制。这个约束一旦叠加到威胁避让和多机协同里，整个问题的搜索空间和可行路径结构会变得很复杂。
多机协同的难点也在“复杂”这个词里。首先，多架无人机之间要保持安全距离，不能因为路径规划各自独立就导致空中相撞。其次，很多任务要求多机同时到达目标区域，或者按严格时间窗到达，这要求路径规划必须考虑速度调整和等待策略。最后，如果任务周期长，通信受限，每架无人机还要具备局部重规划能力，不能所有决策都依赖地面站。把这些需求放在一起，单机路径规划的办法就很难直接套用。
1.2 方案选型：为什么是多段Dubins路径而不是其他方法
选定方案之前，我对比过好几类主流方法。
A*和RRT这类基于搜索的算法在离散栅格或随机采样空间中找路径，优点是通用、容易处理各种障碍，但缺点是生成的路径往往没有考虑固定翼无人机的曲率约束，后续需要额外的平滑步骤，而且平滑之后很可能重新撞上障碍。B样条曲线虽然能生成光滑路径，但对控制点的选取很敏感，在复杂障碍环境下很难保证全程无碰撞。基于Dubins路径的规划则直接从运动学模型出发，把“最小转弯半径”作为硬约束纳入路径生成，所以更适合固定翼无人机。
但单段Dubins路径也有局限。一条Dubins曲线只能保证从A点到B点满足曲率约束的最短飞行，面对多个威胁区时，如果威胁区把直线通道挡住了，单段路径往往会被迫绕很远，甚至无法绕过复杂地形。多段Dubins路径的核心思路是把整条路线切成多个子段，每个子段仍然是一段Dubins路径，但子段之间通过合理的转向点衔接，这样既能保持曲率连续，又能灵活躲避多个威胁区。
我选择多段Dubins还有一个重要考虑：协同策略可以附着在这些分段点上。比如某一段发现两架无人机会同时经过同一区域，我可以直接调整其中一架的分段点位置或该段的速度，而不是推翻整条路径重新规划。这个“局部可调”的特性，在工程实现里非常实用。项目里最终采用的方案，本质上是一个“全局分段+局部Dubins生成+协同约束校正”的框架。
2. 核心细节解析与实操要点
2.1 Dubins路径基础：曲率约束下的最短曲线
理解Dubins路径前，先想一个生活中的场景：开车转弯。车不能像人走路一样原地转向，它必须按一个半径绕圆弧转过去。无人机也一样，尤其是固定翼，受气动和结构限制，存在一个最小转弯半径。Dubins路径解决的就是：在给定起点位置、起点航向角、终点位置、终点航向角和最小转弯半径的情况下，找一条满足曲率约束的最短可行路径。
Dubins路径的理论结论是，最优路径只有两种基本结构：CSC和CCC。C代表圆弧段，S代表直线段。CSC就是圆弧-直线-圆弧，CCC就是圆弧-圆弧-圆弧。常见的具体类型包括LSL、LSR、RSL、RSR、LRL、RLR，其中L表示左转圆弧，R表示右转圆弧，S表示直线。为什么要做这个分类？因为不同起点终点位置关系下，最优结构不同。比如起点和终点航向刚好对齐，且距离较远时，LSL往往是最优的；如果起点终点比较近，可能就需要CCC结构才能回到目标航向。
在实际计算中，我会把所有坐标转换到以起点为原点的局部坐标系，把终点相对起点的位置和航向差作为参数，再枚举六种候选路径，计算每一条的长度，选出最短且满足约束的那条。这里的核心是圆弧段参数的计算：给定圆心、起始角、转向方向，可以算出弧长；直线段则是连接两个圆弧端点的切线段。Matlab里实现时，我用结构体保存每个航路点、每段路径的类型和长度，后续多段拼接时直接复用这些信息。
注意：Dubins路径的“最短”是在曲率约束下成立的，如果你的最小转弯半径设置得过大，六种候选路径都可能很长，最后生成的路径可能看起来像在“绕大圈”。所以转弯半径这个参数必须结合无人机的实际机动能力来标定，不能随手填一个数。
2.2 多段路径拼接：分段点选择和路径重规划
多段Dubins路径不是把几条Dubins路径简单地首尾相接。直接相接会出现航向跳变，因为前一段终点的航向和后一段起点的航向可能不一致。真正的做法是：在关键转折区域生成“转向点”，每个转向点既作为前一段的终点，也作为后一段的起点，并且让这两个航向角保持一致。
分段点的选择是整个多段路径规划里最考验经验的地方。我的做法是先用威胁区的几何外形生成候选绕飞点。比如一个圆形威胁区，最优绕飞方向是沿切线方向切过去，那么切点就是天然的分段点。对于多个威胁区，按照“从起点到目标点依次经过威胁区边缘”的思路，把每个威胁区的左右切点都纳入候选集合，然后用少量路径点规划算法（比如简单枚举或贪心策略）选出一组可行的转向点。
这里有一个很关键的细节：威胁区不是简单的“点障碍”，它是有半径的。所以路径生成时必须把无人机本身也当成一个有尺寸的物体，用“膨胀威胁区”代替原始威胁区。简单说，如果无人机的安全半径是0.5公里，威胁区半径是3公里，那么路径规划时实际使用的威胁半径至少是3.5公里。这样生成的Dubins路径即使贴着威胁区边缘走，也留有安全余量。
多段路径的可重规划性也很重要。我在项目里做过这样的验证：某架无人机沿着三段Dubins路径飞行时，前方突然出现一个新的静态威胁区。这时不需要从起点重新规划，只需要找到当前所在航段，以当前点为起点，目标点不变，重新生成从当前点到下个分段点的新路径，再把后续分段接上。这种局部重规划在仿真中响应时间可以做到几十毫秒量级，比全局重规划快得多。
2.3 协同策略：时间协同和空间避碰
协同策略不是“每架无人机避开威胁区就行”这么简单。多机协同通常分两个维度：时间和空间。
时间协同要解决的是多机同时到达或按时间窗到达的问题。Dubins路径生成了几何路径，但没有给速度曲线。所以我给每架无人机定义了一个可调的速度区间[v_min, v_max]。每条路径的总长度除以某个基准速度，得到一个基础飞行时间；然后通过调节每架无人机的速度，让它们到达目标点的时间严格对齐。如果某架无人机路径明显更长，就让它飞得快一些，但上限不能超过v_max；如果某架无人机路径很短，就让它飞得慢一些，但不能低于v_min。当某架无人机即使以最大速度飞行也无法按时到达时，就需要重新规划路径，缩短它的绕行长度，或者给其他无人机加一段等待航线。
空间避碰的复杂度更高。最简单的做法是高度分层，让不同无人机在不同高度层飞行；但在同一高度层飞行的场景，就必须考虑水平面上的冲突。我在项目里采用“先独立规划，再统一检测，最后局部调整”的顺序。每架无人机先生成自己的Dubins路径，然后把整条路径离散成一系列带时间戳的航路点，做两两配对的空间-时间碰撞检测。检测到冲突时，优先给其中一架无人机加一个短时速度调整，把它的时间戳往后推；如果速度调整解决不了，就在冲突区域之前插入一个额外转向点，让它的路径绕开冲突区域。
这里有个容易被忽略的坑：很多人在做协同规划时只检查终点是否同时到达，忽略中途交叉。我一开始也是这样，结果两架无人机在中段相遇时的间距几乎为零。所以后来我把整条路径按固定步长离散化，每个航路点都带上到达时间，做的是完整的时空四维检测。
3. Matlab仿真实现与关键参数调优
3.1 仿真场景搭建与环境参数定义
Matlab实现的第一步不是写算法，而是搭一个能重复实验的仿真环境。我习惯把所有参数集中在一个结构体中管理，方便批量跑对比实验。
% 场景参数定义 scenario = struct(); scenario.mapWidth = 200; % 地图宽度，单位km scenario.mapHeight = 200; % 地图高度，单位km % 威胁区定义: [x, y, radius, type] % type=1 禁飞区, type=2 探测区 scenario.threats = [ 50, 60, 10, 1; 120, 80, 12, 1; 100, 150, 15, 2; ]; % 无人机参数 UAV = struct(); UAV.num = 3; UAV.start = [20, 20, 45; 20, 30, 90; 30, 20, 135]; % [x, y, heading] UAV.target = [180, 180, 45; 180, 170, 90; 170, 180, 135]; UAV.minTurnRadius = 8; % 最小转弯半径 km UAV.safeRadius = 2; % 无人机安全半径 km UAV.minSpeed = 0.3; % 最小速度 km/s UAV.maxSpeed = 0.8; % 最大速度 km/s威胁区我用圆形来做，因为圆形在计算切点和碰撞检测时最方便。如果你的场景是多边形禁飞区，可以先求出它的外接圆或者用凸包近似成圆形，再参与Dubins分段规划。值得注意的是，探测区并不需要完全避开，只需要减少穿越路径长度，所以我在碰撞检测函数里对type=2的威胁区做了单独处理：穿越威胁区时记录长度，如果长度超过阈值就重新规划路径。
起点和终点的航向角也要小心定义。很多无人机会从跑道起飞或沿着特定航线进入任务区，所以航向角是实际存在的，不能像普通路径规划那样只给位置坐标。如果忽略航向角，生成的路径可能在实际执行时无法衔接。
3.2 核心算法流程与代码结构
整个算法流程我分成五个模块：场景初始化、单段Dubins生成、多段路径拼接、协同调整、碰撞检测与重规划。每个模块对应一个函数，这样后面单独调参和排查问题时非常方便。
核心流程用文字描述大致是：
- 对每架无人机，读取起点、终点、航向角和最小转弯半径。
- 根据威胁区列表生成候选分段点，这里用到威胁区切点计算和威胁区之间相对位置的判断。
- 从起点出发，依次经过候选分段点，最终到达目标点，生成一条完整的多段Dubins路径。
- 对多机路径做空间-时间碰撞检测，检测到冲突后，先尝试调整速度，再尝试调整局部路径。
- 输出每架无人机的位置时间序列，用于画图和统计指标。
单段Dubins生成函数我会重点说一下输入输出。
function path = dubinsSegment(startPt, startHeading, endPt, endHeading, turnRadius) % startPt: 起点坐标 [x, y] % startHeading: 起点航向角，单位度 % endPt: 终点坐标 [x, y] % endHeading: 终点航向角，单位度 % turnRadius: 最小转弯半径 % path: 包含圆弧段和直线段的结构体数组 % 内部实现: 计算圆心、切点、弧长、直段长度 % 枚举 LSL LSR RSL RSR LRL RLR 六种结构，取最小长度 end这个函数是整个项目的地基。我建议在做多段拼接之前，先单独把这一块测试到完全可靠。怎么测？随机生成大量起点终点和航向角组合，和商业求解器或解析公式对比路径长度和终点的航向误差。我最初就是在这里踩了角度的坑：Matlab默认用弧度，但我在数据定义时习惯写度数，结果有一次生成的路径终点航向完全不对，查了半天才发现是角度单位混用了。后来我在结构体命名时统一用HeadingDeg和HeadingRad，从源头避免单位混乱。
多段拼接的核心是一个循环，按分段点顺序依次调用dubinsSegment，同时保证每段终点航向等于下一段起点航向。实现上我让相邻两段共用一个分段点，分段点的航向由该段的进入方向决定。举个例子，从威胁区的左下切点进入，航向朝上，那么下一段起点航向也跟着朝上，这样路径就是连续的。
function fullPath = multiSegmentDubins(waypoints, headings, turnRadius) % waypoints: 所有航路点，包括起点、分段点、终点 % headings: 每个航路点的航向角 fullPath = []; for i = 1:length(waypoints)-1 seg = dubinsSegment(waypoints(i,:), headings(i), ... waypoints(i+1,:), headings(i+1), turnRadius); fullPath = [fullPath; seg.points(1:end-1, :)]; end fullPath = [fullPath; waypoints(end,:)]; end3.3 参数调优：转弯半径、威胁半径和分离距离
三个参数直接决定生成路径的可用性：最小转弯半径、威胁区膨胀半径、无人机之间的最小分离距离。
转弯半径的物理来源是固定翼无人机的水平机动过载。公式可以简化成 r = v^2 / (g * tan(phi))，g是重力加速度，phi是最大滚转角。飞行速度越大，转弯半径越大；最大滚转角越大，半径越小。在仿真里如果你不知道具体机型，可以按实际需要设定，比如作战场察打任务时速度通常在50-100 m/s量级，对应的转弯半径可能从几百米到几公里不等。我在这套仿真里设为8 km，是因为地图范围200 km，这个半径既能保证航向调整灵活，又不至于频繁触发大幅绕飞。
威胁区膨胀半径更偏向工程安全考虑。用 Russel 的话说，任何路径都不能贴着威胁边界飞行，因为定位误差、风场扰动都会让无人机“压线”。我会在原始威胁半径上加上无人机安全半径和至少10%的额外余量。比如原始半径10 km，无人机安全半径2 km，那么参与路径规划的威胁半径至少是 12 km 再加一个固定余量。这样生成的路径视觉上离威胁区有一段距离，实际执行时也更有底气。
机间分离距离则直接来自任务要求。如果要求三架无人机同时进入目标区域，但彼此间距不能小于2 km，那么路径生成时我不仅要考虑到达时间一致，还要在路径离散化后逐点检查间距。这里有个优化技巧：不需要对整条路径做密采样，可以先做粗采样找潜在冲突区间，再对冲突区间做细采样，能省下大量计算时间。
4. 常见问题与排查技巧实录
4.1 Dubins路径计算发散或航向不匹配
这个我刚开始做时几乎天天遇到。问题通常出在三点：一是起点和终点距离太近，小于两倍最小转弯半径，此时CCC结构会出现退化；二是航向角没有做规范化，比如12度、372度、-348度虽然等价，但三角函数计算时容易把符号搞乱；三是终点航向和起点航向完全相反时，部分结构计算会出现圆心重合，导致圆弧段长度变成零。
解决办法比较直接。遇到距离过近时，把终点沿目标航向方向外推一小段，或者添加一个虚拟中间点。航向角统一用 wrapToPi 函数规范化到 [-pi, pi] 区间，这样所有计算都保持一致。圆心重合的情况，在枚举路径时检查弧长是否接近零，如果是就跳过该结构。
我调试时习惯把六种候选路径都画出来，用不同颜色区分。只画最终选中的最短路径，你会发现“为什么突然绕一大圈”的问题很难定位；把六条都画出来，就能一眼看出是哪条路径的长度计算出现了异常。
4.2 多机路径交叉但碰撞检测没发现
这个问题非常隐蔽。路径交叉不等于碰撞，因为碰撞还取决于时间。但路径不交叉也可能碰撞，比如两架无人机同时在弯道内侧和外侧接近。我在做碰撞检测时最初只检测路径线是否相交，结果漏掉了同一航向区间内距离过小的情况。
后来改成时间优先的检测方式：将每架无人机路径离散化成时间步长0.5秒的位置序列，再两两计算同时刻的距离。如果距离小于安全阈值，就标记为冲突。这个办法虽然计算量稍大，但非常可靠，因为它同时涵盖了路径交叉和同向接近两种场景。一个优化技巧是先用航路点级别的粗筛，比如两段路径的最小包围盒不相交就跳过，相交了再做细采样计算。
另外一个容易漏的是“速度调整后的时间戳不同步”。我一开始用速度调整解决冲突，但只调整了冲突点附近的速度，忘记把调整后的速度变化传导到后续航段，导致后面又出现新的时间漂移和二次冲突。现在我的做法是：速度调整必须作用在整条路径上或者完整的一个分段上，并且调整后重新做全路径时间一致性检查。
4.3 威胁区太多导致路径绕路严重
威胁环境越复杂，多段Dubins的路径长度就越容易失控。一个典型的失败案例是：地图上有七八个圆形威胁区，我为了让每架无人机都完全避开所有威胁区，生成了大量分段点，结果路径曲曲折折，飞行时间比最优路径多出40%以上。
复盘之后，我认为问题出在分段点的生成方式上。贪心生成切点会让路径“见障碍就绕”，但很多威胁区之间是可以通过的，完全没有必要每个都绕。后来我改成两步走：第一步先做一个粗略的可见性检查，判断两个关键点之间直线飞行是否只穿过探测型威胁区，如果是就直接连直线，不再插入中间转向点；第二步才针对禁飞型和必须避开的威胁区求解切点。这样整体分段数明显减少，路径也简洁很多。
另外，威胁区存在重叠时也要合并处理。我在项目里先做一个威胁区预处理，把相互重叠的圆形威胁区合并成一个更大的威胁区或者一组用同一个外接圆。这不仅能减少分段数，还能避免无人机在两个威胁区之间的小通道里强行挤过去。
4.4 仿真性能优化：从离线规划到在线重规划
最初版本的Matlab仿真里，每架无人机规划一次完整路径大约需要0.8秒，三架就是2秒多。对于离线仿真还能忍，但一旦引入动态威胁，需要在线重规划，这个速度就完全不够用。
性能瓶颈主要在多段拼接时的枚举计算。单段Dubins生成本身很快，但多段拼接时每个分段都要枚举六种结构并做坐标变换，分段点一多，总耗时线性增长。我的优化分三步：第一，预计算所有威胁区切点，并且按位置关系排序，减少候选点数量；第二，分段点之间的单段Dubins路径增加一个快速长度估计函数，先用解析公式估算长度是否超过当前最优，超了就跳过；第三，把整个场景的地图离散成半径网格，威胁区位置和切点关系全部提前算好，规划阶段只查表。
优化之后，单机路径规划时间降到了约0.15秒，已经可以支撑简单动态场景的重规划。如果还需要更快，在真机上可以考虑用C++或者代码生成工具把核心函数编译成MEX文件，速度还能再上一个台阶。
5. 验证结果与多场景扩展
5.1 仿真验证指标怎么定
路径规划算法不能只看“能飞出来”，还要有一套可量化的指标。我在这套项目里主要看四个指标：平均路径长度、到达时间同步误差、最小机间距离、威胁区穿越时间。四个指标对应四个不同的目标：路径长度反映经济性，时间同步误差反映协同精度，最小机间距离反映安全性，威胁区穿越时间反映隐蔽性。每次修改参数后，我都要把这四个指标打出来对比。
比如有三个威胁区、三架无人机的典型场景，经过协同调整后，到达时间同步误差控制在1秒以内，最小机间距离稳定在2.5公里以上，所有禁飞区完全避开，探测区穿越总时间也比基准方案降低了25%。这个结果说明多段Dubins路径配合合理的分段点选择，确实比单段Dubins暴力绕飞更高效。
5.2 动态威胁场景下的扩展思路
静态威胁环境验证通过后，我尝试加了两个动态威胁源。动态威胁的处理思路是周期重规划：每5秒检查一次威胁位置变化，如果发现当前路径有冲突，就触发局部重规划。局部重规划的范围只限于受影响的航段，其他航段保持不变。这样既保证了实时性，也避免了整机路径频繁抖动的现象。实测下来，在有动态威胁的场景里，重规划响应时间平均为0.3秒左右，路径成功率从静态场景的98%下降到91%，但已经能基本满足仿真任务需求。
如果要把这套方法推广到更大规模无人机集群，比如10架以上，我觉得需要在协同策略上再做改进，不能只用全局速度调整，最好引入领航-跟随结构或者分布式一致性协议，把计算压力分散到每个节点上。这也是我接下来打算继续做的方向。
提示：开发过程中一定不要把“仿真能跑出来”当成“算法没问题”。我最后一次大改就是因为只关注了最终路径，忽略了中间过程，结果有一次单独看某架无人机的路径，发现它在绕飞后重新汇入主航线时发生了很不自然的180度转弯。后来通过分航段回放才定位到是分段点航向设定错误。多花时间做可视化调试，绝对值得。
我个人在实际操作中的体会是，多无人机协同路径规划最考验人的不是某个算法的数学原理，而是把运动学约束、威胁避让、协同策略和代码实现串到一起的时候，如何在细节处保持逻辑一致。多段Dubins路径这套方案的优势在于，它把问题拆成了“分段”和“协同”两层，每一层都可以单独调试和优化。最后再分享一个小技巧：Matlab绘图时用颜色和线型同时区分不同无人机和不同航段，比如第一架用红色实线、第二架用蓝色虚线、第三架用绿色点划线，每段弯路用更浅的同色系填充，调试时一眼就能看出段间衔接问题。希望这个复盘能帮你少踩几个坑。
