多机器人协同避障MATLAB仿真平台搭建实战指南

简介：一套基于MATLAB的多机器人路径规划与协同避障仿真平台，面向人工智能、机器人学及智能交通方向的研究者与本科高年级学生，可用于验证图搜索、遗传算法、粒子群优化等算法在多智能体系统中的路径搜索与避障效果。资源共80个文件，压缩包约1011KB，其中以44个m脚本为核心，包含控制器、仿真器、机器人模型等模块；另有Simulink模型、MATLAB App安装包及Prj工程文件支持直接加载运行；18张png截图与手册PDF、说明文档可辅助理解代码结构与操作流程。已有2968人学习下载，适合需要快速搭建多机器人协同避障实验环境的读者。借助这套平台可直观查看机器人在共享工作区中的动态路径规划，理解分布式通信与避障协调机制；文档对安装、运行及代码逻辑作了说明，便于二次开发与算法替换，是入门多机器人路径规划的实用参考。

1. “可直接运行”到底意味着什么：多个机器人别再互相堵死

接到过一个仓库搬运的模拟需求：两台机器人在一条两格宽的走廊里迎面相遇，单看每一台的路径，A* 都给它们导出了一条“完全正确”的路线，结果一放进去就死锁，谁也让不了谁。做多机器人路径规划的人，基本都经历过这个瞬间——单机规划跑得再顺，也只是起跑线，真正的难点在“多机协同”这四个字上。标题里这个 MATLAB 仿真平台，就是把路径规划、协同避障、可视化验证打包起来，让你不用从零搭轮子，改改地图和参数就能跑。

它解决的是三个具体问题：算法选型怎么落地、协同避障怎么做、仿真参数怎么调才不翻车。适合正在做算法验证、项目预研、毕设或课程设计的人，尤其是需要用仿真结果说服别人接受方案的人。我下面按照自己搭这类平台的实际顺序来讲：先选单机算法和协同策略，再搭仿真框架，盯住关键参数，然后排雷，最后验证可扩展性。

2. 多机器人路径规划选型：先解决单机，再解决协同

很多教程会把 A*、RRT、人工势场法一口气列出来，好像“随便选一个都行”。实际上在多机器人场景里，选型直接决定协同层要填多少坑。先选错，后面怎么写协同避障都是补救。我一般把选型拆成两层：单机规划负责“这条路能不能走”，协同策略负责“我们俩别撞上”。两层分开选，组合起来才有可维护性。

2.1 单机路径规划：A*、RRT、人工势场怎么选

先看三张表的对比。对于静态栅格地图上的多机器人调度，A* 是绝对主力，因为它能给出确定性的最短路径，而且栅格地图正好是它的主场。RRT 的优势在高维或者连续空间，在 2D 栅格里反而有点多余——路径不光不光滑，还要额外做后处理。人工势场法我拿来处理“局部避障”这一段，不太建议用它做全局规划，因为容易陷进极小值点，也就是机器人明明绕一下就能过去，它偏偏在障碍物前来回晃。

一个可用的 A* 核心函数长这样，我简化过，去掉了很多工程细节，只保留关键逻辑：

function path = astar_plan(grid, start, goal) % grid: 0 可通行, 1 障碍物 % start, goal: [row, col]，注意是行列坐标 [rows, cols] = size(grid); gScore = inf(rows, cols); fScore = inf(rows, cols); parent = zeros(rows, cols, 2); % 记录每个节点的父节点 gScore(start(1), start(2)) = 0; fScore(start(1), start(2)) = heu(start, goal); % 用 containers.Map 做 open 集合，示意理解；性能要求高时换成堆 openSet = containers.Map('KeyType','char','ValueType','any'); openSet(keyOf(start)) = start; while ~isempty(openSet) % 从 openSet 中取 fScore 最小的节点 curr = popMinF(openSet, fScore); if isequal(curr, goal) return reconstructPath(parent, curr); % 回溯路径 end % 遍历上下左右四个邻域 for d = [1 0; -1 0; 0 1; 0 -1] nei = curr + d; if ~inBounds(nei, rows, cols), continue; end if grid(nei(1), nei(2)) == 1, continue; end % 障碍 tmpG = gScore(curr(1), curr(2)) + 1; if tmpG < gScore(nei(1), nei(2)) gScore(nei(1), nei(2)) = tmpG; fScore(nei(1), nei(2)) = tmpG + heu(nei, goal); parent(nei(1), nei(2), :) = curr; if ~openSet.isKey(keyOf(nei)) openSet(keyOf(nei)) = nei; end end end end path = []; end

逻辑上它就是一个标准 A*：维护 gScore 表示起点到当前点的真实代价，fScore 表示估算总代价，每次从 open 集合里拿 fScore 最小的节点扩展。这里启发函数 heu 我通常取欧氏距离，因为机器人可以斜着走；如果你只允许四方向移动，曼哈顿距离更合适。参数上需要注意的就是对角线扩展，如果地图允许斜走，邻域要从 4 个方向扩到 8 个，并且对角线方向的代价改成约 1.414，否则规划出来的路径会有锯齿感。

2.2 协同避障思路：速度障碍法、优先级规划与交通规则

单机路径解决后，进入协同层。我在不同项目里试过三种做法：速度障碍法、优先级规划、交通规则。先说结论：小规模、任务可调度的场景，优先级规划最省心；机器人数量多、轨迹不可预知时，速度障碍法更稳；通道特别规整的仓库，交通规则反倒是性价比最高的方案。

速度障碍法（VO）的核心思想是：把其他机器人看成一个圆形障碍，再加上相对速度，判断当前速度是否会在未来某段时间内导致碰撞。如果会，就生成一个可选速度集合，从里面选一个最接近原速度且安全的。下面是一个极简判断函数：

function safe = isVelocitySafe(vSelf, vOther, pSelf, pOther, safeR) % 计算相对速度与相对位置 relV = vSelf - vOther; relP = pOther - pSelf; dist = norm(relP); if dist < safeR safe = false; return; end % 相对速度方向与障碍方向的夹角 angleToObs = atan2(relP(2), relP(1)); angleV = atan2(relV(2), relV(1)); diffAngle = abs(angdiff(angleToObs, angleV)); halfCone = asin(safeR / dist); % 如果相对速度方向落在碰撞锥内，并且相对速度在靠近，则危险 if diffAngle < halfCone && dot(relV, relP) < 0 safe = false; else safe = true; end end

这个函数看着简单，但它是很多 VO 类算法的基础。参数 safeR 是安全半径，要包含机器人本体半径加额外裕量。实际工程里我不会直接丢掉危险速度，而是会收集所有可选速度，选一个“调整量最小”的安全速度。这个调整量可以是角度变化，也可以是速度大小变化，取决于你更在意通行效率还是运行平稳。注意这里“差角”判断用的angdiff在 MATLAB 里可以直接用，如果自己写要处理 ±π 的环绕问题。

优先级规划则简单很多：给每个机器人一个全局唯一编号，编号小的优先走，编号大的看到冲突就让行。具体实现时，每个机器人在路径跟随前会检查“当前点上，谁的优先级更高”。这种方案的坑在于：如果优先级分配不合理，低优先级机器人永远等不到通行机会，也就是所谓的“饿死”。因此我在做优先级表时，会按任务截止时间排，而不是按编号排。

2.3 不同规模场景的算法组合建议

选型不是越复杂越好，是要匹配场景规模。我在表里列一下常见组合，后面参数章节会再细说每个组合里要设什么：

我遇到很多从论文里搬来多机器人算法的开发者，一上来就上强化学习或者分布式一致性算法，结果在 MATLAB 里调了一个月还没跑通基准用例。其实对于绝大多数陆地轮式机器人场景，A* 生成全局路径，配合优先级或 VO 做局部避让，已经能覆盖百分之九十的需求。关键是后续把参数调对，而不是上来就换更复杂的算法。

3. 搭建可运行的 MATLAB 仿真平台：地图、机器人模型与主循环

仿真平台要能做到“可直接运行”，本质上是把三样东西约定好：地图数据格式统一、机器人模型确定、主循环的时序清晰。很多项目死在第一步，地图一个函数生成，另一个函数读取，行列顺序不同，后面对不上。下面这套架构是我重复用过多次的，放在一起能跑一个完整的多机器人协同避障 demo。

3.1 用栅格地图描述环境：makeMap 的写法与坐标约定

地图我统一用矩阵表示，0 表示可通行，1 表示障碍物。为什么不用 occupancyMap？主要原因是用户自定义方便，修改一个矩阵比在交互式地图编辑器里拖动省事，而且矩阵可以直接作为 A* 的输入，不需要转换。

function map = makeMap(rows, cols, obstacles) % 默认全零矩阵，所有格子可通行 map = zeros(rows, cols); % obstacles 是 [row, col] 形式的列表 for i = 1:size(obstacles, 1) r = obstacles(i, 1); c = obstacles(i, 2); if r >= 1 && r <= rows && c >= 1 && c <= cols map(r, c) = 1; end end end

这个函数的核心价值不是矩阵本身，而是强制约定了一个坐标规则：row 表示行，从上往下数；col 表示列，从左往右数。在 MATLAB 里显示图像时，imagesc(map)默认 y 轴也是从上往下递增，这跟数学里常用的 x-y 坐标不一样。我建议从一开始就不要混用两套坐标，统一用行列坐标，直到最后推送控制指令前再转换。换坐标系是很多多机器人仿真翻车的根源，后面第 5 章会专门讲。

3.2 机器人运动模型：为什么差分驱动是默认选择

仿真里的机器人运动模型，我默认选差分驱动。原因很简单：大多数轮式移动平台都是差分驱动底盘，前向运动时容易建模，转弯半径受速度差控制，仿真结果挪到真机上时不需要重新标定运动学。全向轮模型在某些场景下确实方便，但真机落地时底盘不一定支持，容易出现“仿真能跑，真机跑不了”的尴尬。

function state = updateDiffDrive(state, v, w, dt) % state: [x; y; theta]，theta 是朝向角 % v: 线速度, w: 角速度, dt: 时间步长 x = state(1); y = state(2); th = state(3); dx = v * cos(th) * dt; dy = v * sin(th) * dt; dth = w * dt; state = [x + dx; y + dy; th + dth]; end

这个更新函数里三个输出量的物理意义要清楚：线速度 v 决定机器人沿当前朝向走多远，角速度 w 决定朝向转多快。参数上需要注意 v 和 w 的量纲：v 用的是米每秒，w 用的是弧度每秒。很多刚接触的人会把角度直接传进 cos 里，结果机器人不走直线，我排查过太多次这个低级错误。另一个容易忽略的点是 dt，也就是仿真步长，它决定了路径跟踪的平滑度，一般取 0.05 到 0.2 秒之间，太小则仿真太慢，太大则碰撞检测容易漏报。

3.3 仿真主循环：路径跟踪、冲突检测与协同避障的框架

主循环是仿真平台的脚手架。初始化时给每台机器人规划好全局路径，然后循环执行以下步骤：预测下一时刻位置、检查是否与其他机器人冲突、如果有则调整速度或触发重新规划、更新时间步。

% 初始化阶段 for k = 1:nRobots paths{k} = astar_plan(map, starts(k,:), goals(k,:)); states{k} = [starts(k,1); starts(k,2); 0]; % x, y, theta idx{k} = 1; end for t = 1:maxSteps for k = 1:nRobots % 从全局路径中取当前目标点 nextPt = paths{k}(min(idx{k}+1, end), :); % 计算期望速度方向 vDes = computeVelocity(states{k}, nextPt, vMax); % 避障修正：检查是否与其他机器人冲突 [avoidV, avoidW] = avoidConflict(k, states, vDes, safeR); % 状态更新 states{k} = updateDiffDrive(states{k}, avoidV, avoidW, dt); % 判断是否到达路径点 if norm(states{k}(1:2)' - nextPt) < threshold idx{k} = idx{k} + 1; end end % 可视化 plotRobots(states, map); end

这段主循环里，avoidConflict是关键函数，它通常结合优先级和速度障碍法。参数方面需要注意vDes的计算方式：不要直接朝目标点方向开足马力，而是要在接近目标点时减速。我一般会根据当前点到目标点的距离线性降低期望速度，保证到位后不会过冲。另外，idx(k)是路径索引，它代表该机器人已经走过的路径点数，如果重规划发生，这个索引要重新初始化为 1。

4. 协同避障必须盯住的 4 个参数：安全距离、速度、优先级与重规划周期

搭建完框架后，很多人以为算法选对就能跑出好看效果，其实不是。协同避障的表现几乎完全由参数决定。同一套 A* + VO 代码，参数设得不对时机器人要么绕路绕到天际，要么互相顶牛。这里把四个最关键的参数讲透。

4.1 安全距离：设太大会绕路，设太小会碰撞

安全距离 safeR 是协同避障里第一敏感参数。它等于机器人本体半径加仿真中的避障裕量。如果本体半径 0.3 米，我一般初始给 safeR 设 0.5 米，后续按仿真结果调。

调这个参数的技巧：先设一个偏大的值让仿真稳定跑通，再逐渐减小到出现偶发冲突。那个临界值的 1.2 倍就是你的最终设置。不要一上来就追求极小安全距离，那会让多机器人系统频繁死锁，反而是最大的性能瓶颈。

4.2 最大速度与加速度平滑

多机器人系统里，速度上限直接影响追尾风险。如果所有机器人速度上限都一样，前面的机器人减速时，后面的很容易撞上。所以我会给每台机器人设定一个“期望速度”和一个“最小速度”，期望速度是正常通行速度，最小速度是在避让时的底线。

function vDes = computeVelocity(state, nextPt, vMax) dist = norm(nextPt - state(1:2)'); % 距离越近，速度越小，防止过冲 vDes = min(vMax, dist / 1.0); % 保底速度，防止在目标点附近无限停滞 vDes = max(vDes, 0.1); end

这个函数设置了两个关键约束：距离 1.0 米以内时，速度随距离线性下降；保底 0.1 米/秒保证机器人即使离目标很近也能缓慢移动到位。参数 1.0 就是减速距离，可根据机器人惯性和控制周期调整。如果你把减速距离设得太大，机器人还没有到达路径转弯点就开始减速，整体效率下降；设得太小，到位时速度过快，看起来像急刹车，放到真机上会产生比较大的冲击。

4.3 优先级：只设一次，还是运行时调整

优先级表最能体现协同避障的设计思路。静态优先级表在仿真初始化阶段生成一次：

% priority(k) 数值越小，优先级越高 priority = 1:nRobots; % 如果结合任务截止时间，则按 deadline 排序 [~, priority] = sort(deadlines);

如果某个低优先级机器人和高优先级机器人冲突，低优先级机器人执行避让动作。这个方案避免了两台机器人“你让我，我让你”的互让死锁。但是静态优先级在长时间运行中会产生不公平：低优先级的机器人可能长期等待。我建议在仿真中加一个“等待超时”机制，如果低优先级机器人连续等待超过 N 秒，自动提升其优先级，相当于一个动态调度策略。这个 N 一般取 5 到 10 秒，太短会频繁打断高优先级任务，太长则低优先级机器人饿死。

4.4 重规划周期：多久重新算一次路径

多机器人避障还有一个隐藏参数：重规划周期。很多人的误区是“遇到障碍立刻重新规划”，这在动态环境中会导致路径频繁跳变，机器人表现为来回抖动。我一般设重规划周期为 20 个仿真步，也就是每 1 秒（按 dt=0.05 算）检查一次“当前路径是否仍然可行”。只有在路径被阻塞时才重新规划，否则只做局部速度调整。

重规划周期太短的后果可以从代码上看：每次调用astar_plan都是一次完整的图搜索，多个机器人同时重规划会让 CPU 瞬间拉满，而且新路径跟旧路径差距过大时，机器人轨迹会十分诡异。我在某次测试里把重规划周期设成 1 个仿真步，结果地图上三台机器人像跳华尔兹一样绕圈，看起来像是在避障，实际上是在反复修改自己的全局路径。

5. 避坑与排查：从“卡死”到“抖动”的 5 类高频问题

仿真平台最磨人的阶段不是写代码，是把代码跑起来后发现各种不讲道理的现象。下面这五条是从多个项目里抽出来的高频问题，每条都按现象、原因、解决给出，照着排除能省很多时间。

5.1 单机器人正常，多机器人一跑就互相卡死

现象：单台机器人从起点到终点跑得很流畅；一旦放两台进去，它们在某个路口面对面停住，既不前进也不绕开。

原因：协同避障逻辑里没有有效的优先级判断，两台机器人都认为“自己要让路”，或者都认为“自己可以先进”。最典型的原因是避让条件写成了“如果距离小于 safeR 就停下来”，而不是“如果距离小于 safeR 且对方优先级更高”。停下来观望这件事，两台机器人同时做，就变成互相等待。

解决：检查避让函数的触发条件，增加优先级比较。确保只有低优先级机器人执行避让动作，高优先级保持原路径。具体实现就是if priority(me) > priority(other)时执行避让。改完后两台机器人相遇，高优先级的直线通过，低优先级的稍作等待然后跟上。

5.2 地图上看到的位置与实际运动方向对不上

现象：机器人明明在图像上往右走，但坐标系里 x 坐标在减小；或者给机器人下达“向前”命令，它却往图像下方走。

原因：栅格矩阵的行列坐标跟笛卡尔坐标混用了。MATLAB 里imagesc(map)显示图像时，第一行在第 1 行的位置，相当于 y 轴向下；而在做运动学时，你用的是 x-y 坐标系，y 轴向上。这两个坐标系之间相差一个翻转，角度还完全对不上。

解决：统一用一个内部坐标约定。我习惯在仿真内部全部用 x-y 坐标，只在渲染显示时换成行列坐标。具体做法：row = y / resolution + offset，col = x / resolution + offset，或者写一个xy2rc辅助函数，每次显示前调用它。不要在地图生成时用行列坐标，在运动学时又即时转成 x-y 坐标，那样只会越转越乱。

5.3 重规划太频繁导致机器人抖动

现象：机器人在空旷区域左右摇摆，路径不像一条直线，而像一条纺锤形曲线；速度曲线也是毛刺状，时快时慢。

原因：冲突检测触发的条件太敏感。也许我只是距离另一个机器人 0.5 米，触发了重新规划，而那条新路径又让机器人绕了一个多余的弯。同时重规划周期太短，导致机器人没时间执行完新路径就又被重规划打断。

解决：把重规划周期提升到 10 步以上，把“遇到障碍就重新规划”改成“偏离原路径超过阈值才重新规划”。阈值一般设为 1.5 倍安全距离。同时把局部避让和全局重规划分开：小冲突用速度调整解决，只有路径被彻底堵死才触发全局重规划。

5.4 仿真实时性差：机器人多了直接卡成 PPT

现象：启动仿真后，画面一帧一帧跳动，每帧间隔好几秒；或者 CPU 占用率 80% 以上。

原因：最常见的是两层问题。第一，路径规划在每一帧对所有机器人重复执行；第二，可视化函数每帧都重绘整个地图，没有使用图形句柄增量更新。还有一种隐藏原因是 MATLAB 的containers.Map在存储大矩阵时速度极慢。

解决：路径规划改成周期性触发，可视化部分用set(h, 'XData', ...)更新机器人位置，而不是每次循环都调用plot。另外，astar_plan里用containers.Map存储 open 集合，在 500×500 地图上性能不够，可以改成预分配结构的堆或者直接用矩阵维护 open 标志。性能排查的顺序是：先分析主循环哪一行耗时最多，再优化对应瓶颈。

5.5 避让后回不到原路径，绕远路

现象：机器人避让完一个临时障碍，继续往前走到目标点，但路径明显绕了一个大弯，原本可以直接穿过的区域它不去，非要沿着边界走一圈。

原因：避让过程是临时偏移，但偏移结束后，机器人没有把目标点恢复到全局路径上的下一个关键点，而是继续沿用避让前那个目标点。后续路径跟踪被这个偏移带偏，一路偏移到最后。

解决：全局路径的跟踪目标应该是“路径序列上的一个点”，而不是“机器人当前位置附近最近的点”。机器人执行避让后，回到全局路径上最近的点继续走。代码里可以维护globalPath和trackingIndex，避让结束时不修改trackingIndex，而是检查当前机器人位置与globalPath(trackingIndex)的距离，超过阈值则重新找最近的点，这样既能完成避让又不会丢掉全局路径。

6. 进阶验证与扩展：怎么证明这个协同避障平台可靠

平台能跑通后，最重要的问题是：它的可靠性怎么体现？我常用的办法是批量跑参数组合，统计碰撞率与成功到达率，用数据判断参数和平稳性。这个习惯是从一次翻车里学到的：某次仿真里我反复看动画觉得一切正常，放到真机上才发现速度下限设得太高，低速堵车时前车刹停，后车直接顶上去。之后我每次都会批量跑测试，不再只肉眼看动画。

批量验证的骨架代码很简单：

function result = batchVerify(configs) % configs 是结构体数组，每组包含 safeR, vMax, rePlanPeriod for i = 1:numel(configs) cfg = configs(i); [collisions(i), success(i), avgTime(i)] = runSingleSim(cfg); end T = table(collisions', success', avgTime'); writetable(T, 'sim_metrics.csv'); end

我一般会跑三组对照：第一组全部是保守参数，验证系统不会碰撞；第二组是激进参数，找系统瓶颈；第三组在中间值附近做小步长扫描，找到最容易复现冲突的组合。最后把仿真结果里的冲突热点标到地图上，观察它们是否集中在窄通道、交叉口这些位置。对于那些位置，可以增加交通规则或局部调度，比全局调参更有效。

扩展方向上，这个平台可以加两类东西：动态障碍物和动态任务点。动态障碍物就是在主循环中让障碍物位置随时间变化；动态目标点则是让机器人完成一个目标后自动接收下一个目标。这两类扩展不需要改核心算法，只需要在主循环里多两个状态更新分支。

我自己现在做多机器人项目，已经离不开批量验证这套流程。养成这个习惯后，真机调试的痛苦少了一大半，因为大部分问题在仿真阶段就能暴露。希望这篇笔记能帮你在搭建多机器人协同避障仿真平台的路上少走点弯路。

本文还有配套的精品资源，点击获取