【机器人订单履行系统】基于高速公路约束的CBS多智能体避撞规划MATLAB仿真

✅作者简介：热爱科研的Matlab仿真开发者，擅长数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

👇 关注我领取海量matlab电子书和数学建模资料

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍

大家好，今天给大家分享一套带高速公路（Highway）单向通行规则的 CBS 冲突搜索 MAPF 代码。MAPF，也就是多智能体路径规划，是移动机器人、仓储 AGV、集群机器人领域非常经典的问题：在同一个栅格地图内，给一群机器人规划从起点到目标点的时间 - 空间路径，避免顶点冲突、边冲突，同时满足路网通行规则。

这份代码的核心创新点是在传统 CBS（Conflict-Based Search，冲突搜索）基础上，增加了高速公路路网分层约束 CBS-HWY，专门给栅格设置横向、纵向高速通道，高速通道带有单向通行限制，非常适合大型仓储、工厂 AGV 集群、机场自动引导车这类有固定主通道的场景。整套代码可以直接运行，输出机器人运动动画，并且自动导出仿真视频，是 MAPF 方向非常高质量的基线仿真框架。

很多刚接触 MAPF 的同学最先上手的就是 CBS。但原版 CBS 大多假设栅格是无规则的，所有格子通行优先级一致。而真实仓储场景里，一般会设置主干道、支线通道，主干道单向通行，减少会车冲突。这个代码就是把 “高速路网单向规则” 嵌入 A 星底层和 CBS 冲突检测流程，更贴近工业 AGV 落地场景。

一、项目整体概述

这套 MATLAB 仿真解决的问题：给定栅格地图、若干台机器人、各自的起点与目标点，在高速公路单向通行约束下，生成无冲突的时空路径，规避顶点冲突和边交换冲突。

核心模块清单：

1. CBS 冲突搜索主函数MRPP_CBSHWY：MAPF 顶层求解器；

2. 带高速路网约束的时空 A 星AStarSTDiffHWY：单智能体路径规划器，支持时间维度、单向道路约束；

3. 栅格坐标与直角坐标互转函数xy2rc / rc2xy；

4. 地图、机器人、货位、充电站初始化函数initialize；

5. 绘图函数plotAll，实时渲染所有机器人、货位、站点；

6. 动画录制模块，自动输出仿真视频 simulation.mp4；

7. 冲突检测模块：识别顶点冲突（同一时间同一栅格）、边冲突（两个机器人交换栅格）；

8. 约束分支逻辑：检测到冲突后，对机器人增加时间 - 位置约束，重新规划路径。

参数配置：

• 地图尺寸：x 方向 61 格，y 方向 29 格；

• 机器人数量 robotNum = 10；

• 货位 podNum = 800；

• 停靠站点 depotNum = 8；

• Highway 分为横向高速、纵向高速，用不同数字标记，代表不同通行方向；

• 最大时间步 MAX_TIME_STEP=300，CBS 搜索最大开节点上限 MAX_OPEN_SIZE=1000。

✅ 已经稳定实现的功能

• 分层路网：普通通道 + 单向高速通道，底层 A 星遵守高速通行方向；

• 标准 CBS 冲突搜索框架，检测顶点冲突、边冲突；

• 冲突发生后二分分支，给机器人增加时空约束，重规划路径；

• 机器人到达目标后原地保持不动，不会消失；

• 仿真动画逐帧刷新，同时生成未来多步预测热力图 HeatMap；

• 自动保存路径 mat 文件，支持二次加载回放；

• 视频导出，适合论文附图、汇报演示。

⚠️ 方案固有局限（论文创新点直接参考） CBS 本身是完备的 MAPF 算法，但存在几个经典短板，叠加高速约束之后也有新问题：

1. CBS 是基于分支的搜索算法，机器人数量变多之后，冲突节点爆炸，求解速度急剧下降；

2. 高速路网规则是提前静态定义，无法动态调整单向方向；

3. 只处理静态任务，起点、目标、地图全部固定，没有动态目标、动态障碍物；

4. 冲突策略是简单二分约束，没有优先级、时序松弛、冲突优先策略；

5. 没有考虑机器人尺寸，是质点栅格模型，没有拓展到占用栅格、安全距离；

6. 不支持延时、等待代价优化，目标函数只优化路径总长度之和。

这些不是代码 bug，是 CBS 基线方案本身的特性，也是后续改进的突破口。

二、代码结构与变量说明

整套代码分为两大块：主脚本仿真初始化 + 动画回放、CBS 求解函数MRPP_CBSHWY。

1. 主脚本部分

开头先做基础参数、地图尺寸、实体数量定义：

xlength=61;ylength=29;robotNum=10;podNum=800;depotNum = 8;

xy2rc、rc2xy是坐标转换函数：仿真绘图用 xy 直角坐标，路径规划内部用行列 rc 栅格坐标，这是栅格仿真里很常见的设计。

地图占用矩阵：

• MapOccupancy

  ：全局地图占用；

• RobotOccupancy

  ：机器人占用栅格；

• PodOccupancy

  ：货位 pod 占用；

• StorageOccupancy

  ：仓储存储区。

Highway 是核心特色：用 cell 数组存储两层高速矩阵。

• Highway {1}：横向高速层，数字 1、3 代表不同横向通行方向；

• Highway {2}：纵向高速层，数字 2、4 代表不同纵向通行方向； 在 A 星内部，走到高速栅格时，只能沿着规定方向移动，反向是禁止通行的，以此模拟仓储主干道单向通行。

之后随机分配任务：给 10 台机器人随机挑选 800 个货位里的一个作为目标，同时随机生成目标朝向。 调用MRPP_CBSHWY求解所有机器人的无冲突时空路径，保存路径文件。

求解完成之后进入动画循环： 逐时间步读取每台机器人路径上的状态，更新全局机器人位置；同时预读未来若干步位置存入 HeatMap，用来做预测热力图（可以二次开发，画出未来冲突风险区域）；每一帧截图写入视频，循环结束后关闭视频写入器。

2. CBS 核心函数 MRPP_CBSHWY

CBS 分为两层：底层是单智能体 A 星，上层是冲突搜索树。 CBS 的搜索树上每一个节点 Node，保存三样东西：

1. 一套当前所有机器人的路径方案 solution；

2. 一组时空约束 constraints；

3. 总代价 cost（所有机器人路径长度之和）。

CBS 标准流程：

1. 根节点：不给任何机器人加约束，每台机器人单独调用 A 星规划路径；

2. 冲突检测：遍历所有机器人两两配对，检查是否存在顶点冲突、边冲突；

3. 如果没有冲突 → 当前方案就是可行解，直接返回路径；

4. 如果检测到冲突 → 分裂两个子节点，分别给其中一台机器人增加时空约束，禁止它在该时间出现在冲突位置；

5. 把子节点放进 OPEN 开表，继续迭代，直到找到无冲突方案。

冲突检测逻辑（这段是 MAPF 的重点）

代码区分两类经典 MAPF 冲突：

• 顶点冲突 Vertex Conflict

  ：机器人 i 和机器人 j，在同一个时间步，占据同一个栅格；

• 边冲突 Edge Conflict

  ：机器人 i 从栅格 A 走到栅格 B，机器人 j 同时从栅格 B 走到栅格 A，也就是交换位置，这是栅格 MAPF 里非常容易忽略的碰撞。

代码里用 ismember 矩阵匹配，判断两条路径的状态行、边状态行是否重合，识别冲突类型，然后生成对应的约束。

分支重规划逻辑

抓到冲突之后，生成两个子节点： 子节点 1：约束机器人 A，禁止它在冲突时刻出现在冲突位置； 子节点 2：约束机器人 B，禁止它在冲突时刻出现在冲突位置； 被约束的机器人，调用带约束的 AStarSTDiffHWY 重新规划路径，其他机器人路径保持不变。 这就是 CBS 二分分支的核心。

重点：底层 A 星AStarSTDiffHWY和普通时空 A 星不一样。它除了接收时空约束，还会读取 Highway 高速矩阵，判断当前栅格是否属于高速通道，校验移动方向是否符合单向规则，违规方向直接禁止拓展。这就是 HWY（Highway）后缀的含义。

3. 路径后处理

A 星原生路径结束在目标点之后就停止了。但仿真动画需要机器人到达终点之后停在原地，所以代码做了填充： 把路径补齐到 MAX_TIME_STEP，到达目标之后所有后续时刻保持目标位置和朝向不变，避免路径长度不一致导致索引报错。

三、核心原理：CBS 冲突搜索 + 单向高速路网

1. 什么是 CBS 冲突搜索

CBS 是 MAPF 领域非常经典的完备算法。 很多新手会先想到 “集中式一起规划”，但是多机器人状态空间爆炸，直接集中式规划很难。CBS 巧妙拆成两层： 底层：单智能体路径规划，速度快； 上层：只负责检测冲突、添加约束，不直接规划全体。 它的优势是：完备性，只要存在可行解，CBS 一定能找到；适合中小规模机器人集群。 短板就是前面提到的，机器人一多，冲突爆炸，搜索树节点疯狂增长，求解很慢。

2. Highway 单向高速路网的设计意义

普通 MAPF 默认栅格四向均可通行，在仓储 AGV 场景里很容易出现主干道会车拥堵。 工业仓储常用的优化手段就是单向主干道：主干道只允许一个方向通行，支线双向。 这份代码把这个规则硬编码进 A 星的拓展邻域： 横向高速栅格，只能向左或者向右；纵向高速栅格，只能向上或者向下。 高速通道的代价可以设置更低，A 星会优先选择主干道通行，减少支线拥堵，这也是仓储里很常用的路网工程方案。

对比：普通 CBS = 无规则栅格；CBS-HWY = 分层路网 + 单向通行约束，更贴合 AGV 仓储。

3. 时空路径的含义

和普通路径规划不一样，MAPF 路径是时空状态，每一个状态包含：栅格行 r、格列 c、朝向 a、时间 t。 不是只关心 “去哪里”，还要关心 “什么时候到”，冲突本质就是时空状态发生碰撞。

四、仿真结果解读

运行代码之后，会依次发生这些事情：

1. 弹出绘图窗口，初始化所有机器人、货位、仓储站点；

2. 控制台打印 tic-toc，输出 CBS 求解耗时；

3. 求解完成后进入循环动画，机器人沿着规划好的路径运动；

4. 动画同步维护 HeatMap，存储未来几步机器人位置，你可以二次开发绘制风险热力图；

5. 循环结束自动保存 simulation.mp4 视频；

6. 工作区保存 AllPathCell，里面是每一个机器人完整时空路径，可以导出画图、统计指标。

可以做几组对照实验：

1. 开启高速路网 / 关闭高速路网：对比冲突数量、总路径代价、求解耗时；

2. 调整机器人数量：5 台、10 台、15 台，观察 CBS 求解时间增长趋势；

3. 修改高速栅格的通行方向，观察机器人绕行策略变化；

4. 调高 MAX_OPEN_SIZE，看能不能求解更难的冲突场景。

五、优化方向与论文创新点（直接可用）

1. 优化 CBS 开表选择策略：原版这里选代价最大的节点拓展，标准 CBS 一般选代价最小（优先最低代价），你可以修改节点选择逻辑，改为优先最低代价，提升求解效率；

2. 高速路网动态调度：现在高速方向是静态写死，可以增加动态单向切换，高峰期切换主干道方向；

3. 引入优先级 CBS：给机器人增加任务优先级，高优先级机器人优先保留路径，减少无效分支；

4. 融合 ECBS、ICBS：用有界次优 CBS 替代基础 CBS，大幅提升求解速度，适合大规模集群；

5. 增加机器人尺寸和安全距离：从质点模型升级为圆形占用模型，避免擦碰；

6. 增加动态障碍物、动态任务：货位任务动态新增，在线重规划；

7. 指标量化：统计总 makespan（最大完工时间）、SOC（路径总和）、冲突数量、求解耗时，绘制对比曲线；

8. 拓展到连续空间，和 APF、MPC 结合，把栅格 MAPF 输出作为上层参考轨迹。

六、写在最后

MAPF 是机器人集群方向非常热门的研究方向，很多同学入门时会卡在 CBS 的冲突逻辑、时空路径、边冲突识别这几个难点上。 这份代码的价值，不只是实现了标准 CBS，而是把工业仓储里非常实用的单向高速路网约束嵌入 CBS 框架，打通了 “理论 MAPF 算法” 和 “AGV 仓储落地场景” 之间的差距。

很多 MAPF 论文里的仿真都是空白栅格，和真实仓库脱节。而仓库里主干道单向通行、支线分流是行业标配。这个框架刚好补上这个缺口，不管是课程作业、毕设、小论文基线对比，都非常合适。

⛳️ 运行结果

📣 部分代码

clc;

clear;

xlength=3;

ylength=3;

MapMat=zeros(ylength,xlength);

MapMat(2,2)=1;

%allPath=CBS(MapMat,[1 1 1;5 1 1],[5 5 1;1 5 1]);

temp=zeros(ylength,xlength);

temp(1,:)=ones(1,3);

temp(2,:)=3*ones(1,3);

temp(3,:)=ones(1,3);

Highway{1,1}=temp;

temp=zeros(ylength,xlength);

temp(:,1)=2*ones(3,1);

temp(:,2)=4*ones(3,1);

temp(:,3)=4*ones(3,1);

Highway{2,1}=temp;

%% Astar test

path=AStarSTDiffHWY(MapMat,Highway,[1 1 1],[2 3 1],[])

🔗 参考文献

🍅往期回顾扫扫下方二维码