CN109483516A - 一种基于空间距离和极线约束的机械臂手眼标定方法 - Google Patents

Abstract

本发明公开了一种基于空间距离和极线约束的机械臂手眼标定方法。求解手眼转换矩阵的初始值，获得机械臂在两个不同位置时固定在机械臂上的相机的转换关系，通过相机的内参计算图像中特征点的极线误差，并对特征点进行三维重建，计算重建后的特征点间的空间距离，根据实际的空间距离计算空间距离误差，将所有特征点的极线误差和空间距离误差的累加和作为优化目标函数，使用列文伯格‑马夸尔特算法进行迭代优化，获得更高精度的手眼转换矩阵。本发明能构造更符合实际需求的优化目标函数，使得通过手眼转换矩阵计算出的空间尺寸和实际更相吻合，提高迭代优化的收敛速度以及保证优化后的参数满足对极几何约束，精度更高。

Description

一种基于空间距离和极线约束的机械臂手眼标定方法

技术领域

本发明涉及了一种视觉标定方法，涉及一种基于空间距离和极线约束的机械臂手眼标定方法，适用于机械臂和相机融合的机器人视觉引导系统，涉及机械臂视觉装配、机械臂辅助手术以及机械臂视觉检测等技术领域。

背景技术

相机和机械臂融合的、基于视觉引导的机械臂控制系统由于可以提高生产的柔性、自动化和智能化程度，近几年来在自动化生产线、医疗手术以及某些不存在人工操作条件的场合发挥着越来越重要的作用。固定在机械臂末端执行器上的相机可用来完成目标检测任务，检测的结果大多是在相机坐标系下的。因此需要求取高精度的手眼转换矩阵，使得检测结果由相机坐标系转换到机械臂坐标系下，从而可控制机械臂根据检测结果进行相应的运动。由于手眼标定是机械臂利用机器视觉进行后续一系列操作的基础，因此学者们在手眼标定领域展开了大量的研究。

Hanqi Zhuang于1994年在《IEEE Transaction On Robotics And Automation》上的论文“Simultaneous Robot/World and Tool/Flange Calibration by SolvingHomogeneous Transformation Equations of the Form AX＝YB”中构造手眼标定的AX＝YB的数学模型，应用四元数代数对该模型进行线性求解。由于Zhuang将首先求取手眼转换矩阵中的旋转参数，再将求解得到的旋转参数作为已知量求解手眼转换矩阵中的平移参数，这样会导致旋转参数的误差直接传递到平移参数，从而影响最终的标定结果。Aiguo Li于2010年在《International Journal of the Physical Sciences》上的论文“Simultaneous robot-world and hand-eye calibration using dual-quaternions andKronecker product”使用对偶四元数以及克罗内克积这两种运算方法分别求解手眼标定方程AX＝YB，尽管两种方法均可求取手眼转换矩阵的解析解，该论文的实验结果表明，采用克罗内克积求解的手眼转换矩阵更为精确。Nicolas Andreff于2011年在“3dim IEEEComputer Society”上发表的论文“On-line Hand-Eye Calibration”中考虑使用AX＝XB的手眼标定数学模型，并根据克罗内克积的运算性质，将手眼标定数学模型转换为MX＝N的矩阵等式，并通过SVD分解求取手眼转换矩阵X。由于该方法同时求解出手眼转换矩阵的旋转和平移参数，有效避免了由旋转参数到平移参数的误差传递。Mili Shah于2013年在《Journal of Mechanisms and Robotics》上的论文“Solving the Robot-World/Hand-EyeCalibration Problem Using the Kronecker Product”同样是致力于求取手眼标定方程AX＝YB的解析解。首先利用克罗内克积求解手眼转换矩阵中的旋转参数，之后通过矩阵运算，利用旋转参数求解平移参数。由于平移参数是在旋转参数的基础上计算所得，因此这种方法同样会产生误差传递。Amy Tabb于2017年在《Machine Vision and Applications》上的论文“Solving the robot-world hand-eye(s)calibration problem with iterativemethods”总结分析了当前求取手眼转换矩阵的解析解的几种常用的计算方法，包括四元数法、对偶四元数法以及克罗内克积法，由于通过这些方法求取的解析解依赖手眼标定数学模型中的输入量的准确度，易受噪声影响，从而导致在实际应用中误差较大。因此需要对求解得到的解析解施加合理的约束，以优化手眼转换矩阵的参数，从而得到高精度、鲁棒性强的手眼转换矩阵。

在手眼转换矩阵的参数优化方面，现有的手眼标定方法大致采用两类优化目标函数。第一种优化目标函数构造方法是根据手眼标定的数学模型，最小化‖AX-YB‖₂，从而求解得到的手眼转换矩阵尽可能的满足等式AX＝YB，这种方法同样是过于依赖输入参数A和B的精度。另一种优化目标函数构造方法是通过求解得到的手眼转换矩阵反推相机的投影矩阵A，并通过A计算图像上特征点的重投影误差，之后优化手眼转换矩阵中的参数使得重投影误差最小化，得到更高精度的手眼转换矩阵，比如Xiangyang Zhi在“2017 IEEE/RSJInternational Conference on Intelligent Robots and Systems”上发表的论文“Simultaneous Hand-Eye Calibration and Reconstruction”中的方法。尽管这种方法考虑到了使用图像信息来优化矩阵参数，但实际应用中，往往保证三维信息的精度更为重要。目前，现有的手眼标定算法缺少在实际三维层面的约束信息，未将空间尺寸信息以及空间投影几何应当满足的约束信息融合到参数优化的过程中。

发明内容

为了解决背景技术中存在的问题，本发明的目的在于提供一种基于空间距离和极线约束的机械臂手眼标定方法，构造更符合实际需求的优化目标函数，即使得通过手眼转换矩阵计算出的空间尺寸和实际更相吻合。另外根据对极几何原理对手眼标定时移动到不同位置的相机施加极线约束，提高迭代优化的收敛速度以及保证优化后的参数满足对极几何约束。综合利用空间距离约束和极线约束来对手眼转换矩阵进行迭代优化，获得更满足实际需求且精度更高的手眼转换矩阵。

本发明所采用的技术方案它包括以下步骤：

本发明中需要建立四个坐标系：机械臂底座坐标系、末端执行器坐标系、相机坐标系、标定板坐标系。机械臂底座坐标系、末端执行器坐标系、相机坐标系、标定板坐标系分别是以机械臂底座中心、末端执行器中心、相机光心、标定板中心为坐标系原点建立的三维坐标系。

(1)机械臂包括机械臂底座和末端执行器，末端执行器安装于机械臂底座，末端执行器安装有相机，机械臂位于标定板上方，标定板表面具有国际象棋黑白棋盘格图案，通过控制末端执行器移动相机拍摄标定板的完整图像；使用张正友标定法对固定在机械臂的末端执行器上的相机进行标定，获得相机的内参矩阵K和畸变系数D，移动末端执行器使得相机在n个不同拍摄位置朝向同一标定板拍摄，标定板保持不动，n>4，这n个位置进行编号分别定义为位置1、位置2、…、位置n；记录相机在这n个不同位置时控制器上显示的末端执行器的位姿，再处理获得机械臂底座坐标系到末端执行器坐标系的转换矩阵B_i；

(2)采用哈里斯角点检测算法提取n张图像中国际象棋黑白棋盘格图案上的特征点，即位于国际象棋黑白棋盘格图案中每个正方形的四个角上的点，也即正方形之间的交叉点；然后，根据相机的畸变系数D，采用无畸变递归求解方法对提取到的特征点进行畸变矫正，获得无畸变特征点m^o及其图像坐标；

所述的无畸变递归求解方法采用Heikkila在《IEEE Transactions on PatternAnalysis and Machine Intelligence》期刊上发表的论文“Geometric cameracalibration using circular control points”中的方法。

(3)使用张正友标定法计算机械臂在步骤(1)中n个不同位置时相机的外参数矩阵，即标定板坐标系到相机坐标系的转换矩阵A_i；机械臂的末端执行器分别在位置2、位置3、…、位置n时和位置1之间形成了相机之间的n-1组不同位置组合，即1-2，1-3，…，1-n，其中1,2,…,n为步骤(1)中相机的n个不同拍摄位置的位置序号，1-2表示位置1和位置2的相机位置组合，再计算机械臂的末端执行器分别在不同位置组合下两个位置之间的相机坐标系转换矩阵C_i＝A_i+1A₁ ^-1以及末端执行器坐标系转换矩阵D_i＝B_i+1B₁ ^-1，其中i＝1,2,…,n-1；然后根据所有n-1个的相机坐标系转换矩阵C_i和末端执行器坐标系转换矩阵D_i采用基于克罗内克积的矩阵运算方法求解方程C_iX₀＝X₀D_i，获得机械臂的手眼转换矩阵的初始值X₀；

所述的基于克罗内克积的矩阵运算方法采用Nicolas Andreff在“3dim IEEEComputer Society”上发表的论文“On-line Hand-Eye Calibration”中的方法。

(4)利用由步骤(3)获得的手眼转换矩阵的初始值X₀采用以下公式进行反推计算获得各个不同位置组合下两个不同拍摄位置的相机之间的相对位置转换矩阵C′_i(i＝1,2,…,n-1)：

C′_i＝X₀D_iX₀ ^-1

根据以下公式将相对位置转换矩阵C′_i分解为由相对位置转换矩阵C′_i的旋转矩阵和相对位置转换矩阵C′_i的平移矩阵两部分组成的矩阵表达：

根据双目视觉的对极几何原理，相机在两个位置拍摄所得的图像上的特征点满足极线约束。根据双目视觉中基础矩阵的定义，通过旋转矩阵和平移矩阵构建获得基础矩阵F_i。

(5)步骤(3)中，每一个位置组合下对应获得一对图像，由于每个位置均拍摄了一张图像，因此n-1组位置组合包含n-1对图像，计算每对图像中无畸变特征点的极线误差，所有极线误差的累加和记为J_epi，具体计算公式为：

式中，为第i对图像中的其中一副图像上的第j个无畸变特征点的齐次坐标，为第i对图像中的另外一副图像上的第j个无畸变特征点的齐次坐标，d(*,*)²代表两个坐标的几何距离的平方，i表示图像对的序号，j表示矫正后的无畸变特征点的序号，F_i表示第i对图像对应的基础矩阵，m为每张图像中棋盘格特征点的总数，T表示矩阵转置；

(6)机械臂位于两个不同位置时相机间的相对位置转换矩阵C′_i，且每组位置均包含位置1，因此针对每组位置组合，建立投影矩阵：

针对每i组位置组合，令其中处于位置1的相机的投影矩阵为P_i1＝K[I_3×3 0_3×1]，使得另一位置的相机的投影矩阵为K表示相机内参矩阵；

根据相机投影映射关系，无畸变特征点m^o的图像坐标和其对应的空间坐标M满足其中P是相机的投影矩阵，为无畸变特征点的齐次坐标，w为比例因子，x,y分别表示m^o在图像上的横、纵坐标；

设p^j为矩阵P的第j行(j＝1,2,3)，分解为：

通过上式可以得到如下3个等式：

一对图像对应两个相机的投影矩阵P_i1和P_i2，然后建立如下的映射关系矩阵：

其中，p_i1 ¹为投影矩阵P_i1的第1行，p_i1 ²为投影矩阵P_i1的第2行，p_i1 ³为投影矩阵P_i1的第3行；p_i2 ¹为投影矩阵P_i2的第1行，p_i2 ²为投影矩阵P_i2的第2行，p_i2 ³为投影矩阵P_i2的第3行；

对上式进行奇异值SVD分解得到无畸变特征点在机械臂末端执行器处于位置1时相机坐标系下的坐标M＝(a,b,c,d)^T，其中a,b,c是在三维直角坐标系xyz下的分别对应x轴、y轴、z轴的三个坐标分量，d为尺度因子，将尺度因子化为1得坐标M的齐次坐标作为三维重建特征点，完成了无畸变特征点的三维重建；

本发明中，由于棋盘格上的特征点是沿行、列分布的，每行、每列均包含固定数量的特征点，且在每行、每列上的相邻特征点的距离相等，均等于一个黑正方格子或白正方格子的边长。

计算黑白棋盘格图案中沿行方向相邻的每两个特征点所对应的三维重建特征点之间的空间距离和黑白棋盘格图案中沿列方向相邻的每两个特征点所对应的三维重建特征点之间的空间距离，以空间距离和用游标卡尺测量黑白棋盘格图案中相邻两个特征点实际间距之间的差值作为空间中相邻三维重建特征点在行和列方向上的空间距离误差。

接着，对n-1对图像的所有无畸变特征点均进行三维重建并采用公式计算空间距离误差，所有空间距离误差的累加和记为J_3D：

其中，D(M_h,M_v)为沿水平/竖直方向相邻的每两个特征点所对应的三维重建特征点之间的空间距离，M_h表示沿水平方向相邻的两个特征点，M_v表示沿竖直方向相邻的两个特征点，D_adj为用游标卡尺对黑白棋盘格图案中相邻特征点间距测量获得的实际测量值，h代表黑白棋盘格图案水平方向，v代表黑白棋盘格图案水平方向；

(7)构造优化目标函数。

步骤(4)中极线误差的累加和J_epi以及步骤(5)中的空间距离误差的累加和J_3D都受参数和的影响，也就是C′_i的影响。而步骤(4)中已经推出C′_i＝X₀D_iX₀ ^-1，也就是C′_i是通过手眼转换矩阵的初始值X₀求出的。因此，X₀影响了J_epi和J_3D的最终结果。

手眼转换矩阵的初始值X₀由旋转矩阵和平移矩阵两部分组成，可分解为：

为了缩小待优化参数的数量，将旋转矩阵进行罗德里格斯反变换，变换为3行1列的旋转向量

这样X₀可由6个参数组成，其中包括的3个旋转参数以及的3个平移参数。这样J_epi和J_3D也就成为了仅受这6个参数影响的函数，我们记为和

构造如下优化目标函数：

其中，J_opt表示误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的极线误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的空间距离误差累加和，表示手眼转换矩阵的初始值X₀中的旋转矩阵部分罗德里格斯反变换后的结果，表示手眼转换矩阵的初始值X₀中的平移矩阵部分；

采用列文伯格-马夸尔特法对优化目标函数J_opt进行迭代求解，通过优化上述的6个参数，得到使得误差累加和J_opt最小的分别表示对进行优化后的结果和对进行优化后的结果；

最后将进行罗德里格斯变化得到3×3的旋转矩阵采用以下公式组建得到优化后的手眼转换矩阵完成机械臂手眼标定：

所述步骤(1)中，每次移动末端执行器使得相邻相机拍摄位置之间的圆心旋转角度大于10°，间隔平移距离大于10mm。

每个位置拍摄一张图像，并保证标定板上的国际象棋黑白棋盘格图案完整的呈现在每张图像中。

所述步骤(1)中，记录相机在这n个不同位置时控制器上显示的末端执行器的位姿为(α_i,β_i,γ_i,x_i,y_i,z_i)，其中i表示拍摄位置的序数，i＝1,2,…,n，(α_i,β_i,γ_i)为末端执行器的欧拉角(偏航角、俯仰角、翻滚角)，(x_i,y_i,z_i)为末端执行器的空间坐标；

接着将欧拉角转化为机械臂底座坐标系到末端执行器坐标系的旋转矩阵将空间坐标转化为机械臂底座坐标系到末端执行器坐标系的平移矩阵如下：

进而计算机械臂底座坐标系到末端执行器坐标系的转换矩阵B_i：

其中，0_1×3为1行3列的零矩阵。

所述步骤(1)中，使用张正友标定法对固定在机械臂的末端执行器上的相机进行标定。

所述步骤(2)中，采用哈里斯角点检测算法提取n张图像中国际象棋黑白棋盘格图案上的特征点。

本发明具有的有益效果是：

(1)针对手眼转换矩阵的参数优化，构造了新型的优化目标函数，引入三维空间尺寸约束，使得通过手眼转换矩阵计算出的空间尺寸和实际更相吻合，从而更加满足实际空间操作的需求。另外根据对极几何原理对手眼标定时移动到不同位置的相机施加极线约束，提高迭代优化的收敛速度以及保证优化后的参数满足对极几何约束。

(2)减小迭代优化过程中的误差传递。最终的优化目标函数由空间距离误差和极线误差构成，这两种误差的计算均依赖于相机在不同位置间的转换矩阵C′_i，而根据C′_i＝X₀D_iX₀ ^-1可知，C′_i仅由机械臂末端执行器在不同位置间的转换矩阵D_i和待优化手眼转换矩阵X₀计算所得。而D_i是通过机械臂控制器读取的末端执行器的位姿参数计算所得的，精度较高，因此在整个优化过程中的误差传递较小。而传统的优化策略，无论是最小化‖AX-YB‖₂还是根据A＝YBX^-1最小化重投影误差均会引入额外的误差(除了B以外，A或Y的误差也会引入到迭代优化过程)，影响优化结果。因此本发明可以有效地缩小误差传递的影响，从而得到较高精度的参数优化结果。

(3)缩小待优化参数的数量。本发明消除了手眼转换矩阵计算过程中，世界坐标系到机械臂基坐标系间的转换矩阵这个未知量的影响，该矩阵同样包括3和旋转参数和3个平移参数待定。因此，待优化参数数量也由12个缩小到了6个，其中包括手眼转换矩阵中的3个旋转参数和3个平移参数。

(4)标定物仅需提供黑白棋盘格标定板即可，无需额外的光学追踪仪或者其余用来辅助定位的设备。

附图说明

图1是本发明的流程图。

图2是手眼标定基本原理图。

图3是5组实验的旋转误差结果图。

图4是平均旋转误差结果图。

图5是5组实验的平移误差结果图。

图6是平均平移误差结果图。

图7是空间距离误差结果图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

本发明的实施例及其实施过程如下：

(1)图1是本发明方法的流程图。本实验中的机械臂型号为ABB-IRB-120，工业相机型号为Basler-acA2440-20gm，镜头型号为Computar-M1224-MPW2。

如图2所示，机械臂包括机械臂底座和末端执行器，末端执行器安装于机械臂底座，末端执行器安装有相机，机械臂位于标定板上方，标定板表面具有国际象棋黑白棋盘格图案，通过控制末端执行器移动相机拍摄标定板的完整图像；

使用张正友标定法对固定在机械臂末端执行器上的相机进行标定，获得相机的内参矩阵K和畸变系数D。移动机械臂在n(n>4)个不同位置朝向同一国际象棋黑白棋盘格图案拍摄，这n个位置分别定义为位置1，位置2，…，位置n。在本实施例中，n取6，也就是机械臂在6个不同的位置拍摄图像。

每个位置拍摄一张图像，并保证棋盘格图案完整的呈现在每张图像中。所述步骤(1)中，每次移动末端执行器使得相邻相机拍摄位置之间的圆心旋转角度大于10°，间隔平移距离大于10mm。记录机械臂在这6个不同位置时控制器上显示的末端执行器的位姿(α_i,β_i,γ_i,x_i,y_i,z_i)，其中i＝1,2,…,6，(α_i,β_i,γ_i)为欧拉角(偏航角，俯仰角，翻滚角)，(x_i,y_i,z_i)为空间坐标。将欧拉角转化为旋转矩阵如下：

将空间坐标转化为平移矩阵

计算机械臂底座坐标系到末端执行器坐标系的转换矩阵B_i(i＝1,2,…,6)：

其中0_1×3为1行3列的零矩阵。

(2)采用哈里斯角点检测算法提取6张图像中棋盘格上的特征点，也就是位于棋盘格中每个正方形的四个角上的点。根据相机的畸变系数D，采用无畸变递归求解方法对提取到的特征点进行畸变矫正，获得无畸变特征点m^o。

(3)使用张正友标定法计算机械臂在步骤(1)中6个不同位置时相机的外参数矩阵，也就是标定板坐标系到相机坐标系的转换矩阵A_i(i＝1,2,…,6)。

机械臂的末端执行器分别在位置2、位置3、…、位置n时和位置1之间形成了相机之间的n-1组不同位置组合，即1-2，1-3，…，1-n，其中1,2,…,n为步骤(1)中相机的n个不同拍摄位置的位置序号，1-2表示位置1和位置2的相机位置组合，

再计算机械臂的末端执行器分别在不同位置组合下两个位置之间的相机坐标系转换矩阵C_i＝A_i+1A₁ ^-1以及末端执行器坐标系转换矩阵D_i＝B_i+1B₁ ^-1，其中i＝1,2,…,n-1；然后根据所有n-1个的相机坐标系转换矩阵C_i和末端执行器坐标系转换矩阵D_i采用基于克罗内克积的矩阵运算方法求解方程C_iX₀＝X₀D_i，获得机械臂的手眼转换矩阵的初始值X₀；

(4)由步骤(3)估计出的手眼转换矩阵的初始值X₀反推，当机械臂位于两个不同位置时相机间的相对位置转换矩阵C′_i(i＝1,2,…,5)：C′_i＝X₀D_iX₀ ^-1。C′_i由旋转矩阵和平移矩阵两部分组成，也就是：

根据双目视觉的对极几何原理，相机在两个位置拍摄所得的图像上的特征点满足极线约束。根据双目视觉中基础矩阵的定义，通过和获得基础矩阵F_i。

步骤(3)中有5组不同位置的组合，即1-2，1-3，…，1-6，其中1,2,…,6为步骤(1)中的6个不同位置的位置序号。由于每个位置均拍摄了一张图像，因此5组位置组合包含5对图像。计算每对图像中特征点的极线误差，所有极线误差的累加和记为J_epi，具体计算公式为：

式中，为第i对图像中的其中一副图像上的第j个无畸变特征点的齐次坐标，为第i对图像中的另外一副图像上的第j个无畸变特征点的齐次坐标，d(*,*)²代表两个坐标的几何距离的平方，i表示图像对的序号，j表示矫正后的无畸变特征点的序号，F_i表示第i对图像对应的基础矩阵，m为每张图像中棋盘格特征点的总数，T表示矩阵转置。

在本实施例中，m的值为54，。

(5)由于机械臂位于两个不同位置时相机间的相对位置转换矩阵C′_i(i＝1,2,…,5)，且每组位置均包含位置1，因此可令第i组的处于位置1的相机的投影矩阵为P_i1＝K[I_3×30_3×1]，则该组中另外一个位置的相机的投影矩阵根据相机投影映射关系，无畸变特征点坐标m^o和其对应的空间坐标M满足其中P是该相机的投影矩阵，为图像点的齐次坐标，w为未知的比例因子。设p^j为为矩阵P的第j行，可以分解为：

通过上式可以得到如下3个等式：

由于一对图像对应两个相机投影矩阵P_i1和P_i2，那么我们可以得到如下的矩阵形式：

其中p^j _i1、p^j _i2分别为P_i1、P_i2的第j行。对上式进行SVD分解得到棋盘格上的特征点在机械臂处于位置1时相机坐标系下的坐标M＝(a,b,c,d)^T，将尺度因子化为1可得M的齐次坐标完成了特征点的三维重建，也可称为三维重建特征点。

获得5对图像的所有特征点均进行三维重建并计算空间距离误差，误差的累加和记为J_3D：

其中，D(M_h,M_v)为沿水平/竖直方向相邻的每两个特征点所对应的三维重建特征点之间的空间距离，M_h表示沿水平方向相邻的两个特征点，M_v表示沿竖直方向相邻的两个特征点，D_adj为用游标卡尺对黑白棋盘格图案中相邻特征点间距的测量值，h代表行方向，v代表列方向，‖*‖为二范数运算符号。

(6)构造优化目标函数。

构造如下优化目标函数：

其中，J_opt表示误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的极线误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的空间距离误差累加和，表示手眼转换矩阵的初始值X₀中的旋转矩阵部分罗德里格斯反变换后的结果，表示手眼转换矩阵的初始值X₀中的平移矩阵部分；

采用列文伯格-马夸尔特法对优化目标函数J_opt进行迭代求解，通过优化上述的6个参数，得到使得误差累加和J_opt最小的

最后将进行罗德里格斯变化得到3×3的旋转矩阵采用以下公式组建得到优化后的手眼转换矩阵完成机械臂手眼标定：

为了评估本发明方法的标定精度，本实施例采用了另外两种方法做对比实验，分别为Nicolas Andreff在“3dim IEEE Computer Society”上发表的论文“On-line Hand-Eye Calibration”中的方法，记为Andreff法；另外一种对比方法为Xiangyang Zhi在“2017IEEE/RSJ International Conference on Intelligent Robots and Systems”上发表的论文“Simultaneous Hand-Eye Calibration and Reconstruction”中的方法，记为Zhi法。

首先应当观察手眼标定模型CX＝XD的满足程度。计算手眼标定模型的旋转误差：

其中，‖*‖₂为矩阵的二范数运算符号，为步骤(3)中定义的C_i的旋转矩阵部分，旋转部分也就是矩阵的前3行前3列的9个元素。类似的，为的旋转矩阵部分，为D_i的旋转矩阵部分。根据步骤(3)中的定义，i＝1,2,…,5，因此可以得到共计5组误差对比数据，将e_Ri的单位由弧度制化为角度制后，实验结果如图3所示。从图3中可以看出，采用本发明方法有效降低了手眼标定的旋转误差。图4展示了这5组实验的平均旋转误差，可以看出，采用本发明方法使得手眼标定的平均旋转误差得到了有效的降低，且具有较小的标准偏差，也就是误差波动较小。

计算手眼标定模型的平移误差：

其中为的平移矩阵部分，也就是矩阵的第1行第4列到第3行第4列的3个元素。类似的，为矩阵C_i的平移矩阵部分，为矩阵D_i的平移矩阵部分。根据步骤(3)中的定义，i＝1,2,…,5，因此可以得到共计5组误差对比数据，实验结果如图5所示。从图5中可以看出，采用本发明方法有效地降低了手眼转换关系中的平移误差。图6展示了这5组实验的平均平移误差，可以看出，采用本发明方法对机械臂进行手眼标定，这5组实验的平移误差的平均值得到了有效的降低，同时也获得了较小的标准偏差，也就是误差波动较小。

根据步骤(5)所述，手眼转换矩阵的精度实际上也影响着空间距离误差的大小，而在实际的工业应用中，也往往需要保证这个手眼转换关系能保证空间信息的准确，因此空间距离误差也是衡量手眼标定精度的一个重要标准。由于步骤(5)已经给出了空间距离误差累加和J_3D的计算方法，这里我们不将所有空间距离误差进行累加，而是记录步骤(5)中所有的空间距离误差的具体数值，并将所有的空间距离误差的数值以箱形图的形式进行统计和展示。如图7所示，从空间距离误差的箱形图可以看出，采用本发明方法有效地降低了空间距离误差，使得空间距离误差控制在0.2mm到0.5mm之间，且误差波动较小，和实际生产应用的需求更相符合。

上述实施例不应视为对本发明的限制，但任何基于本发明的精神所做的改进，都应在本发明的保护范围之内。

Claims (5)

1.一种基于空间距离和极线约束的机械臂手眼标定方法，其特征在于包括以下步骤：

(1)机械臂包括机械臂底座和末端执行器，末端执行器安装于机械臂底座，末端执行器安装有相机，机械臂位于标定板上方，标定板表面具有国际象棋黑白棋盘格图案，通过控制末端执行器移动相机拍摄标定板的完整图像；对固定在机械臂的末端执行器上的相机进行标定，获得相机的内参矩阵K和畸变系数D，移动末端执行器使得相机在n个不同拍摄位置朝向同一标定板拍摄，n>4，这n个位置进行编号分别定义为位置1、位置2、…、位置n；记录相机在这n个不同位置时末端执行器的位姿，再处理获得机械臂底座坐标系到末端执行器坐标系的转换矩阵B_i；

(2)提取n张图像中国际象棋黑白棋盘格图案上的特征点，即位于国际象棋黑白棋盘格图案中每个正方形的四个角上的点；然后，根据相机的畸变系数D，采用无畸变递归求解方法对提取到的特征点进行畸变矫正，获得无畸变特征点m^o及其图像坐标；

(3)使用张正友标定法计算机械臂在步骤(1)中n个不同位置时相机的外参数矩阵，即标定板坐标系到相机坐标系的转换矩阵A_i；机械臂的末端执行器分别在位置2、位置3、…、位置n时和位置1之间形成了相机之间的n-1组不同位置组合，再计算机械臂的末端执行器分别在不同位置组合下两个位置之间的相机坐标系转换矩阵C_i＝A_i+1A₁ ^-1以及末端执行器坐标系转换矩阵D_i＝B_i+1B₁ ^-1，其中i＝1,2,…,n-1；然后根据所有相机坐标系转换矩阵C_i和末端执行器坐标系转换矩阵D_i采用基于克罗内克积的矩阵运算方法求解方程C_iX₀＝X₀D_i，获得机械臂的手眼转换矩阵的初始值X₀；

(4)利用由步骤(3)获得的手眼转换矩阵的初始值X₀采用以下公式进行反推计算获得各个不同位置组合下两个不同拍摄位置的相机之间的相对位置转换矩阵C′_i：

C′_i＝X₀D_iX₀ ^-1

根据以下公式将相对位置转换矩阵C′_i分解为由相对位置转换矩阵C′_i的旋转矩阵和相对位置转换矩阵C′_i的平移矩阵两部分组成的矩阵表达：

根据双目视觉中基础矩阵的定义，通过旋转矩阵和平移矩阵构建获得基础矩阵F_i；

(5)步骤(3)中，每一个位置组合下对应获得一对图像，计算每对图像中无畸变特征点的极线误差，所有极线误差的累加和记为J_epi，具体计算公式为：

式中，为第i对图像中的其中一副图像上的第j个无畸变特征点的齐次坐标，为第i对图像中的另外一副图像上的第j个无畸变特征点的齐次坐标，d(*,*)²代表两个坐标的几何距离的平方，i表示图像对的序号，j表示矫正后的无畸变特征点的序号，F_i表示第i对图像对应的基础矩阵，m为每张图像中棋盘格特征点的总数，T表示矩阵转置；

(6)针对每组位置组合，建立投影矩阵：针对每i组位置组合，令其中处于位置1的相机的投影矩阵为R_i1＝K[I_3×3 0_3×1]，使得另一位置的相机的投影矩阵为K表示相机内参矩阵；根据相机投影映射关系，无畸变特征点m^o的图像坐标和其对应的空间坐标M满足其中P是相机的投影矩阵，为无畸变特征点的齐次坐标；

然后建立如下的映射关系矩阵：

其中，p_i1 ¹为投影矩阵P_i1的第1行，p_i1 ²为投影矩阵P_i1的第2行，p_i1 ³为投影矩阵P_i1的第3行；p_i2 ¹为投影矩阵P_i2的第1行，p_i2 ²为投影矩阵P_i2的第2行，p_i2 ³为投影矩阵P_i2的第3行；

对上式进行奇异值SVD分解得到无畸变特征点在机械臂末端执行器处于位置1时相机坐标系下的坐标M＝(a,b,c,d)^T，其中a,b,c是在三维直角坐标系xyz下的分别对应x轴、y轴、z轴的三个坐标分量，d为尺度因子，将尺度因子化为1得坐标M的齐次坐标作为三维重建特征点，完成了无畸变特征点的三维重建；

接着，对n-1对图像的所有无畸变特征点均进行三维重建并采用公式计算空间距离误差，所有空间距离误差的累加和记为J_3D：

其中，D(M_h,M_v)为沿水平/竖直方向相邻的每两个特征点所对应的三维重建特征点之间的空间距离，M_h表示沿水平方向相邻的两个特征点，M_v表示沿竖直方向相邻的两个特征点，D_adj为用游标卡尺对黑白棋盘格图案中相邻特征点间距测量获得的实际测量值，h代表水平方向，v代表黑白棋盘格图案水平方向；

(7)构造优化目标函数

构造如下优化目标函数：

其中，J_opt表示误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的极线误差累加和，表示由旋转矩阵部分罗德里格斯反变换后的结果和平移矩阵部分影响构成的空间距离误差累加和，表示手眼转换矩阵的初始值X₀中的旋转矩阵部分罗德里格斯反变换后的结果，表示手眼转换矩阵的初始值X₀中的平移矩阵部分；

采用列文伯格-马夸尔特法对优化目标函数J_opt进行迭代求解，得到使得误差累加和J_opt最小的分别表示对进行优化后的结果和对进行优化后的结果；最后将进行罗德里格斯变化得到3×3的旋转矩阵采用以下公式组建得到优化后的手眼转换矩阵完成机械臂手眼标定：

2.根据权利要求1所述的一种基于空间距离和极线约束的机械臂手眼标定方法，其特征在于：所述步骤(1)中，每次移动末端执行器使得相邻相机拍摄位置之间的圆心旋转角度大于10°，间隔平移距离大于10mm。

3.根据权利要求1所述的一种基于空间距离和极线约束的机械臂手眼标定方法，其特征在于：所述步骤(1)中，记录相机在这n个不同位置时末端执行器的位姿为(α_i,β_i,γ_i,x_i,y_i,z_i)，其中i表示拍摄位置的序数，i＝1,2,…,n，(α_i,β_i,γ_i)为末端执行器的欧拉角，(x_i,y_i,z_i)为末端执行器的空间坐标；接着将欧拉角转化为机械臂底座坐标系到末端执行器坐标系的旋转矩阵将空间坐标转化为机械臂底座坐标系到末端执行器坐标系的平移矩阵如下：

进而计算机械臂底座坐标系到末端执行器坐标系的转换矩阵B_i：

其中，0_1×3为1行3列的零矩阵。

4.根据权利要求1所述的一种基于空间距离和极线约束的机械臂手眼标定方法，其特征在于：所述步骤(1)中，使用张正友标定法对固定在机械臂的末端执行器上的相机进行标定。

5.根据权利要求1所述的一种基于空间距离和极线约束的机械臂手眼标定方法，其特征在于：所述步骤(2)中，采用哈里斯角点检测算法提取n张图像中国际象棋黑白棋盘格图案上的特征点。