空地多无人平台协同路径规划：从论文公式到Matlab代码复现

把一篇算法论文从公式变成能跑通的代码，本质上是另一场战斗。最近我花了两周时间复现了一个“空地多无人平台协同路径规划”方向的论文项目，用 Matlab 完整实现了论文提出的框架，跑通了流程，也重新理解了协同规划里的门道。这篇博文就把我从读论文、拆方案、写代码到调试出图的完整过程讲清楚，尤其会重点说明空地异构平台协同的算法设计和 Matlab 工程实现细节，适合正在读论文但苦于无从下手的同学，也适合想从算法层面理解多平台协作机制的工程师。

复现之前，我一开始最担心的是论文的算法描述太抽象，公式多但缺少细节。结果真正动手之后发现，问题往往出在“你以为你懂了，但代码不会撒谎”。这句话在我们这个领域尤其适用，路径规划算法放到多平台协同场景里，单机路线和整体系统是两套逻辑，单纯把 A 星或 RRT 各跑一遍根本不算协同。

1. 复现前先把问题拆明白

1.1 论文到底在解决什么问题

这篇论文的核心场景是：一个由若干无人机和若干无人车组成的异构编队，在某个野外区域内执行覆盖搜索类任务。无人机视野广、机动速度快，但续航短、载荷能力有限；无人车地形适应能力强、可以携带搜索设备和物资，但在地面上视野受限、移动速度慢。单独使用任何一类平台都有明显短板，所以要把它们组织成一个协同系统，让无人机负责大面积快速侦察，无人车根据侦察信息进入重点区域执行精细作业。

这个场景其实离现实很近。比如，在野外应急巡逻、大范围作物巡检、灾区物资投送等任务里，空地协同都能显著提升效率。该论文用了某高校实验室的仿真环境，我的复现只针对核心算法部分，不依赖具体硬件。

协同要解决的核心问题有三个。第一个是异构性：无人机和无人车的运动学模型完全不同，无人机是三维空间运动，无人车则在地面二维平面内运动，还要考虑转弯半径，它们各自的路径规划算法和约束条件不一样。第二个是任务耦合：无人机侦察得到的区域信息会改变无人车的目标点分配，反过来，无人车的任务优先级也会影响无人机的航迹重规划。第三个是冲突规避：多平台在共享空域和地面区域中运动，平台之间、平台与障碍物之间都可能发生冲突，需要一个统一的冲突消解机制。

如果只做单机路径规划，这三个问题完全不需要考虑。这也是这篇论文的复现价值所在：空地协同不是简单地把单机规划器拼起来，而是要在系统层面设计协作逻辑。我当时复现前就把论文的摘要、引言、结论反复看了三遍，把问题定义和约束条件摘出来，写成一页纸的参数表，后面建模和写代码时反复对照，避免了做一半发现理解偏差的麻烦。

1.2 论文的算法框架是怎么搭的

读完整篇文章之后，我把论文的算法框架归纳为一个三层结构：

• 顶层：任务分配层。根据无人机侦察到的环境信息和平台能力，用集中式分配算法给每个平台分配目标区域或目标点。
• 中层：单平台路径规划层。无人机用基于采样的规划算法在三维空间生成航迹，无人车用考虑运动学约束的算法在二维地面生成轨迹。
• 底层：协同调控层。从时间维度对多条路径做冲突检测，把冲突路径放进时空占用表中比较，通过等待、避让、重新规划等方式消除冲突。

这种分层架构在国内外的多平台协同文献里非常常见，原因是每一层负责一类问题，解耦清晰，可以单独替换算法。比如任务分配层用贪心算法还是用拍卖算法，不影响中层的路径规划器实现；中层把RRT换成A星也不影响顶层的分配逻辑。代码结构天然模块化，非常适合逐层复现、逐层验证。

我需要提醒一点：论文里的“空地协同”有时会被写成“异构多智能体协同决策”，不同论文的术语风格差异很大。复现时务必先确认“协同”到底体现在哪一层，有些论文其实只是在同一张地图上同时跑无人机和无人车路径，并没有真正的交互策略，那种复现难度会小很多，但参考价值也相对有限。这篇论文值得复现，正是因为协同交互在底层是真实发生的。

1.3 从公式到代码的翻译原则

复现论文最忌讳一上来就翻代码。我的做法是先做“翻译”，分三步：

第一步，把论文里出现的数学符号和变量全部列出来，一一对应到 Matlab 里的数据结构。比如栅格地图就对应二维矩阵，无人机的状态就对应位置坐标、路径点和航向角，任务分配结果就对应平台索引到目标点的映射表。

第二步，把算法描述翻译成伪代码。论文里常写“通过迭代更新使路径代价收敛”这类话，实操时要把它展开成具体的循环结构和收敛条件。论文里一句话，代码里可能就是几十行，这一步不做好，后面写代码会非常痛苦。

第三步，把伪代码翻译成 Matlab。我的翻译原则很简单：保持和论文公式的语义一致，不要“优化过头”。比如论文里写“选择距离最近的未访问节点”，代码里就把距离计算写成一个明确的子函数，先用最直观的方式实现，跑通了再考虑性能。

后面我会详细展开任务分配、路径规划和冲突消解三个模块的代码实现。下面先说说我对核心算法选型和技术原理的理解，这一部分决定了代码的整体走向。

2. 核心算法拆解：协同到底协同在哪里

2.1 单机规划器：无人机和无人车该选什么算法

复现到中层的时候，必须先确定每个平台用哪种路径规划算法。我在论文里看到了两类典型选择：

无人机采用一种基于快速搜索随机树的改进算法。RRT 系列算法在三维连续空间里很受欢迎，因为它不需要对空间做精确建模，靠随机采样就能探索可行域，而且在高维空间中比栅格搜索算法更高效。论文用的变体加入了路径代价优化和剪枝策略，也就是 RRT 的渐进最优版本思路：在已有随机树的基础上，对路径点重新选择父节点，经过多次迭代逐渐缩短路径长度。这种算法对无人机的意义是：可以在比较大的三维环境中快速找到一条可行航迹，且不需要对障碍物做保守的膨胀近似。

无人车则采用一种带运动学约束的局部规划方法，思路类似动态窗口法。无人车在二维平面运动，有最大转向角、最大加速度的限制，还不能原地掉头。动态窗口法会从无人车当前状态出发，在速度空间中采样可行速度组合，以前向仿真模拟一小段时间内的轨迹，再根据“接近目标、避开障碍、保持速度”三个指标评分，选择最优的速度指令。它在多层协同里特别合适，因为每次规划只考虑很短的时间窗口，计算量小，适合无人车在已知地图中边查边走。

这两种算法一表一里，分工清楚。我整理了一个对比表格，方便看不同算法在复现时的取舍：

复现的时候我强烈建议保持论文原始算法的选择，不要因为某个算法自己更熟就擅自替换。因为后面的协同逻辑和算法特性是绑定的，比如论文的冲突消解依赖无人机路径的随机树结构，如果换成了 A 星，树结构就没有了，整个协同策略都要跟着改，工作量会翻几倍。先按原文复现通，再去改造，才是最稳的路线。

2.2 冲突消解：这是协同的关键

中层把每个平台的路径规划好之后，整个系统还远没有完成。多条路径在时间维度上叠加，才会暴露真正的“协同”问题。论文的底层设计了一个时空占用表：路径不再是单纯的几何曲线，而是把几何路径离散成路径点，每个路径点附上平台到达该点的时间区间，整条路径就变成了一条“时空轨迹”。

这个设计我非常喜欢，因为它把无序的多条路径放进了统一坐标系里。每个平台知道自己在什么时刻会出现在哪个位置，两个平台比较各自时空占用表，就能发现“它们在第二十三秒会位于同一个栅格”这一类的冲突。

发现冲突之后，论文采取了一种优先级加等待的消解机制。优先级由任务分配层给出，比如侦察关键区域的无人机优先级更高，普通运输任务的无人车优先级低。当两个平台发生时间冲突时，低优先级平台先尝试在冲突点前等待一小段时间，把自己的整条路径在时间轴上向后平移；如果等待时间超过阈值，就触发局部重规划，绕行到冲突点附近的旁路。

这个机制在 Matlab 里实现起来并不复杂，逻辑上其实就是先检查、再等待、最后改路径的三段式流程。真正麻烦的是判断“等待是否触发阈值”的参数设置，复现时论文对阈值只是一笔带过，我在代码里用二分法手动调了几轮才和论文结果对齐。

2.3 任务分配层的替代方案

顶层任务分配我顺手对比了好几种方案。论文本身用的是带均衡约束的匈牙利算法变体，典型的集中式分配方案。匈牙利算法解决的是“把 N 个待执行任务分配给 N 个平台”的组合优化问题，能在多项式时间内找到全局最优。论文在这基础上加了均衡性约束，避免出现某个平台任务过重、其他平台闲置的情况。

我也试过把分配层替换成拍卖算法。拍卖算法的思路是：每个任务标一个价格，平台根据自身情况对任务出价，出价最高者获得任务，价格再迭代上升，直到所有任务分配完毕。它和匈牙利算法的核心区别是，拍卖算法天然适合分布式场景，每个平台只需要知道自己的出价信息，不需要全局通信。在空地协同里，如果通信条件受限，拍卖算法要比匈牙利算法更实用。复现时我两边都写了，代码损失不大，但让我对“协同分配”的理解深了不少。

这里要强调一个容易忽略的细节：任务分配层的输出会直接决定中层的规划起点和终点。分配稍有偏差，无人机就可能被派到一片侦察价值很低的区域，无人车则要穿越危险地带去回收物资。论文里的评价值函数把距离代价、任务收益、平台适配度三个因素加权求和，这一部分在代码里对应一个目标函数子程序。我复现的时候额外加了一个功能，可以动态画出各平台收益值的柱状图，方便直观判断分配算法是否合理。

3. Matlab 代码实现与工程化思路

3.1 环境建模的三种表达方式

开始写代码前，还得先解决“地图怎么存”的问题。实际复现中，环境建模可以直接影响后续所有算法的执行效果。

• 最简单的是二维栅格地图。用二维矩阵表示环境，矩阵中的 0 表示空闲，1 表示障碍物。代价就是栅格间的距离，搜索算法直接在栅格上工作。这种方式代码量少、调试直观，但缺点是空间分辨率受矩阵尺寸限制，地图一大，内存就上涨。
• 第二种是代价地图。和普通栅格地图的区别是，每个栅格除了“可通行/不可通行”，还额外存一个代价数值。代价可以表示地形起伏、危险程度、通信信号强度等因素。这种表达方式非常灵活，空地协同场景尤其适用，无人机可以避开信号干扰强的区域，无人车可以偏好平坦路面。
• 第三种是三维体素地图。用三维数组表示空间，适合无人机这类在三维空间中运动的平台。但体素地图的存储开销惊人，一般的协同仿真环境不需要真的建一个完整三维体素地图，用“栅格地图 + 高度层”的 2.5D 表达就能满足绝大多数场景。

我复现论文时选择的是栅格地图加 2.5D 高度层：地面无人车在二维栅格地图上规划，无人机在同样尺寸的二维地图上规划，但额外附带一个高度属性。障碍物区的无人机必须飞过低于安全高度的区域时会自动避开，高价值区域则设定最低飞行高度。这种混合表达论文里没有明说，但实测下来最贴合原文算法，也最节省计算资源。

3.2 主程序框架设计

整个 Matlab 仿真项目的主循环分为五个阶段：初始化、任务分配、路径规划、冲突消解、可视化输出。我写的主程序框架如下：

%% 空地多无人平台协同路径规划主程序 clear; close all; clc; rng(2024); % 固定随机种子，保证结果可复现 % 1. 环境初始化：加载地图、生成平台 map = createMap(60, 60, 'obstacleRatio', 0.2); uavList = createPlatforms('uav', 3, map); ugvList = createPlatforms('ugv', 2, map); % 2. 任务分配：生成目标点集合 tasks = generateTasks(map, 5); [assignment, score] = taskAssignment([uavList, ugvList], tasks); % 3. 单平台路径规划 for i = 1:length(uavList) uavList(i).path = planUavPath(uavList(i), tasks(assignment(i)), map); end for i = 1:length(ugvList) ugvList(i).path = planUgvPath(ugvList(i), tasks(assignment(length(uavList)+i)), map); end % 4. 协同冲突消解 [allPaths, allTimes] = conflictResolution(uavList, ugvList); % 5. 可视化结果 visualizeTrajectories(map, allPaths, allTimes);

这个框架很朴素，但符合论文的模块化思路。文件结构也按模块拆：

• createMap.m/createPlatforms.m：负责地图和平台初始化
• taskAssignment.m：任务分配模块
• planUavPath.m/planUgvPath.m：单平台规划器
• conflictResolution.m：协同冲突消解模块
• visualizeTrajectories.m：绘图和 GIF 导出

每个模块内部再细分子函数，例如planUavPath.m里包含采样函数、碰撞检测函数、优化重连函数。模块之间通过结构体传递数据，没有全局变量，后续换算法、调参数都方便。

3.3 关键函数编写要点

下面挑几个最核心的函数，讲一讲实现过程中的关键点和踩过的坑。

首先是taskAssignment.m中的匈牙利算法。Matlab 本身没有现成的匈牙利算法函数，网上能找到第三方实现，但我更建议自己写一遍，代码量不大，写的过程还能加深理解。核心思路是：构造一个代价矩阵，矩阵的每一行对应一个任务，每一列对应一个平台，矩阵值表示该平台执行该任务的代价；然后对矩阵做行列归约，寻找独立的零元素，直到得到一个完整的匹配。

代价函数的定义直接影响分配效果。我复现的时候把代价拆成三项：平台到目标点的欧氏距离、目标点的任务收益权重、平台执行该任务的风险值。这三项加权相加，权重系数一开始取的论文默认值，但结果中一个无人机总被分配到最远的任务，原因是距离项的权重偏低。后来我调整了距离项的权重，分配结果才变得合理。这个坑提醒我：论文里的权重参数通常只是一个参考值，复现时要在整体系统目标下重新校准。

然后是planUavPath.m。RRT 系列算法的核心是随机树数据结构。我用的是一个结构体数组，树的每个节点保存坐标、父节点索引、从起点到该节点的路径代价。采样时在地图范围内随机生成点，查找树上离该点最近的节点，然后从最近节点向采样点方向延伸固定步长，生成新节点；如果新节点与最近节点的连线不穿过障碍物，就把新节点加入树中。

这里有两个性能药方。一个是距离计算用向量化方式，不要写 for 循环逐节点算距离，否则规划一帧可能要好几秒；另一个是在碰撞检测阶段用“步长细分采样”，把新生成的一长段连线均匀分成若干小段，逐段检测是否落入障碍物栅格，这样既准确又快速。

planUgvPath.m相对简单。动态窗口法的核心在一个滚动优化的循环里：每个控制周期，从当前速度空间采样若干组线速度和角速度，对每一组做时间前向仿真，生成一小段轨迹；然后用评价函数打分，选出得分最高的那组速度作为当前控制命令。打分时需要归一化处理，因为距离代价和障碍代价不在同一个数量级。我在实现时设置了一个参数“目标距离权重”和“障碍距离权重”，实测下来权重比 4:1 左右比较合适，这是调参调出来的经验值。

3.4 可视化技巧：把协同过程“演”出来

复现论文的正确输出不只是一条直线路径，而是动态的、带时间维度的协同过程。可视化的好坏直接影响读者对结果的理解，也直接决定论文复现的完成度。我的可视化模块包含三部分：

• 地图和障碍物绘制，用imagesc显示栅格地图，障碍物区域标记为黑色。
• 无人机和无人车的轨迹绘制，用animatedline增量画线，配合drawnow逐帧刷新，可以看路径生成的动画过程。
• 当前平台位置标注：用scatter或自绘符号标注无人机/无人车的实时位置，再叠加文字说明，标识平台编号和当前状态。

每帧结束后可以用getframe抓取图形窗口，配合imwrite循环写入帧数据，输出 Gih 动图。这个动图在汇报时特别加分，因为评审人一眼就能看出协同策略是否有效。

% 动态绘制多平台轨迹的示意代码 figure; ax = gca; hold(ax, 'on'); % 假设 allPaths 是元胞数组，存了各平台的路径 for i = 1:numel(allPaths) plot3(allPaths{i}(:,1), allPaths{i}(:,2), allPaths{i}(:,3), 'LineWidth', 1.5); end hold(ax, 'off');

编写可视化时有一个常见误区：直接用plot画整条路径，完全看不出平台运动的变化过程。应该模拟实时任务推进，逐步展示平台位置和已完成路径。这样才发现协同冲突是否真的被有效消解。

4. 复现过程中遇到的最典型问题

复现类项目，调试和排错的时间往往比写代码还长。我把这次实操中遇到的高频问题整理成了一张速查表，每一个都是真实踩出来的坑。

4.1 地图分辨率与算法无解

刚开始地图栅格划得特别细，想追求精度，结果无人机规划器频繁找不到路径，运行时间翻了近四倍。深层原因在于过度细致的栅格让可行通道变窄，随机树探索极难穿过狭窄区域，同时碰撞检测的计算量巨大。

后面对策是两步：第一，把地图分辨率调到与平台尺寸匹配的程度，不要追求视觉精度，栅格太细只是自找麻烦；第二，在障碍物周围做侵蚀处理，把无人机的安全半径考虑进去。经此调整，算法无解的概率从百分之三十左右降到了百分之五以内。

4.2 随机树搜索慢与代价函数权重失衡

RRT 系列的随机性在带来高维探索能力的同时，也带来规划结果的不稳定。同一个地图、同样的参数，这次规划 3 秒找到路径，下次规划 12 秒才找到，差异非常大。

缓解措施是给搜索树加一个目标偏置：每次采样时，有百分之十到二十的概率直接把目标点作为采样点，这样树会大概率向目标方向生长，搜索效率大幅提升。改造后规划时间稳定在 2 到 4 秒区间。代价函数权重失衡的问题在动态窗口法上更突出，目标距离权重过大时无人车容易直冲障碍边缘，引入转向代价项后系统才恢复稳定。

4.3 数值对不上的“隐藏凶手”

复现论文时最常问的问题是“为什么我的结果和论文的图不一样？”经验告诉我，大概率不是算法逻辑错了，而是下面这几个细节：

• 随机数种子不同。RRT 和动态窗口法都有随机性，没有固定随机种子，任何一次运行结果都不同，和论文数值自然对不上。
• 起始点或目标点占了障碍物栅格。规划器一开始就返回“无路径”，但地图画出来看起来完全正常，需要检查栅格坐标是否越界或重叠。
• 邻域半径的定义方式不同。论文里可能指欧氏半径，代码里误用曼哈顿距离或切比雪夫距离，导致搜索范围差异巨大。
• 路径代价中忽略了转弯代价。单纯比较路径长度时，一条多个锐角转弯的短路径可能被选中，但在真实平台里它消耗的时间、能量都更高。

这些隐藏在细节里的“坑”都很隐蔽。我建议每次复现关键数据前做一个baseline sanity check：先把地图简化成无障碍物环境，跑一次路径规划，看结果是否接近两点间直线；再逐步加障碍物，看路径是否合理绕行。这种从简到繁的验证方式能有效定位问题。

4.4 程序越跑越慢的瓶颈优化

复现中期，整个仿真循环跑一次要近一分钟，完全无法接受。定位瓶颈后发现，拖慢速度的主要是三处：

• 每个平台规划完路径之后，保存路径时用了path = [path; newPoint]这种逐步拼接写法，数组反复扩容导致严重性能消耗。预分配路径矩阵后，速度提升了约三倍。
• 距离计算里用循环逐点遍历树节点，改成pdist2一次性计算所有节点到采样点的距离后，耗时降了一个数量级。
• 冲突检测比较的是所有平台两两之间的时空占用链表，用了嵌套循环，地图大、路径长时复杂度接近 O(N²·M)。优化方式是先用粗粒度时间窗口判断，只有时间窗口重叠的组合才进入细粒度检测，该模块耗时降低了约百分之七十。

Matlab 这类向量化友好的语言，能向量化的地方一定要向量化，逐点循环是性能杀手。

4.5 常见问题速查表

5. 这类复现项目后续还可以怎么扩展

代码跑通之后，我又花了两天时间做了几个扩展实验，发现这个项目的扩展空间比想象中大得多。

第一，把固定任务分配变成动态重分配。论文假设任务在执行前就分配完成，但真实任务里随时可能发现新目标点或被障碍阻断，需要在线重分配。我把任务分配模块拆出来，每次执行完一轮规划后检查任务完成度，未完成任务的区域回收到待分配池，重新触发分配流程。扩展后的系统更能体现“协同”的连续性和动态性。

第二，加入通信距离约束。空地协同最现实的限制是通信。无人机的图传距离、无人车之间的信息中继能力都会影响整体规划。我在路径代价里加入通信惩罚项，平台离中继节点太远时路径代价会升高，系统自然倾向于保持编队通信畅通。这一改动让路径规划的仿真结果更像真实工程实践。

第三，把二维地图推展到三维地形。论文的环境相对理想化，但真实地形有高程起伏。我用数字高程模型数据生成了带地形的栅格地图，无人车规划时自动倾向平地路线，无人机则把高度变化纳入代价。这个扩展让“空地协同”真正做到了不同空间维度的协作，而不仅仅是名字里带“空地”两字。

第四，测试异构平台的动力学约束。原论文的路径规划偏几何层面，平台运动学约束主要在无人车上体现。我给无人机加入了最大俯仰角约束，给无人车加入了最大转向角约束，再把轨迹交给仿真运动学时，发现原本平滑的路径在极限约束处出现了小幅度扰动。这部分联调工作让复现从“算法能跑”进阶到“算法在真实平台上基本可用”。

个人实操中最后的体会

把这篇论文从零复现到跑通，我最深的感受是：读论文是“看懂”，写代码是“做对”，而调参和排错才是“理解”。论文里的很多参数细节并不会白纸黑字写出来，比如冲突等待的阈值、代价项权重、随机树的目标偏置概率，都需要在代码实验中自己找出来。面对这类项目，我建议复现时务必坚持“模块化、逐层验证、先跑通再优化”的三条原则，这样论文复现才不只是把公式敲进 Matlab，而是真正吃透它的设计思想。

最后再分享一个小技巧：复现全程把关键实验数据都记录下来。路径长度、耗时、冲突次数、成功率这些指标，不单是用来写报告的素材，更是判断自己的代码是否真的达到论文要求的硬指标。后续往这个方向扩展研究时，这些数据会是第一手参照系。