logo资料库

基于静态环境下的空间多自由度机械臂碰撞检测.pdf

发布时间：2022-05-30 发布人：admin 分类：说明书 资料大小：0.40M 资料格式：pdf 举报 版权申诉
第1页 / 共9页
第2页 / 共9页
第3页 / 共9页
第4页 / 共9页
第5页 / 共9页
第6页 / 共9页
第7页 / 共9页
第8页 / 共9页
资料共9页，剩余部分请下载后查看
    5 10 15 20 25 30 35 40 中国科技论文在线 http://www.paper.edu.cn 基于静态环境下的空间多自由度机械臂碰 撞检测# 宗成星1，陈波芝1，陈诺2，赵萍1** （1. 合肥工业大学机械工程学院，安徽合肥，230009； 2. 合肥工业大学材料科学与工程学院，安徽宣城，242000） 摘要：针对某串联的空间多自由度机械臂运动过程中与障碍物发生碰撞的问题，本文提出了 一种静态碰撞检测算法。通过 D-H 法对机械臂建模，分析得到其运动学方程，并导出了机 械臂各关节和末端的坐标，对可能与给定障碍物发生碰撞的机械臂的几个连杆，将上面各关 节点表示成连杆直线段方程。其次，对于机械臂各杆件自身以及与障碍物之间的干涉问题， 本文做了必要的简化处理，将不规则的障碍物包络成多个长方体模型，机械臂各杆件包络成 圆柱体，并将各关节的径向半径叠加到障碍物上，这样机械臂各杆件之间及机械臂本体与障 碍物之间的碰撞问题，也就转化为空间两直线段及空间直线段与空间四边形之间的位置关系 判断。最后通过仿真实验验证了该方法的可行性与有效性。 关键词：空间机械臂；长方体包络；碰撞检测；空间直线段；空间平面 中图分类号：TP391.9 Collision Detection for Multi-DOF Spatial Robot Arm in Static Environment Zong Chengxing1, Chen Bozhi1, Chen Nuo2, Zhao Ping1 (1. School of Mechanical Engineering, Hefei University of Technology,Anhui,Hefei,230009; 2. School of Materials Engineering，Hefei University of Technology，Anhui，Xuanchen，242000) Abstract: For the problem of collision with obstacles during the movement of series multi-DOF spatial manipulator，this essay present a new algorithm about detecting stationary obstacles. Through Modeling the manipulator by the way of D-H, analyzing the kinematics equations of it and deducing the coordinates of every articulation and endpoint of manipulator, we use line segment equations of links which collide with given obstacles to express the points that have been mentioned. Then, this essay simplifying the intervene problem of manipulator and obstacles by modeling the obstacle with cuboids and the manipulator with cylinder, it also add the radial radius to obstacles. By this way, the problem of collision between each link of manipulator and obstacles can be transformed into a judgment of location of spatial straight line segment and spatial quadrilateral. We also can use two spatial straight lines to solve the intervene problem of manipulator itself. In the end, we prove the feasibility and effectiveness of the mention by simulation.(10 Points, Times New Roman) Key words: Spatial robot; Rectangular envelope; Collision detection; Straight line segment; space plane 0 引言 多臂机器人关节运动过程中与障碍物的碰撞检测问题是机器人路径规划技术研究中的 一个重要内容。在计算机仿真，虚拟现实场景中，一方面由于物体与物体之间的运动与交互， 它们之间不可避免的产生碰撞，对此系统必须要实时检测到碰撞；另一方面碰撞检测得到的 实时数据可以直接运用于路径规划的研究中，所以精确的碰撞检测算法对于机器人避碰路径 规划技术研究具有重要的意义。碰撞检测算法方法从时间域的层面来看，主要分为两大类[1]： 基金项目：国家自然科学基金资助项目 (51405128)；安徽省自然科学基金资助项目（1508085QE82） 作者简介：宗成星（1991-），男，在读研究生，机器人运动与控制 通信联系人：赵萍（1988-），女，副教授，机器人与机构学. E-mail: ping.zhao@hfut.edu.cn - 1 -
    45 50 55 60 65 中国科技论文在线 http://www.paper.edu.cn 静态碰撞检测方法和动态碰撞检测方法，静态碰撞检测方法主要用来检测各物体位姿在不随 时间变化前提下，它们之间是否发生碰撞，该算法计算速度快，效率高，一般不太注重实时 性，但对计算精度要求颇高。动态碰撞检测方法主要检测物体位姿随时间变化下，各物体之 间的碰撞，该算法计算速度慢，注重实时性。 目前，国内外许多研究人员都做了机器人碰撞检测有关方面的工作，研究出了许多模型 和算法[2]。北京理工大学学者汪首坤等人，针对机械臂在运动过程中可能与工作空间的障碍 物发生碰撞的问题，提出了圆柱体包络检测方法[3,4]，该作者将机械臂整体与障碍物都用一 定大小的圆柱体包络进行简化，这样就将机械臂与不规则或规则障碍物之间的碰撞检测问题 就转化为了两圆柱体之间的位置关系判断。中国科技大学学者祁若龙等人，采用了一种严格 的机械臂简化方法，即将机械臂连杆的径向最大半径叠加到障碍物的厚度上，这里的障碍物 则运用球体进行包络的[5]，这样将机械臂与障碍物两个实体间的碰撞问题转化为了线段与实 体间的位置关系判断，同时对难以描述的实体外形，进一步转化为一个用球心和球半径描述 的球体序列，这样连杆直线段与障碍物实体间的干涉判断又转化为直线段与一系列球心点之 间的距离判断。 多关节机械臂避碰问题主要包括两个方面：机械臂各关节间的碰撞与机械臂整体与障碍 物的碰撞。针对机械臂关节间的碰撞问题，本文采用了圆柱体包络法，这样机械臂碰撞检测 可简化为多个两圆柱体间最短距离的求解，在不影响计算精度的和提高计算效率前提下，对 上述圆柱体进一步简化成空间两线段，这样机械臂各关节之间的碰撞问题就转化为求空间两 直线段相互关系的空间解析几何问题；另一方面对于机械臂本体与障碍物之间的碰撞问题 [6]，本文提出了多长方体包络检测方法。首先基于给定的空间机械臂，通过 D-H 法建模，分 析得到机械臂的运动学方程，在此基础上导出机械臂各关节和末端的坐标；其次，采用多长 方体包络检测法，将不规则障碍物模型包络成一定大小的长方体，且对机械臂整体进行简化 处理，将机械臂连杆的径向最大半径叠加到障碍物的厚度上，这样就把机械臂与障碍物之间 的实体碰撞问题转化为了空间直线段与空间平面之间的位置关系判断，而空间直线段方程可 以通过机械臂关节坐标得到，最后，通过计算机仿真验证该方法的可行性。 1 空间机械臂建模及简化 1.1 关节坐标 70 空间多自由度机械臂各关节示意如图 1 所示，根据 D-H 法建模[7]，分析得到该机械臂的 运动学方程，其齐次变换矩阵的通式为： T i 1 − i = sin cos 0 − θ θ ⎡ i i ⎢ sin cos sin cos cos α θ α θ α − ⎢ i i i 1 1 − − sin sin cos sin cos ⎢ θ α θ α α i i 1 − ⎢ 0 0 0 ⎣ i 1 − i 1 − i 1 − i i a i 1 − d sin α − i i 1 − d cos α i i 1 − 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ (1) 式中：θi,αi-1,ai-1,di 为相应的连杆参数，即各关节的转角相邻关节之间的扭转角，连杆长度及 相邻关节之间的偏距。且 75 - 2 -
    中国科技论文在线 c i = c o s θ i , s i = s in ( i i = 1, 2 ...6 ) 根据以上信息分析，可以得到各关节的空间坐标。 80 基座坐标系原点为 ⎡ ⎢ ⎢ ⎢ ⎣ x y z 0 0 0 0 0 0 ⎤ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎣ 0 0 d 0 ⎤ ⎥ ⎥ ⎥ ⎦ − 关节 1 坐标为 ⎡ ⎢ ⎢ ⎢ ⎣ x y z 0 1 0 1 0 1 ⎤ ⎥ ⎥ ⎥ ⎦ = 0 0 0 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ 85 关节 2 坐标为 x 02 y z 02 02 ⎡ ⎢ ⎢ ⎢ ⎣ = ⎤ ⎥ ⎥ ⎥ ⎦ d s − 2 1 d c 2 2 0 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ 关节 3 坐标为 90 ⎡ ⎢ ⎢ ⎢ ⎣ x y z 0 3 0 3 0 3 ⎤ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎣ 关节 4 坐标为 2 a c c - d s 2 1 d c + a c s 2 2 1 2 1 2 - a s 2 2 ⎤ ⎥ ⎥ ⎥ ⎦ 1 x 04 y 04 z 04 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎣ 1 2 3 1 2 3 a (c c c - c c c ) - d c - d (c c c + c c c ) + a c c 3 2 1 2 d c - d (c c c + c c c ) - a (s s s - c c s ) + a c s 2 1 2 2 1 4 - a (c s + c s ) - d (c c - s s ) - a s 2 2 1 2 3 1 3 2 2 1 3 3 1 2 2 3 1 3 1 2 3 3 2 3 2 1 4 3 2 4 2 3 2 3 http://www.paper.edu.cn (2) (3) (4) (5) (6) (7) ⎤ ⎥ ⎥ ⎥ ⎦ - 3 -
    中国科技论文在线 http://www.paper.edu.cn 95 关节 5 坐标为 x 05 y 05 z 05 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎣ 1 2 3 1 2 3 a (c c c - c c c ) - d c - d (c c c + c c c ) + a c c 3 2 1 2 d c - d (c c c + c c c ) - a (s s s - c c s ) + a c s 2 1 2 2 1 4 - a (c s + c s ) - d (c c - s s ) - a s 2 2 1 2 3 1 3 2 2 1 3 3 1 2 2 3 1 1 2 3 2 3 2 1 2 3 3 2 2 3 4 3 3 4 ⎤ ⎥ ⎥ ⎥ ⎦ 关节 6 坐标为 x 06 y 06 z 06 ⎡ ⎢ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎣ 100 1 2 3 1 2 3 a (c c c - c c c ) - d c - d (c c c + c c c ) + a c c 2 1 2 3 d c - d (c c c + c c c ) - a (s s s - c c s ) + a c s 2 1 2 2 1 4 - a (c s + c s ) - d (c c - s s ) - a s 2 2 1 2 3 1 3 2 2 1 3 3 1 2 2 3 1 1 2 3 2 3 2 1 2 3 2 3 3 2 4 3 3 4 ⎤ ⎥ ⎥ ⎥ ⎦ (8) (9) 105 110 Fig.1 Schematic diagram of mechanical arm joint 图 1 机械臂关节示意图 1.2 关节坐标 多关节空间串联机械臂碰撞检测主要包括两个方面：机械臂各关节间碰撞和机械臂本体 与障碍物间的碰撞。在碰撞检测之前，首先必须要分析机械臂的结构，对机械臂结构进行适 度简化，建立机械臂各关节简化模型。关于机械臂各关节间的碰撞问题，已转化为空间两直 线段之间的位置关系判断，而机械臂本体与障碍物间的碰撞问题，已转化为空间直线段与空 间四边形之间的位置关系判断，为此本文将机械臂各关节实体简化为空间直线段，如图 2， 3 所示。 - 4 -
    中国科技论文在线 http://www.paper.edu.cn 115 120 125 130 135 140 图 2 机械臂实体 图 3 机械臂简化模型 Fig.2 Mechanical arm Fig.3 Simplified model of manipulator 2 静态环境下的障碍物建模 2.1 障碍物模型 空间障碍物一般都是不规则的几何形状物，为了方便的判断机械臂碰撞问题，必然要对 障碍物进行规则化处理，基于空间机械臂的模型，障碍物模型采用多长方体包络，如图 4 所示。当然，采用长方体包络障碍物，必然会扩大障碍物的区域，但它大大简化了碰撞检测 方法，对规划出来的路径更具有安全性。 Fig.4 Multiple rectangular envelope 图 4 多长方体包络 2.2 长方体包络 本文机械臂本体与障碍物间的碰撞采用的是多长方体包络，即将空间不规则障碍物模型 包络成一定大小的长方体，这样将机械臂与障碍物两个实体间的碰撞问题转化为了空间平面 之间的位置关系判断，考虑到了该方法的计算量大，过程繁琐，对机械臂进一步简化，将机 械臂连杆的径向最大半径叠加到障碍物的厚度上，这样空间平面之间的碰撞判断就转化为了 空间直线段与空间平面之间的位置关系判断，而机械臂各关节间碰撞[4]是将多臂机器人机械 臂和各运动关节简化为圆柱体，通过计算两圆柱体中心线间最短距离来进行机器人的碰撞检 测研究，考虑到前人工作做的非常成熟，本文利用其结论不展开研究。 针对机械臂本体与障碍物间的碰撞问题，不妨设长方体的长、宽、高依次为 Lx，Ly, Lz， 一个顶点坐标为(x0,y0,z0),则根据空间解析几何知识易得该长方体的其它 7 个顶点坐标为： (x0,y0,z0+Lz),(x0,y0+Ly,z0+Lz),(x0,y0+Ly,z0),(x0+Lx,y0,z0),(x0+Lx,y0,z0+Lz),(x0+Lx,y0+Ly,z0+Lz),(x0 +Lx,y0+Ly,z0)，将以上各点按顺序连接可得到空间任意大小的长方体。在此基础上，将机械 臂连杆的径向最大半径叠加到障碍物的厚度上，此时根据空间向量的方法，可得新的长方体 - 5 -
    中国科技论文在线 http://www.paper.edu.cn 八个顶点坐标，具体就是根据原先的长方体八个顶点坐标，分别求得沿 x,y,z 方向的单位向 量，再整体乘以叠加厚度。不妨设原先长方体的顶点坐标 (x1,y1,z1),(x2,y2,z2),…(x8,y8,z8),设叠加厚度为 r,则第一个顶点沿 x 方向单位向量 xN 为 xK x 2 y 1 x 1 z 1 = − y ] [ ] [ z 2 2 145 150 N x = r K K x x 第一个顶点沿 y 方向单位向量 yN 为 yK y 5 x 5 y 1 x 1 z 1 = − [ ] [ ] z 5 N y = r K K y y 第一个顶点沿 z 方向单位向量 zN 为 zK x 4 y 1 x 1 z 1 = − y ] [ ] [ z 4 4 N z = r K K z z 则沿第一个顶点的叠加后新的坐标为 p new 1 = [ x 1 y 1 z 1 ] + N x + N y + N z (10) (11) (12) (13) 155 其他 7 个顶点情况类似处理，最后运用 MATLAB 编写程序。如图 5 所示为一个厚度叠加的 实例，设长方体的长、宽、高分别为 Lx=1mm，Ly=2mm, Lz=3mm，一个顶点坐标为(1,1,1), 叠加厚度 r=0.5mm,仿真运行结果如图。 160 图 5 长方体包络模型 Fig.5 Rectangular envelope model - 6 -
    中国科技论文在线 3 机械臂关节碰撞检测算法 3.1 空间直线段与空间平面位置关系 http://www.paper.edu.cn 本文研究的机械臂为空间串联链式结构，为了研究方便，将机械臂整体骨架化，通过严 格的简化处理已转化为空间直线段；而空间障碍物一般不具有规则结构，采用多长方体包络 更能接近实际的障碍物模型，因此机械臂本体与障碍物间的碰撞问题就转化为空间直线段与 空间四边形之间的位置关系。空间的线面位置关系主要有三种:线段与平面相交，线段与平 面平行，线段与平面异面。这里线段与平面相交又要分为三种情况讨论，即交点在平面内， 交点在某条边上，线段在面内。 已知空间线段上两个端点 p1(x1,y1,z1)和 p2(x2,y2,z2)，空间四边形的法向量 M(m1,m2,m3) 和四个顶点的坐标，由空间解析几何知识，直线段与平面位置关系就可以用空间直线段方向 向量与空间四边形法向量之间关系判断，其中 N=(x2-x1,y2-y1,z2-z1),（1）若 直线段与平面平行，没有交点。（2）若 ，则 ，则直线段与平面有交点，设为 P0(x0,y0,z0), 接下来需要判断该交点是否就是碰撞点，有几何关系得知，它必须满足两个条件:(i)交点必 M N =i M N ≠i 0 0 须要在线段上。(ii)交点在四边形内部。障碍物是长方体包络而成的，前后面平行于 z 轴， 左右面平行于 y 轴，上下面平行于 x 轴。对平行于 x 轴的上平面来说，设平面四个顶点 s1(x3,y3,z3),s2(x4,y4,z4), s3(x5,y5,z5),s4(x6,y6,z6), 如果同时满足以下条件： max( ⎧ ⎪ max( ⎨ ⎪ max( ⎩ min( min( min( z z , 1 x x , 3 4 y y , 3 z 0 x 0 y 0 ≤ ≤ ≤ ≤ ≤ ≤ ) ) ) 2 4 z z , 1 2 x x , 3 4 y y , 3 ) ) ) 4 ，那么 p0 就是线段与四边形的交点，其它情况类似。 3.2 碰撞检测算法实现 本文采用了长方体包络检测算法。其主要的思想就是把机械臂各杆件简化成空间直线段 和障碍物包络成长方体，并将机械臂的实体半径叠加到障碍物的包络长方体上。假设包络 N 自由度机械臂各杆件的空间直线段方程为{R1,R2,…RN-1}，而包络 L 个障碍物的长方体分别 为{O1,O2,…OL}。通过机械臂各杆件的空间直线段方程为{R1,R2,…RN-1}与{O1,O2,…OL}各个 面是否有交点的检测，把上述碰撞问题转化为空间直线段与四边形之间的位置关系的判断。 165 170 175 180 185 其算法步骤如下： 190 Step1 初始化机械臂的起始点和终止点，及每个关节的初始关节角，并且已知 n 个关节 195 点即 n-1 个连杆最大径向半径依次为{r1,r2,r3,…rn-1}。 Step2 障碍物建模。具体就是采用长方体包络法，将不规则障碍物包络成基于八个顶点 构成逼近障碍物实体的长方体,其次根据机械臂模型的简化，即将每个机械臂连杆的径向最 大半径叠加到障碍物的厚度上，公式如(9)-(11)，重新构成为新的长方体模型。 Step3 机械臂运动学正解。运用 D-H 法建模，得到机械臂的齐次变换矩阵，进而求得每 个关节的坐标及末端的位置坐标，公式如(2)-(8)，再根据机械臂简化模型，可以获得每两个 相邻关节间的空间直线度方程。 Step4 碰撞检测。由 Step3 中得到的各关节的空间位置坐标，可以确定具体机械臂相邻两关 节 间 的 直 线 段 方 程 ， 检 测 机 机 械 臂 各 杆 件 的 空 间 直 线 段 方 程 为{R1,R2,…RN-1} 与 - 7 -
    中国科技论文在线 http://www.paper.edu.cn 200 205 210 {O1,O2,…OL}各个面是否有交点的情况。接下来需要判断该交点是否就是碰撞点，有几何关 系得知，它必须满足两个条件:(i)交点必须要在线段上。(ii)交点在四边形内部。如果同时满 足，即发生碰撞。 4 仿真结果分析 本文仿真实验所选用的机械臂是仿真实验常用的一款机械臂 PUMA560。首先设置机械 臂的 D-H 参数，即 a2=4mm,a3=1mm,d2=3mm,d4=4mm,初始关节角 ϴ1=90°,ϴ2=0°,ϴ3=-90°, ϴ4=0°,ϴ5=0°，ϴ6=0°,机械臂各连杆的包络最大半径为 0.5mm；其次，用 MATLAB 语言编程并 对上述碰撞检测算法做了一个仿真分析，如果发生碰撞，设输出为 1，反之，为 0。 下 面 结 合 碰 撞 检 测 仿 真 实 验 ， 给 了 两 组 机 械 臂 某 一 位 姿 各 关 节 角 度 ： (1)( 27°, 39°,11°,0°,0°,0°),根据机械臂正运动学方程，可得各关节的坐标,仿真结果如图 6 所示，机械 臂本体不与障碍物发生碰撞，仿真结果输出为 0,通过 ADAMS 软件建立静态障碍物环境和机 械臂实体模型，验证仿真结果如下图 7 所示；(2)(12°,5°,17°,0°,0°,0°)，仿真结果如图 8 所示， 机械臂本体与障碍物发生碰撞，仿真结果输出为 1,通过 ADAMS 软件建立静态障碍物环境和 机械臂实体模型，验证仿真结果如下图 9 所示。 215 图 6 机械臂简化模型与障碍物无碰示图 图 7 机械臂实体与障碍物无碰示图 Fig.6 Robot arm simplified model and obstacles Fig.7 The mechanical arm and the obstacles without collision diagram without collision 220 图 8 机械臂简化模型与障碍物碰撞示图 图 9 机械臂实体与障碍物碰撞示图 Fig.8 A simplified model of the manipulator Fig.9 A simplified model of the manipulator and the collision of obstacles and the collision of obstacles - 8 -
    分享到：
    赞 收藏

    相关推荐

    资料库

    开发技术

    共收录19306份资料，累计14个分类，关注成员有19位，主要包括：Actionscript,Javascript,PHP,VB,Perl,Delphi,Python,C#,C,Java,C++,Web开发,硬件开发,其它

    热门标签

    Actionscript Javascript PHP VB Perl Delphi Python C# C Java C++ Web开发 硬件开发 其它

    最新资料