工业机器人仿真系统碰撞检测快速算法

摘要: 根据 6R 机器人杆系的特有结构, 提出了 三角形包围法和端点矢量投影法来提高双机器人协 调工作时连杆碰撞检测的效率 。通过改进公共面法 来进行几何元素的碰撞和干涉检测 , 给出了相关的 数值仿真结果。 关键词: 机器人 ; 碰撞检测 ; 三角形包围法 ; 公共 面法 中图分类号 : TP 24 文献标识码 : A 文章编号: 1001 2257( 2004) 09 0045 03 Abstract: A fast method for collision detection is presented , which is based on special links structure of 6R robots To speed up the collision detection links are bounded by triangles And improved method of common place is used in collision detection between geometric ele ments Computer simulation results have verified that method and algorithm is correct and efficient Key words: robots; collision detect ion, ; bounding - triangle; common plane
工业机器人仿真系统碰撞检测快速算法
张智勇, 沈俊杰 ( 上海大学, 上海 200072)
Fast Collision Detection Algorithm for Industrial Robots Simulation System
ZHANG Zhi - yong, SHENG Jun- jie ( Shanghai University, Shanghai 200072, China)
( i) C (j ) ( i)
n
3,
n 4} 。且取 :
a1 max = max { m 0 , m 1 , m 2 } a max = max { m 2 , m 3 , m 4 }
1 b min = 2 b min =
( 2)
A 的所有顶点到 C- P 的距离 ( i = 1, 2 m) A 的顶点个数 B 的所有顶点到 C- P 的距离 ( j = 1, 2 n)
行干涉检测。
2
几何元素的相交检测算法
采用 C- P 法进行几何元素的相交检测。在软
件方面, 只需要编制一个通用的 C- P 算法函数就 可实现所有几何元素的相交检测。 2 1 几何元素公共面的概念 参照 2 个任意 形状多面体之 间 公共面 的概 念 , 定义 1 个平分 任意几何元素之 间间隙的 公共 面 。几何元素间的干涉检测问题转化为检测几何 元素与该公共面之间关系问题, 且具有以下特点 : a 只检查几何元素顶点 ( 直线段为端点 ) 与公 共面的关系。 b 检查次数与几何元素的的顶点数成正比。 c C- P 平分 2 个几何元素之间的最小间隙 , 2 个几何元素之间的最小距离即为 C- P 到 2 个几何 元素距离之和。 2 2 公共面初始位置的确定 C- P 的初始位置满足如下条件: 法向单位矢量 为 n, 且过参考点 C 。 C= ( CA + CB) / 2 n= Z / | Z| 式中 CA CB 几何元素 A 的形心位置矢量 几何元素 B 的形心位置矢量 ( 1)
2
意姿态的三角形对和长方体对, 如图 3、 图 4 所示。
图 3 平面 平面的干涉
( 4)
图 4 空间任意方向凸 多面体间的干涉
几何元素顶点数据及计算机数值仿真结果如表 1、 表 2 所示。 k min = 0 01。当 d > 0 时几何元素不干 涉 , d < 0 将发生干涉。表中 lp 为算法中的循环迭 代次数。从 lp 的数值和干涉关系可以知道, 公共面 法可以快速排除不相干涉的情况。
空间凸多面体 A 顶点位置矢量
- 565 9223 - 548 655 0 - 548 655 0 - 425 832 0 - 486 685 6 - 486 685 6 - 425 832 0 - 461 879 9 - 522 733 5 - 522 733 5 - 461 879 9 - 406 977 1 - 467 830 7 - 467 830 7 - 406 977 1 - 503 878 7 - 503 878 7 - 443 025 0 - 443 025 0 4 085 8 4 085 8 49 783 8 49 783 8 49 783 8 49 783 8 4 085 8 4 085 8 4 085 8 4 085 8 49 783 8 49 783 8 49 783 8 4 085 8 4 085 8 49 783 8
平面 A 顶点位置矢量 48 308 1 100 48 308 1 - 50 - 50
80 391 7 92 388 5 209 688 1 209 688 1 119 927 8
58 451 9 100 58 451 9 - 50 102 532 3 - 50
表 2 多面体间干涉 C- P 法计算结果
n
B 的顶点个数
46
将 C- P 按下式给出参考点位置矢量作平移 : C = ( P A + P B ) / 2+ ( dA + d B ) n / 2 ( 3) 式中 2 4 PA , P B 几何元素 A , B 与公共面 取得最 小距离的顶点的位置矢量。 公共面的旋转 选择 2 个正交矢量 , 记其法向单位矢量分别为 p 和 q 。并且 p , q 及 C- P 的法向单位矢量 n 两两 正交。实验表明 , p 和 q 的选择将影响 C- P 的扰动 次数, 甚至结果的正确性。将 n 分别沿 p 和 q 方向 进行正负扰动, 产生 4 个扰动量 : n 1 = ( n + kp ) / ( 1+ k 2 ) n 2 = ( n - kp ) / ( 1+ k 2 ) n 3 = ( n + kq ) / ( 1+ k 2 ) n 4 = ( n - kq ) / ( 1+ k ) 式中 k 控制扰动幅度的参数 一般情况下取 k max = 0 174 rad 可得到较好的效
0
引言
1
连杆间碰撞预检测的三角形包围法
对机器人的手臂作如下约定 : 将机器人的手臂
在机器人仿真系统中, 经常需要对 2 个或更多 个机器人在同一工作单元中协同作业进行仿真。通 常情况下多机器人工作空间存在重叠交叉区域 , 这 就要解决机器人之间相互干涉问题。传统的三维物 体碰撞与干涉检测方法多是进行 2 个空间物体的求 交运算[ 1- 3] , 往往比较复杂与费时。为提高检测效 率, 进行碰撞检测的一个常用方法是层次包围盒法 ( hierarchy bounding volume) 。其 核心思 想是用 体 积略大而几何特征简单的包围盒来近似地描述复杂
机械与电子 2004( 9)
min{ n 0, n 1, n 2} min{ n 2, n 3, n 4}
1 a max , 2 a max 和 1 b m in , 2 b min 在 O 1 O 2 上的叠置 (j) d B- C
根据 和 情况来决定是否对三角对进行求交计算。例如 , 若
2 仅有 a 2 max 和 b min 在 O 1 O 2 叠置 , 则只对 A 2 与 B 2 进
1 1 2 2 2 2 2 2 2 2 2 1 1 1 1 1 1 1
机械与电子 2004( 9)
若直接对其进行碰撞检测 , 不但费时, 且在其中大部 分连杆不干涉情况下 , 将浪费大量计算资源。 为了快速排除不相交的连 杆对, 针对 6R 机器 人特有的结构 , 将其划分为 2 个三角形。长度不为 零的连杆依次形成三角形的 2 条边如图 1 所示。这
表1
不 相 交 相 交 170 152 1 170 152 1
果。搜索最大间隙 d = max ( dB - dA ) 。若 d > 0 则 2 个元素不干涉, 若 d 0 则发生干涉。 公共面法的完整流程如图 2 所示。
平面间干涉 C- P 法计算结果
平面 B 顶点 位置矢量 191 148 9 77 504 5 274 092 1 162 902 1 223 194 5 152 619 8 191 148 9 77 504 5 223 194 5 152 619 8 140 251 4 67 222 2 0 - 85 461 8 0 85 461 8 - 85 461 8 85 461 8 - 22 70 9 34 39 1 d lp
[ 7]
。每个机器人的连杆分别被包围在 2 个三角形
中。通过对三角形对的干涉测试快速排除不相交的 连杆对, 以减少相交测试次数。采用公共面法对几 何元素进行碰撞与干涉检测。包括三角形包围法中 平面 平面的干涉检测 , 及 将用到的直线段 直线段、 连杆精确碰撞检测时所用到的直线段 凸多面体、 凸 多面体 凸多面体的干涉检测。该法最初由 Cundall 提出[ 8] , 主要用于研究边坡岩体的力学行为。我们 进一步改进了 C- P 法 , 使其成为机器人仿真系统 中通用的几何元素碰撞检测算法。
2 2,
直线段的形心为某中心点。 Z = CB - CA 。 当 C- P 初始位置确定后, 为了得到正确的 CP 位置, 必须对 C- P 进行一系列微调, 即平移和旋 转初始 C- P。 2 3 公共面的平移 求 C- P 与每个几何元素最近顶点的有向距离 dA 和 d B 。其方向为平行于 n 同向时取正, 异向时 取负。 dA 取负, dB 取正。 dA = max [ d A - C ] dB = min[ d B - C ] 式中 dA m