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

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 可得到较好的效
平面 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 法计算结果
空间凸多面体 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
2
wenku.baidu.com
意姿态的三角形对和长方体对, 如图 3、 图 4 所示。
图 3 平面 平面的干涉
( 4)
图 4 空间任意方向凸 多面体间的干涉
几何元素顶点数据及计算机数值仿真结果如表 1、 表 2 所示。 k min = 0 01。当 d > 0 时几何元素不干 涉 , d < 0 将发生干涉。表中 lp 为算法中的循环迭 代次数。从 lp 的数值和干涉关系可以知道, 公共面 法可以快速排除不相干涉的情况。
行干涉检测。
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)
[ 7]
。每个机器人的连杆分别被包围在 2 个三角形
中。通过对三角形对的干涉测试快速排除不相交的 连杆对, 以减少相交测试次数。采用公共面法对几 何元素进行碰撞与干涉检测。包括三角形包围法中 平面 平面的干涉检测 , 及 将用到的直线段 直线段、 连杆精确碰撞检测时所用到的直线段 凸多面体、 凸 多面体 凸多面体的干涉检测。该法最初由 Cundall 提出[ 8] , 主要用于研究边坡岩体的力学行为。我们 进一步改进了 C- P 法 , 使其成为机器人仿真系统 中通用的几何元素碰撞检测算法。
的几何对象, 从而只需对包围盒重叠对象进行进一 步的相交测试。此外 , 针对操作手的碰撞与干涉问 题 , Faverjon 提 出 虚 拟 操 作 手 法 ( virtual manipula tor) [ 5] 。Henrich 给出了 动态层次 法 ( dynamic hierar chy) [ 6] 。虚拟操作手法是将 n 自由度的操作手分成 多级 , 每一级用自由度为 k 的虚拟操作手代替, 0 k n 。对于一个 k 自由度虚拟操作手其后所有连 杆均被它们的扫描体代替。这样关节 k + 1 到 n 将 消失, 最后一连杆将成为子树为扫描体的装配树 [ 5] 。 针对 6R 机 器 人 杆 系 结 构 提 出 三 角 形 包 围 法
机械与电子 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 进
2 2,
直线段的形心为某中心点。 Z = CB - CA 。 当 C- P 初始位置确定后, 为了得到正确的 CP 位置, 必须对 C- P 进行一系列微调, 即平移和旋 转初始 C- P。 2 3 公共面的平移 求 C- P 与每个几何元素最近顶点的有向距离 dA 和 d B 。其方向为平行于 C- P 的法向单位矢量, 且由 C- P 指向该顶点 , 当与 n 同向时取正, 异向时 取负。 dA 取负, dB 取正。 dA = max [ d A - C ] dB = min[ d B - C ] 式中 dA m
0
引言
1
连杆间碰撞预检测的三角形包围法
对机器人的手臂作如下约定 : 将机器人的手臂
在机器人仿真系统中, 经常需要对 2 个或更多 个机器人在同一工作单元中协同作业进行仿真。通 常情况下多机器人工作空间存在重叠交叉区域 , 这 就要解决机器人之间相互干涉问题。传统的三维物 体碰撞与干涉检测方法多是进行 2 个空间物体的求 交运算[ 1- 3] , 往往比较复杂与费时。为提高检测效 率, 进行碰撞检测的一个常用方法是层次包围盒法 ( hierarchy bounding volume) 。其 核心思 想是用 体 积略大而几何特征简单的包围盒来近似地描述复杂
表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
( 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)
图 1 机器人的端点矢量
样, 排除不干涉连杆对的计算量将减少。在没有碰 撞的情况 下, 只需 进行 4 次三角形 对的干涉检测。 而直接检测则要进行 10 次凸多面体对的干涉检测。 1 2 三角形包围法流程 首先用 C- P 法对三角形对进行碰撞检测。若
不干涉 , 则该三角形对所包含的连杆间无碰撞。若 发生干涉, 则进一步对三角形对包含的连杆直线段 进行干涉检测。对满足碰撞条件的直线段所代表的 连杆进行精确的碰撞检测。 为了进一步提高检测效率 , 采用连杆端点矢量 投影的方法来排除完全不可能相交的三角形对。机 器人的端点可用矢量来表示, 如图 1 所示。机器人 每个三角形与 3 个端点矢量相对应机器人 A 的 A 1 与矢量 { m 0 , m 1 , m 2} , A 2 与矢 量 { m 2 , m 3, m 4 } ; 机 器人 B 的 B 1 与矢量 { n 0 , n 1 , n 2} , B 2 与矢量 { n 2 , n 3, n 4} 。 将 2 个机器人的端点矢量分别向O1O2 作投 影, 记 为 { m 0, m 1, m 2} , { m 2 , m 3 , m 4 } , { n 0 , n 1, n 2 } 和 { n
摘要: 根据 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)
1 1 2 2 2 2 2 2 2 2 2 1 1 1 1 1 1 1
机械与电子 2004( 9)
若直接对其进行碰撞检测 , 不但费时, 且在其中大部 分连杆不干涉情况下 , 将浪费大量计算资源。 为了快速排除不相交的连 杆对, 针对 6R 机器 人特有的结构 , 将其划分为 2 个三角形。长度不为 零的连杆依次形成三角形的 2 条边如图 1 所示。这
收稿日期 : 2004 07 15
[ 4]
进行细化, 用等长的直线段来代替 ; 同时 , 将手臂用 凸多面体包围 , 以弥补手臂细化的尺寸 , 2 个机器人 的坐标原点位于 xoy 平面。 1 1 三角形的建立 对于 2 个 6R 机器人 , 其连杆长度可表示为: l = [ l 1, l 2 , l 3 , l4 , l 5 , l 6] l = [ l 1, l 2 , l 3 , l4 , l 5 , l 6] 其中, l 4 = l 5= l4 = l 5 = 0。因此 , 6R 机器人的实 际连杆线 段数为 4。 发生碰 撞的 形式 有 10 种 [ 9] 。 45