CN105014667B - 一种基于像素空间优化的相机与机器人相对位姿标定方法 - Google Patents

Abstract

本发明公开了一种基于像素空间优化的相机与机器人相对位姿标定方法。本发明采用机器人末端携带标定板在固定相机视野内运动，利用标定板的空间运动约束信息，标定得到最优的相机与机器人基座的相对位姿关系。首先通过利用旋转矩阵的线性不变性求解齐次变换矩阵，得到的初步的标定结果；然后将初步标定结果作为优化初值，在像素空间进行优化，使得重投影误差最小。本发明采用了迭代优化算法，无需精密的外部测量设备，利用图像像素空间的模型约束，结合有效的优化初值获取方法，得到了相对高精度的标定结果，可满足工业应用中视觉伺服机器人完成定位和抓取工作的需求。

Description

一种基于像素空间优化的相机与机器人相对位姿标定方法

技术领域

本发明属于先进制造与自动化领域，具体地说，是一种基于像素空间优化的相机与机器人相对位姿标定方法。

背景技术

随着工业机器人的发展，基于视觉伺服的机器人/机械臂作业在越来越多的场景得到应用，是目前“机器换人”和产业转型的核心问题。具备视觉系统的工业机器人可以更加灵活智能的与环境交互，取代人工完成更多的任务，把工人从流水线上的重复工作中解放出来。视觉传感器与机器人空间位姿标定的精度是保证手眼系统可以正常运行的关键，标定的精度决定了视觉伺服机器人定位和抓取的准确度。

相机与机器人之间相对位置和方向关系的求解问题是机器人学与计算机视觉领域的一个基本问题。以往的方法把这类问题归结为求解齐次变换方程AX＝XB，对于此方程的求解一般分为两类：一类是基于旋转矩阵的线性不变性[1]，将方程线性化并采用线性最小二乘求解，此类方法先求解旋转变换部分，再利用旋转变换的结果求解平移变换，这会导致误差传递，因此此类方法对噪声敏感，精度不高，一类是通过迭代优化的方法[2]，此类方法依赖于优化初值的选择，选择不当的初值会使优化结果陷入局部最优，无法得到满意的标定精度。标定数据通过相机来观测标定板的运动(此运动由机器人的运动引起)来获得，标定板上的特征点对应的图像坐标是直接的观测数据，以上方法并没有利用相机的成像信息。

[1]Tsai R Y,Lenz R K,A new technique for fully autonomous andefficient 3D robotics hand/eye calibration,in\emph{IEEE Trans.on Robotics andAutomation},1989,5(3):345-358.

[2]Zuang H,Shiu Y C,A noise-tolerant algorithm for robotic hand-eyecalibration with or without sensor orientation measurement,in\emph{IEEETrans.on Systems,Man and Cybernetics},1993,23(4):1168-1175.

发明内容

本发明的目的是针对现有的机器人与相机标定方法中存在的不足，提出了一种基于像素空间优化的相机与机器人相对位姿标定方法。

本发明是通过以下技术方案来实现的：

本发明的有益效果如下：

本发明无需外部测量设备，借助低成本的标定板完成相机与机器人相对位姿的标定，首先通过利用旋转矩阵的线性不变性求解齐次变换矩阵，得到初步的标定结果。然后将初步标定结果作为优化初值，在像素空间进行优化，使得重投影误差最小，充分利用了图像信息。由于存在两个未知参数，因此采用了迭代优化的方法，假设其中一参数为已知量，求解另一个未知参数，并将求解的结果互相迭代，当标定结果收敛时停止迭代，可得到精确的标定结果，本发明针对工业应用场景的实际需求，提供了高精度的相机与机器人相对位姿标定方法。

本发明无需依赖标定初值的选择，又兼顾了相机与机器人相对位姿标定的精度，使得标定结果不受噪声的影响，克服了传统方法中存在的不足。实验证明应用本发明中的方法得到的标定结果建立机器人和相机之间的位置关系，机器人可以在视觉系统的伺服下完成高精度的抓取任务，平均抓取误差为0.873mm。

附图说明

图1为基于像素空间优化的相机与机器人相对位姿标定方法流程示意图；

图2为适用本发明技术方案的Hand-to-eye手眼标定装置示意图；

图3为本发明技术方案标定过程中运动状态转移示意图；

具体实施方法

本发明公开了一种基于像素空间优化的相机与机器人相对位姿标定方法，图1为基于像素空间优化的相机与机器人相对位姿标定方法流程示意图，具体步骤如下：

(1)固定标定板在机器人末端，控制机器人运动，使得运动停止时，标定板在相机视野内；

(2)当运动停止后，记录机器人末端坐标系到基座坐标系的转换关系T_e ^r，采集图像得到标定板上棋盘格角点像坐标{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}，计算标定板坐标系到相机坐标系的转换关系T_b ^c；

(3)再控制机器人运动n若干次，得到n+1运动停止时的状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}；

(4)根据运动状态转移中相机、标定板、机器人末端与机器人基座坐标系之间的转换关系，建立以下状态转移方程：

(T_b ^c)₍₁₎＝T_r ^c·(T_e ^r)₍₁₎·T_b ^e

(T_b ^c)₍₂₎＝T_r ^c·(T_e ^r)₍₂₎·T_b ^e

(T_b ^c)_(n+1)＝T_r ^c·(T_e ^r)_(n+1)·T_b ^e

T_r ^c是机器人基座坐标系到相机坐标系的齐次转换矩阵，T_b ^e是标定板坐标系到机器人末端坐标系的齐次转换矩阵；

(5)消去T_b ^e，令T_b表示标定板的状态转移矩阵，T_e表示机器人末端的转移矩阵，得到n个齐次转换方程:(T_b)_(i)·T_r ^c＝T_r ^c·(T_e)_(i)，即AX＝XB；

(6)利用旋转矩阵的线性不变性求解齐次转换方程AX＝XB，将求得的T_r ^c'作为优化求解的初始值；

(7)在像素空间迭代优化，得到精确的T_r ^c和T_b ^e，根据相机的成像模型以及方程(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e，可得：

此方程为优化求解的目标方程，其中相机的内参数矩阵是已知参数，迭代优化求解的步骤如下：

(7-1)迭代过程包括两个阶段：标定板到机器人末端位姿关系求解阶段和机器人基座到相机位姿关系求解阶段；

(7-2)将(6)中得到的结果作为初始值(T_r ^c)₍₀₎＝T_r ^c'，将代入方程

(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e得到T_b ^e的初始值(T_b ^e)₍₀₎；

(7-3)标定板到机器人末端位姿关系求解阶段：n+1组状态数据作为输入数据，将(T_r ^c)_(k-1)作为已知量带入优化目标方程，(T_b ^e)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_b ^e)_(k)；

(7-4)机器人基座到相机位姿关系求解阶段：n+1组状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}作为输入数据，将(T_b ^e)_(k)作为已知量带入优化目标方程，(T_r ^c)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_r ^c)_(k)；

(7-5)根据(T_b ^e)_(k)，(T_r ^c)_(k)和(T_b ^e)_(k-1)，(T_r ^c)_(k-1)判断标定结果是否收敛：计算

err＝||(T_r ^c)_(k)-(T_r ^c)_(k-1)||_f+||(T_b ^e)_(k)-(T_b ^e)_(k-1)||_f

||·||_f表示矩阵的Frobenius范数，如果err小于设定的阈值，则停止迭代，得到最终的标定结果T_r ^c＝(T_r ^c)_(k)，即否则置k＝k+1；

下面通过具体实施例对本发明的技术方案作进一步地说明：

图2适用本发明技术方案的Hand-to-eye手眼标定装置示意图，基于像素空间优化相机与机器人相对位姿标定方法的步骤如下：

(1)工业机器人基座固定，相机固定安装在机器人外部工作区域的上方，已知相机的内参数，在机器人末端固定棋盘格标定板，装置图如图2所示；

(2)控制机器人运动，使标定板运动到视野范围内，当运动停止时，可以得到机器人末端坐标系到基座坐标系的转换关系，用齐次变换矩阵T_e ^r表示，包含旋转和平移变换；

(3)相机采集图像，得到棋盘格m个角点在像平面中的坐标

{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}，结合相机内参数计算标定板坐标系到相机坐标系的转换关系，用齐次变换矩阵T_b ^c表示；

(4)再控制机器人进行n(n＞2)次转轴非平行的运动，同样要保证标定板在相机视野范围，每次运动后采集图像，根据步骤(2)(3)可以得到n+1组状态数据{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}；

(5)根据运动状态转移中相机、标定板、机器人末端与机器人基座坐标系之间的转换关系，建立以下状态转移方程：

(T_b ^c)₍₁₎＝T_r ^c·(T_e ^r)₍₁₎·T_b ^e

(T_b ^c)₍₂₎＝T_r ^c·(T_e ^r)₍₂₎·T_b ^e

(T_b ^c)_(n+1)＝T_r ^c·(T_e ^r)_(n+1)·T_b ^e

运动转移状态示意图如图3所示，图3为本发明技术方案标定过程中运动状态转移示意图，其中T_r ^c是机器人基座坐标系到相机坐标系的齐次转换矩阵，T_b ^e是标定板坐标系到机器人末端坐标系的齐次转换矩阵，T_r ^c与T_b ^e都是固定不变的；

(6)将(5)中的方程两两顺序组合，消去T_b ^e，令 T_b表示标定板的状态转移矩阵，T_e表示机器人末端的转移矩阵，可得n个齐次变换方程：(T_b)_(i)·T_r ^c＝T_r ^c·(T_e)_(i)，其中i表示状态索引；

(7)将状态转移矩阵分块表示，齐次变换方程可表示为：

其中R表示旋转矩阵，t表示平移向量。

(8)旋转矩阵R等价的旋转向量表示为：其中θ为旋转角，(n₁,n₂,n₃)^t为转轴方向的单位向量，表示P_r的反对称矩阵，由齐次变换方程可得(R_b)_(i)R_r ^c＝R_r ^c(R_e)_(i)，将其中的旋转矩阵都变换为旋转向量可得n个线性方程Skew((P_b)_(i)+(P_e)_(i))P_r ^c'＝(P_e)_(i)-(P_b)_(i)；

(9)由线性最小二乘算法求解得到P_r ^c'，根据公式求解出P_r ^c，将P_r ^c转换为欧拉角表示的旋转矩阵R_r ^c，再将R_r ^c带回齐次变换方程，求得t_r ^c，将此结果作为迭代优化的初始值；

(10)在像素空间迭代优化，得到精确的T_r ^c和T_b ^e，根据相机的成像模型以及方程(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e，可得：

此方程为优化求解的目标方程，其中相机的内参数矩阵是已知参数，迭代优化求解的步骤如下：

(10-1)迭代过程包括两个阶段：标定板到机器人末端位姿关系求解阶段和机器人基座到相机位姿关系求解阶段；

(10-2)将(9)中得到的结果作为初始值(T_r ^c)₍₀₎＝T_r ^c'，将代入方程

(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e得到T_b ^e的初始值(T_b ^e)₍₀₎；

(10-3)标定板到机器人末端位姿关系求解阶段：n+1组状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}作为输入数据，将(T_r ^c)_(k-1)作为已知量带入优化目标方程，(T_b ^e)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_b ^e)_(k)；

(10-4)机器人基座到相机位姿关系求解阶段：n+1组状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}作为输入数据，将(T_b ^e)_(k)作为已知量带入优化目标方程，(T_r ^c)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_r ^c)_(k)；

(10-5)根据(T_b ^e)_(k)，(T_r ^c)_(k)和(T_b ^e)_(k-1)，(T_r ^c)_(k-1)判断标定结果是否收敛：计算

err＝||(T_r ^c)_(k)-(T_r ^c)_(k-1)||_f+||(T_b ^e)_(k)-(T_b ^e)_(k-1)||_f

||·||_f表示矩阵的Frobenius范数，如果err小于设定的阈值，则停止迭代，得到最终的标定结果T_r ^c＝(T_r ^c)_(k)，否则置k＝k+1。

以上例举的仅是本发明的优选实施方式，本发明并不限于以上实施例，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围内。

Claims (3)

1.一种基于像素空间优化的相机与机器人相对位姿标定方法，其特征在于，机器人在末端固定标定板，通过运动控制使标定板在固定安装的相机视野下摆放不同位姿，结合各个姿态下已知的机器人末端运动信息和标定板在图像中的识别结果，迭代优化相机与机器人基座和标定板与机器人末端两个参数，最终获得精确的相机与机器人基座相对位姿关系，具体标定步骤如下：

1)、固定标定板在机器人末端，控制机器人运动，使得运动停止时，标定板在相机视野内；

2)、当运动停止后，记录机器人末端坐标系到基座坐标系的转换关系T_e ^r，采集图像得到标定板上棋盘格角点像坐标{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}，计算标定板坐标系到相机坐标系的转换关系T_b ^c；

3)、再控制机器人运动n次，得到n+1运动停止时的状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}；

4)、根据运动状态转移中相机、标定板、机器人与机器人基座坐标系之间的转换关系，建立以下状态转移方程：

(T_b ^c)₍₁₎＝T_r ^c·(T_e ^r)₍₁₎·T_b ^e

(T_b ^c)₍₂₎＝T_r ^c·(T_e ^r)₍₂₎·T_b ^e

(T_b ^c)_(n+1)＝T_r ^c·(T_e ^r)_(n+1)·T_b ^e

T_r ^c是机器人基座坐标系到相机坐标系的齐次转换矩阵，T_b ^e是标定板坐标系到机器人末端坐标系的齐次转换矩阵；

5)、消去T_b ^e，令T_b表示标定板的状态转移矩阵，T_e表示机器人末端的转移矩阵，得到n个齐次转换方程:(T_b)_(i)·T_r ^c＝T_r ^c·(T_e)_(i)，即AX＝XB；

6)、利用旋转矩阵的线性不变性求解齐次转换方程AX＝XB，将求得的T_r ^c'作为优化求解的初始值；

7)、在像素空间迭代优化，得到精确的T_r ^c和T_b ^e，根据相机的成像模型以及方程(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e，可得：

$s\left[\begin{array}{c}{u_j}^{\′}\\ {v_j}^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& c_x& 0\\ 0& f_y& c_y& 0\\ 0& 0& 1& 0\end{array}\right]{T_r}^c\·{\left({T_e}^r\right)}_{\left(i\right)}\·{T_b}^e$

此方程为优化求解的目标方程，其中相机的内参数矩阵是已知参数。

2.根据权利要求1所述的基于像素空间优化的相机与机器人相对位姿标定方法，其特征在于，所述的步骤7)中的迭代优化过程包括两个阶段：标定板到机器人末端位姿关系求解阶段和机器人基座到相机位姿关系求解阶段。

3.根据权利要求1或2所述的基于像素空间优化的相机与机器人相对位姿标定方法，其特征在于，所述的迭代优化求解的具体步骤如下：

1)、将权利要求1中步骤6)中得到的结果作为初始值(T_r ^c)₍₀₎＝T_r ^c'，将代入方程(T_b ^c)_(i)＝T_r ^c·(T_e ^r)_(i)·T_b ^e得到T_b ^e的初始值(T_b ^e)₍₀₎；

2)、标定板到机器人末端位姿关系求解阶段：n+1组状态数据作为输入数据，将(T_r ^c)_(k-1)作为已知量带入优化目标方程，(T_b ^e)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_b ^e)_(k)；

3)、机器人基座到相机位姿关系求解阶段：n+1组状态数据

{T_e ^r,T_b ^c,{(u₁,v₁),(u₂,v₂),...,(u_m,v_m)}}_{(1,2,...,n+1)}作为输入数据，将(T_b ^e)_(k)作为已知量带入优化目标方程，(T_r ^c)_(k-1)作为优化的初始值，优化的目标为最小化重投影误差其中(u_j,v_j)为观测角点像坐标，(u_j',v_j')为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解(T_r ^c)_(k)；

4)、根据(T_b ^e)_(k)，(T_r ^c)_(k)和(T_b ^e)_(k-1)，(T_r ^c)_(k-1)判断标定结果是否收敛：计算

err＝||(T_r ^c)_(k)-(T_r ^c)_(k-1)||_f+||(T_b ^e)_(k)-(T_b ^e)_(k-1)||_f

||·||_f表示矩阵的Frobenius范数，如果err小于设定的阈值，则停止迭代，得到最终的标定结果T_r ^c＝(T_r ^c)_(k)，否则置k＝k+1。