公开/公告号CN112669434A
专利类型发明专利
公开/公告日2021-04-16
原文格式PDF
申请/专利权人 山东华数智能科技有限公司;滕州华数智能制造研究院;
申请/专利号CN202011519586.4
申请日2020-12-21
分类号G06T15/30(20110101);G06T17/20(20060101);G06T17/00(20060101);
代理机构13113 石家庄科诚专利事务所(普通合伙);
代理人左燕生;贺寿元
地址 277500 山东省枣庄市滕州市荆河街道奚仲路滕州墨子科创园内A3厂房
入库时间 2023-06-19 10:38:35
技术领域
本发明属于虚拟现实和工业机器人领域,具体涉及一种基于网格与包围盒的碰撞检测方法。
背景技术
碰撞检测影响虚拟世界中的沉浸感和交互性,是虚拟现实中核心研究问题之一。为了防止工业机器人在实际作业中发生碰撞,对工人和设备造成损害,因此需预先在虚拟环境中对机器人运动轨迹进行预先验证,以方便改正轨迹和排除有问题的轨迹点,从而方便改正和调整轨迹点或设备的布局等。传统的包围盒方法在工业机器人应用环境中碰撞检测效率不高,呈现出碰撞检测实时性和精确性巨大的矛盾。由于工业机器人在虚拟仿真时一直处于动态变化的,需要对工业机器人轨迹上大量点进行碰撞检测,因此对实时性要求较高,而传统的包围盒算法,在模型过大时,会造成碰撞检测时间复杂度过高,检测效率低下;同时因为精确性不高,可能出现机器人与工件之间穿透现象,造成真实感下降。机器人轨迹规划需要对多条轨迹线的大量的点位进行碰撞检测,高效的检测将节省大量时间,因此为了提高检测效率,需要提出一种适用于复杂模型的高效检测方法。
发明内容
本发明的目的,是要提供一种基于网格与包围盒的碰撞检测方法,通过采用网格划分、外接球包围盒及OBB包围盒混合的方法提升了碰撞检测的精确性。
本发明为实现上述目的,所采用的技术方案如下:
一种基于网格与包围盒的碰撞检测方法,包括以下步骤:
S1、读取三维模型中的三角形面片顶点坐标和法向量信息;
S2、模型处理阶段,对三维模型进行网格划分,根据读取的三角形面片顶点坐标和法向量信息,在每个网格内先构造OBB包围盒,再用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,依次类推直到层次OBB包围盒二叉树不能再分为止,然后以在每个网格内先构造的OBB包围盒为基础构建外接球包围盒;
S3、碰撞检测阶段,遍历检测每个网格,通过进行网格相交测试,定位可能碰撞的网格区域,在该网格区域内,依次对每个网格的外接球包围盒、内部的层次OBB包围盒二叉树、三角形面片进行相交测试,判断是否发生碰撞。
作为限定,步骤S1中,读取三维模型的三角形面片顶点坐标和法向量信息时,依次读取三角形面片的法向量、三角形面片的顶点坐标,对读取的三角形面片进行索引标号,并在最后通过以红黑树为底层的关联式Map容器数据结构去除冗余点减少存储的消耗。
作为第二种限定,步骤S2中,具体包括以下步骤:
S21、对三维模型进行等尺寸网格划分,根据读取的三角形面片顶点坐标和法向量信息,在每个等尺寸的网格内先构造OBB包围盒,用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,直到层次OBB包围盒二叉树不能再分为止;
S22、以在每个等尺寸的网格内先构造的OBB包围盒为基础,以其中心坐标作为外接球中心坐标,并以该OBB包围盒中心到该OBB包围盒顶点的最远距离作为外接球的半径构建每个网格最外层的外接球包围盒。
作为第三种限定,步骤S21中,对三维模型进行等尺寸网格划分是采用二分回溯法,即下一次划分网格的边长是上一次网格边长的二分之一,若边长过大使网格未包含三角形面片,则改变网格边长为上次网格边长和此次网格边长的和的均值,继续划分网格。
作为第四种限定,步骤S21中,在每个等尺寸的网格内构造OBB包围盒时,设第i个三角形面片的三个顶点为(p
其中,p
然后用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,其中分离轴选择该OBB包围盒根节点的最长轴,分离点选择分离轴的中点,分离面即为通过分离点垂直于分离轴的面,分离面将此OBB包围盒根节点剖分为两个OBB包围盒;若该方式不能剖分此OBB包围盒根节点,则选择次长轴实现剖分,若仍是不能剖分,则选择最短轴,分离出的两个OBB包围盒即为OBB包围盒根节点的两个OBB包围盒子节点,再用这两个OBB包围盒子节点继续用“分离轴”法继续分离,通过此递归过程,直到层次OBB包围盒二叉树子节点中的OBB包围盒中的三角形面片不能再分为止,即构造为层次OBB包围盒二叉树。
作为第五种限定,步骤S3中,具体包括以下步骤:
S31、遍历检测每个网格,通过进行网格相交测试,定位可能碰撞的网格区域;在网格相交测试中,计算两网格的中心点距离L与网格的边长rmin、网格的顶点到网格中心的距离rmax的关系,两网格的中心坐标为C1(x1,y1,z1),C2(x2,y2,z2);
a、若L>2rmax,则两网格必不相交,不必进行下一步的相交测试;
b、若L<2rmin,则两网格必定相交,通过步骤S32进行外接球包围盒的相交测试;
c、若2rmin≤L≤2rmax,则两网格可能相交也可能不相交,通过步骤S32进行外接球包围盒的相交测试;
其中,
S32、外接球包围盒的相交测试:
设两个外接球包围盒的半径分别为R1、R2,若R1+R2>L1,则两外接球包围盒必不相交,否则通过步骤S33进行层次OBB包围盒二叉树相交测试;
S33、层次OBB包围盒二叉树相交测试,判断OBB包围盒是否相交;
计算两个OBB包围盒A、B的三个坐标轴方向以及两个OBB包围盒的三个坐标轴方向两两叉乘得到的十五条分离轴上投影的情况,如果在其中一条分离轴上的投影满足下式,则两OBB包围盒分离,只有当两个OBB包围盒在所有分离轴上的投影均不满足下式,则判定两OBB包围盒相交,通过步骤S34进行两三角形面片间的相交测试;
其中,T为从OBB包围盒A中心指向OBB包围盒B中心的向量,L为待检测的分离轴,r
S34、三角形面片的相交测试
给定两个三角形面片T1和T2,两个三角形面片所在的平面分别为F1和F2,计算T1所有顶点a(a
A、若所有行列式符号相同且均不为0,表明两三角形面片不相交,否则两三角形面片T1与T2相交,继续进行下面的判断;
B、若所有行列式值都为0,则两三角形面片T1与T2共面;
C、若其中一个行列式值为0,另外两个同号,则行列式值为0的行列式所包括的三角形面片T2的顶点在F1内,然后通过上述三个行列式计算该顶点是否在三角形面片T1内,若在三角形面片T1内,则两三角形面片相交,否则不相交;
D、若均不满足步骤A、B、C,则三角形面片T2的三个顶点位于F1两侧,计算三角形面片T1、T2与F1、F2的交线是否重叠,若重叠,则两三角形相交。
作为第六种限定,步骤S3中,若检测到发生碰撞的网格后,则对网格内部的外接球包围盒进行坐标和方向的更新,只有检测到外接球包围盒碰撞才会对其中的OBB包围盒进行坐标和方向的更新。
本发明由于采用了上述的技术方案,其与现有技术相比,所取得的技术进步在于:
(1)本发明采用网格与层次OBB包围盒二叉树结合的方法,很好的中和了包围盒紧密性与精确性之间的矛盾,提高了虚拟仿真中机器人与周围环境间的检测速度和精确性;
(2)本发明通过采用网格划分、外接球包围盒及OBB包围盒混合的方法提升了碰撞检测的精确性,碰撞检测一开始检测到碰撞的网格以后便会定位检测区域到某个网格,然后对网格内的层次OBB包围盒二叉树进行深度优先遍历,大大提升了碰撞检测的实时性。
本发明属于虚拟现实和工业机器人领域,通过网格与层次OBB包围盒二叉树结合的方法,提高虚拟仿真中机器人与周围环境间的检测速度和精确性。
附图说明
图1为本发明实施例的流程示意图;
图2为本发明实施例层次OBB包围盒二叉树的构建流程图;
图3为本发明实施例OBB包围盒相交检测的投影示意图;
图4为本发明实施例三角形面片相交的示意图。
具体实施方式
下面结合实施例对本发明作进一步说明,但本领域的技术人员应当理解,本发明并不限于以下实施例,任何在本发明具体实施例基础上做出的改进和变化都在本发明权利要求保护的范围之内。
实施例 一种基于网格与包围盒的碰撞检测方法
如图1所示,本实施例包括以下步骤:
S1、读取三维模型中的三角形面片顶点坐标和法向量信息;
其中,用程序读取三维模型例如.stp,.stl等中性格式文件的信息时,依次读取三角形面片的法向量、三角形面片的顶点坐标,对读取的三角形面片进行索引标号,并在最后通过以红黑树为底层的关联式Map容器数据结构去除冗余点减少存储的消耗;
S2、模型处理阶段,对三维模型进行网格划分,根据读取的三角形面片顶点坐标和法向量信息,在每个网格内先构造OBB包围盒,再用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,依次类推直到层次OBB包围盒二叉树不能再分为止,如图2所示,然后以在每个网格内先构造的OBB包围盒为基础构建外接球包围盒;
本步骤具体包括以下步骤:
S21、对三维模型进行等尺寸的正方体网格划分,根据读取的三角形面片顶点坐标和法向量信息,在每个等尺寸的正方体网格内先构造OBB包围盒,用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,以分离出的OBB包围盒继续“分离”出OBB子节点直到层次OBB包围盒二叉树不能再分为止,构成一个层次OBB包围盒二叉树;
其中,对三维模型进行等尺寸正方体网格划分是采用二分回溯法,即下一次划分网格的边长是上一次网格边长的二分之一,若边长过大使网格未包含三角形面片,则改变网格边长为上次网格边长和此次网格边长的和的均值,继续划分网格;网格存储了其中包含的三角面片的索引号及网格中心坐标,不包含三角面片的网格则可以从内存中去掉,减小消耗,提高检测效率;
本步骤在,在每个等尺寸的网格内构造OBB包围盒时,设第i个三角形面片的三个顶点为(p
其中,p
然后用“分离轴”法构造以该OBB包围盒为根节点,分离出的OBB包围盒为子节点的层次OBB包围盒二叉树,其中分离轴选择该OBB包围盒根节点的最长轴,分离点选择分离轴的中点,分离面即为通过分离点垂直于分离轴的面,分离面将此OBB包围盒根节点剖分为两个OBB包围盒;若该方式不能剖分此OBB包围盒根节点,则选择次长轴实现剖分,若仍是不能剖分,则选择最短轴,分离出的两个OBB包围盒即为OBB包围盒根节点的两个OBB包围盒子节点,再用这两个OBB包围盒子节点继续用“分离轴”法继续分离,通过此递归过程,直到层次OBB包围盒二叉树子节点中的OBB包围盒中的三角形面片不能再分为止,即构造为层次OBB包围盒二叉树;
S22、以在每个等尺寸的网格内先构造的OBB包围盒为基础,以其中心坐标作为外接球中心坐标,并以该OBB包围盒中心到该OBB包围盒顶点的最远距离作为外接球的半径构建每个网格最外层的外接球包围盒;
碰撞检测阶段,遍历检测每个网格,通过进行网格相交测试,定位可能碰撞的网格区域,在该网格区域内,依次对每个网格的外接球包围盒、内部的层次OBB包围盒二叉树、三角形面片进行相交测试,判断是否发生碰撞;
本步骤中,具体包括以下步骤:
S31、遍历检测每个网格,通过进行网格相交测试,定位可能碰撞的网格区域;在网格相交测试中,计算两网格的中心点距离L与网格的边长rmin、网格的顶点到网格中心的距离rmax的关系,两网格的中心坐标为C1(x1,y1,z1),C2(x2,y2,z2);
a、若L>2rmax,则两网格必不相交,不必进行下一步的相交测试;
b、若L<2rmin,则两网格必定相交,通过步骤S32进行外接球包围盒的相交测试;
c、若2rmin≤L≤2rmax,则两网格可能相交也可能不相交,通过步骤S32进行外接球包围盒的相交测试;
其中,
S32、外接球包围盒的相交测试:
设两个外接球包围盒的半径分别为R1、R2,若R1+R2>L1,则两外接球包围盒必不相交,否则通过步骤S33进行层次OBB包围盒二叉树相交测试;
S33、层次OBB包围盒二叉树相交测试,判断OBB包围盒是否相交,如图3所示;
计算两个OBB包围盒A、B的三个坐标轴方向以及两个OBB包围盒的三个坐标轴方向两两叉乘得到的十五条分离轴上投影的情况,如果在其中一条分离轴上的投影满足下式,则两OBB包围盒分离,只有当两个OBB包围盒在所有分离轴上的投影均不满足下式,则判定两OBB包围盒相交,通过步骤S34进行两三角形面片间的相交测试;
其中,T为从OBB包围盒A中心指向OBB包围盒B中心的向量,L为待检测的分离轴,r
S34、三角形面片的相交测试
如图4所示,给定两个三角形面片T1和T2,两个三角形面片所在的平面分别为F1和F2,计算T1所有顶点a(a
A、若所有行列式符号相同且均不为0,表明两三角形面片不相交,否则两三角形面片T1与T2相交,继续进行下面的判断;
B、若所有行列式值都为0,则两三角形面片T1与T2共面;
C、若其中一个行列式值为0,另外两个同号,则行列式值为0的行列式所包括的三角形面片T2的顶点在F1内,然后通过上述三个行列式计算该顶点是否在三角形面片T1内,若在三角形面片T1内,则两三角形面片相交,否则不相交;
D、若均不满足步骤A、B、C,则三角形面片E2的三个顶点位于F1两侧,计算三角形面片T1、T2与F1、F2的交线是否重叠,若重叠,则两三角形相交。
本步骤中,若检测到发生碰撞的网格后,则对网格内部的外接球包围盒进行坐标和方向的更新,只有检测到外接球包围盒碰撞才会对其中的OBB包围盒进行坐标和方向的更新;
其中,物体在三维空间中的运动可分为平移和旋转,平移变换为3x1的列向量t=(x,y,z)
假设旋转顺序为x,y,z轴,则合成的旋转矩阵为
本领域的技术人员容易理解,以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
机译: 机动车碰撞的早期检测方法包括:基于确定的碰撞时间来估计碰撞危险,并引入超过预定碰撞危险的减少碰撞的措施。
机译: 基于概率方案的碰撞网格图谱学习和识别碰撞情况的装置和方法
机译: 基于网格目标的低和高有机移动相组成的基于碰撞的电离源的可复制性提高