CN111716340B - 3d相机与机械手臂坐标系统的校正装置及方法 - Google Patents

Abstract

本发明公开一种3D相机与机械手臂坐标系统的校正装置及方法，利用3D相机摄取三平板校正装置的点云图，根据相邻点云的相同Z值差度与向量夹角，划分及排除点云群，形成三个平面点云群，以最小平方法建构三个平面方程式，并计算三个平面方程式的交点，作为外部参数校正的定位点。

Description

3D相机与机械手臂坐标系统的校正装置及方法

技术领域

本发明涉及一种机器手臂，尤其是涉及利用校正装置，校正机器手臂与3D相机相对坐标系统的方法。

背景技术

随着人工智能的快速发展，工厂利用设置3D(3-Dimensional)相机，使机器手臂自动化进行加工组装制造作业，以提高工厂生产效率。而3D相机的坐标系统与机器手臂的坐标系统正确的相对关系，影响工厂生产的精密性。

如图7所示，为现有技术机器手臂与3D相机的校正。机器手臂1在基座2形成手臂坐标系统R，并在机器手臂1的工作环境中安装3D相机3，形成相机坐标系统C，以进行人工智能的自动化组装制造。相机坐标系统C与手臂坐标系统R进行校正时，需要一个校正装置4让3D相机3进行识别定位。安装3D相机3时，先将校正装置4设置在机器手臂1的工作环境，且让校正装置4出现在3D相机3的视野内。

3D相机3又称为深度(Depth)相机，根据原理不同，可能会同时带有RGB彩色信息或单色灰度信息等2D信息供识别定位校正装置4。假如3D相机3不带有彩色信息，或2D信息不足以进行精准识别定位时，则会以校正装置4形状的3D信息进行校正。校正装置4的形状3D信息为深度图(Depth Map)或点云图(Point Cloud)两种位置信息，深度图与点云图可以通过相机的内部参数进行换算，实质上属于相同的3D信息。3D相机3摄取校正装置4的3D信息，形成校正装置4在相机坐标系统C的空间位置信息。分析校正装置4的形状特征，利用外形上容易识别的边缘角点，作为机器手臂1与3D相机2校正的定位点K1-K5。当已知定位点K1-K5在相机坐标系统C的坐标下，只要将机器手臂1上的工具中心点5接触定位点K1-K5，即可经由机器手臂1移动工具中心点5在手臂坐标系统R的坐标，校正手臂坐标系统R与相机坐标系统C。

然而，前述现有技术进行坐标系统校正时，因为3D相机3的3D信息，通常在中间部位较为精确，并无法得到精准的校正装置4边缘位置信息，且当校正装置4的表面斜率变化较大时，边缘位置信息的精度也会跟着下降，致使位于边缘角点的定位点K1-K5的精度较差，若仅使用3D信息进行校正，并无法获得良好的机器手臂与3D相机间的坐标转换关系。因此，3D相机与机械手臂坐标在校正的方法上，仍有问题亟待解决。

发明内容

本发明的目的提供一种3D相机与机械手臂坐标的校正装置，在校正装置上设置相对位置固定及相互分离不平行的三平板，且三平板延伸的三个平面相交于一点，作为校正定位点，以提升校正的精准度。

本发明的另一目的提供一种3D相机与机械手臂坐标的校正方法，利用3D相机的点云3D信息，根据相邻点云的Z值差度与向量夹角，划分点云群形成三个平面，以避免边缘误差。

本发明的另一目的提供一种3D相机与机械手臂坐标的校正方法，由三个点云群平面，利用最小平方法建构三个平面方程式，计算三个平面方程式的交点，作为校正定位点，以提高定位点的正确性。

为了达到前述发明的目的，本发明的3D相机与机械手臂坐标系统的校正装置，在座架上设置相对位置固定及相互分离不平行的三平板，且让三平板延伸的三个空间平面相交于一点，作为外部参数校正的定位点。

本发明的3D相机与机械手臂坐标系统的校正方法，将三平板的校正装置放置在机器手臂工作环境及3D相机的视野，通过3D相机摄取校正装置的3D信息，利用3D信息点云图或深度图，计算各相邻点云的Z值差距及向量夹角，根据相同Z值差距及向量夹角将点云分群，排除较少量的点云群，形成三个平面点云群，以最小平方法建立三个平面方程式，利用三平面交点公式计算三个平面方程式的交点，将交点作为外部参数校正的定位点。

本发明在建立定位点后，可将校正装置固定在机械手臂上，使校正装置与机械手臂维持相对固定的位置关系，再控制机械手臂驱动校正装置在3D相机视野中移动数点，通过3D相机获得各定位点相对在相机坐标系统中的坐标，以进行坐标系统校正。

本发明3D相机与机械手臂坐标系统的校正方法，利用最小平方法建立的一平面方程式为：z＝Ax+By+C

系数

其中

是一主平面的点云群所有点云坐标x值的平均、

是所有y值的平均、

是所有z值的平均、

是所有z和x值相乘的平均、

是所有z和x值相乘的平均、

是所有x和y值相乘的平均。

本发明3D相机与机械手臂坐标系统的校正方法，改写三平面的平面方程式，为下列三个平面方程式：

a₁x+b₁y+c₁z+d₁＝0

a₂x+b₂y+c₂z+d₂＝0

a₃x+b₃y+c₃z+d₃＝0

再求解三平面交点T，利用三平面交点公式：

其中Det为

且Det≠0

附图说明

图1为本发明机器手臂与3D相机的校正示意图；

图2为本发明校正装置的点云图；

图3为本发明确认点云群成三个平面的示意图；

图4为本发明形成三个平面方程式交点的校正定位点的示意图；

图5为本发明3D相机与机械手臂坐标系统校正方法的流程图；

图6为本发明校正装置固定在机械手臂的校正示意图；

图7为现有技术D3相机与机械手臂坐标系统的校正示意图。

符号说明

10 机器手臂

11 基座

12 轴节

13 端末部

14 工具

15 致动马达

16 控制装置

17 3D相机

20 校正装置

21 座架

22,23,24 平板

25,27,28 主平面

26 边缘平面

具体实施方式

有关本发明为达成上述目的，所采用的技术手段及其功效，现举优选实施例，并配合附图加以说明如下。

请同时参图1、图2、图3及图4，图1为本发明机器手臂与3D相机的校正示意图，图2为本发明校正装置的点云图，图3为本发明确认点云群成三个平面的示意图，图4为本发明形成三个平面方程式交点的校正定位点的示意图。图1中，本发明的机器手臂10一端为固定的基座11，串接多轴节12形成另一端活动的端末部13，端末部13上设工具14，各轴节12设致动马达15，并连线至控制装置16。机器手臂10经由控制装置16控制各轴节12的致动马达15转动角度，移动机器手臂10端末部13的工具14。本发明的机器手臂10利用固定的基座11作为基准点，形成机器手臂10的手臂坐标系统R。并通过机器手臂10已知的各轴节12与端末部13的工具14长度，以及控制各肘节12致动马达15转动的角度，利用控制装置16计算出工具14的移动位置，定位工具14在手臂坐标系统R的坐标，以精确控制移动工具14。

本发明另在机器手臂10的工作环境中安装3D相机17，3D相机17视窗拍摄的空间自成相机坐标系统C，并将3D相机17拍摄的信息，连线至控制装置16进行处理，但是3D相机17的相机坐标系统C，相对于机器手臂10的手臂坐标系统R的位置关系不明，需要进行相机坐标系统C与手臂坐标系统R坐标间的转换校正，才能协调统合3D相机17与机器手臂10的作业。

本发明机器手臂10与3D相机17进行校正时，先将校正装置20设置在机器手臂10的工作环境中，并使校正装置20出现在3D相机17的视野内。本发明的校正装置20，在座架21上设置相对位置固定而相互分离不平行的三平板22,23,24，且让三平板22,23,24延伸的三个空间平面相交于一点，作为外部参数校正的定位点。

由于本发明的校正装置20的三平板22,23,24延伸三个空间平面相交的定位点为虚拟，必须经由计算取得。首先利用3D相机17摄取校正装置20的3D信息，如图2取得校正装置20的点云图，本实施例虽以点云图为例，但包含且不限于点云图，深度图也可适用。为确认校正装置20的三平板22,23,24的点云图位置，本发明针对3D相机17摄取点云图的各点云，在相机坐标系统C中的坐标(X,Y,Z)，计算相邻点云的Z值差距及向量夹角θ，其中Z值差距为相邻点云在相机坐标系统C中的Z轴坐标的差距，向量夹角θ为相邻点云构成的位置向量V与3D相机17水平或垂直轴线的夹角。

以平板22的点云图举例说明，图3中以侧面展示平板22的点云，平板22主平面25上的相邻点云Pn-1,Pn,Pn+1，由于3D相机17设定点云图的分辨率及平板22的主平面25斜率相同，相邻点云的Pn-1与Pn，或Pn与Pn+1在相机坐标系统C中的Z轴坐标的差距△Z0都相同，因此在平板22的同一主平面25上的相邻点云的Z值差距均相同。同理由于3D相机17设定点云图的分辨率及平板22的主平面25斜率相同，平板22主平面25上的相邻点云的Pn-1与Pn，或Pn与Pn+1构成的位置向量V0，方向及大小会相同一致，位置向量V0与3D相机17水平或垂直轴线的夹角θ0也会相同，因此在平板22的同一主平面25上的相邻点云的向量夹角θ均相同。

至于在平板22边缘平面26上的相邻点云Pk-1，Pk，由于与主平面25斜率不相同，在Z值差距△Z1及位置向量V1的向量夹角θ1，与主平面25的Z值差距△Z0及向量夹角θ均不同。因此极易利用相同的Z值差距及向量夹角划分同类的点云群，并如图4所示，排除较少的点云群，加以区分保留主平面25的点云群。同理也可区分保留其他平板23，24的主平面27，28的点云群。如图5所示，为本发明机器手臂校正工具中心点的方法的流程图。本发明校正机器手臂工具中心点的详细步骤说明如下：步骤S1，本发明进行校正时，首先在机器手臂完成安装工具；在步骤S2，机器手臂移动工具至任一姿势，利用机器手臂的力传感器，检测及记录工具重力及工具力矩；步骤S3，使工具的工具中心点进行碰触；接着步骤S4，由力传感器检测力的改变，判断工具中心点受到碰触时，记录活动端的坐标，同时记录碰触力及碰触力矩；步骤S5，由记录的碰触力扣除工具重力计算净碰触力，记录的碰触力矩扣除工具力矩计算净碰触力矩；步骤S6，将净碰触力矩除以净碰触力，计算净力臂；步骤S7，由净力臂及活动端的坐标，计算工具中心点的坐标，校正工具中心点。

接着利用各平板22，23，24的主平面25，27，28的点云群，分别利用现有最小平方法建构各主平面25，27，28的平面方程式。首先设每一平面方程式为:z＝Ax+By+C

根据最小平方法，系数

其中

是一主平面的点云群所有点云坐标x值的平均、

是所有y值的平均、

是所有z值的平均、

是所有z和x值相乘的平均、

是所有z和x值相乘的平均、

是所有x和y值相乘的平均。

将前述获得的三个主平面25，27，28的平面方程式，改写为下列三个平面方程式:

a₁x+b₁y+c₁z+d₁＝0

a₂x+b₂y+c₂z+d₂＝0

a₃x+b₃y+c₃z+d₃＝0

再求解三个主平面25，27，28交点T，利用现有三平面交点公式:

其中Det为

且Det≠0

本发明使用3D相机17中间部位最佳精度的3D信息，计算三个主平面25，27，28的交点T，在相机坐标系统C中的坐标，作为机器手臂10外部参数校正的定位点。在建立定位点后，举例说明使用前述定位点的校正方式，但包含且不限于举例。如图5为本发明校正装置固定在机械手臂的校正示意图。本发明可将校正装置20的座架21固定在机械手臂10的端末部13上，使校正装置20与机械手臂10维持已知相对固定的位置关系，则可确定校正装置20形成的定位点在手臂坐标系统R的坐标，再控制机械手臂10驱动校正装置20在3D相机17视野中移动数点，通过3D相机17利用前述定位点的方法，一一获得各定位点相对在相机坐标系统C中的坐标，即可达到3D相机17与机器手臂10的坐标系统转换的校正。

如图6所示，为本发明3D相机与机械手臂坐标系统校正方法的流程图。本发明3D相机与机械手臂坐标系统进行校正的详细步骤如下:在步骤M1，将三平板的校正装置放置在机器手臂工作环境及3D相机视野；步骤M2，利用3D相机摄取校正装置的3D信息；步骤M3，利用3D信息中的点云图，计算各相邻点云的Z值差距及向量夹角；步骤M4，根据相同Z值差距及向量夹角将点云分群；步骤M5，排除较少量的点云群，形成三个主平面点云群；步骤M6，利用最小平方法建立三个平面方程式；步骤M7，利用三平面交点公式计算三个平面方程式的交点；步骤M8，将交点作为外部参数校正的定位点。

因此本发明3D相机与机械手臂坐标的校正装置及方法，在校正装置上相对位置固定及相互分离不平行的三平板，且三平板延伸的三个平面相交于一点，接着利用3D相机摄取的点云3D信息，根据相邻点云的相同Z值差度与向量夹角，划分及排除点云群，形成三个主平面点云群，再由三个主平面的点云群，利用最小平方法建构三个平面方程式，并利用公式计算三个平面方程式的交点，作为校正定位点。由于本发明利用3D相机最佳精度的3D信息，直接计算定位点，可避免仪器测量的误差及成本，也避免选定边缘形状特征的变形误差，达到提高定位点的正确性的发明目的。

以上所述的仅为用以方便说明本发明的优选实施例，本发明的范围不限于该等优选实施例，凡依本发明所做的任何变更，在不脱离本发明的精神下，都属本发明申请专利的范围。

Claims (8)

1.一种3D相机与机械手臂坐标系统的校正装置，其特征在于，在座架上设置相对位置固定及相互分离不平行的三平板，且让该三平板延伸的三个空间平面相交于一点，作为外部参数校正的定位点，其中所述校正装置配置为通过3D相机摄取校正装置的3D信息，其中利用3D相机设定点云图的分辨率及该三平板的同一平面斜率相同，同一平板的同一平面上相邻点云在相机坐标系统中的Z轴坐标的差距都相同，形成相邻点云的Z值差距及向量夹角均相同。

2.一种3D相机与机械手臂坐标系统的校正方法，包含:

将三平板的校正装置放置在机器手臂工作环境及3D相机的视野；

通过3D相机摄取校正装置的3D信息；

利用3D信息点云图，计算各相邻点云的Z值差距及向量夹角；

根据相同Z值差距及向量夹角将点云分群；

排除较少量的点云群，形成三个平面点云群；

以最小平方法建立三个平面方程式；

利用三平面交点公式计算三个平面方程式的交点；

将交点作为外部参数校正的定位点；

其中利用3D相机设定点云图的分辨率及平板的同一平面斜率相同，同一平板的同一平面上相邻点云在相机坐标系统中的Z轴坐标的差距都相同，形成相邻点云的Z值差距及向量夹角均相同。

3.如权利要求2所述的3D相机与机械手臂坐标系统的校正方法，其中利用3D信息的深度图，通过3D相机的内部参数进行换算点云图。

4.如权利要求2所述的3D相机与机械手臂坐标系统的校正方法，其中该Z值差距为相邻点云在相机坐标系统C中的Z轴坐标的差距。

5.如权利要求4所述的3D相机与机械手臂坐标系统的校正方法，其中该向量夹角为相邻点云构成的位置向量与3D相机水平或垂直轴线的夹角。

6.如权利要求2所述的3D相机与机械手臂坐标系统的校正方法，其中该最小平方法建立的一平面方程式为:z＝Ax+By+C

系数

其中

是一主平面的点云群所有点云坐标x值的平均、

是所有y值的平均、

是所有z值的平均、

是所有z和x值相乘的平均、

是所有z和y 值相乘的平均、

是所有x和y值相乘的平均。

7.如权利要求2所述的3D相机与机械手臂坐标系统的校正方法，其中改写三平面的平面方程式，为下列三个平面方程式：

a₁x+b₁y+c₁Z+d₁＝0

a₂x+b₂y+c₂Z+d₂＝0

a₃x+b₃y+c₃Z+d₃＝0

再求解三平面交点T，利用三平面交点公式：

其中Det为

且Det≠0。

8.如权利要求2所述的3D相机与机械手臂坐标系统的校正方法，其中该校正装置固定在机械手臂上，使校正装置与机械手臂维持相对固定的位置关系，再控制机械手臂驱动校正装置在3D相机视野中移动数点，通过3D相机获得各定位点相对在相机坐标系统中的坐标，以进行3D相机与机器手臂坐标系统校正。

Images