最近在调一个基于多段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 一组典型场景的仿真参数与结果我做过一组比较典型的验证场景参数如下参数取值说明无人机数量3三架同时出动最小转弯半径60 m按速度50m/s、最大坡度约40度估算速度范围[40, 60] m/s飞机巡航速度包络机间安全距离40 m低于该距离判定违规威胁圆数量5半径从100m到200m不等安全余量15 m路径与威胁圆边界的额外间隙粒子数量60种群规模迭代次数50粒子群迭代上限三架无人机从不同位置起飞目标点各不相同但任务要求同时到达。跑完后的结果是总航程大约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样条过渡让路径更适合直接给自动驾驶仪执行。如果你也在做类似的事情先从本文这套静态协同框架着手把基础指标和调参手感打通再往动态方向走会顺很多。