CN109333520A - 一种四轴scara机器人逆解方法 - Google Patents

Abstract

本申请公开了一种四轴SCARA机器人逆解方法，包括步骤：确定目标点P的姿态；在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；根据所述第一臂长L₁、第二臂长L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂；计算第三关节角度θ₃和第四关节角度θ₄。本发明实施例使用空间坐标系转平面坐标系的逆解方式，与常规的逆矩阵求逆解算法相比，提高了逆解的计算效率，并且降低了逆矩阵算法的复杂性，降低了四轴SCARA机器人控制器在频繁调用逆解算法计算点位的运算负担，可以满足实时计算的需求。

Description

一种四轴SCARA机器人逆解方法

技术领域

本申请涉及自动控制领域，尤其涉及四轴SCARA机器人逆解方法。

背景技术

随着科学技术的不断发展，工业机器人已经成为《中国制造2025》重点发展的十大领域之一，是工业自动化水平和工业4.0的重要标志，而机器人相关领域技术也逐渐成为各国共同关注的课题。机器人在抓取、焊接等一系列工作时需要对目标物的位置进行定位，再根据用户设定的工具坐标系或者是机械手的基坐标系进行计算，得到各个轴的角度值，从而移动到目标点进行工作。因此，机器人能否迅速地通过逆解精确可靠地计算出轴的位置是机器人控制技术的基础，同样是实现机器人控制的重要因素。

目前SCARA机器人大多数使用逆矩阵解法，计算复杂，运算效率低，而且计算求解结果不直观，容易出现多解导致难以检查错误，影响控制程序的运算效率。因此，发明人认为很有必要设计出一种简单直接且通用的四轴 SCARA机器人逆解工程算法来替代这些复杂的逆解算法。

发明内容

本申请提供一种四轴SCARA机器人逆解方法，提高逆解的计算效率，并且降低逆矩阵算法的复杂性，以满足实时计算的需求。

根据本申请实施例的提供的四轴SCARA机器人逆解方法包括步骤：

确定目标点P的姿态，所述目标点P的姿态包括左偏置和右偏置；

在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；

根据所述第一臂长L₁、第二臂长L₂和直线L_AB长度、夹角α以及目标点P 的姿态确定第一关节角度θ₁和第二关节角度θ₂；

根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄。

在一些例子中，所述目标点P由人工进行找点或者由机器人视觉寻找特征点提供。

在一些例子中，所述目标点P的姿态由第2关节的实际位置进行计算获得或者根据机器人实际运动需要决定。

在一些例子中，所述在机器人的俯视图方向建立平面坐标系，计算第一臂长 L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α，包括：

将目标点定位在机械手抓的中心，建立平面直角坐标系；

将目标点P的X轴和Y轴坐标定位至所述平面直角坐标系；

根据目标点P的X轴坐标和Y轴坐标计算获得第一臂长L₁与第二臂长L₂终点之间的直线L_AB长度；

根据所述直线L_AB长度计算直线L_AB与Y轴的夹角α。

在一些例子中，所述根据所述第一臂长L₁、第二臂长L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂，包括：

利用余弦定理计算获得第一关节角度θ₁，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取正值，右偏置时取负值；

利用余弦定理计算获得第二关节角度θ₂，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取负值，右偏置时取正值。

在一些例子中，若所述第一臂长L₁处于所述平面直角坐标系的第三象限，而目标点P在所述平面直角坐标系的定位点B位于第四象限，则计算公式变为：

在一些例子中，若所述第一臂长L₁处于所述平面直角坐标系的第四象限，而目标点P在所述平面直角坐标系的定位点B位于第三象限，则计算公式变为：

在一些例子中，所述根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄，包括：

获取SCARA机器人的滚珠丝杠导程D；

根据所述滚珠丝杠导程D和目标点P的Z轴坐标值计算获得第三关节角度θ₃，公式为：其中，PZ为目标点P的Z轴坐标值；

根据目标点P的U轴坐标值、第一关节角度第二关节角度计算获得所述第四关节角度θ₄，公式为：θ₄＝PU-θ₁-θ₂，其中，PU为目标点P的U坐标，θ₁、θ₂为目标点P所对应关节目标角度。

本发明实施例通过确定目标点P的姿态，然后在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；之后再，根据所述第一臂长L₁、第二臂长 L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂；最后，根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄。本发明实施例使用空间坐标系转平面坐标系的逆解方式，与常规的逆矩阵求逆解算法相比，大大提高了逆解的计算效率，并且降低了逆矩阵算法的复杂性，降低了四轴SCARA机器人控制器在频繁调用逆解算法计算点位的运算负担，可以满足实时计算的需求。

附图说明

图1是本申请的一种四轴SCARA机器人逆解方法中四轴SCARA机器人的空间坐标系结构简图；

图2是本申请的一种四轴SCARA机器人逆解方法的流程图；

图3是本申请一种四轴SCARA机器人逆解方法中平面直角坐标系的结构示意图；

图4是本申请的一种四轴SCARA机器人逆解方法中目标点P的姿态的示意图；

图5是本申请的一种四轴SCARA机器人逆解方法中目标点P在平面直角坐标系对应点B在不同象限的示意图；

图6是本申请的一种四轴SCARA机器人逆解方法中目标点P在平面直角坐标系B处于特殊情况之一的示意图；

图7是本申请的一种四轴SCARA机器人逆解方法中目标点P在平面直角坐标系B处于特殊情况之二的示意图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。

在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。

图1是本申请的一种四轴SCARA机器人逆解方法中四轴SCARA机器人的空间坐标系结构简图。在空间直角坐标系中，A点为第一关节，C点为第二关节，B点为第三关节。P点是四轴SCARA机器人的目标点，为简化问题，本申请实施例中，将P点定位在四轴SCARA机器人手抓的中心，因此在图1中，P点为第四关节，目标点P的空间坐标可以表示为(X，Y，Z， U)。可以看出，B点和目标点P的X轴和Y轴坐标相同。图1中，L₁为第一臂长，L₂为第二臂长，L_AB为所述第一臂长起点至第二臂长终点的直线。θ₁、θ₂、θ₃、θ₄分别为第一关节角度、第二关节角度、第三关节角度、第四关节角度。D为SCARA机器人的滚珠丝杠导程。

为了使本申请实施例的四轴SCARA机器人逆解方法，提高逆解的计算效率，并且降低逆矩阵算法的复杂性，以满足实时计算的需求。本申请提出以下技术方案，图2是部分步骤：

S101：确定目标点P的姿态，所述目标点P的姿态包括左偏置和右偏置；

本申请实施例可以根据机器人实际运动需求或第2关节的实际姿态来确定目标点P的姿态。所述目标点P的姿态分为左偏置和右偏置，请参看图4，当所述目标点P的姿态为左偏置时，θ₂＜0；右偏置时，θ₂＞0。

在一些实施例中，所述目标点P可以由人工进行找点或者由机器人视觉寻找特征点提供。

在一些实施例中，目标点P的姿态由第2关节的实际位置进行计算获得或者根据机器人实际运动需要决定。

S102：在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；

将目标点定位在机械手抓的中心，建立平面直角坐标系。在图1中的空间直角坐标系的基础上，在机器人的俯视方向，以第一臂长L₁的起点，第二臂长L₁的终点、第二关节C为平面建立平面直角坐标系，所述平面直角坐标系如图3所示。

将目标点P的X轴和Y轴坐标定位至所述平面直角坐标系，由于B点和目标点P的X轴和Y轴坐标相同，因此，图3中，B点即为目标点P的对应点。

参考图3，可以根据目标点P的X轴坐标和Y轴坐标计算获得第一臂长 L₁与第二臂长L₂终点之间的直线L_AB长度。

第一臂长L₁起点A与第二臂长L₂终点B之间的直线L_AB长度表达式：

式中，PX为点B的X坐标，PY为点B的Y坐标。

根据空间几何知识，求出L_AB与平面直角坐标系Y轴之间的夹角α，计算表达式：

α＝Atan2(PX，PY)

式中，夹角α为角度制，PX为点B的X坐标，PY为点B的Y坐标。以下反三角函数计算结果均弧度转角度。

S103：根据所述第一臂长L₁、第二臂长L₂和直线LA_B长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂；

参考图3，可以看出第一关节角度θ₁和第二关节角度θ₂可以利用余弦定理计算获得。

所述第一关节角度θ₁，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取正值，右偏置时取负值；

所述第二关节角度θ₂，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取负值，右偏置时取正值。

请参看图5，当B点在平面坐标系第2象限时，以右偏置为例，此时 Atan2(PX，PY)解为负值，而的解为正值，因此算出的θ₁为负值，此结果符合实际情况，而B点在第3象限、第4象限也同样能用该公式计算出正确结果。

但发明人发现，有两种特殊的情况需要特殊处理。

情况一请参看图6，若所述第一臂长L₁处于所述平面直角坐标系的第三象限，而目标点P在所述平面直角坐标系的定位点B位于第四象限，此时，使用上述公式求解产生的值我们先称为θ₁′，此时θ₁′＞180且为正值，但SCARA 机器人的θ₁限位为±180°，因此对于这种特殊情况，上述表达式求解出θ₁′后需要多运算一步表达式θ₁＝θ₁′-360°才能求得正确的解。此时公式变为：

由于目标点P的空间坐标(X，Y，Z)中的X、Y和B点所在的平面坐标系中的X、Y相同，所以在本步骤中求出的θ₁、θ₂即为目标点P所对应的关节角度。

情况二请参看图7，若所述第一臂长L₁处于所述平面直角坐标系的第四象限，而目标点P在所述平面直角坐标系的定位点B位于第三象限，使用此前的步骤求解时会求出θ₁′，此时θ₁′≤-180°且为负值，因此对于这种特殊情况，上述表达式求解出θ₁′后需要多运算一步表达式θ₁＝θ₁′+360°才能求得正确的解，此时计算公式变为：

S104：根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄。

从图1可以看出，第三关节角度θ₃作用只在Z轴方向。在一个实施例中，现获取SCARA机器人的滚珠丝杠导程D；然后根据所述滚珠丝杠导程 D和目标点P的Z轴坐标值计算获得第三关节角度θ₃。

由于已知SCARA机器人机械参数中滚珠丝杠的导程为D，则向Z正方向移动每1毫米所需要的角度为360/D，此时θ₃的表达式为：

式中，PZ为目标点P的Z坐标。

所述第四关节角度θ₄为SCARA机器人手抓自身的旋转角度，在一个实施例中，所述第四关节角度θ4可以由实际要求指定获得。

在另一实施例中，根据目标点P的U轴坐标值、第一关节角度第二关节角度计算获得所述第四关节角度θ₄，公式为：θ₄＝PU-θ₁-θ₂，其中，PU 为目标点P的U坐标，θ₁、θ₂为目标点P所对应关节目标角度。

在获得第一关节角度θ₁、第二关节角度θ₂、第二关节角度θ₃后，由于目标点P的U轴起始位置始终为以SCARA机器人手抓末端点为起点向Y轴正方向延伸且平行于Y轴的直线，因此第四关节角度θ₄除了受目标点P的U轴控制外还与第一关节角度θ₁和第二关节角度θ₂造成机器人末端朝向有关，此时第四关节角度θ₄的表达式为：

θ₄＝PU-θ₁-θ₂

式中，PU为目标点P的U轴坐标值，θ₁、θ₂为目标点P所对应第一关节角度和第二关节角度。

本发明实施例通过确定目标点P的姿态，然后在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；之后再，根据所述第一臂长L₁、第二臂长 L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂；最后，计算第三关节角度θ₃和第四关节角度θ₄。本发明实施例使用空间坐标系转平面坐标系的逆解方式，与常规的逆矩阵求逆解算法相比，大大提高了逆解的计算效率，并且降低了逆矩阵算法的复杂性，降低了四轴SCARA机器人控制器在频繁调用逆解算法计算点位的运算负担，可以满足实时计算的需求。

以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

Claims (8)

1.一种四轴SCARA机器人逆解方法，其特征在于，所述方法包括步骤：

确定目标点P的姿态，所述目标点P的姿态包括左偏置和右偏置；

在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α；

根据所述第一臂长L₁、第二臂长L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂；

根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄。

2.根据权利要求1所述的方法，其特征在于，所述目标点P由人工进行找点或者由机器人视觉寻找特征点提供。

3.根据权利要求1所述的方法，其特征在于，所述目标点P的姿态由第2关节的实际位置进行计算获得或者根据机器人实际运动需要决定。

4.根据权利要求1所述的方法，其特征在于，所述在机器人的俯视图方向建立平面坐标系，计算第一臂长L₁起点与第二臂长L₂终点之间的直线L_AB长度以及直线L_AB与Y轴的夹角α，包括：

将目标点定位在机械手抓的中心，建立平面直角坐标系；

将目标点P的X轴和Y轴坐标定位至所述平面直角坐标系；

根据目标点P的X轴坐标和Y轴坐标计算获得第一臂长L₁与第二臂长L₂终点之间的直线L_AB长度；

根据所述直线L_AB长度计算直线L_AB与Y轴的夹角α。

5.根据权利要求4所述的方法，其特征在于，所述根据所述第一臂长L₁、第二臂长L₂和直线L_AB长度、夹角α以及目标点P的姿态确定第一关节角度θ₁和第二关节角度θ₂，包括：

利用余弦定理计算获得第一关节角度θ₁，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取正值，右偏置时取负值；

利用余弦定理计算获得第二关节角度θ₂，计算公式为：

当所述目标点P的姿态为左偏置时，上述公式中正负取负值，右偏置时取正值。

6.根据权利要求5所述的方法，其特征在于，若所述第一臂长L₁处于所述平面直角坐标系的第三象限，而目标点P在所述平面直角坐标系的定位点B位于第四象限，则计算公式变为：

7.根据权利要求5所述的方法，其特征在于，若所述第一臂长L₁处于所述平面直角坐标系的第四象限，而目标点P在所述平面直角坐标系的定位点B位于第三象限，则计算公式变为：

8.根据权利要求1所述的方法，其特征在于，所述根据SCARA机器人的滚珠丝杠导程D计算第三关节角度θ₃和根据所述第一关节角度θ₁和第二关节角度θ₂计算第四关节角度θ₄，包括：

获取SCARA机器人的滚珠丝杠导程D；

根据所述滚珠丝杠导程D和目标点P的Z轴坐标值计算获得第三关节角度θ₃，公式为：其中，PZ为目标点P的Z轴坐标值；

根据目标点P的U轴坐标值、第一关节角度第二关节角度计算获得所述第四关节角度θ₄，公式为：θ₄＝PU-θ₁-θ₂，其中，PU为目标点P的U坐标，θ₁、θ₂为目标点P所对应关节目标角度。