【机器人控制】缓冲不确定性感知Voronoi单元多机器人碰撞避免附Matlab代码

最新推荐文章于 2025-09-04 23:02:08 发布

原创最新推荐文章于 2025-09-04 23:02:08 发布 · 374 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#cocos2d #游戏引擎

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

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

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

🔥 内容介绍

在物流仓库里，十几台 AGV 机器人穿梭搬运却从不相撞；在工厂车间中，多台协作机器人同步作业却能精准避让；在室外巡检场景下，无人机集群飞越复杂地形仍能保持安全距离…… 这些 “默契配合” 的背后，藏着多机器人控制的核心难题 ——碰撞避免。而 “缓冲不确定性感知 Voronoi 单元” 技术，就像给每台机器人装上了 “智能防撞雷达”，让集群在复杂环境中既能高效协作，又能杜绝碰撞风险。今天我们就来拆解这项技术，看看它如何解决多机器人 “互不干扰” 的核心需求。

一、多机器人碰撞避免：不止 “不撞”，还要 “抗干扰”

提到多机器人碰撞避免，很多人会觉得 “不就是让机器人互相躲开吗？” 但实际场景远比想象中复杂 —— 机器人的感知、定位、运动都存在 “不确定性”，这让碰撞避免从 “简单避让” 变成了 “抗干扰的动态协调” 问题。我们先看看多机器人集群面临的三大核心挑战：

1. 感知有 “误差”：机器人的 “眼睛” 不完美

无论是激光雷达、视觉摄像头还是毫米波雷达，机器人感知环境时总会存在误差。比如 AGV 机器人定位精度可能是 ±0.3 米，在仓库货架密集区，这个误差可能让机器人误判与同伴的距离；室外无人机受风速影响，感知到的障碍物位置可能偏差 0.5 米以上，若按 “绝对精确” 的距离计算，很容易出现 “以为安全却相撞” 的情况。

2. 环境有 “动态”：突发状况防不胜防

真实场景中总有意外：仓库突然出现的搬运工人、车间里临时摆放的工具箱、室外巡检时突然飞过的鸟类…… 这些 “动态障碍物” 不会提前 “报备”，若机器人只能按预设路线行驶，很可能因无法及时响应而碰撞。

3. 集群有 “规模”：越多越难协调

当机器人数量从 3 台增加到 30 台，碰撞避免的复杂度会呈指数级上升。比如 10 台 AGV 在交叉路口交汇时，需要同时考虑 9 台同伴的运动方向、速度，还要兼顾自身任务（如 “优先送达紧急订单”），传统 “一对一避让” 策略会导致效率骤降，甚至出现 “全员停滞” 的僵局。

传统碰撞避免方法（如人工设定安全距离、基于预设路径避让）要么忽略 “不确定性”，要么难以应对大规模集群，而 “缓冲不确定性感知 Voronoi 单元” 技术，正是为解决这些痛点而生。

二、先懂基础：Voronoi 单元 —— 机器人的 “专属安全区”

要理解 “缓冲不确定性感知 Voronoi 单元”，首先得搞懂它的 “基础款”——Voronoi 单元。简单来说，Voronoi 单元是给多机器人集群划分 “专属活动区域” 的数学工具，核心逻辑是 “让每台机器人只在自己的‘区域’内运动，区域边界就是与其他机器人的安全界限”。

我们用一个通俗的例子解释：假设仓库里有 3 台 AGV 机器人（A、B、C），Voronoi 单元会根据它们的实时位置，用 “垂直平分线” 划分出 3 个互不重叠的区域 —— 机器人 A 的区域内，任意一点到 A 的距离都比到 B、C 的距离近；同理，B 和 C 的区域也遵循这个规则。这样一来，只要每台机器人只在自己的 Voronoi 单元内运动，理论上就不会与同伴碰撞。

Voronoi 单元的优势很明显：

分布式协调：不需要中央控制器统一指挥，每台机器人根据自身位置和同伴位置就能计算出自己的单元，减少 “中心化故障” 风险；

动态适配：当机器人运动时，Voronoi 单元会实时更新边界，比如 A 向 B 靠近，A 的单元会缩小、B 的单元会扩大，始终保持 “区域不重叠”；

效率优先：机器人在自己的单元内可自主规划路径，不用频繁等待其他机器人，兼顾安全与效率。

但问题来了 —— 传统 Voronoi 单元没考虑 “不确定性”！如果机器人 A 的定位误差是 ±0.3 米，它实际位置可能比计算出的单元边界 “偏出” 0.3 米，此时即使严格在单元内运动，仍可能与相邻的 B 机器人碰撞。这就是 “缓冲不确定性感知” 要解决的核心问题。

三、核心升级：缓冲不确定性感知 —— 给 “安全区” 加层 “防护垫”

“缓冲不确定性感知 Voronoi 单元”，本质是在传统 Voronoi 单元的基础上，增加了一层 “基于感知误差的缓冲区域”，就像给机器人的 “安全区” 裹上了一层弹性防护垫 —— 即使存在感知误差或轻微扰动，缓冲区域也能挡住碰撞风险。

1. 第一步：量化 “不确定性”—— 知道 “误差有多大”

要加缓冲，首先得知道 “需要加多大的缓冲”。技术上会通过两种方式量化不确定性：

感知误差建模：根据机器人的传感器类型，建立误差模型。比如 AGV 用二维码定位时，误差与行驶速度、地面平整度相关，可通过历史数据拟合出 “速度越快，误差越大” 的函数（如速度 v 时，误差 σ=0.1+0.05v）；无人机用 GPS 定位时，误差与卫星信号强度相关，信号弱时误差 σ 可设为 0.5 米，信号强时设为 0.2 米。

动态障碍物概率预测：对于突然出现的动态障碍物（如仓库工人），通过传感器实时跟踪其运动轨迹，预测未来 1-3 秒内的位置范围（如 “工人未来 2 秒可能在 [X1,X2]×[Y1,Y2] 区域内”），将这个范围纳入不确定性考量。

2. 第二步：构建缓冲 Voronoi 单元 ——“安全区” 扩大 “误差半径”

在量化不确定性后，就可以调整 Voronoi 单元的边界：将传统单元的边界 “向外扩展” 一个 “缓冲距离”，这个距离等于 “机器人自身感知误差 + 相邻机器人感知误差 + 动态障碍物预测范围”。

举个具体例子：

机器人 A 的定位误差 σ_A=0.3 米，相邻机器人 B 的定位误差 σ_B=0.2 米；

两者之间传统 Voronoi 单元的边界是 L，那么缓冲后的边界会向 A 的方向外扩 0.3 米，向 B 的方向外扩 0.2 米；

最终 A 的缓冲单元边界是 L_A = L + 0.3 米，B 的缓冲单元边界是 L_B = L - 0.2 米，A 和 B 的缓冲单元之间会形成一个 “无重叠的间隙”（宽度 0.3+0.2=0.5 米）。

这层 “间隙” 就是碰撞避免的关键 —— 即使 A 因误差偏出原单元 0.3 米，B 偏出 0.2 米，两者也不会进入对方的缓冲单元，从根本上杜绝碰撞。

3. 第三步：动态调整与路径规划 —— 让 “缓冲区” 跟着场景变

缓冲 Voronoi 单元不是固定不变的，会根据场景实时调整：

误差变大时，缓冲区扩大：比如 AGV 从平坦路面驶入颠簸区域，定位误差从 0.3 米增至 0.5 米，缓冲距离会同步调整为 0.5 米，确保间隙足够；

动态障碍物靠近时，优先避让：若检测到工人向机器人 A 的缓冲单元靠近，A 会主动缩小自身缓冲单元（同时降低速度），给工人留出更多安全空间；

任务紧急时，优化缓冲策略：若某台 AGV 需优先送达紧急订单，可在 “不突破安全底线” 的前提下，适当缩小缓冲距离（如从 0.5 米减至 0.4 米），同时提高相邻机器人的避让优先级，平衡 “效率” 与 “安全”。

⛳️ 运行结果

📣 部分代码

%% initialization script

% common pre-run set up for simulation and experiment

%% Setup path

setPath;

%% Problem setup

global pr

% environment dimension

pr.dim = mode_dim; % problem dimension

xdim = [-5, 5]; % workspace

ydim = [-5, 5];

zdim = [ 0, 3];

if pr.dim == 2

pr.ws = [xdim; ydim];

elseif pr.dim == 3

zdim = [0, 10];

pr.ws = [xdim; ydim; zdim];

end

% robot physics

pr.robot_type = 0; % robot type

pr.radius = 0.2; % robot radius

pr.maxSpeed = 0.4; % robot maximal speed

pr.boundLocalRadius = 2.0; % local bound radius, m

% simulation setup

pr.dtSim = 0.1; % time step, s

pr.N = 10; % number of stage

pr.ifSimRobotNoise = 1; % if simulating measurement noise

pr.ifSimBoxObsNoise= 1;

% collision probability threshold

pr.collision_probability = 0.10;

pr.collision_parameter = erfinv(2*sqrt(1-pr.collision_probability) - 1);

% BVC or BUAVC

pr.method = mode_region; % 0 - BVC; 1 - BUAVC

if pr.ifSimRobotNoise == 0

pr.method = 0;

fprintf('Simulate robot localization noise: No. \n');

fprintf('Computation of safe region: BVC. \n');

else

fprintf('Simulate robot localization noise: Yes. \n');

if pr.method == 0

fprintf('Computation of safe region: BVC. \n');

elseif pr.method == 1

fprintf('Computation of safe region: BUAVC. \n');

else

error('Safe region mode is not defined!');

end

pr.control = mode_control; % 0 - one-step; 1 - mpc

if pr.control == 0

fprintf('Controller mode: Feedback reactive. \n');

elseif pr.control == 1

fprintf('Controller mode: MPC. \n');

else

error('Controller mode is not defined!');

end

% weights

pr.weights = [0.0, 0.0; 1.0, 0.0]'; % w_pos, w_input, stage and terminal weights

%% Simulation configuration

global cfg

% visualization setup

cfg.ifShowHistoryTra = 1; % if plotting the history trajectory of the robot

cfg.ifShowSafeRegion = 1; % if plotting the obstacle-free safe region for each robot

cfg.ifShowVelocity = 0; % if plotting robot velocity

%% Scenario setup

% static obstacles

if pr.dim == 2

vert_m = 4;

[nBoxObs, boxPos, boxSize, boxYaw] = box_initial_2D(3);

elseif pr.dim == 3

vert_m = 8;

[nBoxObs, boxPos, boxSize, boxYaw] = box_initial_3D(3);

end

boxVert = zeros(pr.dim, vert_m, nBoxObs);

for iBox = 1 : nBoxObs

[temp_vert, ~] = box2PolyVertsCons(pr.dim, boxPos(:, iBox), ...

boxSize(:, iBox), boxYaw(:, iBox));

boxVert(:, :, iBox) = temp_vert; % dim * m * nBoxObs

end

% multiple robot, robot initial and end position should not collide with each other and with obstacles

nRobot = 5; % number of robots

collision_mtx = ones(nRobot, nRobot+nBoxObs);

while (sum(sum(collision_mtx)) > 0)

fprintf('Generating robot initial positions and goals ... \n');

if pr.dim == 2

robotStartPos = robotStartPos_2D(nRobot, 3, pr.ws(1,:), pr.ws(2,:), pr.radius);

% robotEndPos = robotStartPos_2D(nRobot, 2, pr.ws(1,:), pr.ws(2,:), pr.radius);

robotEndPos = -robotStartPos; % robot final goal position

elseif pr.dim == 3

robotStartPos = robotStartPos_3D(nRobot, 2, pr.ws(1,:), pr.ws(2,:), pr.ws(3,:), pr.radius);

robotEndPos(1:2, :) = -robotStartPos(1:2, :); % robot final goal position

robotEndPos(3, :) = zdim(1) + zdim(2) - robotStartPos(3, :);

end

collision_mtx_start = collision_check(nRobot, nBoxObs, robotStartPos, 1.2*pr.radius, boxVert);

collision_mtx_end = collision_check(nRobot, nBoxObs, robotEndPos, 1.2*pr.radius, boxVert);

collision_mtx = [collision_mtx_start; collision_mtx_end];

end

% robot localization measurement noise

robotPosNoise = zeros(pr.dim, pr.dim, nRobot);

for iRobot = 1 : nRobot

robotPosNoise(:, :, iRobot) = 0.10^2 * eye(pr.dim);

end

% obs localization measurment noise

boxPosNoise = zeros(pr.dim, pr.dim, nBoxObs);

for iBox = 1 : nBoxObs

boxPosNoise(:, :, iBox) = 0.10^2 * eye(pr.dim);

end

pr.nRobot = nRobot;

pr.robotStartPos = robotStartPos;

pr.robotEndPos = robotEndPos;

pr.robotPosNoise = robotPosNoise;

pr.nBoxObs = nBoxObs;

pr.boxPos = boxPos;

pr.boxSize = boxSize;

pr.boxYaw = boxYaw;

pr.boxPosNoise = boxPosNoise;

%% For mpc

si_mpc_setup;

🔗 参考文献

[1] Zhu H , Brito B , Alonso-Mora J .Decentralized Probabilistic Multi-Robot Collision Avoidance Using Buffered Uncertainty-Aware Voronoi Cells[J]. 2022.DOI:10.1007/s10514-021-10029-2.