空地协同路径规划实践：从任务分配到时间同步

1. 项目整体拆解：空地协同不是“会飞”加“会跑”

拿到“空地多无人平台协同路径规划”这个课题，很多人的第一反应是：无人机管天上、无人车管地上，各跑各的，最后把路径拼在一起不就行了？真这么想就错了。我当初复现完这篇论文的最大感触是——空地协同难的不是单平台路径规划，而是怎么让两种动力学特性完全不同的平台，在同一个任务里做到时间上对齐、空间上解耦、冲突上可控。

先说这个项目解决的核心问题。在典型的多区域侦察、物资转运或搜救场景里，空中平台速度快、视角广、不受地面障碍限制，但续航短、载荷小；地面平台载重大、续航长，但速度慢、受地形约束严重。把这两类平台放在同一个任务里，天然存在三个矛盾：

• 速度差异：无人机巡航速度可能是无人车的3到5倍，同样的任务区域，无人机几分钟能到的地方，无人车要跑几十分钟
• 约束差异：无人车必须严格跟随路网或可通行地形，无人机除了禁飞区和高层障碍，几乎到处能走
• 任务耦合：很多任务必须是空地平台在同一时刻到达同一区域才能完成，比如无人机先抵近侦察，无人车再进场作业——先后关系一旦颠倒，任务就失败

论文的核心贡献，是把这个问题形式化成了一个“先分配、后规划”的两层优化结构。上层用任务分配算法决定哪架无人机、哪辆无人车去执行哪个目标点，下层用路径规划算法在满足各自运动学约束的前提下，算出从当前位置到目标点的可行路径。两层之间通过时间协同变量耦合——这个“时间协同”四个字，是整个项目的灵魂，也是我在复现过程中花时间最多的地方。

为什么这种分层结构是合理的选择？因为路径规划和任务分配耦合在一起，本质上是一个混合整数优化问题，直接求解在计算上是不可行的。比如5个平台对8个目标点，任务分配组合数量就已经是天文数字，还要叠加路径规划的空间约束，全局求解器的计算时间会让人怀疑人生。拆成两层，上层只负责“谁去”，下层只负责“怎么走”，中间通过代价矩阵和时间窗做迭代，虽然未必是全局最优，但工程上收敛快、可解释性强、调试起来不会“一崩全崩”。

我在复现时直接用Matlab而不是Python，有一个很实际的原因：Matlab的绘图和可视化调试能力在这个场景下太顺手了。路径规划算法调试的本质，是反复看“路径为什么弯了”“为什么穿墙了”“为什么两辆车在同一个栅格里打架”，Python用matplotlib也能做，但交互性和实时更新的流畅度确实差了一截。加上Matlab的Global Optimization Toolbox里面现成的ga、particleswarm可以直接用来做任务分配层的求解，能省掉不少造轮子的时间。

再提醒一下，这个课题的代码实现有两条截然不同的路线：一种是完全按照论文的伪代码逐步实现，把每个公式都落地；另一种是调用现成的路径规划工具箱混搭，比如用自动驾驶工具箱里的规划模块再套一层自己的协同逻辑。我这次选择的是前者，因为论文复现的第一目标是“理解”而不是“跑通”——用现成工具箱搭出来，可能一天就能出结果，但论文里那些关键公式你依然看不懂，出了问题也不知道去哪里排查。做科研复现，慢就是快，这个学费值得交。

2. 核心算法解析：任务分配和路径规划，两条腿缺一不可

2.1 任务分配层：不是简单的目标点“抢单”

任务分配层要回答的问题很朴素：这么多目标点，哪一个平台去哪个最划算？但如果真把这个问题展开，里面全是细节。

论文里的任务分配建模方式我建议按“收益最大化”的思路去理解，而不是“路程最短”。很多复现者的直觉是把所有平台到所有目标的距离算出来，然后做一个指派问题求解，让总距离最小。这在单平台场景成立，但在多平台协同场景里会翻车——因为不同平台的代价度量根本不统一。无人机飞1公里和无人车开1公里消耗的资源不同，任务收益也不同，简单加总没有意义。

我复现时采用的建模方式是：

• 目标函数=任务覆盖率+时间惩罚项
• 约束包括：平台性能约束、目标点访问次数约束、时间窗约束
• 时间惩罚项用于惩罚“到达过早”和“到达过晚”——协同任务里，到太早和到太晚一样糟糕，甚至更糟，因为早到的平台可能暴露位置

这里有一个看起来不起眼但影响很大的细节：距离矩阵的计算要不要考虑地形差异？我一开始用的是欧氏距离，结果任务分配结果经常“看起来很美，跑起来就废”——分配器觉得某辆车去某个目标点最近，但实际路径上有一片沼泽或建筑群，真实路径长度是分配器预估的两倍，时间协同直接崩掉。后来我改成用下一层路径规划器的首轮输出作为代价矩阵的输入，做一个循环迭代，效果立刻好了很多。这个“分配-规划-再分配”的交替优化结构，我觉得是论文里没有明说、但实际效果最关键的工程决策之一。

对于求解算法，论文给的框架是遗传算法。Matlab的ga函数我用了，但说实话，直接套用的效果不好——标准遗传算法在平台数量多、约束复杂的时候收敛很慢，而且容易陷入局部最优。我的做法是做了两点改进：

• 编码方式改成双层编码：上层是目标点序列，下层是平台分配掩码，交叉和变异操作分别处理两层，避免产生大量非法子代
• 引入精英保留策略：每代保留前几个最优个体直接进入下一代，防止优秀解被变异掉

这个改进不需要动ga的底层逻辑，只用ga的MutationFcn和CrossoverFcn回调函数接口就行。

2.2 路径规划层：A*框架下的三种形态

路径规划层是论文最核心的算法部分，也是我复现时调试时间最长的地方。论文在这一层用了A*框架，并根据平台类型分化出了三种形态：

无人机路径规划

固定翼无人机有最小转弯半径约束，A的直接栅格移动方式不适用。论文的做法是在A的节点扩展阶段加入运动学约束——每次扩展的不是四周邻居栅格，而是根据当前航向角和最小转弯半径，生成一组可行的航向扇区节点。这种思路本质上是把A*从几何空间搜索变成了状态空间搜索。

我复现时用了一个比较取巧的等价实现：先在几何空间用标准A搜出一条折线路径，再做路径平滑的后处理，用Dubins曲线把折线转角替换成满足最小转弯半径的圆弧过渡。结果在算法层面可能和论文的“带约束A”不完全等价，但在最终路径形态和代价上基本一致。而且这样写的代码要简单很多——标准A加一个平滑后处理模块，比在A内部做状态空间扩展要容易调试得多。

无人车路径规划

无人车用标准A*栅格搜索即可，但有一个非常关键的细节：代价函数的权重要和车的实际通过能力匹配。

复现时我用的是：

• G代价=累计路径长度
• H代价=当前节点到目标的欧氏距离
• 附加代价=经过不同类型地形时的惩罚系数

地形惩罚系数这个设计来源于户外场景的实际需求。同一片区域的路径，草地、土路、沥青路对无人车的通行成本完全不一样，如果不加这个惩罚，A*出来的路径经常会让你气笑——沿着河堤走最短的线路，实际上那是一条车根本开不上去的人行栈道。

空地协同的速度协调

这就是论文标题里“协同”二字的实际落点。无人机路径短、速度快，如果按照各自的默认速度跑，无人机早早就到目标点悬停等待，不仅费电还暴露轨迹。论文用速度协调层来对齐到达时间：先算出所有平台路径的实际长度，再根据最长耗时路径反推统一的任务完成时间，最后给每个平台算出一个参考巡航速度。

这个逻辑看起来简单，实际执行时会遇到一个非常容易被忽略的问题：速度调整幅度不能太大。无人机把速度降到原本的60%以下，或者无人车把速度提到120%以上，都会偏离各自的经济巡航区间，实际耗能反而上升。我复现时给速度协调层加了一个安全域判断，如果平台的最优速度偏离度超过阈值，就通知任务分配层重新做时间窗调整，而不是硬性推一个物理上不合理的速度。

注意：速度协调层不是简单的代数计算，它必须在路径生成之后运行，并且要把输出反馈回任务分配层做第二轮验证。很多初写者只做单向流程（分配->规划->出图），等真实验发现协同时间对不上，又开始怀疑是算法问题。实际上缺的就是这个反馈回路。

2.3 三种算法的适用边界对比

3. 工程实现：从论文公式到可运行代码的完整链路

3.1 框架结构设计：怎么分模块最容易调试

这一节说说代码层面的工程实现。我的实践是把整个项目拆成五个独立模块，模块之间通过接口交互，用主脚本串联：

• Config模块：集中管理地图参数、平台参数、算法参数的配置文件
• Map模块：地图加载、栅格化、障碍标记、可通行区域提取
• TaskAssign模块：任务分配层的遗传算法实现
• PathPlan模块：A*路径规划及路径平滑实现
• TimeSync模块：速度协调和时间对齐计算

这样的分层有几个实际好处。第一，模块间拆开后，每个模块可以单独调试和验证。PathPlan模块可以脱离任务分配单独测试路径效果，TaskAssign模块可以用预设代价矩阵验证分配逻辑，定位问题不需要每次都跑全流程。第二，后续做算法改进时可以只替换单个模块，比如把A换成RRT，改PathPlan模块就够了，其他模块不动。

配置管理模块的信号我建议都用统一的struct来组织，不要在各模块里散写参数。实际复现时，我吃过这个亏：改了参数以后算法行为变了，但找半天不知道哪里的参数改了，回滚都没有方向。把所有参数全部集中到Config模块之后，这个问题的排查效率一下提升了好几倍。

3.2 代价地图构建：所有上层算法的地基

代价地图是整个系统的地基，这个模块的准确性直接决定上层算法会不会做出蠢事。

我构造地图的步骤如下：

• 第一步：读取二值地图图源作为原始数据，白色可通行，黑色障碍
• 第二步：对地图做膨胀处理。膨胀半径根据无人机和无人车的物理尺寸分别设置——无人机膨胀5格（对应物理空间的飞行安全距离），无人车膨胀8格（车身宽+两侧安全余量）
• 第三步：根据图源中的灰度值或者区域标签，设置通行代价系数，形成代价值地图
• 第四步：在代价值地图上叠加禁飞区、禁行区等逻辑约束区域

膨胀处理是必须做的，不然A*规划的路径会贴着障碍物边缘走，实际物理平台上根本执行不了。但膨胀半径不是越大越好——膨胀过大，窄通道会被封死，原本可以通行的路径直接变成断头路；膨胀过小，路径安全距离不足。调试经验是：先用小膨胀半径跑一遍路径，观察实际平台上最窄的通过距离，再逐步加大直到路径形态稳定。

这一层我强烈建议把可视化做顺——把膨胀前后的地图对比、代价热力图、最终路径叠加这三层图画出来放在一个figure窗口里。做路径规划的调试，快速看清地图和路径的关系比什么都重要，Matlab的交互式缩放在这种场景下真的比Python好用太多。

3.3 A*路径规划模块的核心实现

这里给出A*模块的关键代码结构，代码按“可运行骨架”的标准来写，方便直接复现：

function path = AstarPlan(map, start, goal, options) % map: 代价地图矩阵 % start, goal: [x, y] 下标坐标 % options: 结构体，包含启发函数权重、搜索邻域类型等 % 初始化 nRows = size(map, 1); nCols = size(map, 2); gScore = inf(nRows, nCols); gScore(start(1), start(2)) = 0; fScore = inf(nRows, nCols); fScore(start(1), start(2)) = hFunc(start, goal, options); cameFrom = cell(nRows, nCols); directions = [1,0; -1,0; 0,1; 0,-1; 1,1; 1,-1; -1,1; -1,-1]; % 使用优先队列：priority queue可以用最小堆实现 openQueue = InfPriorityQueue(); push(openQueue, [start, fScore(start(1), start(2))]); visitedSet = zeros(nRows, nCols); while ~isEmpty(openQueue) current = pop(openQueue); if isequal(current(1:2), goal) path = reconstructPath(cameFrom, start, goal); return; end visitedSet(current(1), current(2)) = 1; for i = 1:size(directions, 1) neighbor = current(1:2) + directions(i, :); % 越界检查 + 障碍检查（默认map中0为可通行） if neighbor(1) < 1 || neighbor(1) > nRows || ... neighbor(2) < 1 || neighbor(2) > nCols continue; end if map(neighbor(1), neighbor(2)) == 1 continue; end if visitedSet(neighbor(1), neighbor(2)) == 1 continue; end % 计算g代价：考虑方向不同的路劲长度差异 if abs(directions(i,1)) + abs(directions(i,2)) == 2 stepCost = sqrt(2) * map(neighbor(1), neighbor(2)); else stepCost = 1 * map(neighbor(1), neighbor(2)); end tentativeG = gScore(current(1), current(2)) + stepCost; if tentativeG < gScore(neighbor(1), neighbor(2)) cameFrom{neighbor(1), neighbor(2)} = current(1:2); gScore(neighbor(1), neighbor(2)) = tentativeG; fScore(neighbor(1), neighbor(2)) = tentativeG + hFunc(neighbor, goal, options); push(openQueue, [neighbor, fScore(neighbor(1), neighbor(2))]); end end end path = []; % 无路径返回空 end

这段代码有几个值得注意的细节：

• 对角线移动的代价是√2而不是1，这个看起来是常识，但很多初写的代码里会忘记，导致路径倾向走斜线，形态不对
• 优先队列的效率至关重要。我前几个版本用的是普通数组加排序，A*在500×500的地图上跑一次要600ms，换成堆实现以后降到80ms左右。在Matlab里用java.util.PriorityQueue或自写最小堆都可以，千万别用排序数组硬扛
• hFunc的选择直接影响搜索效率。无人机场景用欧氏距离（因为可以任意方向飞行），无人车场景用切比雪夫距离更贴近栅格实际移动代价

3.4 路径平滑与速度分配的衔接

A*出来的路径是折线段，无人机飞这种路径转弯角是苛刻的，无人车也受不了这种原地打折的转向。这一步做Dubins平滑：

function smoothPath = dubinsSmooth(path, turningRadius) smoothPath = []; for i = 1:length(path)-2 p1 = path(i, :); p2 = path(i+1, :); p3 = path(i+2, :); % 计算向量夹角，如果转弯过大，插入Dubins过渡弧 v1 = p2 - p1; v2 = p3 - p2; angle = atan2(abs(v1(1)*v2(2) - v1(2)*v2(1)), dot(v1, v2)); if angle > deg2rad(30) % 插入圆弧过渡 [arcPoints, ~] = dubinsArc(p1, p2, p3, turningRadius); smoothPath = [smoothPath; arcPoints]; else smoothPath = [smoothPath; p2]; end end end

速度协调的实现在平滑之后、任务执行模拟之前。流程是：先根据平滑后的路径逐段累加长度，除以每个平台的经济巡航速度，得到各自的自然完成时间；取最大完成时间作为协同参考时间；最后反算每个平台的参考速度，并做物理可行性校验。

注意：路径长度和轮速执行都是在仿真层面做的，真实硬件平台上还有电机响应延迟和里程计漂移的问题。仿真里速度同步做得再精确，硬件上跑起来都会有偏差。所以代码里要留一个时间缓冲接口，在目标点附近设置等待区，既有的平台可以提前到，但不能早到目标正上方暴露位置，而是停在等待区内，等所有平台就绪后再统一进场。

4. 实操过程：从零搭建到场景复现的完整步骤

4.1 场景定义与仿真参数设置

我复现时用的测试场景定义如下：

• 地图尺寸：400×400像素，对应物理区域400×400米
• 无人机参数：巡航速度15m/s，最小转弯半径30m，最大航程3000m
• 无人车参数：巡航速度3m/s，最小转弯半径5m，最大续驶里程2000m
• 任务目标：覆盖10个目标点，每个目标点都需要至少一个空中平台和一个地面平台在时间窗±10秒内到达

配置参数里的一个关键细节，是协同时间窗的宽窄设定。时间窗±10秒在物理世界已经是很严苛的要求了——无人机和无人车的速度差异摆在那里，能对齐到这个精度说明协同算法刷得很正常。初学时可以把时间窗放宽到±30秒，先把逻辑跑通，再逐步收紧看算法的边界能力。

4.2 需要逐步执行的四个阶段

阶段一：地图预处理与可视化验证

把地图读进来以后，第一件事不是跑算法，而是验证膨胀处理后的地图是否正确。把原始地图和膨胀地图画在一起对比，看哪些窄通道被合理保留了，哪些不该切断的通道被切断了。这一步每次都值得做15分钟，能防止后续所有问题。

阶段二：单平台路径规划验证

在膨胀地图上分别跑无人机和无人车的单平台路径规划，先不启用协同逻辑，确认每一条单路径本身是合理可行的。这一步如果看到路径穿障碍、过大折线、绕远路的问题，直接在这一层解决，不要带着问题进协同层。

阶段三：开启任务分配，验证目标分配结果

把10个目标点加进去，分配器计算结果以后，人工审视分配结果是否合理。我在这一步发现过一个很有意思的现象：分配器倾向于把所有目标都分配给无人机，因为它的速度快、欧氏距离近。人工一眼就能看出这不行——无人机续航根本不够跑完10个点。原因就是代价矩阵缺少续航约束的惩罚项。在代价矩阵里加上平台最大航程的软约束之后，分配结果就明显合理了。

阶段四：协同时间对齐与迭代验证

在最终分配结果和路径都确定后，跑速度同步算法。把10个目标点的到达时间差画成柱状图，应该能看到所有目标点的时间误差都在容限范围内。如果有个别点超限，要么是路径长度估算误差太大，要么是速度调整超了安全域，需要回到分配-规划回路重新迭代。

4.3 全流程效果的可视化呈现

调试的时候把协同过程可视化，我做了四个主视图：

• 左上方视图：全局任务态势图，地图与所有平台路径
• 左下方视图：各平台随时间变化的位置轨迹（带拖影）
• 右上方视图：时间对齐误差图，各平台到达每个目标点的时间偏差
• 右下方视图：速度曲线图

这四个视图在联调的时候帮了我大忙。时间对齐误差图尤其好用——哪个环节有问题，从误差柱状图上能一眼定位是分配问题还是路径问题还是速度同步问题。

5. 常见问题与排查技巧实录

5.1 八个高频问题速查表

5.2 问题3的深入讲解：为什么分配结果经常“偏心”

目标点总分配给无人机这个问题，值得展开讲讲。

根本原因是遗传算法的目标函数是总收益最大化，而无人机的“单位时间收益”天然高于无人车。在平台数量和任务量都不大的时候，求解器会本能地把任务往高效平台身上堆。这在单目标优化视角下没有错，但它违背了“多平台协同”这个前提——协同的意义就是让不同类型平台各展所长，而不是让一类平台包圆所有任务。

我的解决方案是显式加入平台利用率均衡项。目标函数从：

总收益 = Σ(各平台任务收益)

改成：

总收益 = Σ(各平台任务收益) - λ ×(平台间任务量方差)

λ是一个手调的惩罚权重。加入这一项以后，分配结果在协同性上明显改善，无人机和无人车各司其职，从最终路径形态上看也更合理，像是团队作战而不是单兵逞英雄。

5.3 问题7的动态防撞处理

栅格A*规划的都是静态路径，但在多平台同时执行时，即使静态路径不重叠，时间上也可能出现两个平台同时占用同一个栅格的情况。离散事件仿真里这表现为平台在同一栅格的轨迹穿插。

最简单的解决方式是在速度同步层做时间窗避让——对于每对可能占用的公共节点，调整一个平台在该段的通过时间。代价是可能破坏全局时间同步精度，所以要做迭代平衡。更复杂的方案是引入优先级机制：比如无人机在空中，和无人车不冲突；但如果两辆无人车争用同一个节点，让先到达占位的车先通行，另一个在节点附近等待。

实操心得：动态防撞的代码量不大，但调试时最花时间的不是算法逻辑，而是让仿真事件队列里的时间戳和数据结构的行列索引保持一致。出错几乎都是因为这些细节两边对不上，用断点单步追查又慢又烦。我的做法是写一个专门的时间戳快照打印函数，每个平台每运行一个时间步就记录一次位置和时间，把数据导入表格排序后人工检查是否有时空碰撞。一个测试下来几千行数据，但定位问题的速度比手动查快几十倍。

5.4 复现过程中的三条核心避坑经验

第一条：先跑通，再调优。论文里的算法是理论最优的表述，但真实复现时第一次跑通的时候大概率是次优结果。不要急着在第一次跑通时就调参调到完美，先把全流程跑起来、数据记录下来、可视化做出来，再回头逐项优化。这个过程会让调试效率和心情都好很多。

第二条：代码里所有坐标单位统一。地图像素坐标、物理坐标、路径坐标、时间戳索引，这几个之间要保持清晰、可追溯的换算关系。我原代码里有一版混用了像素坐标和物理坐标，排查了好久才发现，简直欲哭无泪。从一开始就在地图模块里写坐标转换函数，后面所有模块一律调用转换函数，这个问题就再也不存在了。

第三条：订阅参数日志。每跑完一次实验，把能记录的都记录下来——地图版本、膨胀半径、遗传代数、每轮的目标函数值、每条的路径长度、速度同步后的到达时间误差。后期做对比分析时，这些日志是判断改进是有利还是有害的唯一依据。没有日志的算法实验，基本等于白跑。

6. 根据我的实际经验，说几个总结性的体会

做这类论文复现项目，真正的收获往往不在“把论文的代码抄通”，而在于复现过程中逼着自己把论文里一笔带过的“废话”重新思考一遍。比如论文可能用一句话说“设置协同时间窗口”，你实现的时候才会发现这句话背后藏着一堆问题：时间窗宽窄怎么定、哪个平台先到怎么办、时间同步不满足时是让快的等还是让慢的赶。这些细节论文通常不会逐字写清楚，但恰恰是这些细节决定了算法能不能在真实任务里落地。

从投入产出比看，多远平台协同路径规划这个方向的技术栈极其通用。A*、遗传算法、Dubins曲线、时间同步，这些组件拆开来看都是非常成熟的技术，寒假里练手也好、组会汇报做demo也好，这套项目做完之后的代码积累，在后续做多智能体仿真、编队控制、分布式决策这些课题时都能直接复用。Matlab代码在算法迭代和可视化验证这个环节的开发效率，也确实让它成为这类复现项目的合理第一选择。

最后讲一个小经验：这个项目做完以后，可以往两个方向继续扩展。一个方向是加动态障碍物，把静态地图换成有移动物体的仿真环境，让平台在路径规划中引入滚动时域重新规划逻辑；另一个方向是加通信约束，模拟真实通信环境下平台之间信息交换受限时的协同策略。两个方向的工程量都在可控范围内，但对这块知识的理解深度会有质的提升。如果你复现完这个项目以后觉得还不过瘾，优先挑其中一个方向继续往下做就对了。