CN113386136B - 一种基于标准球阵目标估计的机器人位姿矫正方法及*** - Google Patents

Abstract

本发明属于机器人相关技术领域，并公开了一种基于标准球阵目标估计的机器人位姿矫正方法及***。该方法包括下列步骤：S1建立机器人基坐标系、机器人末端坐标系、扫描仪测量坐标系和标准球阵局部坐标系；S2机器人带动扫描仪多角度扫描标准球阵，读取并记录机器人位姿和在该位姿下的标准球阵点云；计算获得机器人末端坐标系相对于机器人基坐标系的变换矩阵；S3将机器人末端坐标系相对于机器人基坐标系的变换矩阵转换为实际六维矢量，该实际六维矢量即为机器人实际位姿，计算机器人实际位姿与步骤S2中读取位姿之间的误差，实现机器人位姿矫正。通过本发明，突破了现有方法存在的瓶颈，具有成本低，实用性强，矫正效率高，适用范围广等优点。

Description

一种基于标准球阵目标估计的机器人位姿矫正方法及***

技术领域

本发明属于机器人相关技术领域，更具体地，涉及一种基于标准球阵目标估计的机器人位姿矫正方法及***。

背景技术

机器人具有操作灵活、柔性高等特点，以工业机器人代替人完成码垛、焊接、装配等操作已成为工业机器人应用领域的主流发展趋势，然而工业机器人较低的绝对定位精度限制了其作业的精准性。

为此，许多学者针对机器人的运动学参数对工业机器人展开了精度补偿方法理论方面的研究，这种方式大多涉及到雅可比矩阵计算、微分运动求解等复杂的数学公式推导过程，对于只需要矫正特定机器人位姿的情况而言效率较低，而且测量设备(激光跟踪仪)的成本会大大增加。相应地，本领域存在着发展一种成本低、实用性强、矫正效率高的基于标准球阵目标估计的机器人位姿快速矫正方法的技术需求。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于标准球阵目标估计的机器人位姿矫正方法及***，实现对特定的机器人位姿进行快速矫正，突破了现有方法存在的瓶颈，具有成本低，实用性强，矫正效率高，适用范围广等优点。

为实现上述目的，按照本发明的一个方面，提供了一种基于标准球阵目标估计的机器人位姿矫正方法，该方法包括下列步骤：

S1将扫描仪固连在机器人末端，标准球阵放置在工作台上，建立机器人基坐标系{B}、机器人末端坐标系{E}和扫描仪测量坐标系{S}和标准球阵局部坐标系{W}；

S2机器人带动扫描仪多角度扫描所述标准球阵，读取并记录机器人位姿和在该位姿下的标准球阵点云；基于记录的位姿和点云信息，计算手眼矩阵；将记录的点云转换至机器人基坐标系下，并与标准球阵设计模型进行匹配；利用匹配的结果完成标准球阵目标估计；基于尺寸链传递模型计算获得机器人末端坐标系{E}相对于机器人基坐标系{B}的变换矩阵

S3通过矢量-矩阵转换关系，将所述机器人末端坐标系相对于机器人基坐标系的变换矩阵

转换为实际六维矢量

该实际六维矢量

即为通过坐标变换计算获得的机器人实际位姿，计算并获得该机器人实际位姿与步骤S2中读取的机器人位姿之间的误差，以此实现机器人位姿的矫正。

在步骤S2中，所述变换矩阵

按照下列方式获得：

S21基于所述标准球阵点云建立标准球阵局部坐标系{W}，计算获得标准球阵局部坐标系{W}相对于扫描仪测量坐标系{S}的变换矩阵

S22对机器人进行手眼标定，以此获得机器人的手眼关系矩阵，即扫描测量坐标系{S}相对于机器人末端坐标系{E}之间的变换矩阵

S23基于刚度变换矩阵和点云匹配，求取机器人基坐标系{B}相对于标准球阵局部坐标系{W}的变换矩阵

S24构建变换矩阵

和

与变换矩阵

之间的关系，利用该关系式计算获得所需的变换矩阵

进一步优选地，在步骤S21中，所述变换矩阵

按照下列方式计算获得：

(a)根据每个姿态下扫描的标准球阵点云的数据，拟合每个标准球球心的坐标；

(b)选取其中一个标准球的球心作为原点建立标准球阵局部坐标系，该坐标系的原点三维坐标和坐标轴方向形成变换矩阵

进一步优选地，在步骤S22中，变换矩阵

按照下列步骤获得：

(a)构建机器人位姿与机器人末端坐标系相对于机器人基坐标系的变换矩阵之间的关系式，利用读取的机器人位姿计算获得机器人末端坐标系 {E}相对于机器人基坐标系{B}的变换矩阵

(b)构建变换矩阵

和

之间的关系，以此计算获得变换矩阵

进一步优选地，在步骤(a)中，所述变换矩阵

按照下列关系式计算：

其中，

表示第i次测量时{E}相对于{B}的变换矩阵，R(z,Ez_i)表示绕着z轴旋转Ez_i的旋转矩阵(R(y,Ey_i)、R(x,Ex_i)类推)，

是一个三维矢量，表示第i次测量时{E}的原点在{B}下的位置坐标。

进一步优选地，在步骤S23中，所述变换矩阵

按照下列方式获得：

(a)通过刚体变换矩阵乘积运算，将每一幅扫描仪测量坐标系下的点云^SP_i转换至机器人基坐标系下，以此获得在机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n}；

(b)将机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n}作为测试模型，将标准球阵的三维实体模型作为参考模型，采用ADF算法进行匹配，以此获取机器人基坐标系相对于标准球阵局部坐标系的变换矩阵为

(c)将机器人基坐标系相对于标准球阵局部坐标系的变换矩阵

转换为六维矢量组

求解该六维矢量组的均值

通过矢量-矩阵变换，由六维矢量

求解变换矩阵

进一步优选地，步骤(a)中，所述将每一幅扫描仪测量坐标系下的点云^SP_i转换至机器人基坐标系下，按照下列关系式进行：

其中，^BP_i是第i次扫描获取的机器人基坐标系下的标准球阵点云，^SP_i是第i次扫描获取的扫描仪测量坐标系下的标准球阵点云，

是第i次扫描机器人末端坐标系相对于机器人基坐标系的变换矩阵。

进一步优选地，在步骤S24中，所述变换矩阵

阵

和

与变换矩阵

之间的关系按照下列关系式进行：

其中，i是扫描测量次数，n表示扫描测量总次数。

进一步优选地，所述误差按照下列关系是进行计算：

其中，

是第i次扫描在基坐标系中机器人位姿误差，

是第i次扫描在机器人基坐标系中机器人的实际位姿，

是第i次扫描在机器人控制器中读取的机器人位姿，i是扫描次数，n是扫描次数总数。

按照本发明的另一个方面，提供了一种上述所述的机器人位姿矫正方法进行矫正的***，其特征在于，该***包括机器人、扫描仪和标准球阵，所述扫描仪固连在所述机器人末端，所述标准球阵包括多个大小不同且非线性排列的标准球，该标准球阵设置在所述扫描仪的扫描范围内。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：

1.本发明只需将待矫正的机器人位姿添加到手眼标定操作过程中即可对特定的机器人位姿进行矫正，易于实施；只需进行矩阵乘积正/逆运算及矩阵-矢量变换即可快速矫正若干特定的机器人位姿，基于尺寸链传递模型确定新的机器人位姿，完成机器人位姿快速矫正，可对若干特定的机器人位姿进行快速矫正，突破了现有方法存在的瓶颈，具有成本低，实用性强，矫正效率高，适用范围广等优点；

2.本发明中采用扫描仪和标准标定球的方式实现机器人位姿的矫正，与现有技术中采用价格昂贵的激光跟踪仪和靶球相比，极大地降低了成本，同时，通过扫描仪多角度扫描标准球阵的实现方式快，与现有技术中规划机器人网格式的空间位姿相比，耗时更短；

3.本发明中基于包含至少三个不同直径大小标准球的标准球阵完成机器人手眼标定，刚体变换矩阵易于求解；另外，采用ADF算法进行点云匹配，并基于若干匹配变换矩阵完成标准球阵目标估计，ADF匹配算法融合了点与点的距离函数以及点与平面的距离函数，相对于ICP点点匹配，不易陷入局部最优，计算效率高且适用范围广。

附图说明

图1为本发明实施例提供的一种基于标准球阵目标估计的机器人位姿快速矫正方法的流程示意图；

图2为本发明实施例提供的基于标准球阵目标估计完成机器人位姿快速矫正的***示意图；

图3为本发明实施例提供的标准球阵设计模型示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提供的一种基于标准球阵目标估计的机器人位姿快速矫正方法，方法适用于机器人位姿的快速矫正，其利用六自由度工业机器人带动光栅式双目面阵扫描仪多角度扫描测量标准球阵完成机器人手眼标定，通过测量点云与设计模型匹配完成标准球阵目标估计，基于尺寸链传递模型确定新的机器人位姿，完成机器人位姿快速矫正。

如图2所示，图中是基于标准球阵目标估计完成机器人位姿快速矫正的***，机器人为六自由度工业机器人，扫描仪为光栅式双目面阵扫描仪，如图3所示，在本实施例中，标准球阵包含三个大小不同且非线性排列的标准球(标准亚光陶瓷球)。其中，{B}表示机器人基坐标系，{E}表示机器人末端法兰坐标系，{S}表示扫描仪测量坐标系，{W}表示标准球阵局部坐标系(或工件坐标系)，

表示机器人末端法兰坐标系相对于机器人基坐标系的变换矩阵，

表示扫描仪测量坐标系相对于机器人末端法兰坐标系的变换矩阵，

表示标准球阵局部坐标系相对于扫描仪测量坐标系的变换矩阵，

表示机器人基坐标系相对于标准球阵局部坐标系的变换矩阵，其中，

和

是定值，

与扫描次数有关，不同的扫描次数对应不同的

变换矩阵，因此，记为

如图1所示，一种基于标准球阵目标估计的机器人位姿快速矫正方法，该方法包括以下步骤：

步骤一，机器人带动扫描仪多角度扫描测量标准球阵，完成机器人手眼标定。

具体地，首先，控制机器人带动扫描仪多角度扫描测量标准球阵，记获取的标准球阵点云为{^SP₁,^SP₂,^SP₃,...,^SP_i,...,^SP_n}，其中，i＝1,2,3,...n，n表示扫描测量次数，每一幅扫描仪测量坐标系下的点云^SP_i由N个点{^Sp₁，^Sp₂，^Sp₃，…，^Sp_m，…，^Sp_N}组成，其中，m＝1,2,3,...,N，三维矢量^Sp_m＝[x_m y_m z_m]^T表示第m个点的坐标，同时，依次记机器人位姿为

为一个六维矢量，表示第i个机器人位姿，

表示机器人的位置，ψ_i＝[Ex_i Ey_i Ez_i]^T表示机器人的姿态；上述扫描过程中，至少包含三个或三个以上非线性相关的机器人位姿，且保证机器人位姿非奇异。

其次，基于标准球阵点云创建标准球阵局部坐标系：基于测量点云拟合标准球的球心并拟合球心所在的平面，以其中一个球的球心为原点，选取另一个球的球心建立x轴方向，以球心所在平面的其中一个法矢为z轴，通过右手定则获取y轴方向。以局部坐标系的原点为位置矢量，以x、y、z 轴的方向为姿态矢量，计算若干标准球阵局部坐标系相对于扫描仪测量坐标系的变换矩阵

同时，通过矢量-矩阵转换，由六维矢量

求解机器人末端坐标系相对于机器人基坐标系的变换矩阵

(该变换矩阵是通过直接读取的机器人位姿ζ_i获得的)，即

其中，R(·,E·_i)表示绕着·轴旋转E·_i角的旋转矩阵，由此可得到若干机器人末端坐标系相对于机器人基坐标系的变换矩阵

最后，基于AX＝XB模型，其中，

j＝1,2,3,...,n-1，计算手眼关系矩阵

完成机器人手眼标定。

步骤二，基于坐标系之间的刚体变换矩阵获得多组机器人基坐标系下的标准球阵点云。

具体地，通过刚体变换矩阵乘积运算，将每一幅扫描仪测量坐标系下的点云^SP_i转换至机器人基坐标系下，即

其中，^BP_i表示第i次扫描获取的机器人基坐标系下的标准球阵点云，由公式(2)可得n组机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n}。

步骤三，通过点云匹配得到若干机器人基坐标系相对于标准球阵局部坐标系的变换矩阵集合。

具体地，将机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n} 作为测试模型，将标准球阵的三维实体模型作为参考模型，采用ADF算法进行匹配，依次获取若干机器人基坐标系相对于标准球阵局部坐标系的变换矩阵集合，记为

步骤四，基于匹配结果确定机器人基坐标系相对于标准球阵局部坐标系的变换矩阵，完成目标估计。

具体地，首先，将机器人基坐标系相对于标准球阵局部坐标系的变换矩阵组

转换为六维矢量组

即：

其次，求解六维矢量组

的均值

即

最后，通过矢量-矩阵变换，由六维矢量

求解变换矩阵

即机器人基坐标系相对于标准球阵局部坐标系的变换矩阵，完成标准球阵目标估计。

步骤五，基于尺寸链传递模型确定若干机器人末端法兰坐标系相对于机器人基坐标系的变换矩阵。

具体地，基于机器人基坐标系、标准球阵局部坐标系、扫描仪测量坐标系及机器人末端法兰坐标系之间的相对位姿关系，依次求解机器人末端法兰坐标系相对于机器人基坐标系的变换矩阵

即

变换矩阵中的姿态矩阵为正交矩阵。

步骤六，通过矩阵-矢量变换得到新的机器人位姿，完成机器人位姿快速矫正。

具体地，将变换矩阵

转换为六维矢量

由此可得到若干新的机器人位姿

计算n个机器人位姿的位姿偏差

完成机器人位姿矫正，即

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，该方法包括下列步骤：

S1将扫描仪固连在机器人末端，标准球阵放置在工作台上，建立机器人基坐标系{B}、机器人末端坐标系{E}和扫描仪测量坐标系{S}和标准球阵局部坐标系{W}；

S2机器人带动扫描仪多角度扫描所述标准球阵，读取并记录机器人位姿和在该位姿下的标准球阵点云；基于尺寸链传递模型计算获得机器人末端坐标系{E}相对于机器人基坐标系{B}的变换矩阵

S3通过矢量-矩阵转换关系，将所述机器人末端坐标系相对于机器人基坐标系的变换矩阵

转换为实际六维矢量

该实际六维矢量

即为通过坐标变换计算获得的机器人实际位姿，计算并获得该机器人实际位姿与步骤S2中读取的机器人位姿之间的误差，以此实现机器人位姿的矫正；

在步骤S2中，所述变换矩阵

按照下列方式获得：

S21基于所述标准球阵点云建立标准球阵局部坐标系{W}，计算获得标准球阵局部坐标系{W}相对于扫描仪测量坐标系{S}的变换矩阵

S22对机器人进行手眼标定，以此获得机器人的手眼关系矩阵，即扫描测量坐标系{S}相对于机器人末端坐标系{E}之间的变换矩阵

S23基于刚体变换矩阵和点云匹配，求取机器人基坐标系{B}相对于标准球阵局部坐标系{W}的变换矩阵

S24构建变换矩阵

和

与变换矩阵

之间的关系，利用该关系计算获得所需的变换矩阵

在步骤S23中，所述变换矩阵

按照下列方式获得：

(a)通过刚体变换矩阵乘积运算，将每一幅扫描仪测量坐标系下的点云^SP_i转换至机器人基坐标系下，以此获得在机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n}；

(b)将机器人基坐标系下的标准球阵点云{^BP₁,^BP₂,^BP₃,...,^BP_i,...,^BP_n}作为测试模型，将标准球阵的三维实体模型作为参考模型，采用ADF算法进行匹配，以此获取机器人基坐标系相对于标准球阵局部坐标系的变换矩阵为

(c)将机器人基坐标系相对于标准球阵局部坐标系的变换矩阵

转换为六维矢量组

求解该六维矢量组的均值

通过矢量-矩阵变换，由六维矢量

求解变换矩阵

2.如权利要求1所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，在步骤S21中，所述变换矩阵

按照下列方式计算获得：

(a)根据每个姿态下扫描的标准球阵点云的数据，拟合每个标准球球心的坐标；

(b)选取其中一个标准球的球心作为原点建立标准球阵局部坐标系，该坐标系的原点三维坐标和坐标轴方向形成变换矩阵

3.如权利要求1所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，在步骤S22中，变换矩阵

按照下列步骤获得：

(a)构建机器人位姿与机器人末端坐标系相对于机器人基坐标系的变换矩阵之间的关系式，利用读取的机器人位姿计算获得机器人末端坐标系{E}相对于机器人基坐标系{B}的变换矩阵

(b)构建变换矩阵

和

之间的关系，以此计算获得变换矩阵

4.如权利要求3所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，在步骤(a)中，所述变换矩阵

按照下列关系式计算：

其中，

表示第i次测量时{E}相对于{B}的变换矩阵，R(z,Ez_i)表示绕着z轴旋转Ez_i的旋转矩阵，R(y,Ey_i)、R(x,Ex_i)类推，

是一个三维矢量，表示第i次测量时{E}的原点在{B}下的位置坐标。

5.如权利要求4所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，步骤(a)中，所述将每一幅扫描仪测量坐标系下的点云^SP_i转换至机器人基坐标系下，按照下列关系式进行：

其中，^BP_i是第i次扫描获取的机器人基坐标系下的标准球阵点云，^SP_i是第i次扫描获取的扫描仪测量坐标系下的标准球阵点云，

是第i次扫描机器人末端坐标系相对于机器人基坐标系的变换矩阵。

6.如权利要求1所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，在步骤S24中，所述变换矩阵

阵

和

与变换矩阵

之间的关系按照下列关系式进行：

其中，i是扫描测量次数，n表示扫描测量总次数。

7.如权利要求1所述的一种基于标准球阵目标估计的机器人位姿矫正方法，其特征在于，所述误差按照下列关系是进行计算：

其中，

是第i次扫描在基坐标系中机器人位姿误差，

是第i次扫描在机器人基坐标系中机器人的实际位姿，

是第i次扫描在机器人控制器中读取的机器人位姿，i是扫描次数，n是扫描次数总数。

8.一种应用权利要求1-7任一项所述的机器人位姿矫正方法进行矫正的***，其特征在于，该***包括机器人、扫描仪和标准球阵，所述扫描仪固连在所述机器人末端，所述标准球阵包括多个大小不同且非线性排列的标准球，该标准球阵设置在所述扫描仪的扫描范围内。

Images