CN112792816B - 基于几何的手眼标定方法、机器人、计算机及存储介质 - Google Patents

Abstract

本发明提供一种涉及基于几何的手眼标定方法、机器人、计算机以及存储介质，其以Tsai的方法为基础，一次性可以标定出手眼标定问题的手眼矩阵的旋转矩阵R_x、平移矢量t_x、机械臂末端到靶标的旋转矩阵S_t和平移矢量p_t四个矩阵。其中，求解手眼矩阵的旋转矩阵R_x的方法是根据矢量几何图像的启发而完成的，其中依据手眼矩阵的旋转矢量K_x垂直于所有A矩阵的旋转矢量K_Ai和B矩阵的旋转矢量K_Bi矢量对的差矢量K_Ci。并且采用了基于最优化目标函数的方法，采用拉格朗日乘数法完成求解。还提出一种全新的方法来求取平移矢量t_x。

Description

基于几何的手眼标定方法、机器人、计算机及存储介质

技术领域

本发明涉及一种新颖的基于几何的手眼标定方法，能以Tsai的方法为基础，一次性标定出手眼矩阵的旋转矩阵R_x和平移矢量t_x、机械臂末端到靶标的旋转矩阵S_t和平移矢量p_t四个矩阵。还涉及执行该方法的机器人、计算机及计算机可读存储介质.

背景技术

在实际应用中，通常需要将相机中看到的靶标(marker)的位姿转换到机械臂坐标系下，进而辅助机械臂规划一些后续动作，因此，为了得到两个坐标系之间的转换矩阵，提出了机械臂的手眼标定方法。

通常机械臂的手眼关系可以分为“眼在手上”(eye-in-hand)和“眼在手外”(eye-to-hand)两种。

眼在手上是指：相机安装在机械臂的末端，通过移动机械臂，让相机观察外部的靶标，得到变换矩阵S1、S2。然后根据机械臂自己的正运动学矩阵R1、R2求出手眼矩阵X。最终可以得到手眼标定方程：R2^-1·R1·X＝X·S2^-1·S1。可以简化为AX＝XB。

同理，眼在手外是指：相机安装在机械臂外部，标定前在机械臂末端安装靶标，然后驱动机械臂运动，得到正运动学矩阵R1、R2，而相机拍摄不同位置的机械臂的靶标来得到变换矩阵S1、S2。然后据此求出手眼标定方程：R2·R1^-1·X＝X·S2·S1^-1。也可以简化为AX＝XB。

Tsai-Lenz方法是手眼标定中最常用的方法，它利用将手眼标定方程分解为旋转部分和平移部分，然后用罗德里格斯方程将旋转部分化简为矢量的矩阵下的旋转，通过几何关系求解X的旋转部分R，再然后代回到方程的平移部分求解平移矢量。Park,Frank C.利用李群理论将手眼标定方程化为最小二乘问题加以求解。Daniilidis采用的是对偶四元数的知识，用对偶四元数表达旋转和平移，从而进行统一计算。

以上三种方法对于眼在手上都具有相同的精度。但是计算方法偏于复杂。

对于眼在手外的手眼标定情况，在传统工业生产中，通常采用一种简明清晰的所谓“九点标定法”。即机械臂在相机视野范围内运动末端到九个点，然后根据正运动学得到的九个末端点和相机得到的九个点代入到转换方程中，将转换矩阵的参数变成待求解的未知矢量x，得到Ax＝b的一个简单矩阵方程，然后用最小二乘解求出x继而得到手眼标定矩阵X。

发明内容

根据本发明的一方面，提供一种基于几何的手眼标定方法，用于标定机器人的手眼矩阵，该机器人具有基座、机械臂，在所述机械臂的末端安装有靶标标定支撑器，在所述靶标标定支撑器上安装有靶标，利用安装在所述机械臂外的双目立体相机，将所述机械臂的末端和所述靶标置于视野范围内的空间中，

其特征在于，包括下述步骤：

获得所述机械臂在所述空间中的第一位置的位姿T₁(R₁，t₁)，并相应地获得所述靶标相对于所述双目立体相机的第一位姿T_c1(R_c1,t_c1)，

获得所述机械臂在所述空间中的第二位置的位姿T₂(R₂,t₂)，并相应地获得所述靶标相对于所述双目立体相机的第二位姿T_c2(R_c2,t_c2)，

设所述靶标标定支撑器在所述机械臂的末端的位姿是：T'(S_t,p_t)，手眼变换位姿为：T_x(R_x,t_x)，

其中，R₁、R₂表示所述机械臂的末端的坐标系相对于所述机器人的基座的基坐标系的旋转矩阵，t₁、t₂表示平移矢量，R_c1、R_c2表示所述靶标的坐标系相对于所述双目立体相机的坐标系的旋转矩阵，t_c1、t_c2表示平移矢量，S_t表示所述靶标的坐标系相对于所述末端的坐标系的旋转矩阵，p_t表示平移矢量，R_x表示所述双目立体相机的坐标系相对于所述基坐标系的旋转矩阵，t_x表示平移矢量，

在由所述的各个坐标系构成的多坐标系转换闭环中，成立方程组：

T₁·T'＝T_x·T_c1........................................(1)

Τ₂·Τ'＝T_x·T_c2.........................................(2)

根据式(1)、式(2)推导出：

(T₂·T₁ ^-1)·T_x＝T_x·(T_c2·T_c1 ^-1)...................................(4)

已知：

由此简化得到：

另外，已知有：

将(9)(10)(11)带入到(4)中，可以得到：

R_At_x+t_A＝R_xt_B+t_x...........................................(13)

R_A＝R_x·R_B·R_x ^-1...........................................(14)

基于R_A与R_B的相似性，设：

其中，K_A与K_B是旋转轴矢量，α与β是各自的旋转角度，根据相

似性，得出下式(18)和(19)：

α＝β.................................................(18)

K_A＝R_x·K_B.............................................(19)

对于通过一次手眼标定获得的多个K_Ai和K_Bi的矢量对，设K_Ai和K_Bi的矢量对的差矢量为K_Ci，在此，i＝1、2、3、……、n，n为自然数，

基于手眼旋转矩阵R_x的旋转轴矢量K_x垂直于所述多个K_Ai和K_Bi的矢量对的几何关系，设所述差矢量K_Ci为：

设手眼旋转矩阵R_x的旋转轴矢量为：

采用如下优化目标函数：

应用拉格朗日乘数法，求解得到手眼旋转矩阵R_x。

根据本发明的基于几何的手眼标定方法、机器人、计算机和存储介质，与本申请前述实施例提供的标定方法与系统出于相同的发明构思，具有相同的有益效果。

附图说明

图1是表示简易的眼在手外式机械臂标定场景的图。

图2是表示手眼标定矩阵的旋转轴矢量K_x与K_Ai和K_Bi矢量对之间的几何关系的图。

图3是表示求解旋转角度的图。

具体实施方式

下面结合附图详细描述本发明的示例性实施例。下文描述的和附图示出的示例性实施例旨在教导本发明的原理，使本领域技术人员能够在若干不同环境中和对于若干不同应用实施和使用本发明。因此，本发明的保护范围由所附的权利要求来限定，示例性实施例并不意在、并且不应该被认为是对本发明保护的范围的限制性描述。而且，术语“第一”和“第二”、“步骤”等旨在区别不同对象非特定顺序。

图1是表示简易的眼在手外式机械臂标定场景的图。

<标定系统的构成>

根据本发明的标定系统包括：机器人1，其具有基座2、关节4、机械臂3；安装于机械臂3的末端9的靶标标定支撑器5；精密地安装在靶标标定支撑器5上的靶标7，在此，靶标7利用4颗反光球6制成；和双目立体相机8，其安装在机械臂3外，使得机械臂3的末端9置于其视野范围内，利用双目视觉进行位姿测量。

设于机械臂3的靶标7随机械臂3的移动而移动。由此双目立体相机8能够采集机械臂末端9的运动图像，并将其传送给计算机进行图像处理，计算出机械臂末端位姿。其中，利用靶标7的相关特征点在机械臂末端9的坐标系和相机8的坐标系的位置，计算机械臂末端9的坐标系在相机8的坐标系的旋转平移矩阵以及位姿，如后述。

<标定原理>

设机器人1的基座2的基坐标系为{M}(等同于世界坐标系)，其坐标原点位于机器人1的基座中心。在图1中的标定场景图中，选择性地示出了机械臂3运动的第一位姿W1：T₁(R₁，t₁)，和第二位姿W2：T₂(R₂,t₂)。而这两个位姿在双目立体相机8看来的位姿分别是：T_c1(R_c1,t_c1)以及T_c2(R_c2,t_c2)。

此外，靶标标定支撑器5在机械臂3的末端9的位姿是：T'(S_t,p_t)，而手眼变换位姿为：T_x(R_x,t_x)。

其中，R₁、R₂表示机械臂3的末端9的坐标系{N}相对于基坐标系{M}的旋转矩阵，t₁、t₂表示平移矢量。R_c1、R_c2表示靶标7(或靶标标定支撑器5)的坐标系{J₁}(或{J₂})相对于双目立体相机8的坐标系{K}的旋转矩阵，t_c1、t_c2表示平移矢量。S_t表示靶标7的坐标系{J₁}相对于末端9的坐标系{N}的旋转矩阵，p_t表示平移矢量。R_x表示双目立体相机8的坐标系{K}相对于基坐标系{M}的旋转矩阵，t_x表示平移矢量。

根据几何关系，在由坐标系{M}、{N}、{J₁}(或{J₂})、{K}构成的多坐标系转换闭环中，可以建立方程组：

T₁·T'＝T_x·T_c1......................................(1)

Τ₂·Τ'＝T_x·T_c2....................................(2)

根据(1)可以推导出：

T'＝T₁ ^-1·T_x·T_c1.....................................(3)

代入(2)可以推导出：

(T₂·T₁ ^-1)·T_x＝T_x·(T_c2·T_c1 ^-1).........................(4)

已知：

可以得到：

其中，利用R_A、R_B、t_A、t_B、T_A、T_B来进行简化表示。

另外已知有：

将(9)(10)(11)带入到(4)中，可以得到：

R_A·R_x＝R_x·R_B.........................................(12)

R_At_x+t_A＝R_xt_B+t_x...................................(13)

<手眼矩阵之矩阵R_x的求解>

为了得到手眼矩阵，必须首先求解公式(12)，据(12)可以推导出：

R_A＝R_x·R_B·R_x ^-1...............................(14)

由此可以得知，R_A与R_B是相似矩阵，有着共同的特征值，因而又有：

其中，K_A与K_B是旋转轴矢量，α与β是各自的旋转角度。于是根据相似性，可以很自然得出结论：

α＝β.....................................................(18)

K_A＝R_x·K_B................................................(19)

本发明人由此可以得出结论，待求解的手眼旋转矩阵R_x的旋转轴矢量K_x一定垂直于两个旋转轴矢量K_A和K_B的垂直平分面。

做一次手眼标定会获得许多K_Ai和K_Bi矢量对，这些矢量对的差矢量K_Ci末端理论上会位于同垂直平分面平行的各个平面上(如图2所示的平面Pm1～Pm4)，但实际上肯定会有误差。

因此，目标就是寻找最优的那个近似地垂直于所有的矢量对的旋转轴矢量K_x。

在此，i＝1,2,3,……n，n为任意自然数，可根据需要适当地选择。

假设矢量对的差矢量可以表示为：

而手眼旋转矩阵的旋转轴矢量可以表示为：

为了找到这个最优的旋转轴矢量，本发明人在此假定一个优化目标函数如下：

应用拉格朗日乘数法，可以得到以下四个方程：

设：

这样，方程组(23)～(26)可以化简为：

上述方程的求解可以化为系数行列式矩阵等于0：

上式可以进一步化为：

上述一元三次方程的求解可以用高斯-牛顿法等数值方法求解。求解得到λ后，返代回方程组(27)，化简得：

先通过式(32)求出z然后再代入式(30)、(31)求出x和y。这样就求出了K_x，进一步，再根据如图3所示的几何关系可以求出旋转角度θ、矢量c。

如图3所示，首先可以根据K_A和K_B求解出沿着K_x的投影矢量a和b：

a＝K_A-(K_A·K_x)K_x.....................................(33)

b＝K_B-(K_B·K_x)K_x...................................(33’)

这样，可以根据余弦定理求出旋转角度θ：

这样，根据式(17)就可以得到R_x。

<手眼矩阵之平移矢量t_x和p_t的求解>

接下来就是去求取平移矢量，传统的手眼标定方法都是采用公式(13)来求取t_x。但是这种方法在实际中效果并不好。

本文提出一种全新的方法来求取平移矢量t_x。

根据图1，针对手眼标定过程中经过的第i个路径点，可以得到关系：

T_x＝Τ_i·T'·T_ci ^-1.......................................(35)

展开得到：

上式左上角的矩阵元对应相等可以得到：

S_t＝R_i ^-1·R_x·R_ci...........................................(37)

将式(37)带入到(36)式的右上角的矩阵元对应相等得到的方程，可以得到：

R_ip_t-t_x＝R_xt_ci-t_i.................................(38)

这样，当手眼标定过程中经过了许多路径点时，可以用最小二乘法去求解一个近似精确的t_x和p_t：

求解以上方程，用伪逆解可以得到：

X＝(E^TE)^-1E^TF..........................................(40)

这样，就可以得到近似精确的手眼矩阵的平移矢量t_x和以及机械臂末端9到靶标7的平移矢量p_t。

<手眼矩阵之矩阵S_t的求解>

最后，还要求解机械臂末端到靶标的旋转矩阵S_t。根据式(37)可以得到：

R_iS_t＝R_xR_ci.................................................(41)

当手眼标定过程中经过了许多路径点时，可以用最小二乘法求解S_t：

求解以上方程，由于伪逆解可以提供最小二乘解，故用伪逆解可以得到：

S_t＝(E_s ^TE_s)^-1E_s ^TF_s.........................................(43)

至此为止，手眼标定问题的四个矩阵R_x、t_x、S_t和p_t已经全部求出。

<创新点>

由上述可知，根据本发明包括如下创新点：

(1)求解手眼矩阵的旋转矩阵R_x的算法是根据矢量几何图像的启发而完成的，主要是依据手眼矩阵的旋转矢量K_x垂直于所有A矩阵的旋转矢量K_Ai和B矩阵的旋转矢量K_Bi矢量对的差矢量K_Ci。并且采用了基于最优化目标函数的方法，采用拉格朗日乘数法完成求解。

(2)求解手眼矩阵的平移矢量t_x并没有根据以往的AX＝XB的分量式进行求解，而是采用不同的机械臂底座(基座)到相机坐标系路径的转换关系相等来建立等量关系求解，最后用最小二乘伪逆来求平均值。

从而，根据本发明的基于几何的手眼标定方法，可以分别更优地求解标定出手眼标定问题的矩阵R_x；t_x；S_t和p_t，乃至于一次性标定出手眼标定问题的四个矩阵R_x、t_x、S_t和p_t。从而能够快速有效精确地实现标定，提高了机器人的自动化程度。

上述实施例中以机械臂3运动的两个位姿为例进行了说明，然而并不限于此，也可以考虑多个位姿的情况来进行。

此外，本发明涉及的手眼标定方法也可以通过软件、硬件或软硬结合的方式来实现。例如，可以包括集成的或分开的功能模块或单元来执行上述方法中的对应步骤。

相应地，本发明提供机器人，通过上述手眼标定方法进行了标定。

相应地，本发明提供一种计算机(装置)，包括控制器，该控制器用于执行存储器中存储的计算机程序来实现上述的手眼标定方法。

相应地，本发明提供一种存储介质，其为计算机可读存储介质，存储有计算机程序，该计算机程序被控制器执行来实现上述的手眼标定方法。

上述计算机程序可以包含多个模块，例如是执行相应功能的一系列指令段，以描述程序在计算机装置中的执行过程，处理器可与模块连接，用于处理双目立体相机8的图像，计算位姿，求解矩阵R_x、矢量t_x和矢量p_t以及矩阵S_t等。

尽管已经参考各种具体实施例描述了本发明，但是应当理解，可以在所描述的发明构思的精神和范围内做出变形。因此，意图是本发明不限于所描述的实施例，而是将具有由所附权利要求的语言所定义的全部范围。对于本领域技术人员不言而喻的是，根据本发明公开的内容而能够容易想到的技术方案也应当视为等同或相当而落在本发明的范围内。

Claims (8)

1.一种基于几何的手眼标定方法，用于标定机器人的手眼矩阵，该机器人具有基座、机械臂，在所述机械臂的末端安装有靶标标定支撑器，在所述靶标标定支撑器上安装有靶标，利用安装在所述机械臂外的双目立体相机，将所述机械臂的末端和所述靶标置于视野范围内的空间中，

其特征在于，包括下述步骤：

获得所述机械臂在所述空间中的第一位置的位姿T₁(R₁，t₁)，并相应地获得所述靶标相对于所述双目立体相机的第一位姿T_c1(R_c1,t_c1)，

获得所述机械臂在所述空间中的第二位置的位姿T₂(R₂,t₂)，并相应地获得所述靶标相对于所述双目立体相机的第二位姿T_c2(R_c2,t_c2)，

设所述靶标标定支撑器在所述机械臂的末端的位姿是：T'(S_t,p_t)，手眼变换位姿为：T_x(R_x,t_x)，

其中，R₁、R₂表示所述机械臂的末端的坐标系相对于所述机器人的基座的基坐标系的旋转矩阵，t₁、t₂表示平移矢量，R_c1、R_c2表示所述靶标的坐标系相对于所述双目立体相机的坐标系的旋转矩阵，t_c1、t_c2表示平移矢量，S_t表示所述靶标的坐标系相对于所述末端的坐标系的旋转矩阵，p_t表示平移矢量，R_x表示所述双目立体相机的坐标系相对于所述基坐标系的旋转矩阵，t_x表示平移矢量，

在由所述的各个坐标系构成的多坐标系转换闭环中，成立方程组：

T₁·T'＝T_x·T_c1............................................(1)

Τ₂·Τ'＝T_x·T_c2.............................................(2)

根据式(1)、式(2)推导出：

(T₂·T₁ ^-1)·T_x＝T_x·(T_c2·T_c1 ^-1)....................................(4)

已知：

由此简化得到：

另外，已知有：

将(9)(10)(11)带入到(4)中，可以得到：

R_At_x+t_A＝R_xt_B+t_x.....................................(13)

R_A＝R_x·R_B·R_x ^-1.......................................(14)

基于R_A与R_B的相似性，设：

其中，K_A与K_B是旋转轴矢量，α与β是各自的旋转角度，根据相似性，得出下式(18)和(19)：

α＝β.........................................................(18)

K_A＝R_x·K_B....................................................(19)

对于通过一次手眼标定获得的多个K_Ai和K_Bi的矢量对，设K_Ai和K_Bi的矢量对的差矢量为K_Ci，在此，i＝1、2、3、……、n，n为自然数，

基于手眼旋转矩阵R_x的旋转轴矢量K_x垂直于所述多个K_Ai和K_Bi的矢量对的几何关系，设所述差矢量K_Ci为：

设手眼旋转矩阵R_x的旋转轴矢量为：

采用如下优化目标函数：

应用拉格朗日乘数法，求解得到手眼旋转矩阵R_x。

2.根据权利要求1所述的基于几何的手眼标定方法，其特征在于，

通过应用拉格朗日乘数法，得到以下四个方程：

设：

将方程组(23)～(26)化简为：

将该方程组的求解化为系数行列式矩阵等于0：

进一步化为：

对上述一元三次方程进行数值方法求解，求解得到λ后，代回到方程组(27)，化简得：

通过(32)求出z，然后代入到式(30)、(31)来求出x和y，由此求出K_x，

进一步，根据K_A和K_B求解出沿着K_x的投影矢量a和b：

a＝K_A-(K_A·K_x)K_x.....................................(33)

b＝K_B-(K_B·K_x)K_x.....................................(33’)

根据余弦定理求出旋转角度θ：

由此，根据式(17)得到手眼旋转矩阵R_x。

3.根据权利要求1或2所述的基于几何的手眼标定方法，其特征在于，

基于式(1)、(2)，针对手眼标定过程中经过的第i个路径点，可以得到关系：

T_x＝Τ_i·T'·T_ci ^-1.........................................(35)

展开得到：

基于上式左上角的矩阵元对应相等而得到：

S_t＝R_i ^-1·R_x·R_ci...........................................(37)

将式(37)带入到(36)式的右上角的矩阵元对应相等得到的方程，得到：

R_ip_t-t_x＝R_xt_ci-t_i.....................................(38)

当手眼标定过程中经过了多个路径点时，用最小二乘法去对下式(39)求解手眼矩阵的平移矢量t_x和平移矢量p_t：

求解以上方程，通过伪逆解得到：

X＝(E^TE)^-1E^TF............................................(40)

由此，得到近似精确的平移矢量t_x和平移矢量p_t。

4.根据权利要求3所述的基于几何的手眼标定方法，其特征在于，

根据式(37)得到：

R_iS_t＝R_xR_ci.........................................(41)

当手眼标定过程中经过了多个路径点时，通过最小二乘法对下式(42)求解旋转矩阵S_t：

对于上式(42)，通过伪逆解得到：

S_t＝(E_s ^TE_s)^-1E_s ^TF_s.........................................(43)。

5.根据权利要求1～4中任一项所述的基于几何的手眼标定方法，其特征在于，

所述靶标利用4颗反光球制成。

6.一种机器人，其特征在于，通过执行权利要求1～4中任一项所述的基于几何的手眼标定方法进行了标定。

7.一种计算机，包括控制器，其特征在于，所述控制器用于执行存储器中存储的计算机程序来实现权利要求1～4中任一项所述的基于几何的手眼标定方法。

8.一种存储介质，其为计算机可读存储介质，存储有计算机程序，其特征在于，该计算机程序被控制器执行来实现权利要求1～4中任一项所述的基于几何的手眼标定方法的步骤。

Images