一种机器人关节与几何体碰撞的检测方法技术

本发明专利技术属于碰撞检测技术领域，具体涉及一种机器人关节与几何体碰撞的检测方法，包括下列步骤：S1、将机器人各运动关节等效为最大包络圆柱体；S2、将几何体有可能与关节碰撞的面等效为长为l，宽为s的最大包络矩形；S3、检测最大包络矩形的四条边是否与最大包络圆柱体碰撞；S4、检测最大包络矩形平面是否与最大包络圆柱体碰撞；S5、若S3和S4中最大包络矩形的四条边、最大包络矩形平面与最大包络圆柱体均不发生碰撞，则该最大包络矩形与最大包络圆柱体不碰撞；否则发生碰撞。本发明专利技术在检测过程中无需附加任何传感器，对机器人的结构无需任何修改，对工况没有任何限制，成本低，且程序简单，执行效率高。本发明专利技术用于机器人关节与几何体碰撞的检测。

【技术实现步骤摘要】
一种机器人关节与几何体碰撞的检测方法
本专利技术属于碰撞检测
，具体涉及一种机器人关节与几何体碰撞的检测方法。
技术介绍
随着机器人产业的不断发展，人们对机器人的要求越来越高，尤其是在机器人安全性能方面。因此，在实际应用过程中，碰撞检测是非常必要的，它不仅能保障机器人本体的安全，更能起到保护外界物体或人员的安全。机器人碰撞检测一般分为两个部分：机器人关节间的碰撞检测和机器人与外界环境间的碰撞检测。对于机器人关节间的碰撞检测，最常见的方法是圆柱体模型法，该方法中将机器人各运动关节的具体结构简化为具有一定直径的最大包络圆柱体，并按照有界圆柱体的几何要素将两圆柱体的碰撞类型分为：平行碰撞，不平行时的柱面与柱面碰撞、端面与柱面碰撞和端面与端面碰撞，通过比较两圆柱轴线距离与两圆柱半径之和大小、两圆柱中心之间距离与两圆柱长度一半之和大小、端面圆心到另一圆柱轴线距离与两圆柱半径之和大小等等，进而判断属于哪种碰撞类型，从而实现机器人各关节间的碰撞检测研究。对于机器人与外界环境间的碰撞检测，最常见的方法主要有基于路径规划和力矩传感器的检测方法。基于路径规划的检测方法计算量往往很大，且需要针对特定的工作空间；基于力矩传感器的检测方法，由于需要额外增加传感器，增加了碰撞检测成本，而且由于安装的需要有可能需要改变机器人的结构，操作较为不便。专利CN2010260907公开了一种通过计算一个碰撞标量来衡量机器人在实时运作中是否发生碰撞，这种检测方法涉及两个变量，一个是位置误差，一个是电机的电流值。由于电机的电流值无法获得，需要在电机上安装电流传感器，这无疑增加了碰撞检测的成本。
技术实现思路
针对上述碰撞检测方法检测成本高的技术问题，本专利技术提供了一种无需附加传感器、对机器人的结构无需修改、对工况没有任何限制的机器人关节与几何体碰撞的检测方法。为了解决上述技术问题，本专利技术采用的技术方案为：一种机器人关节与几何体碰撞的检测方法，包括下列步骤：S1、将机器人各运动关节等效为最大包络圆柱体；S2、将几何体有可能与关节碰撞的面等效为长为l，宽为s的最大包络矩形；S3、检测最大包络矩形的四条边是否与最大包络圆柱体碰撞；S4、检测最大包络矩形平面是否与最大包络圆柱体碰撞；S5、若S3和S4中最大包络矩形的四条边、最大包络矩形平面与最大包络圆柱体均不发生碰撞，则该最大包络矩形与最大包络圆柱体不碰撞；否则发生碰撞。所述S1中最大包络圆柱体的底面半径为r、高为h，所述最大包络圆柱体的轴线中点坐标为C(xc，yc，zc)，所述最大包络圆柱体的轴线单位方向向量为所述最大包络圆柱体的端点坐标为C1(xc1，yc1，zc1)和C2(xc2，yc2，zc2)。所述S2中最大包络矩形的中心坐标为O(xo，yo，zo)，所述最大包络矩形的法向量为所述最大包络矩形的长边和短边的中点坐标分别为Cl(xcl，ycl，zcl)和Cs(xcs，ycs，zcs)，所述最大包络矩形的长边和短边的单位方向向量分别为和所述S3中检测最大包络矩形的四条边是否与圆柱体碰撞的方法为：将最大包络矩形的四条边等效为底面半径r＝0的圆柱体，与机器人n个关节的最大包络圆柱体依次进行碰撞检测。所述S4中检测最大包络矩形平面是否与圆柱体碰撞的方法为：S4.1、设最大包络矩形平面α的方程为xf·x+yf·y+zf·z+df＝0，将最大包络矩形的中心坐标为O(xo，yo，zo)代入xf·x+yf·y+zf·z+df＝0，得到df＝-(xf·xo+yf·yo+zf·zo)；S4.2、设以最大包络矩形长边的方向向量为法向量且过最大包络矩形长边的中点Cl(xcl，ycl，zcl)的平面β的方程为xl·x+yl·y+zl·z+dl＝0，将最大包络矩形长边的中点Cl(xcl，ycl，zcl)代入xl·x+yl·y+zl·z+dl＝0，得到dl＝-(xl·xcl+yl·ycl+zl·zcl)；S4.3、设以最大包络矩形短边的方向向量为法向量且过最大包络矩形短边的中点Cs(xcs，ycs，zcs)的平面γ方程为xs·x+ys·y+zs·z+ds＝0，将最大包络矩形短边的中点Cs(xcs，ycs，zcs)代入xs·x+ys·y+zs·z+ds＝0，得到ds＝-(xs·xcs+ys·ycs+zs·zcs)；S4.4、选择机器人关节的最大包络圆柱体的两个端点，所述两个端点分别为D1(xd1，yd1，zd1)、D2(xd2，yd2，zd2)，计算D1到平面α的距离S4.5、根据公式计算D1点的投影点Ds的坐标(xds，yds，zds)，即xds＝xd1+dα·xf，yds＝yd1+dα·yf，zds＝zd1+dα·zf；S4.6、计算投影点Ds(xds，yds，zds)到平面β的距离到平面γ的距离S4.7、如果满足dβ<s/2且dγ<l/2，说明D1点的投影点Ds在矩形内，则判断dα，若dα>r，则最大包络圆柱体的端点D1不与最大包络矩形平面α发生碰撞，若dα≤r，则与最大包络矩形平面α发生碰撞；如果不满足dβ<s/2且dγ<l/2，说明D1点的投影点Ds不在最大包络矩形内，则最大包络圆柱体的端点D1不与最大包络矩形平面α发生碰撞。所述S4.4中判断D2点是否与最大包络矩形平面α发生碰撞的方法与判断D1点是否与最大包络矩形平面α发生碰撞的方法相同，若两个端点D1、D2均不与最大包络矩形平面α发生碰撞，那么最大包络圆柱体与最大包络矩形平面α不发生碰撞；否则发生碰撞。所述S2中的几何体可以是规则几何体，也可以是不规则几何体，当几何体是不规则几何体时，将其表面简化为多个矩形的组合或最大包络矩形。本专利技术与现有技术相比，具有的有益效果是：本专利技术在检测过程中无需附加任何传感器，对机器人的结构无需任何修改，适用于机器人的任何运行工况，对工况没有任何限制，成本低，且程序简单，执行效率高。本专利技术的方法不仅可以应用于机器人关节末端连接的几何体与各关节间的碰撞检测，也可应用于机器人关节与外界环境空间中固定几何体的碰撞检测。附图说明图1为本专利技术最大包络圆柱体的简化模型；图2为本专利技术最大包络矩形的简化模型；图3为本专利技术的碰撞检测模型示意图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。一种机器人关节与几何体碰撞的检测方法，包括下列步骤：S1、将机器人各运动关节等效为最大包络圆柱体，最大包络圆柱体的底面半径为r、高为h，最大包络圆柱体的轴线中点坐标为C(xc，yc，zc)，最大包络圆柱体的轴线单位方向向量为最大包络圆柱体的端点坐标为C1(x本文档来自技高网...

【技术保护点】
1.一种机器人关节与几何体碰撞的检测方法，其特征在于：包括下列步骤：/nS1、将机器人各运动关节等效为最大包络圆柱体；/nS2、将几何体有可能与关节碰撞的面等效为长为l，宽为s的最大包络矩形；/nS3、检测最大包络矩形的四条边是否与最大包络圆柱体碰撞；/nS4、检测最大包络矩形平面是否与最大包络圆柱体碰撞；/nS5、若S3和S4中最大包络矩形的四条边、最大包络矩形平面与最大包络圆柱体均不发生碰撞，则该最大包络矩形与最大包络圆柱体不碰撞；否则发生碰撞。/n

【技术特征摘要】
1.一种机器人关节与几何体碰撞的检测方法，其特征在于：包括下列步骤：
S1、将机器人各运动关节等效为最大包络圆柱体；
S2、将几何体有可能与关节碰撞的面等效为长为l，宽为s的最大包络矩形；
S3、检测最大包络矩形的四条边是否与最大包络圆柱体碰撞；
S4、检测最大包络矩形平面是否与最大包络圆柱体碰撞；
S5、若S3和S4中最大包络矩形的四条边、最大包络矩形平面与最大包络圆柱体均不发生碰撞，则该最大包络矩形与最大包络圆柱体不碰撞；否则发生碰撞。


2.根据权利要求1所述的一种机器人关节与几何体碰撞的检测方法，其特征在于：所述S1中最大包络圆柱体的底面半径为r、高为h，所述最大包络圆柱体的轴线中点坐标为C(xc，yc，zc)，所述最大包络圆柱体的轴线单位方向向量为所述最大包络圆柱体的端点坐标为C1(xc1，yc1，zc1)和C2(xc2，yc2，zc2)。


3.根据权利要求1所述的一种机器人关节与几何体碰撞的检测方法，其特征在于：所述S2中最大包络矩形的中心坐标为O(xo，yo，zo)，所述最大包络矩形的法向量为所述最大包络矩形的长边和短边的中点坐标分别为Cl(xcl，ycl，zcl)和Cs(xcs，ycs，zcs)，所述最大包络矩形的长边和短边的单位方向向量分别为和


4.根据权利要求1所述的一种机器人关节与几何体碰撞的检测方法，其特征在于：所述S3中检测最大包络矩形的四条边是否与圆柱体碰撞的方法为：将最大包络矩形的四条边等效为底面半径r＝0的圆柱体，与机器人n个关节的最大包络圆柱体依次进行碰撞检测。


5.根据权利要求1所述的一种机器人关节与几何体碰撞的检测方法，其特征在于：所述S4中检测最大包络矩形平面是否与圆柱体碰撞的方法为：
S4.1、设最大包络矩形平面α的方程为xf·x+yf·y+zf·z+df＝0，将最大包络矩形的中心坐标为O(xo，yo，zo)代入xf·x+yf·y+zf·z+df＝0，得到df＝-(xf·xo+yf·yo+zf·zo)；
S4.2、设以最大包络矩形长边的方向向量为法向量且过最大包络矩形长边的中点Cl(xcl，ycl，zcl)的平...

【专利技术属性】
技术研发人员：陈晶晶，李济甫，王素钢，尉小雪，
申请(专利权)人：山西万合智能科技有限公司，
类型：发明
国别省市：山西;14