CN114474058A - 视觉引导的工业机器人系统标定方法 - Google Patents

Abstract

本发明属于工业机器人技术领域，具体涉及一种视觉引导的工业机器人系统标定方法，旨在解决视觉引导的工业机器人系统标定精度较低的问题。本方法包括求出工业机器人基座坐标系与相机坐标系的相对位姿初值；利用四点标定法获取末端执行器坐标系与工业机器人法兰盘坐标系的相对位姿初值；并通过预获取的D‑H参数计算工业机器人关节间固连关系的相对位姿矩阵；控制工业机器人实现N_m种不同姿态，记录各姿态下的各关节角数据，获取N_m种姿态下工业机器人末端执行器中心点在相机坐标系下的坐标；求解误差校正矩阵，实现对视觉引导的工业机器人系统的标定。本发明解决了视觉引导的工业机器人系统标定精度较低的问题，提升了标定精度。

Description

视觉引导的工业机器人系统标定方法

技术领域

本发明属于工业机器人技术领域，具体涉及一种视觉引导的工业机器人系统标定方法。

背景技术

工业机器人作为工业自动化的重要设备，在提高生产效率、解放劳动力等方面发挥重要作用，并且已广泛应用于焊接、磨削、抓取等工业制造中。为保证工业机器人在复杂场景下进行精密作业的质量，基于视觉引导进行作业是一种有效的解决方案，这就需要对工业机器人和视觉单元组成的整个系统(即视觉引导的工业机器人系统)进行精确标定。

视觉引导的工业机器人系统在作业时，其误差的来源往往是多元的。工业机器人在加工、装配中难免会产生装配误差，具体表现为实际和理想运动参数之间的差异，这部分误差一般使用D-H参数误差模型或旋量误差模型来标定。同时，工业机器人与相机之间存在相对位姿误差，对工业机器人与相机的相对位姿误差的标定称为手眼标定。此外，对于末端执行器来说，还存在末端执行器安装误差，对其标定一般使用四点标定法、七点标定法等方法进行处理。在实际应用中，研究人员经常借助OpenCV库来进行标定，例如，OpenCV库中的findChessboardCorners函数用来检测标定板的角点，solvePnP函数可以求解标定板与相机间的位姿关系，手眼标定可以由calibrateHandEye函数完成；OpenCV库中的StereoSGBM类通过立体匹配获得末端执行器的点云，ppf_match_3d::ICP类用来实现点云匹配。

需要说明的是，只考虑单一误差的标定方法因为忽略了其它误差，无法保证标定的质量。为提高标定质量，研究人员关注对不同误差进行联合标定。现有的联合标定方法主要考虑装配误差和工业机器人与相机的相对位姿误差之间的联合标定，但是这类方法假定末端执行器是理想的，忽略了末端执行器的安装误差，这带来了标定精度的损失。末端执行器安装误差、装配误差和工业机器人与相机的相对位姿误差是耦合的，这种耦合性使得要想获得高精度的标定结果就需要对这三种误差联合标定。为此，需要对末端执行器安装误差、装配误差和工业机器人与相机的相对位姿误差的联合标定方法进行更深入的研究，以解决视觉引导的工业机器人系统标定精度较低的问题。

因此，如何提出一种解决上述问题的方案是本领域技术人员目前需要解决的问题。

发明内容

为了解决现有技术中的上述问题，即为了解决视觉引导的工业机器人系统标定精度较低的问题，本发明提出了一种视觉引导的工业机器人系统标定方法，包括以下步骤：

步骤S100，控制工业机器人使固定于其法兰盘上的棋盘格标定板在双目相机的左相机的视野内运动，并采集棋盘格标定板的图像、工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，进而求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0；

步骤S200，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0；并通过预获取的D-H参数计算工业机器人关节间固连关系的相对位姿矩阵^i-1T_i，i＝1，2，...，n，n为工业机器人的自由度；

步骤S300，控制工业机器人实现N_m种不同姿态，记录各姿态下的各关节角数据θ_ij，j＝1，2，...，N_m，并通过双目相机的左相机和右相机分别采集各姿态下工业机器人末端执行器的图像，获取N_m对图像；对N_m对图像中的每对图像进行处理，得到每对图像对应的末端执行器的点云；对第一对图像处理后的末端执行器的点云，指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁，进而通过点云匹配，获取其它N_m-1种姿态下工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，...，N_m；

步骤S400，结合^CT_B0、^i-1T_i、^ET_K0、^CP_j和θ_ij，求解误差校正矩阵E_BC、E_KE和E_i，实现对视觉引导的工业机器人系统的标定；

其中，所述工业机器人基座坐标系O_BX_BY_BZ_B和工业机器人法兰盘坐标系O_EX_EY_EZ_E均通过厂商提供的工业机器人使用手册加以获得；所述相机坐标系O_CX_CY_CZ_C以双目相机的左相机的光心作为原点O_C，以左相机图像的横轴方向和纵轴方向分别作为X_C轴方向和Y_C轴方向，以左相机的光轴作为Z_C轴；所述末端执行器坐标系O_KX_KY_KZ_K，其坐标系原点为末端执行器中心点O_K，以末端执行器的轴向方向作为Z_K轴方向，任选经过O_K且垂直于Z_K轴的方向作为X_K轴方向，Y_K轴方向由右手定则确定。

在一些优选的实施方式中，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，其方法为：

通过OpenCV库的findChessboardCorners函数对采集的棋盘格标定板的图像进行处理，得到每张图像中棋盘格标定板的各个角点的图像坐标p_k(s，t)，k＝1，2，...，m，s＝0，1，...，r-1，t＝0，1，...，c-1，m为采集的棋盘格标定板的图像的数量；r、c分别为棋盘格标定板的行、列角点数；

基于坐标p_k(s，t)，结合棋盘格标定板的各个角点的世界坐标，利用OpenCV库中的solvePnP函数求解出每张图像中棋盘格标定板到相机坐标系O_CX_CY_CZ_C的相对位姿^CT_Ok。

在一些优选的实施方式中，求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0，其方法为：

基于采集的棋盘格标定板的图像对应的工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，结合每张棋盘格标定板的图像中棋盘格标定板到相机坐标系O_CX_CY_CZ_C的相对位姿^CT_Ok，通过OpenCV库中的calibrateHandEye函数，得到工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0。

在一些优选的实施方式中，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0，其方法为：

在工业机器人的工作空间中设置一个固定位置，控制工业机器人以四种不同的姿态使其末端执行器中心点到达该固定位置，并分别记录工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E1、^BR_E2、^BR_E3、^BR_E4以及位移向量^Bt_E1、^Bt_E2、^Bt_E3、^Bt_E4；

基于旋转矩阵^BR_E1、^BR_E2、^BR_E3、^BR_E4以及位移向量^Bt_E1、^Bt_E2、^Bt_E3、^Bt_E4，计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的位移向量^Et_K；

控制工业机器人使得末端执行器坐标系O_KX_KY_KZ_K与工业机器人基座坐标系O_BX_BY_BZ_B对应各坐标轴的方向保持一致，记录此时工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E，进而计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的旋转矩阵^ER_K：^ER_K＝(^BR_E)^-1；

基于旋转矩阵^ER_K、位移向量^Et_K，得到末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0：

在一些优选的实施方式中，计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的位移向量^Et_K，其方法为：

在一些优选的实施方式中，计算工业机器人关节间固连关系的相对位姿矩阵^{i- 1}T_i，其方法为：

其中，i＝1，2，...，n，a_i、α_i、d_i为工业机器人的D-H参数值。

在一些优选的实施方式中，工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，其获取方法为：

对于N_m对图像，通过OpenCV库中的StereoSGBM类分别进行处理，获得每对图像对应的末端执行器的点云D_j，j＝1，2，...，N_m；

在第一个点云D₁中指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁；

通过OpenCV库中的ppf_match_3d::ICP类进行点云匹配得到D_j与D₁之间的位姿变换关系

根据公式

求出点云D_j中末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，...，N_m。

在一些优选的实施方式中，所述误差校正矩阵E_BC，其求解方法为：

其中，E_BC0是上一次迭代时E_BC的值，第一次迭代时E_BC0＝I_4×4，I_4×4为4×4的单位矩阵，

R_BC和t_BC表示E_BC0的旋转矩阵和平移分量，U_BCj＝^CT_B0，

E_KE0是上一次迭代时E_KE的值，第一次迭代时E_KE0＝I_4×4，E_i0是上一次迭代时E_i的值，第一次迭代时E_i0＝I_4×4，i＝1，2，...，n，^KP_j＝^KP，^KP＝[0，0，0，1]^T是末端执行器中心点在末端执行器坐标系下的坐标，Rot(θ_ij)是以θ_ij为自变量的旋转矩阵，符号∧表示反对称符号。

在一些优选的实施方式中，所述误差校正矩阵E_KE，其求解方法为：

其中，

R_KE和t_KE表示E_KE0的旋转矩阵和平移分量，

V_KEj＝^KP_j。

在一些优选的实施方式中，所述误差校正矩阵E_i，其求解方法为：

其中，δξ_i^的求解分为以下三种情况：

第一种情况：当i＝1时，

R₁和t₁表示E₁₀的旋转矩阵和平移分量，U_1j＝^CT_B0E_BC0 ⁰T₁，

第二种情况：当i＝p，1<p<n时，

R_i和t_i表示E_i0的旋转矩阵和平移分量，

第三种情况：当i＝n时，

R_n和t_n表示E_n0的旋转矩阵和平移分量，

V_nj＝Rot(θ_nj)^ET_K0E_KE0 ^KP_j。

本发明的有益效果：

本发明通过对末端执行器安装误差、装配误差和工业机器人与相机的相对位姿误差的联合标定，解决了视觉引导的工业机器人系统标定精度较低的问题，提升了标定精度，为视觉引导下的工业机器人作业提供技术支持。

附图说明

图1是本发明一种实施例的视觉引导的工业机器人系统标定方法的流程示意图。

具体实施方式

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

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明的一种视觉引导的工业机器人系统标定方法，包括以下步骤：

步骤S100，控制工业机器人使固定于其法兰盘上的棋盘格标定板在双目相机的左相机的视野内运动，并采集棋盘格标定板的图像、工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，进而求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0；

步骤S200，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0；并通过预获取的D-H参数计算工业机器人关节间固连关系的相对位姿矩阵^i-1T_i，i＝1，2，...，n，n为工业机器人的自由度；

步骤S300，控制工业机器人实现N_m种不同姿态，记录各姿态下的各关节角数据θ_ij，j＝1，2，...，N_m，并通过双目相机的左相机和右相机分别采集各姿态下工业机器人末端执行器的图像，获取N_m对图像；对N_m对图像中的每对图像进行处理，得到每对图像对应的末端执行器的点云；对第一对图像处理后的末端执行器的点云，指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁，进而通过点云匹配，获取其它N_m-1种姿态下工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，...，N_m；

步骤S400，结合^CT_B0、^i-1T_i、^ET_K0、^CP_j和θ_ij，求解误差校正矩阵E_BC、E_KE和E_i，实现对视觉引导的工业机器人系统的标定；

其中，所述工业机器人基座坐标系O_BX_BY_BZ_B和工业机器人法兰盘坐标系O_EX_EY_EZ_E均通过厂商提供的工业机器人使用手册加以获得；所述相机坐标系O_CX_CY_CZ_C以双目相机的左相机的光心作为原点O_C，以左相机图像的横轴方向和纵轴方向分别作为X_C轴方向和Y_C轴方向，以左相机的光轴作为Z_C轴；所述末端执行器坐标系O_KX_KY_KZ_K，其坐标系原点为末端执行器中心点O_K，以末端执行器的轴向方向作为Z_K轴方向，任选经过O_K且垂直于Z_K轴的方向作为X_K轴方向，Y_K轴方向由右手定则确定。

为了更清晰地对本发明视觉引导的工业机器人系统标定方法进行说明，下面对本发明方法一种实施例中各步骤进行展开详述。

该实施例为一种较优的实现方式，通过在式(1)中同时考虑末端执行器安装误差、装配误差和工业机器人与相机的相对位姿误差，结合末端执行器中心点在相机坐标系下的观测位置与理想位置，进行视觉引导的工业机器人系统的标定，末端执行器中心点在相机坐标系下的理想位置^CP计算如下：

其中^CT_B0是工业机器人基座坐标系O_bX_bY_bZ_b与相机坐标系O_cX_cY_cZ_c的相对位姿初值，^i-1T_i是工业机器人关节间固连关系的相对位姿矩阵，^ET_K0是末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值，^KP＝[0，0，0，1]^T是末端执行器中心点在末端执行器坐标系下的坐标，n为工业机器人的自由度，θ_i是工业机器人的关节角，i＝1，2，...，n，Rot(θ)表示以θ为自变量的旋转矩阵，

E_BC、E_KE和E_i(i＝1，2，...，n)是误差校正矩阵；所述工业机器人基座坐标系O_BX_BY_BZ_B和工业机器人法兰盘坐标系O_EX_EY_EZ_E均通过厂商提供的工业机器人使用手册加以获得；所述相机坐标系O_CX_CY_CZ_C以双目相机的左相机的光心作为原点O_C，以左相机图像的横轴方向和纵轴方向分别作为X_C轴方向和Y_C轴方向，以左相机的光轴作为Z_C轴；所述末端执行器坐标系O_KX_KY_KZ_K，其坐标系原点为末端执行器中心点O_K，以末端执行器的轴向方向作为Z_K轴方向，任选经过O_K且垂直于Z_K轴的方向作为X_K轴方向，Y_K轴方向由右手定则确定。视觉引导的工业机器人系统的标定的目的就是求解误差校正矩阵E_BC、E_KE和E_i(i＝1，2，...，n)。如图1所示，具体标定过程如下：

步骤S100，控制工业机器人使固定于其法兰盘上的棋盘格标定板在双目相机的左相机的视野内运动，并采集棋盘格标定板的图像、工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，进而求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0。

在本实施例中，将具有r行c列个角点的棋盘格标定板固定于工业机器人的法兰盘上，其中水平相邻角点的间隔都为d，竖直相邻角点的间隔也都为d，r、c和d为预设常数，本发明中优选设置分别为9、9和7mm。控制工业机器人使棋盘格标定板在双目相机的左相机的视野内运动m次，采集棋盘格标定板的图像m张，并同时记录下对应的工业机器人法兰盘坐标系O_EX_EY_EZ_E在工业机器人基座坐标系O_BX_BY_BZ_B下的位姿m个：^BT_E1，^BT_E2，...，^BT_Em，m为预设值，本发明中优选设置为20。

通过OpenCV库的findChessboardCorners函数对这m张图像进行处理，得到m张图像中所有棋盘格标定板的各个角点的图像坐标p_1(s，t)，p_2(s，t)，...，p_m(s，t)，其中s＝0，1，...，r-1，t＝0，1，...，c-1；另外，m张图像中棋盘格标定板的各个角点的世界坐标计算如下：w_(s，t)＝[t·d s·d 0]^T，s＝0，1，…，r-1，t＝0，1，…，c-1；基于p_k(s，t)(k＝1，2，...，m)和w_(s，t)(s＝0，1，...，r-1，t＝0，1，...，c-1)，利用OpenCV库中的solvePnP函数求解出每张图像中棋盘格标定板到相机坐标系O_CX_CY_CZ_C的相对位姿^CT_Ok(k＝1，2，...，m)。

基于^BT_E1，^BT_E2，...，^BT_Em和^CT_O1，^CT_O2，...，^CT_Om，通过OpenCV库中的calibrateHandEye函数即可求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0。

步骤S200，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0；并通过预获取的D-H参数计算工业机器人关节间固连关系的相对位姿矩阵^i-1T_i，i＝1，2，...，n，n为工业机器人的自由度。

在本实施例中，在工业机器人的工作空间中设置一个固定位置，控制工业机器人以四种不同的姿态使其末端执行器中心点到达该固定位置，分别记录工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E1、^BR_E2、^BR_E3、^BR_E4以及位移向量^Bt_E1、^Bt_E2、^Bt_E3、^Bt_E4。计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的位移向量^Et_K，如公式(2)所示：

控制工业机器人使得末端执行器坐标系O_KX_KY_KZ_K与工业机器人基座坐标系O_BX_BY_BZ_B的X_K轴方向与X_B轴方向保持一致、Y_K轴方向与Y_B轴方向保持一致、Z_K轴方向与Z_B轴方向保持一致，记录此时工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E，计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的旋转矩阵^ER_K如下：^ER_K＝(^BR_E)^-1。将^ER_K和^Et_K组合起来得到末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值

工业机器人关节间固连关系的相对位姿矩阵计算如下：

本实施例中n优选设置为6，其中a_i、α_i、d_i为工业机器人的D-H参数值，由厂家提供的D-H参数得到。

步骤S300，控制工业机器人实现N_m种不同姿态，记录各姿态下的各关节角数据θ_ij，i＝1，2，...，n，j＝1，2，...，N_m，并通过双目相机的左相机和右相机分别采集各姿态下工业机器人末端执行器的图像，获取N_m对图像；对N_m对图像中的每对图像进行处理，得到每对图像对应的末端执行器的点云；对第一对图像处理后的末端执行器的点云，指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁，进而通过点云匹配，获取其它N_m-1种姿态下工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，…，N_m。

在本实施例中，以工业机器人末端执行器在双目相机的左相机和右相机公共视野范围内为前提，控制工业机器人实现N_m种不同姿态，对于每种姿态，记录各关节角数据θ_ij(i＝1，2，…，n，j＝1，2，...，N_m)，并通过双目相机的左相机和右相机分别采集末端执行器的图像构成一对图像，从而N_m种不同姿态共获得N_m对图像；N_m为预设值，本发明中优选为100。

对于N_m对图像，通过OpenCV库中的StereoSGBM类分别进行处理，获得每对图像对应的末端执行器的点云D_j(j＝1，2，...，N_m)。人为在第一个点云D₁中选择末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁，通过OpenCV库中的ppf_match_3d::ICP类进行点云匹配得到D_j(j＝2，3，...，N_m)与D₁之间的位姿变换关系

然后根据公式

求出点云D_j(j＝2，3，…，N_m)中末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j(j＝2，3，…，N_m)。

步骤S400，结合^CT_B0、^i-1T_i、^ET_K0、^CP_j和θ_ij，求解误差校正矩阵E_BC、E_KE和E_i，i＝1，2，...，n，j＝1，2，...，N_m，实现对视觉引导的工业机器人系统的标定。

在本实施例中，结合^CT_B0、^i-1T_i(i＝1，2，...，n)、^ET_K0、^CP_j(j＝1，2，...，N_m)和θ_ij(i＝1，2，...，n，j＝1，2，...，N_m)，使用公式(3)对E_BC、E_KE和E_i(i＝1，2，...，n)进行迭代更新：

其中，

E_BC0是上一次迭代时E_BC的值，第一次迭代时E_BC0＝I_4×4，R_BC和t_BC表示E_BC0的旋转矩阵和平移分量，U_BCj＝^CT_B0，

E_i0是上一次迭代时E_i的值(i＝1，2，...，n)，第一次迭代时E_i0＝I_4×4(i＝1，2，...，n)，E_KE0是上一次迭代时E_KE的值，第一次迭代时E_KE0＝I_4×4，I_4×4为4×4的单位矩阵，^KP_j＝^KP，符号∧表示反对称符号，具体可参考文献：高翔,张涛,刘毅,颜沁睿.视觉SLAM十四讲:从理论到实践.电子工业出版社,2017中的第3讲三维空间刚体运动和第4讲李群与李代数；

R_KE和t_KE表示E_KE0的旋转矩阵和平移分量，

V_KEj＝^KP_j；

δξ_i^(i＝1，2，...，n)的求解分为以下三种情况：

第一种情况：当i＝1时，

R₁和t₁表示E₁₀的旋转矩阵和平移分量，

第二种情况：当i＝p(1<p<n)时，

R_i和t_i表示E_i0的旋转矩阵和平移分量，

第三种情况：当i＝n时，

R_n和t_n表示E_n0的旋转矩阵和平移分量，

V_nj＝Rot(θ_nj)^ET_K0E_KE0 ^KP_j。

当利用公式(3)对E_BC、E_KE和E_i(i＝1，2，...，n)进行迭代时，如果迭代次数超过N_max或迭代过程中满足

停止迭代，其中N_max是预设最大迭代次数，|·|为向量的模长算子，本实施例优选为100，ε是预设阈值，本实施例优选为0.001。迭代结束后的E_BC、E_KE和E_i(i＝1，2，...，n)即为所求。

采用本发明一种视觉引导的工业机器人系统标定方法，工业机器人获得误差校正矩阵E_BC、E_KE和E_i(i＝1，2，...，n)，从而实现末端执行器安装误差、装配误差和工业机器人与相机的相对位姿误差的联合标定。本发明解决了视觉引导的工业机器人系统标定精度较低的问题，提升了标定精度。在此基础上，视觉引导的工业机器人系统可以获得场景中目标物体的准确空间位置或者对检测到的目标物体进行稠密的三维重建，进而工业机器人可以通过路径规划无碰的实现对目标物体的作业任务，其中，检测目标物体、稠密的三维重建、路径规划等为现有技术，此处不再一一阐述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”、“第三”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1.一种视觉引导的工业机器人系统标定方法，其特征在于，该方法包括以下步骤：

步骤S100，控制工业机器人使固定于其法兰盘上的棋盘格标定板在双目相机的左相机的视野内运动，并采集棋盘格标定板的图像、工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，进而求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值CT_B0；

步骤S200，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0；并通过预获取的D-H参数计算工业机器人关节间固连关系的相对位姿矩阵^i-1T_i，i＝1，2，...，n，n为工业机器人的自由度；

步骤S300，控制工业机器人实现N_m种不同姿态，记录各姿态下的各关节角数据θ_ij，j＝1，2，...，N_m，并通过双目相机的左相机和右相机分别采集各姿态下工业机器人末端执行器的图像，获取N_m对图像；对N_m对图像中的每对图像进行处理，得到每对图像对应的末端执行器的点云；对第一对图像处理后的末端执行器的点云，指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁，进而通过点云匹配，获取其它N_m-1种姿态下工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，...，N_m；

步骤S400，结合^CT_B0、^i-1T_i、^ET_K0、^CP_j和θ_ij，求解误差校正矩阵E_BC、E_KE和E_i，实现对视觉引导的工业机器人系统的标定；

其中，所述工业机器人基座坐标系O_BX_BY_BZ_B和工业机器人法兰盘坐标系O_EX_EY_EZ_E均通过厂商提供的工业机器人使用手册加以获得；所述相机坐标系O_CX_CY_CZ_C以双目相机的左相机的光心作为原点O_C，以左相机图像的横轴方向和纵轴方向分别作为X_C轴方向和Y_C轴方向，以左相机的光轴作为Z_C轴；所述末端执行器坐标系O_KX_KY_KZ_K，其坐标系原点为末端执行器中心点O_K，以末端执行器的轴向方向作为Z_K轴方向，任选经过O_K且垂直于Z_K轴的方向作为X_K轴方向，Y_K轴方向由右手定则确定。

2.根据权利要求1所述的视觉引导的工业机器人系统标定方法，其特征在于，求解出每张图像中棋盘格标定板到相机坐标系的相对位姿，其方法为：

通过OpenCV库的findChessboardCorners函数对采集的棋盘格标定板的图像进行处理，得到每张图像中棋盘格标定板的各个角点的图像坐标p_k(s，t)，k＝1，2，...，m，s＝0，1，...，r-1，t＝0，1，...，c-1，m为采集的棋盘格标定板的图像的数量；r、c分别为棋盘格标定板的行、列角点数；

基于坐标p_k(s，t)，结合棋盘格标定板的各个角点的世界坐标，利用OpenCV库中的solvePnP函数求解出每张图像中棋盘格标定板到相机坐标系O_CX_CY_CZ_C的相对位姿^CT_Ok。

3.根据权利要求1所述的视觉引导的工业机器人系统标定方法，其特征在于，求出工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0，其方法为：

基于采集的棋盘格标定板的图像对应的工业机器人法兰盘坐标系在工业机器人基座坐标系下的位姿，结合每张棋盘格标定板的图像中棋盘格标定板到相机坐标系O_CX_CY_CZ_C的相对位姿^CT_Ok，通过OpenCV库中的calibrateHandEye函数，得到工业机器人基座坐标系O_BX_BY_BZ_B与相机坐标系O_CX_CY_CZ_C的相对位姿初值^CT_B0。

4.根据权利要求1所述的视觉引导的工业机器人系统标定方法，其特征在于，利用四点标定法获取末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0，其方法为：

在工业机器人的工作空间中设置一个固定位置，控制工业机器人以四种不同的姿态使其末端执行器中心点到达该固定位置，并分别记录工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E1、^BR_E2、^BR_E3、^BR_E4以及位移向量^Bt_E1、^Bt_E2、^Bt_E3、^Bt_E4；

基于旋转矩阵^BR_E1、^BR_E2、^BR_E3、^BR_E4以及位移向量^Bt_E1、^Bt_E2、^Bt_E3、^Bt_E4，计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的位移向量^Et_K；

控制工业机器人使得末端执行器坐标系O_KX_KY_KZ_K与工业机器人基座坐标系O_BX_BY_BZ_B对应各坐标轴的方向保持一致，记录此时工业机器人法兰盘坐标系O_EX_EY_EZ_E与工业机器人基座坐标系O_BX_BY_BZ_B的旋转矩阵^BR_E，进而计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的旋转矩阵^ER_K：^ER_K＝(^BR_E)^-1；

基于旋转矩阵^ER_K、位移向量^Et_K，得到末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的相对位姿初值^ET_K0：

5.根据权利要求4所述的视觉引导的工业机器人系统标定方法，其特征在于，计算末端执行器坐标系O_KX_KY_KZ_K与工业机器人法兰盘坐标系O_EX_EY_EZ_E的位移向量^Et_K，其方法为：

6.根据权利要求1中所述的视觉引导的工业机器人系统标定方法，其特征在于，计算工业机器人关节间固连关系的相对位姿矩阵^i-1T_i，其方法为：

其中，i＝1，2，...，n，a_i、α_i、d_i为工业机器人的D-H参数值。

7.根据权利要求1中所述的视觉引导的工业机器人系统标定方法，其特征在于，工业机器人末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，其获取方法为：

对于N_m对图像，通过OpenCV库中的StereoSGBM类分别进行处理，获得每对图像对应的末端执行器的点云D_j，j＝1，2，...，N_m；

在第一个点云D₁中指定末端执行器中心点，将其在相机坐标系O_CX_CY_CZ_C下的坐标作为^CP₁；

通过OpenCV库中的ppf_match_3d：：ICP类进行点云匹配得到D_j与D₁之间的位姿变换关系

根据公式

求出点云D_j中末端执行器中心点在相机坐标系O_CX_CY_CZ_C下的坐标^CP_j，j＝2，3，...，N_m。

8.根据权利要求1中所述的视觉引导的工业机器人系统标定方法，其特征在于，所述误差校正矩阵E_BC，其求解方法为：

其中，E_BC0是上一次迭代时E_BC的值，第一次迭代时E_BC0＝I_4×4，I_4×4为4×4的单位矩阵，

R_BC和t_BC表示E_BC0的旋转矩阵和平移分量，U_BCj＝^CT_B0，

E_KE0是上一次迭代时E_KE的值，第一次迭代时E_KE0＝I_4×4，E_i0是上一次迭代时E_i的值，第一次迭代时E_i0＝I_4×4，i＝1，2，...，n，^KP_j＝^KP，^KP＝[0，0，0，1]^T是末端执行器中心点在末端执行器坐标系下的坐标，Rot(θ_ij)是以θ_ij为自变量的旋转矩阵，符号∧表示反对称符号。

9.根据权利要求8中所述的视觉引导的工业机器人系统标定方法，其特征在于，所述误差校正矩阵E_KE，其求解方法为：

其中，

R_KE和t_KE表示E_KE0的旋转矩阵和平移分量，

V_KEj＝^KP_j。

10.根据权利要求9中所述的视觉引导的工业机器人系统标定方法，其特征在于，所述误差校正矩阵E_i，其求解方法为：

其中，δξ_i^的求解分为以下三种情况：

第一种情况：当i＝1时，

R₁和t₁表示E₁₀的旋转矩阵和平移分量，U_1j＝^CT_B0 ^E _BC0 ⁰T₁，

第二种情况：当i＝p，1＜p＜n时，

R_i和t_i表示E_i0的旋转矩阵和平移分量，

第三种情况：当i＝n时，

R_n和t_n表示E_n0的旋转矩阵和平移分量，

Images