CN111421573B - 一种工业机器人拐角偏差性能测试方法 - Google Patents

Abstract

本发明涉及工业机器人位姿特性测量技术领域，尤其为一种工业机器人拐角偏差性能测试方法，通过机器人拐角偏差功能测试计算连续三次测量循环计算所得的拐角点与实到轨迹间的最小距离中的最大值，即为圆角偏差；计算连续三次测量循环计算所得的机器人不减速地以设定的恒定轨迹速度进入第二条轨迹后偏离指令轨迹的最大值的最大值，即为拐角超调。

Description

一种工业机器人拐角偏差性能测试方法

技术领域

本发明涉及工业机器人位姿特性测量技术领域，具体为一种工业机器人拐角偏差性能测试方法。

背景技术

工业机器人因同时具备通用性、高柔性、高精度等诸多特点，在全球制造业智能化趋势的推动下蓬勃发展。目前，工业机器人大多采用开环控制，为保证机器人的末端精度，在出厂前或使用一段时间后需要对机器人性能起显著影响的特性进行测试与调试。

机器人的拐角偏差性能参数是其中之一特性。机器人按程序设定的轨迹从第一条轨迹转到与之垂直的第二条轨迹时，便会出现拐角偏差。机器人拐角偏差性能功能测试参数分为圆角误差(CR)和拐角超调(CO)。专门用于机器人的测量系统价使用和维护成本高昂，且测量的机理均不清楚。

发明内容

本发明的目的在于提供一种工业机器人拐角偏差性能测试方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种工业机器人拐角偏差性能测试方法，包括如下步骤：

步骤1、选取测试点位：根据机器人的工作空间，顺时针选取E4、E1、E2、E3四个点，采用连续轨迹编程以得到矩形实验轨迹，每个拐角离平面各顶点的距离为该平面对角线长度的(10±2)％；

步骤2、使用机器人控制器驱动机器人末端从E4与E1的中点依次经过E1至E2至E3至E4，循环3次，记录跟踪仪采集数据和指令位姿数据；

步骤3、将机器人运动轨迹按拐角分为4部分，分别为：E4与E1的中点至E1与E2的中点，E1至E2的中点至E2至E3的中点，E2至E3的中点到E3至E4的中点，E3与E4的终点至E4与E1的中点；

步骤4、由于机器人在通过拐角、起点和终点的时候速度会减小，因此可以通过判断机器人连续三个点位变动大小判断机器每次循环运动的起始点和停止点，对于机器经过的第一个角来说，它的起点是每次循环的第一个动点；对于机器经过的最后一个角来说，它的终点是每次循环的最后一个静点；对于其他角，它们的起点是相对于该角的起点和终点的中点；

步骤5、当机器人坐标系不一致时候，需要做出判断并将坐标系转换为统一方向的机器人坐标系；

步骤6、求出四个指令点的中点坐标，将指令点投影到XOY面上，通过判断X,Y与重点的大小关系，求出当前机器人坐标系类型，之后进行旋转变换，对三类情况，分别对其坐标系逆时针旋转90°、180°、270°，将机器人坐标系旋转到默认坐标系方向；

步骤7、对每一部分，遍历每个点，求出测试点位到当前部分指令点位的最小距离，即求出每次循环每个拐角的最小值；

步骤8、根据CR计算公式求出CR，CR值即为每个拐角的三次循环的值的最大值；

步骤9、对测试数据点进行投影变换，将它们投影变换到指令点所在的参考平面上；

步骤10、求出投影后新的指令轨迹向量：E34，E41，E12，E23，向量的x，y，z坐标分别存在Ex[]，Ey[]，Ez[]中，用于计算CO；

步骤11、通过零点定理判断在每个拐角是否有CO值；

步骤12、对每一部分，从零点开始，到该部分的终点结束，用计算点到直线的距离方法计算测试数据点位到指令轨迹的最大距离，即求出每次循环每个拐角的值，最终结果为每个拐角的中的最大值；

步骤13、根据CO计算公式求出CO。

进一步的，步骤7中的最小距离采用两点间的距离公式：

求出。

进一步的，步骤8中的CR计算公式：

CR＝maxCR_j，j＝1,2,3

xe、ye、ze是指令拐角点的坐标；

xi、yi、zi是实到轨迹上的指令拐角附近第i个点的坐标。

进一步的，步骤9中的投影变换方法如下：

三维空间平面的一般方程为：

Ax+By+Cz+D＝0 (1)

假定不在平面上的三维空间点坐标为(x₀,y₀,z₀)，其在平面上的投影点坐标为(x_p,y_p,z_p)，因为投影点到当前点与平面垂直，根据垂直约束条件，易知y_p与z_p满足如下条件：

将(2)和(3)代入(1)，可以解得：

将(4)代入(2)、(3)，可以解得：

由此解得空间三维点到平面的投影坐标(x_p,y_p,z_p)。

进一步的，步骤11中的零点定理如下：

对于在XOY面上的点，若(f(x_i)-ins(x_i))×(f(x_i+1)-ins(x_i+1))<＝0，

则存在零点x＝i+1，

在YOZ面上同理，

此处应每个角投影后的超调方向都可能不一样，因此分为四种情况讨论，每一种情况类似，只需计算投影点分别在XOY面上和YOZ面上是否具有超调，若存在零点，则存在超调，记录零点下标；若无零点，则不存在超调，记录零点下标为-1，当拐角是E1时，其轨迹在XOY面的超调量是在X方向，在YOZ面的超调量是在Z方向，因此，计算这两个方向是否存在零点。

进一步的，步骤12中的求最大值的方法如下：假设给出三个点，A，B和C，要求点B到点A、C定出的直线间距离，第一步是找出A到B的向量AB和A到C的向量AC，该两向量的叉积除以|AC|，就能求出点到直线的距离，即：

进一步的，步骤13中的CO计算公式为：

CO＝maxCO_j,j＝1,2,3

xci、yci是指令轨迹上对应于zci的点的坐标；

xi、yi是实到轨迹上对应于zi的点的坐标；

此公式仅当(yi-yci)为正时才是正确的，若(yi-yci)为负，则不存在拐角超调。

进一步的，选取测试点位需要先完成三维空间测量仪坐标系与机器人坐标系的坐标转换，该坐标转换包括建立两个坐标系对应关系，SVD法算出R、T。

进一步的，建立两个坐标系对应关系包括首先，三维空间测量仪采集机器人末端法兰盘的球极坐标点位数据：

方位角

仰角θ和距离γ数据，然后通过球极坐标系与直角坐标系转换，可以获取测量仪的直角坐标点位数据，最后，再完成三维空间测量仪坐标系与机器人坐标系的坐标转换；

SVD法算出R、T包括：

(1)机器人坐标系下指令坐标点位的点集为：

指令坐标点位在测量仪下测得的实际坐标点位构成的点集：

(2)分别计算机器人和测量仪坐标系下的坐标点位点集P^r、P^t的重心，即坐标点位点集包含的全部点的坐标的平均值，分别为：

(3)将两个坐标点位点集的重心对齐重合，分别计算各点集相对重心的相对坐标构成新的点集：

记：

为：

记：

为：

(4)由坐标点位点集

构造协矩阵

对协矩阵进行SVD分解：

(5)旋转矩阵即为R_3×3＝VU^T，平移矩阵为T_3×1＝μ_r-Rμ_t，当n≥3时既可求出R矩阵，R的各列为长度为3的单位向量，且两两相互垂直。

进一步的，球极坐标系

与直角坐标系(x、y、z)转换公式如下：

z＝γ·cosθ

三维空间测量仪坐标系与机器人坐标系坐标转换公式如下：

P^t＝RP^r+T

R——旋转矩阵，T——平移矩阵；

P^t——三维空间测量仪坐标系下的坐标点位；

P^r——机器人坐标系下的坐标点位；

任意点P_i坐标的矩阵表示：

三维空间测量仪坐标系下的坐标点位：

机器人坐标系下的坐标点位：

与现有技术相比，本发明的有益效果是：

本发明所述的是测量机器人拐角偏差性能指标的方法，本方案中对计算原理进行了详细的推导说明，基于该方法可以开发相应的程序实现机器人的拐角偏差性能指标测试。机器人拐角偏差功能测试计算连续三次测量循环计算所得的拐角点与实到轨迹间的最小距离中的最大值，即为圆角偏差；计算连续三次测量循环计算所得的机器人不减速地以设定的恒定轨迹速度进入第二条轨迹后偏离指令轨迹的最大值的最大值，即为拐角超调。

附图说明

图1为本发明测量点的选择示意图。

图2为本发明的方法测量实到轨迹示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，术语“上/下端”、“内”、“外”“前端”、“后端”、“两端”、“一端”、“另一端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“设置/套设有”、“套接”、“连接”等，应做广义理解，例如“连接”，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

请参阅图1-2，本发明提供一种技术方案：

一、坐标转换

1.建立两个坐标系对应关系

工业机器人要进行位姿特性测量的话，需要先完成三维空间测量仪坐标系与机器人坐标系的坐标转换。首先，三维空间测量仪采集机器人末端法兰盘的球极坐标点位数据——方位角

仰角θ和距离γ数据。然后通过球极坐标系与直角坐标系转换，可以获取测量仪的直角坐标点位数据。最后，再完成三维空间测量仪坐标系与机器人坐标系的坐标转换。

球极坐标系

与直角坐标系(x、y、z)转换公式如下：

z＝γ·cosθ

三维空间测量仪坐标系与机器人坐标系坐标转换公式如下：

P^t＝RP^r+T

R——旋转矩阵，T——平移矩阵；

P^t——三维空间测量仪坐标系下的坐标点位；

P^r——机器人坐标系下的坐标点位。

任意点P_i坐标的矩阵表示：

三维空间测量仪坐标系下的坐标点位：

机器人坐标系下的坐标点位：

2.SVD法(奇异值分解法)算出R、T

(1)机器人坐标系下指令坐标点位的点集为：

指令坐标点位在测量仪下测得的实际坐标点位构成的点集：

(2)分别计算机器人和测量仪坐标系下的坐标点位点集P^r、P^t的重心，即坐标点位点集包含的全部点的坐标的平均值，分别为：

(3)将两个坐标点位点集的重心对齐重合，分别计算各点集相对重心的相对坐标构成新的点集：

记：

为：

记：

为：

(4)由坐标点位点集

构造协矩阵

对协矩阵进行SVD分解：

(5)旋转矩阵即为R_3×3＝VU^T，平移矩阵为T_3×1＝μ_r-Rμ_t，当n≥3时既可求出R矩阵，R的各列为长度为3的单位向量，且两两相互垂直。为保证拟合的优度，n取5。也可根据需要适当的增加点数。

二、测量方法

1、选取测试点位：根据机器人的工作空间，顺时针选取E4、E1、E2、E3四个点，采用连续轨迹编程以得到矩形实验轨迹，每个拐角离平面各顶点的距离为该平面对角线长度的(10±2)％(如附图1所示)。

2、使用机器人控制器驱动机器人末端从E4与E1的中点依次经过E1至E2至E3至E4，循环3次，记录跟踪仪采集数据和指令位姿数据。

3、将机器人运动轨迹按拐角分为4部分，分别为：E4与E1的中点至E1与E2的中点，E1至E2的中点至E2至E3的中点，E2至E3的中点到E3至E4的中点，E3与E4的终点至E4与E1的中点。分块后，计算每一个拐角的CR与CO值时候，便可只计算当前拐角部分的数据，减少数据量。

4、由于机器人在通过拐角、起点和终点的时候速度会减小，因此可以通过判断机器人连续三个点位变动大小判断机器每次循环运动的起始点和停止点。对于机器经过的第一个角(E1)来说，它的起点是每次循环的第一个动点；对于机器经过的最后一个角(E3)来说，它的终点是每次循环的最后一个静点；对于其他角，它们的起点是相对于该角的起点和终点的中点。

5、由于不同测试机型有不同的机器人坐标系，而拐角超调的计算算法与机器人坐标系有关，因此当机器人坐标系不一致时候，需要做出判断并将坐标系转换为统一方向的机器人坐标系。

6、求出四个指令点的中点坐标，将指令点投影到XOY面上，通过判断X,Y与重点的大小关系，求出当前机器人坐标系类型，之后进行旋转变换，对三类情况，分别对其坐标系逆时针旋转90°、180°、270°，将机器人坐标系旋转到默认坐标系方向。

7、对每一部分，遍历每个点，用两点间的距离公式：

求出测试点位到当前部分指令点位的最小距离，即求出每次循环每个拐角的最小值。

8、根据CR计算公式：

CR＝maxCR_j，j＝1,2,3

xe、ye、ze是指令拐角点的坐标；

xi、yi、zi是实到轨迹上的指令拐角附近第i个点的坐标。

求出CR，CR值即为每个拐角的三次循环的值的最大值。

9、对测试数据点进行投影变换，将它们投影变换到指令点所在的参考平面上，方法如下：

三维空间平面的一般方程为：

Ax+By+Cz+D＝0 (1)

假定不在平面上的三维空间点坐标为(x₀,y₀,z₀)，其在平面上的投影点坐标为(x_p,y_p,z_p)。因为投影点到当前点与平面垂直，根据垂直约束条件，易知y_p与z_p满足如下条件：

将(2)和(3)代入(1)，可以解得：

将(4)代入(2)、(3)，可以解得：

由此解得空间三维点到平面的投影坐标(x_p,y_p,z_p)

10、求出投影后新的指令轨迹向量：E34，E41，E12，E23，向量的x，y，z坐标分别存在Ex[]，Ey[]，Ez[]中，用于计算CO。

11、通过零点定理判断在每个拐角是否有CO值，零点定理如下：

对于在XOY面上的点，若(f(x_i)-ins(x_i))×(f(x_i+1)-ins(x_i+1))<＝0，

则存在零点x＝i+1。

在YOZ面上同理。

此处应每个角投影后的超调方向都可能不一样，因此分为四种情况讨论，每一种情况类似，只需计算投影点分别在XOY面上和YOZ面上是否具有超调，若存在零点，则存在超调，记录零点下标；若无零点，则不存在超调，记录零点下标为-1。当拐角是E1时，其轨迹在XOY面的超调量是在X方向，在YOZ面的超调量是在Z方向，因此，计算这两个方向是否存在零点。

12、对每一部分，从零点开始，到该部分的终点结束，用计算点到直线的距离方法计算测试数据点位到指令轨迹的最大距离，即求出每次循环每个拐角的值，最终结果为每个拐角的中的最大值。具体方法如下：

假设给出三个点，A，B和C，要求点B到点A、C定出的直线间距离。第一步是找出A到B的向量AB和A到C的向量AC，该两向量的叉积除以|AC|，就能求出点到直线的距离，即：

13、根据CO计算公式求出CO：

CO＝maxCO_j,j＝1,2,3

xci、yci是指令轨迹上对应于zci的点的坐标；

xi、yi是实到轨迹上对应于zi的点的坐标；

此公式仅当(yi-yci)为正时才是正确的，若(yi-yci)为负，则不存在拐角超调。

本发明，图1中的C1-C8为机器人最大空间的立方体，E1、E2、E3、E4为测量点。

本发明，所述的是测量机器人拐角偏差性能指标的方法，文中对计算原理进行了详细的推导说明，基于该方法可以开发相应的程序实现机器人的拐角偏差性能指标测试。机器人拐角偏差功能测试计算连续三次测量循环计算所得的拐角点与实到轨迹间的最小距离中的最大值，即为圆角偏差；计算连续三次测量循环计算所得的机器人不减速地以设定的恒定轨迹速度进入第二条轨迹后偏离指令轨迹的最大值的最大值，即为拐角超调。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (10)

1.一种工业机器人拐角偏差性能测试方法，其特征在于，包括如下步骤：

步骤1、C1-C8为机器人最大空间的立方体，平面为该立方体的对角面，矩形轨迹位于该平面上，点E1、E2、E3、E4为矩形轨迹的四个拐角，选取测试点位：根据机器人的工作空间，顺时针选取E4、E1、E2、E3四个点，采用连续轨迹编程以得到矩形实验轨迹，每个拐角离平面各顶点的距离为该平面对角线长度的(10±2)％；

步骤2、使用机器人控制器驱动机器人末端从E4与E1的中点依次经过E1至E2至E3至E4，循环3次，记录跟踪仪采集数据和指令位姿数据；

步骤3、将机器人运动轨迹按拐角分为4部分，分别为：E4与E1的中点至E1与E2的中点，E1至E2的中点至E2至E3的中点，E2至E3的中点到E3至E4的中点，E3与E4的终点至E4与E1的中点；

步骤4、由于机器人在通过拐角、起点和终点的时候速度会减小，因此可以通过判断机器人连续三个点位变动大小判断机器每次循环运动的起始点和停止点，对于机器经过的第一个角来说，它的起点是每次循环的第一个动点；对于机器经过的最后一个角来说，它的终点是每次循环的最后一个静点；对于其他角，它们的起点是相对于该角的起点和终点的中点；

步骤5、当机器人坐标系不一致时，需要做出判断并将坐标系转换为统一方向的机器人坐标系；

步骤6、求出四个指令点的中点坐标，将指令点投影到XOY面上，通过判断X,Y与重点的大小关系，求出当前机器人坐标系类型，之后进行旋转变换，对三类情况，分别对其坐标系逆时针旋转90°、180°、270°，将机器人坐标系旋转到默认坐标系方向；

步骤7、对每一部分，遍历每个点，求出测试点位到当前部分指令点位的最小距离，即求出每次循环每个拐角的最小值；

步骤8、根据CR计算公式求出CR，CR值即为每个拐角的三次循环的值的最大值，CR表示圆角误差；

步骤9、对测试数据点进行投影变换，将它们投影变换到指令点所在的参考平面上；

步骤10、求出投影后新的指令轨迹向量：E34，E41，E12，E23，向量的x，y，z坐标分别存在Ex[]，Ey[]，Ez[]中，用于计算CO，CO表示拐角超调；

步骤11、通过零点定理判断在每个拐角是否有CO值；

步骤12、对每一部分，从零点开始，到该部分的终点结束，用计算点到直线的距离方法计算测试数据点位到指令轨迹的最大距离，即求出每次循环每个拐角的值，最终结果为每个拐角的中的最大值；

步骤13、根据CO计算公式求出CO。

2.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤7中的最小距离采用两点间的距离公式：

求出。

3.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤8中的CR计算公式：

CR＝maxCR_j，j＝1,2,3

xe、ye、ze是指令拐角点的坐标；

xi、yi、zi是实到轨迹上的指令拐角附近第i个点的坐标。

4.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤9中的投影变换方法如下：

三维空间平面的一般方程为：

Ax+By+Cz+D＝0 (1)

假定不在平面上的三维空间点坐标为(x₀,y₀,z₀)，其在平面上的投影点坐标为(x_p,y_p,z_p)，因为投影点到当前点与平面垂直，根据垂直约束条件，易知y_p与z_p满足如下条件：

将(2)和(3)代入(1)，可以解得：

将(4)代入(2)、(3)，可以解得：

由此解得空间三维点到平面的投影坐标(x_p,y_p,z_p)。

5.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤11中的零点定理如下：

对于在XOY面上的点，若(f(x_i)-ins(x_i))×(f(x_i+1)-ins(x_i+1))<＝0，

则存在零点x＝i+1，

在YOZ面上同理，

此处应每个角投影后的超调方向都可能不一样，因此分为四种情况讨论，每一种情况类似，只需计算投影点分别在XOY面上和YOZ面上是否具有超调，若存在零点，则存在超调，记录零点下标；若无零点，则不存在超调，记录零点下标为-1，当拐角是E1时，其轨迹在XOY面的超调量是在X方向，在YOZ面的超调量是在Z方向，因此，计算这两个方向是否存在零点。

6.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤12中的求最大值的方法如下：假设给出三个点，A，B和C，要求点B到点A、C定出的直线间距离，第一步是找出A到B的向量AB和A到C的向量AC，该两向量的叉积除以|AC|，就能求出点到直线的距离，即：

7.根据权利要求1所述的工业机器人拐角偏差性能测试方法，其特征在于，步骤13中的CO计算公式为：

xci、yci是指令轨迹上对应于zci的点的坐标；

xi、yi是实到轨迹上对应于zi的点的坐标；

此公式仅当(yi-yci)为正时才是正确的，若(yi-yci)为负，则不存在拐角超调。

8.根据权利要求1～7中任意一项所述的工业机器人拐角偏差性能测试方法，其特征在于，选取测试点位需要先完成三维空间测量仪坐标系与机器人坐标系的坐标转换，该坐标转换包括建立两个坐标系对应关系，SVD法算出R、T，R、T分别指代坐标转换公式中的旋转矩阵和平移矩阵。

9.根据权利要求8所述的工业机器人拐角偏差性能测试方法，其特征在于，建立两个坐标系对应关系包括首先，三维空间测量仪采集机器人末端法兰盘的球极坐标点位数据：

方位角

仰角θ和距离γ数据，然后通过球极坐标系与直角坐标系转换，可以获取测量仪的直角坐标点位数据，最后，再完成三维空间测量仪坐标系与机器人坐标系的坐标转换；

SVD法算出R、T包括：

(1)机器人坐标系下指令坐标点位的点集为：

指令坐标点位在测量仪下测得的实际坐标点位构成的点集：

(2)分别计算机器人和测量仪坐标系下的坐标点位点集P^r、P^t的重心，即坐标点位点集包含的全部点的坐标的平均值，分别为：

(3)将两个坐标点位点集的重心对齐重合，分别计算各点集相对重心的相对坐标构成新的点集：

记：

为：

记：

为：

(4)由坐标点位点集

构造协矩阵

对协矩阵进行SVD分解：

(5)旋转矩阵即为R_3×3＝VU^T，平移矩阵为T_3×1＝μ_r-Rμ_t，当n≥3时既可求出R矩阵，R的各列为长度为3的单位向量，且两两相互垂直。

10.根据权利要求9所述的工业机器人拐角偏差性能测试方法，其特征在于，球极坐标系

与直角坐标系(x、y、z)转换公式如下：

z＝γ·cosθ

三维空间测量仪坐标系与机器人坐标系坐标转换公式如下：

P^t＝RP^r+T

R——旋转矩阵，T——平移矩阵；

P^t——三维空间测量仪坐标系下的坐标点位；

P^r——机器人坐标系下的坐标点位；

任意点P_i坐标的矩阵表示：

三维空间测量仪坐标系下的坐标点位：

机器人坐标系下的坐标点位：

Images