一种用于多杆件多关节机构的干涉检测预警方法与流程

本发明属于接触式测量设备检测领域，具体涉及一种用于多杆件多关节机构的干涉检测预警方法。

背景技术：

工业机器人的末端执行器定位精度和轨迹误差是机器人的重要技术指标，因此测量、标定其性能的测量系统的研发具有重要的理论研究意义和工程实用价值。与基于光学原理的激光跟踪测量技术和视觉测量技术相比，基于拉线式编码器的机械式测量系统具有结构简单、操作方便、对测量环境要求低的特点，在复杂恶劣条件下易于应用，因此在复杂环境下机器人或大型运动部件的末端定位精度标定、轨迹误差测量等方面越来越凸显出重要的应用价值。

现有拉线式编码器的机械式测量系统一般基于三边测量原理设计，上述测量原理要求多个拉线编码器通过测量适配器引出的拉线延长线在空间交于一点。在实际使用中，测量适配器由于结构原因在某些测量点位会产生干涉问题，即杆件或关节发生干涉碰撞，影响了测量系统测量数据的可靠性。因此对干涉问题进行分析研究，设计可靠的检测预警方法，对该类拉线式测量系统具有重要的应用价值。

在多关节机械手碰撞干涉领域，国内比较早的提出判别空间两线段干涉的快速算法的是周友行，他将三维空间中的两线段投影到二维平面空间，提出一个简单的算法，不求解交点的坐标值，也不求解两空间线段的距离，就可以很容易地判断出空间两线段是否产生干涉，此算法易于进行模块化程序设计，计算量也大为减小，从而将问题简化。

西华大学程晓峰以一个六自由度的双臂机器人为例，在给定了机器人的末端运动轨迹末端夹持器的姿态及位置关于时间的函数的前提下，建立各杆件干涉区分析的数学模型，求解出计算结果，使用pro/engineer，完成该双臂机器人的三维建模和装配，为运动仿真分析提供正确的虚拟三维实体模型同时，使用msc公司提供的pro/engineer与adams的专用接口模块mechanism/pro，将在pro/engineer中建立的虚拟样机导入adams中，通过必要的几何参数及约束条件的设置，进行运动仿真分析并得出仿真结果。

北京邮电大学陈智链提出空间机械臂在轨运行碰撞干涉分析方法，它是适合空间机械臂干涉分析的新方法。该方法基于碰撞干涉空间对空间机械臂的干涉情况进行分析，通过判断机械臂当前构型对应的关节角是否存在于碰撞干涉空间来判断是否发生杆件之间的干涉。由于在进行干涉分析时不需要进行复杂的计算，因此该方法比较高效，既能够保证空间机械臂运动的安全性，也能满足空间机械臂控制的实时性。

技术实现要素：

为了解决工业机器人拉线式精度测量系统中测量适配器在测量过程中杆件或关节干涉检测问题，本发明提供一种用于多杆件多关节机构的干涉检测预警方法，用于在其测量过程中对于测量适配器干涉情况进行检测和预警，以克服杆件发生碰撞干涉对测量结果的影响，从而提高测量系统的可靠性。

为达到上述目的，本发明采用如下技术方案：

一种用于多杆件多关节机构的干涉检测预警方法，包括如下步骤：

s1、根据工业机器人拉线式精度测量系统的拉线长度，由三边测量技术原理求解测量适配器手爪末端的位置坐标；

s2、根据手爪末端位置坐标反解手爪各安装柱和关节的位姿，在位姿的多组解中，根据上一时刻的位姿选择合理解；

s3、根据各安装柱的位姿，求解相邻安装柱轴线的夹角，并判断是否超过角度阈值；

s4、根据手爪杆件关节的位姿，求解手爪部分的刚体轴线两端坐标，进而求得两个刚体的轴线距离，并判断该值是否超过距离阈值。

所述步骤s3具体包括如下步骤：

s31、根据相邻的两个安装柱末端的坐标，由余弦定理及反三角函数求解安装柱杆件i与安装柱杆件j轴线之间的夹角；

s32、判断所求的相邻安装柱轴线夹角是否超过设定的角度阈值β，若是，则系统发出预警，并提示干涉安装柱杆件i和j，同时工业机器人停止运行；否则，计算下一对相邻的安装柱杆件夹角，并跟阈值比较，依次循环，直至所有的相邻安装柱参与计算和比较。

所述步骤s4具体包括如下步骤：

s41、根据手爪部分刚体的位姿，分别求解任一手爪杆件m1上任一个刚体n1的轴线两端坐标和任一手爪杆件m2上任一个刚体n2的轴线两端坐标；

s42、根据s41求得的两个刚体轴线两端坐标，求解两个刚体的轴线距离dm1n1，m2n2；

s43、判断所求的两个刚体轴线距离是否超过设定的距离阈值a，若是，则系统发出预警，并提示干涉碰撞手爪杆件刚体编号，同时工业机器人停止运行；否则，计算下一对刚体轴线的距离，依次循环，直至所有杆件上的所有刚体都参与计算和比较。

与现有技术相比，本发明具有如下的突出的实质性特点和显著的优点：

本发明方法通过一一比较每个安装柱夹角与阈值的大小，以及每个杆件手爪部分的每个刚体的轴线距离与阈值的大小，有效的解决了工业机器人拉线式精度测量系统的干涉问题，提高了该系统的可靠性，同时预警系统可以保障测量适配器及其他部件的安全。除此之外，该方法通用性强，不仅仅可以用于拉线式测量机构测量适配器杆件和关节较少的情况下，还可以用于其他类似结构的杆件数和关节数更多更复杂的情况。

附图说明

图1为工业机器人精度测量系统的测量适配器单个手爪杆件图。

图2为工业机器人精度测量系统的测量适配器剖视图。

图3为工业机器人精度测量系统的测量适配器正视图。

图4为本发明方法的整体流程图。

图5为本发明的两安装柱杆件轴线角度检测方法及干涉预警系统的流程示意图。

图6为本发明的两杆件的两刚体的轴线距离检测方法及干涉预警系统的流程示意图。

具体实施方式

以下参照附图并举实施例对本发明进一步详细说明。

如图1所示为工业机器人精度测量系统的测量适配器单个手爪杆件图，其中，杆件on为测量适配器的安装轴线，杆件om为测量适配器的安装柱，即关节1，杆件mp为测量适配器的手爪部分，即关节2，由三个刚体组成，从上至下依次命名为刚体1、刚体2、刚体3。

如图2所示为工业机器人精度测量系统的测量适配器剖视图，图中数字按照逆时针顺序为测量适配器的安装柱命名。在工业机器人精度测量系统的测量过程中，相邻的安装柱靠近测量适配器安装轴线的一端容易产生干涉碰撞，影响工业机器人的测量可靠性。本实施例中，测量适配器的两个安装柱轴线的实测夹角与设定的角度阈值比较，超过阈值则预警，否则计算下一组安装柱轴线夹角，再进行比较，直至比较完所有的安装柱轴线夹角。

如图3所示为工业机器人精度测量系统的测量适配器正视图，图中数字为手爪杆件刚体名。杆件刚体名由两位数字组成，前一个数字表示测量适配器单个手爪的部分，如图1中的杆件mp，命名方式和图2中的安装柱相同，后一位数字表示杆件的刚体部分，数字从上向下依次增大。例如刚体23表示测量适配器手爪的第二个关节的第三个刚体。在工业机器人精度测量系统的测量过程中，相近的杆件或关节容易产生干涉碰撞，影响工业机器人的精度测量的可靠性。在本实施例中，测量适配器手爪的两个刚体轴线的实测距离与设定的距离阈值比较，超过阈值则预警，否则计算下一组刚体轴线距离，再进行比较，直至比较完所有的刚体轴线距离。

如图4所示，一种用于多杆件多关节机构的干涉检测预警方法，包括如下步骤：

s1、根据工业机器人拉线式精度测量系统的拉线长度，由三边测量技术原理求解测量适配器手爪末端的位置坐标。

s2、根据手爪末端位置坐标反解手爪各安装柱和关节的位姿，在位姿的多组解中，根据上一时刻的位姿选择合理解。

s3、根据各安装柱的位姿，求解相邻安装柱轴线的夹角，并判断是否超过角度阈值；

s31、根据相邻的两个安装柱末端的坐标，由余弦定理及反三角函数求解安装柱杆件i与安装柱杆件j轴线之间的夹角；

s32、判断所求的相邻安装柱轴线夹角是否超过设定的角度阈值β，若是，则系统发出预警，并提示干涉安装柱杆件i和j，同时工业机器人停止运行；否则，计算下一对相邻的安装柱杆件夹角，并跟阈值比较，依次循环，直至所有的相邻安装柱参与计算和比较，如图5所示。

图5为本发明的两安装柱杆件轴线角度检测方法及干涉预警系统的流程示意图，杆件i和杆件j为相邻的两个安装柱杆件；m为安装柱杆件的总数，在本实施例中m为3；αi，j为安装柱杆件i和安装柱杆件j的轴线夹角，基于计算机编程语言在线计算技术，根据杆件i和杆件j的两端空间坐标实时计算而得出；β为设定的角度阈值，根据测量适配器安装柱的极限位置，即安装柱杆件刚发生干涉时得出。在本实施例中β为46°。

s4、根据手爪杆件关节的位姿，求解手爪部分的刚体轴线两端坐标，进而求得两个刚体的轴线距离，并判断该值是否超过距离阈值；

s41、根据手爪部分刚体的位姿，分别求解任一手爪杆件m1上任一个刚体n1的轴线两端坐标和任一手爪杆件m2上任一个刚体n2的轴线两端坐标；

s42、根据s41求得的两个刚体轴线两端坐标，求解两个刚体的轴线距离dm1n1，m2n2；

s43、判断所求的两个刚体轴线距离是否超过设定的距离阈值a，若是，则系统发出预警，并提示干涉碰撞手爪杆件刚体编号，同时工业机器人停止运行；否则，计算下一对刚体轴线的距离，依次循环，直至所有杆件上的所有刚体都参与计算和比较，如图6所示。

图6为本发明的任意两手爪的两刚体的轴线距离检测方法及干涉预警系统的流程示意图，m1和m2为末端执行器手爪杆件名，与之对应的n1和n2分别为杆件m1和杆件m2上的刚体名，即关节m1n1表示手爪的第m1手爪杆件上的刚体n1；m为手爪杆件的总数，在本实施例中m为3；n为手爪杆件上刚体总数，在本实施例中n为3；dm1n1，m2n2表示刚体m1n1和刚体m2n2轴线的距离，基于计算机编程语言在线计算技术，根据刚体m1n1和刚体m2n2的两端空间坐标实时计算而得出；a为设定的刚体距离阈值，根据测量适配器手爪关节的极限位置，即关节刚发生干涉时得出。在本实施例中a为10mm。

在工业机器人拉线式精度测量系统中，相邻的安装柱干涉碰撞更容易发生，故在本发明方法中，将安装柱的干涉碰撞检测在前，关节的干涉碰撞检测安排在后，这有利于检测方法提高效率，同时最大程度上保证测量系统的可靠性。