本文主要是讲解经典手眼标定问题中的TSAI-LENZ 文献方法,参考文献为“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”,并且实现了基于OpenCV的C++代码程序,code可去CSDN资源下载,MATLAB版本作者为苏黎世理工的Christian Wengert,也可在此处下载。
在机器人校准测量、机器人手眼协调以及机器人辅助测量等领域,都要求知道机器人执行器末端(抓取臂)坐标系和传感器(比如用来测量三维空间中目标位置和方向并固定在机器人手上的摄像机)坐标系之间的相互关系,确定这种转换关系在机器人领域就是通常所说的手眼标定。
将手眼标定系统如下图所示,其中 Hgij 为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵; Hcij 为摄像机坐标系之间相对位置姿态的齐次变换矩阵; Hcg 为像机与机器人执行器末端之间的相对位置姿态齐次矩阵。
经过坐标系变换, Hgij 、 Hcij 和 Hcg 满足如下关系:
一般用“两步法”求解基本方程,即先从基本方程上式求解出 Rcg ,再代入下式求解出 Tcg 。在TSAI文献中引入旋转轴-旋转角系统来描述旋转运动来进行求解该方程组,具体的公式推导可以查看原始文献,这里只归纳计算步骤,不明白的地方可阅读文献,计算步骤如下:
其中,skew为反对称运算,假设一个三维向量 V=[vx;vy;vz] ,其反对称矩阵为:
根据上述基本计算步骤,在利用OpenCV 2.0开源库的基础上,编写Tsai手眼标定方法的c++程序,其实现函数代码如下:
void Tsai_HandEye(Mat Hcg, vector Hgij, vector Hcij)
{
CV_Assert(Hgij.size() == Hcij.size());
int nStatus = Hgij.size();
Mat Rgij(3, 3, CV_64FC1);
Mat Rcij(3, 3, CV_64FC1);
Mat rgij(3, 1, CV_64FC1);
Mat rcij(3, 1, CV_64FC1);
double theta_gij;
double theta_cij;
Mat rngij(3, 1, CV_64FC1);
Mat rncij(3, 1, CV_64FC1);
Mat Pgij(3, 1, CV_64FC1);
Mat Pcij(3, 1, CV_64FC1);
Mat tempA(3, 3, CV_64FC1);
Mat tempb(3, 1, CV_64FC1);
Mat A;
Mat b;
Mat pinA;
Mat Pcg_prime(3, 1, CV_64FC1);
Mat Pcg(3, 1, CV_64FC1);
Mat PcgTrs(1, 3, CV_64FC1);
Mat Rcg(3, 3, CV_64FC1);
Mat eyeM = Mat::eye(3, 3, CV_64FC1);
Mat Tgij(3, 1, CV_64FC1);
Mat Tcij(3, 1, CV_64FC1);
Mat tempAA(3, 3, CV_64FC1);
Mat tempbb(3, 1, CV_64FC1);
Mat AA;
Mat bb;
Mat pinAA;
Mat Tcg(3, 1, CV_64FC1);
for (int i = 0; i < nStatus; i++)
{
Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
Rodrigues(Rgij, rgij);
Rodrigues(Rcij, rcij);
theta_gij = norm(rgij);
theta_cij = norm(rcij);
rngij = rgij / theta_gij;
rncij = rcij / theta_cij;
Pgij = 2 * sin(theta_gij / 2)*rngij;
Pcij = 2 * sin(theta_cij / 2)*rncij;
tempA = skew(Pgij + Pcij);
tempb = Pcij - Pgij;
A.push_back(tempA);
b.push_back(tempb);
}
//Compute rotation
invert(A, pinA, DECOMP_SVD);
Pcg_prime = pinA * b;
Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
PcgTrs = Pcg.t();
Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));
//Computer Translation
for (int i = 0; i < nStatus; i++)
{
Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);
tempAA = Rgij - eyeM;
tempbb = Rcg * Tcij - Tgij;
AA.push_back(tempAA);
bb.push_back(tempbb);
}
invert(AA, pinAA, DECOMP_SVD);
Tcg = pinAA * bb;
Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));
Hcg.at<double>(3, 0) = 0.0;
Hcg.at<double>(3, 1) = 0.0;
Hcg.at<double>(3, 2) = 0.0;
Hcg.at<double>(3, 3) = 1.0;
}