碰撞检测
碰撞检测是机器人安全运行的核心保障。Darra 在仿真阶段和运行时阶段分别提供不同级别的碰撞检测, 覆盖自碰撞、环境碰撞、干涉区碰撞三大场景。
碰撞检测原理
包围盒碰撞 (Bounding Box)
原理: 用规则几何体 (AABB / OBB / 球体) 近似物体形状, 检测包围盒之间是否重叠。
- AABB (Axis-Aligned Bounding Box): 轴对齐包围盒, 检测最快 (6 个比较操作), 但紧致性差
- OBB (Oriented Bounding Box): 有向包围盒, 精度更高, 需要分离轴定理 (SAT) 检测
- 球体 (Sphere): 检测最快, 紧致性最差, 适合远距离粗筛
层级包围盒 (BVH, Bounding Volume Hierarchy): 将物体拆成树状结构, 先测粗包围盒, 重叠才下沉到子节点细测。典型 BVH 深度为 8~12, 检测复杂度从 O(n²) 降到 O(n·log n)。
网格碰撞 (Mesh Collision)
原理: 对物体三角网格, 逐三角形检测相交。Darra 使用分离轴定理 (SAT) 对凸包快速检测, 非凸物体预先分解为凸包集合 (V-HACD 算法)。
凸包分解 (V-HACD):
- 输入: 原始三角网格 (任意形状, 可能非凸)
- 输出: 4~16 个凸包 (每个凸包三角面数 ≤ 64)
- 每个凸包用 SAT 检测, 整体碰撞 = 任意一个凸包碰撞
碰撞检测管线
每周期 (1 ms / 2 ms / 4 ms):
1. 粗筛 (Broad Phase) — BVH + AABB
└─ 快速排除明显不相交的物体对
└─ 输出候选碰撞对 (通常 < 5% 总物体对)
2. 精检 (Narrow Phase) — SAT + 三角形相交
└─ 对候选对做精确网格碰撞
└─ 输出碰撞点 + 法线 + 穿透深度
3. 分类 (Collision Classification)
└─ 自碰撞 (机器人本体 vs 本体)
└─ 环境碰撞 (机器人 vs 工装 / 料框 / 其他设备)
└─ 干涉区碰撞 (多机器人协作)
自碰撞检测
自碰撞检测检测机器人自身连杆之间的碰撞。
碰撞体模型
每个连杆关联一个碰撞体 — 简化几何体 (凸包 / 胶囊体 / 圆柱) 或原始三角网格:
| 连杆 | 碰撞体类型 | 尺寸依据 |
|---|---|---|
| 基座 | 凸包 (6~8 面) | 3D 模型自动生成 |
| 大臂 | 胶囊体 (半径 R) | 最小包围半径 |
| 小臂 | 胶囊体 | 包含电机壳 |
| 腕部 | 凸包 | 包含腕部外壳 |
| 末端 | 球体 | TCP 区域 |
自碰撞对: 排除相邻连杆 (Jᵢ 与 Jᵢ₊₁ 相邻, 不构成碰撞), 检测非相邻连杆对:
6 轴机器人非相邻对: (J1,J3), (J1,J4), (J1,J5), (J1,J6),
(J2,J4), (J2,J5), (J2,J6),
(J3,J5), (J3,J6), (J4,J6) — 共 10 对
配置
// 启用自碰撞检测
SetSelfCollisionCheck(Robot := Robot1, Enabled := TRUE);
// 配置碰撞体安全裕量 (默认 5 mm)
SetCollisionMargin(Robot := Robot1, Margin := 8.0); // mm
// 设置碰撞体对排除 (例如: 以下对不检测)
ExcludeCollisionPair(Robot := Robot1, LinkA := "J3", LinkB := "J5");
环境碰撞检测
环境碰撞检测机器人与工作空间中所有静态/动态物体之间的碰撞。
环境对象
在 IDE 中导入或创建以下环境对象:
| 对象类型 | 来源 | 碰撞体 |
|---|---|---|
| 固定工装 | STL / STEP 导入 | 凸包 (V-HACD) |
| 传送带 | 参数化生成 | AABB |
| 料框 / 托盘 | 参数化生成 | AABB |
| 安全围栏 | 参数化生成 | AABB |
| 其他机器人 | 机器人模型 | 每个机器人单独碰撞体 |
| 动态工件 | 程序定义 | 球体 / OBB |
环境碰撞检测配置
// 启用环境碰撞
SetEnvCollisionCheck(Robot := Robot1, Enabled := TRUE);
// 添加环境物体
AddCollisionObject(
Name := "Feeder_01",
Type := COLL_OBJ_MESH,
Mesh := "Models/feeder.stl",
Pose := FeederPose,
Margin := 5.0
);
// 添加动态物体 (位置可在运行时更新)
AddCollisionObject(
Name := "Workpiece",
Type := COLL_OBJ_SPHERE,
Radius := 30.0,
Pose := WorkpiecePose
);
碰撞检测事件
// 碰撞事件回调 (在 PLC 程序中响应)
VAR
CollEvent : COLLISION_EVENT;
CollRobot : STRING;
CollObj : STRING;
CollDist : LREAL;
END_VAR
CollEvent := GetCollisionEvent(Robot := Robot1);
IF CollEvent <> COLL_NONE THEN
// 获取碰撞信息
GetCollisionDetail(
Robot := Robot1,
Event := CollEvent,
ObjectName => CollObj,
Distance => CollDist
);
Log.Warn('Collision detected: robot vs %s, distance = %.2f mm', CollObj, CollDist);
// 减速逼近
IF CollDist < 20.0 THEN
OverrideSpeed(50); // 降到 50%
END_IF;
// 即将碰撞
IF CollDist < 5.0 THEN
StopMotion();
END_IF;
END_IF;
碰撞响应与避让
碰撞响应策略
| 响应策略 | 说明 | 适用场景 |
|---|---|---|
| 急停 (E-Stop) | 立即停止所有运动, 伺服断电 | 运行时碰撞 |
| 减速逼近 | 接近碰撞时自动降速, 不触发急停 | 仿真验证 |
| 路径重规划 | 检测到碰撞路径 → 自动绕行 | 离线编程 |
| 姿态调整 | 在保持 TCP 位置下调整姿态避开 | 狭小空间 |
| 报警不停止 | 仅报警, 不停止运动 | 调试阶段 |
仿真阶段碰撞响应
// 仿真时碰撞检测策略
SetCollisionResponse(
Robot := Robot1,
Mode := COLL_MODE_SLOWDOWN, // 减速逼近模式
WarnDist := 30.0, // 30 mm 警告
SlowDist := 15.0, // 15 mm 开始降速
StopDist := 3.0 // 3 mm 停止
);
运行时碰撞检测
运行时碰撞检测是安全关键功能, 与仿真碰撞检测有本质区别:
| 维度 | 仿真碰撞检测 | 运行时碰撞检测 |
|---|---|---|
| 执行位置 | IDE (用户态) | Service + 内核驱动 |
| 实时性 | 非实时 | 硬实时 (1 ms 周期) |
| 碰撞模型 | 精细网格 | 简化凸包 |
| 检测范围 | 全量 | 仅关键对 (自碰撞 + 环境关键物体) |
| 响应 | 减速 / 报警 | 急停 (E-Stop) + 安全输出 |
| 物理触发 | 不继承 | 可配置安全继电器输出 |
运行时碰撞检测配置:
// 运行时启用碰撞检测
SetRunCollisionCheck(
Robot := Robot1,
Enabled := TRUE,
SelfColl := TRUE, // 自碰撞
EnvColl := TRUE, // 环境碰撞 (仅关键物体)
SafetyOutput := DO[5], // 碰撞检测到 → 输出急停信号
ReactionTime := 2 // 2 ms 内响应
);
路径重规划避让
// 路径规划时启用碰撞避让
VAR
PathResult : PATH_PLAN_RESULT;
Waypoints : ARRAY[1..100] OF ROBOT_POSE;
WpCount : INT;
END_VAR
// 指定起点和终点, 系统自动规划无碰撞路径
PathResult := PlanCollisionFreePath(
Robot := Robot1,
Start := CurrentPose,
Goal := TargetPose,
Waypoints => Waypoints,
Count => WpCount,
MaxIter := 1000,
Timeout := 2.0 // 秒
);
路径规划使用RRT-Connect (快速随机扩展树) 算法, 在配置空间 (C-space) 中搜索无碰撞路径。典型成功率 > 95%, 搜索时间 < 500 ms。
性能考量
| 场景 | 碰撞体对数 | 检测时间 (1 ms 周期) | CPU 占用 |
|---|---|---|---|
| 单机器人自碰撞 (6 轴) | 10 | < 15 μs | < 2% |
| 单机器人 + 10 环境物体 | 10 + 10 | < 30 μs | < 5% |
| 双机器人 + 20 环境物体 | 40 + 20 | < 80 μs | < 10% |
| 仿真精细网格 (全部) | 数百 | 2~5 ms | 非实时线程 |
运行时只使用简化凸包碰撞体, 保证 1 ms 周期内完成检测。
参数表
| 参数 | 类型 | 单位 | 范围 | 默认 | 说明 |
|---|---|---|---|---|---|
| SelfCollisionEnabled | BOOL | - | - | FALSE | 自碰撞检测 |
| EnvCollisionEnabled | BOOL | - | - | FALSE | 环境碰撞检测 |
| CollisionMargin | LREAL | mm | 0..50 | 5.0 | 安全裕量 |
| CollisionResponseMode | ENUM | - | - | SLOWDOWN | 碰撞响应策略 |
| WarnDistance | LREAL | mm | 5..100 | 30.0 | 警告距离 |
| SlowDistance | LREAL | mm | 2..50 | 15.0 | 减速距离 |
| StopDistance | LREAL | mm | 0..10 | 3.0 | 停止距离 |
| RunCollisionEnabled | BOOL | - | - | FALSE | 运行时碰撞检测 |
| RunCollisionReactionTime | UDINT | ms | 1..10 | 2 | 运行时响应时间 |
排错清单
| 现象 | 原因 | 解决 |
|---|---|---|
| 仿真碰撞检测报错但实际无碰撞 | 碰撞体过于保守 | 用 V-HACD 重新生成凸包 |
| 运行时碰撞检测不触发 | 未启用或碰撞体未简化 | 检查 RunCollisionEnabled = TRUE |
| 碰撞检测导致 CPU 超时 | 碰撞体过多 (精细网格) | 转用简化凸包, 减少物体对 |
| 急停时机器人抖动 | 制动响应时间不足 | 调 ReactionTime 到 1 ms |
| 路径重规划失败 | 搜索空间过大或起点在障碍物内 | 调整初始位姿, 增加 MaxIter |
| 自碰撞漏报 | 碰撞体排除对配置错误 | 检查 ExcludeCollisionPair 配置 |
最佳实践
- 仿真用精细网格, 运行时用简化凸包: 仿真阶段追求精度, 运行时追求性能。
- 关键物体优先: 运行时只对关键物体 (工装、安全围栏、其他机器人) 开启碰撞检测, 非关键物体不检测以节省 CPU。
- 安全裕量: 运行时碰撞检测的安全裕量建议 5~10 mm, 避免传感器噪声和机械间隙导致误报。
- 急停响应: 碰撞检测到后必须输出到安全继电器 (硬接线), 不能仅靠软件停止。
- 碰撞后复位: 碰撞触发后需要人工确认 + 手动示教离开碰撞区, 不允许自动复位重跑。