一、概論

現(xiàn)在的機器人少不了有各種傳感器，傳感器之間的標定是機器人感知環(huán)境的一個重要前提。所謂標定，是指確定傳感器之間的坐標轉(zhuǎn)換關系。由于標定的傳感器各異，好像沒有特別通用的方法。

手眼標定法是標定攝像頭與機械臂的一個經(jīng)典方法，不過這個思想也適用于其他傳感器，比如自動駕駛中激光雷達與攝像頭之間的標定，比如東京大學的這篇工作《LiDAR and Camera Calibration using Motion Estimated by Sensor Fusion Odometry》。

手眼標定法的核心公式只有一個，??，這里的 X 就是指手（機械臂末端）與眼（攝像頭）之間的坐標轉(zhuǎn)換關系。下面結(jié)合機械臂的兩種使用場景，講一下這個公式的由來。

用Base表示機械臂的底座（可以認為是世界坐標系），用End表示機械臂的末端，用Camera表示攝像頭，用Object表示標定板。

Eye-In-Hand

所謂Eye-In-Hand，是指攝像頭被安裝在機械臂上。此時要求取的是，End到Camera之間的坐標轉(zhuǎn)換關系，也就是。這種情況下，有兩個變量是不變的：

攝像頭與機械臂末端之間的坐標轉(zhuǎn)換關系不變，也就是說，??始終不變；

標定板與機械臂底座之間的坐標轉(zhuǎn)換關系不變，也就是說，??也是不變的。

把按照前后兩次運動展開，有

記

就得到了

也就是，如果能夠計算移動前后，機械臂末端的坐標變換關系??以及相機的坐標變換關系??，即可求出機械臂末端到相機之間的坐標變換關系??。

Eye-To-Hand

所謂Eye-To-Hand，是指攝像頭被安裝在一個固定不動的位置，而標定板被拿在機械臂手上。此時要求取的是，Base到Camera之間的坐標轉(zhuǎn)換關系，也就是。這種情況下，有兩個變量是不變的：

攝像頭與機械臂底座之間的坐標轉(zhuǎn)換關系不變，也就是說，??始終不變；

標定板與機械臂末端之間的坐標轉(zhuǎn)換關系不變，也就是說，??始終不變。

把按照前后兩次運動展開，有

記

就得到了

本文主要是講解經(jīng)典手眼標定問題中的TSAI-LENZ 文獻方法，參考文獻為“A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration”，并且實現(xiàn)了基于OpenCV的C++代碼程序

二、Eye in hand 手眼標定問題

? ? ? ?在機器人校準測量、機器人手眼協(xié)調(diào)以及機器人輔助測量等領域，都要求知道機器人執(zhí)行器末端（抓取臂）坐標系和傳感器（比如用來測量三維空間中目標位置和方向并固定在機器人手上的攝像機）坐標系之間的相互關系，確定這種轉(zhuǎn)換關系在機器人領域就是通常所說的手眼標定。

? ? ? ?將手眼標定系統(tǒng)如下圖所示，其中Hgi為機器人執(zhí)行器末端坐標系之間相對位置姿態(tài)的齊次變換矩陣；Hcij為攝像機坐標系之間相對位置姿態(tài)的齊次變換矩陣；Hcg為像機與機器人執(zhí)行器末端之間的相對位置姿態(tài)齊次矩陣。

? ? ? ?經(jīng)過坐標系變換，Hgij、Hcij和Hcg滿足上文所述的AX=XB關系：

? ? ? ?將上式展開，可以得到手眼標定的基本方程：

? ? ? ?因此，手眼標定問題也就轉(zhuǎn)化為從上述方程組中求解出RcgRcg和TcgTcg，下面就按照TSAI文獻所述求解該方程組。

三、“兩步法”手眼標定

? ? ? ?一般用“兩步法”求解基本方程，即先從基本方程上式求解出Rcg，再代入下式求解出Tcg。在TSAI文獻中引入旋轉(zhuǎn)軸-旋轉(zhuǎn)角系統(tǒng)來描述旋轉(zhuǎn)運動來進行求解該方程組，具體的公式推導可以查看原始文獻，這里只歸納計算步驟，不明白的地方可閱讀文獻，計算步驟如下：

Step1：利用羅德里格斯變換將旋轉(zhuǎn)矩陣轉(zhuǎn)換為旋轉(zhuǎn)向量

Step2：向量歸一化

Step3：修正的羅德里格斯參數(shù)表示姿態(tài)變化

Step4：計算初始旋轉(zhuǎn)向量P′cg

????其中，skew為反對稱運算，假設一個三維向量V=[vx;vy;vz]，其反對稱矩陣為：

Step5：計算旋轉(zhuǎn)向量Pcg

Step6：計算旋轉(zhuǎn)矩陣Rcg

Step7：計算平移向量TcgTcg

四、MATLAB算法源代碼

根據(jù)上述基本計算步驟，MATLAB實現(xiàn)代碼為：

代碼1：tsai函數(shù)（求解AX=XB）

?

function X = tsai(A,B)
% Calculates the least squares solution of
% AX = XB
% 
% A New Technique for Fully Autonomous 
% and Efficient 3D Robotics Hand/Eye Calibration
% Lenz Tsai
%
% Mili Shah
% July 2014


[m,n] = size(A); n = n/4;
S = zeros(3*n,3);
v = zeros(3*n,1);
%Calculate best rotation R
for i = 1:n
    A1 = logm(A(1:3,4*i-3:4*i-1)); 
    B1 = logm(B(1:3,4*i-3:4*i-1));
    a = [A1(3,2) A1(1,3) A1(2,1)]'; a = a/norm(a);
    b = [B1(3,2) B1(1,3) B1(2,1)]'; b = b/norm(b);
    S(3*i-2:3*i,:) = skew(a+b);
    v(3*i-2:3*i,:) = a-b;
end
x = Sv;
theta = 2*atan(norm(x));
x = x/norm(x);
R = (eye(3)*cos(theta) + sin(theta)*skew(x) + (1-cos(theta))*x*x')';
%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);
for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end
t = Cd;
%Put everything together to form X
X = [R t;0 0 0 1];

?

代碼2：skew函數(shù)（求向量反對稱矩陣）

?

function Sk = skew( x )
%SKEW 此處顯示有關此函數(shù)的摘要
%   此處顯示詳細說明
   Sk = [0,-x(3),x(2);x(3),0,-x(1);-x(2),x(1),0];
end

?

代碼3：計算手眼矩陣Tm

?

clc
clear
close all


%%%%%%%%%%%%%%%%%%%%%%% T6矩陣參數(shù)%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%位姿1的時候機器人末端相對于機器人基坐標系下變換矩陣
Pose1=[1141.243,-15.261,-97.721,178.91,0.47,92.37];
Px = Pose1(1);
Py = Pose1(2);
Pz = Pose1(3);
rota = Pose1(4)*pi/180;
rotb = Pose1(5)*pi/180;
rotc = Pose1(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R1 = Rz*Ry*Rx;
T1= [Px Py Pz]';
%%%%%%%%%%位姿2的時候機器人末端相對于機器人基坐標系下變換矩陣
Pose2=[1103.946,-163.910,-107.673,-160.90,-0.14,-91.62];
Px = Pose2(1);
Py = Pose2(2);
Pz = Pose2(3);
rota = Pose2(4)*pi/180;
rotb = Pose2(5)*pi/180;
rotc = Pose2(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R2 = Rz*Ry*Rx;
T2= [Px Py Pz]';
%%%%%%%%%%位姿3的時候機器人末端相對于機器人基坐標系下變換矩陣
Pose3=[1073.714,2.669,-142.448,-142.86,0.84,-178.55];
Px = Pose3(1);
Py = Pose3(2);
Pz = Pose3(3);
rota = Pose3(4)*pi/180;
rotb = Pose3(5)*pi/180;
rotc = Pose3(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R3 = Rz*Ry*Rx;
T3= [Px Py Pz]';
%%%%%%%%%位姿1,2,3時候機器人末端相對于機器人基坐標系下變換矩陣
T61=[R1 T1;0 0 0 1]   ;      
T62=[R2 T2;0 0 0 1];
T63=[R3 T3;0 0 0 1];


%%%%%%攝像機外參數(shù)矩陣（平面靶標在攝像機坐標系下表示）%%%%%%%%
Extrinsic1=[0.051678,-0.998634,0.007660,21.747985;
         -0.998617,-0.051600,0.010060,27.391246;
        -0.009651,-0.008169,-0.999920,319.071378];%%%3行4列矩陣
Extrinsic2=[0.014949,0.999738,0.017361,-35.869608 
        0.949779,-0.019626,0.312304,-20.701811
        0.312563,0.011821,-0.949823,306.463155];
Extrinsic3=[0.999176,0.039246,0.010343,-26.361812
        0.025037,-0.796606,0.603980,20.533884
        0.031943,-0.603223,-0.796933,318.110756];


%%%%%%%
TC1=[Extrinsic1; 0 0 0 1];     
TC2=[Extrinsic2; 0 0 0 1];
TC3=[Extrinsic3; 0 0 0 1];


TL1=inv(T61)*T62;
TL2=inv(T62)*T63;
 
TR1=TC1*inv(TC2);
TR2=TC2*inv(TC3);


A=[TL1,TL2]
B=[TR1,TR2]
X=?tsai(A,B)

?

結(jié)果如下：

?

A =
   -0.9976    0.0676   -0.0173 -146.8929    0.0535   -0.7980    0.6003 -165.7422
   -0.0697   -0.9488    0.3082  -43.6165    0.9483    0.2289    0.2197   44.2528
    0.0044    0.3087    0.9512   10.3295   -0.3127    0.5575    0.7690   21.0485
         0         0         0    1.0000         0         0         0    1.0000
B =
   -0.9975    0.0711   -0.0029  -11.6622    0.0544   -0.7855   -0.6164  177.7842
   -0.0663   -0.9443   -0.3223  104.2345    0.9515    0.2280   -0.2067   65.4536
   -0.0257   -0.3213    0.9466   21.3907    0.3029   -0.5753    0.7598   84.5619
         0         0         0    1.0000         0         0         0    1.0000
X =
   -0.9998    0.0187   -0.0076  -78.8694
   -0.0187   -0.9998   -0.0073   14.2711
   -0.0078   -0.0071    0.9999 -124.6709
         0         0         0    1.0000

?

五、C++算法源代碼

在利用OpenCV 2.0開源庫的基礎上，編寫Tsai手眼標定方法的c++程序，其實現(xiàn)函數(shù)代碼如下：

代碼1：Tsai_HandEye函數(shù)，求解AX=XB

?

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(3, 0) = 0.0;
  Hcg.at(3, 1) = 0.0;
  Hcg.at(3, 2) = 0.0;
??Hcg.at(3,?3)?=?1.0;
}

?

代碼2：skew函數(shù)（將3x1向量轉(zhuǎn)換成3x3反對稱矩陣）

?

Mat skew(Mat A)
{
  CV_Assert(A.cols == 1 && A.rows == 3);
  Mat B(3, 3, CV_64FC1);


  B.at(0, 0) = 0.0;
  B.at(0, 1) = -A.at(2, 0);
  B.at(0, 2) = A.at(1, 0);


  B.at(1, 0) = A.at(2, 0);
  B.at(1, 1) = 0.0;
  B.at(1, 2) = -A.at(0, 0);


  B.at(2, 0) = -A.at(1, 0);
  B.at(2, 1) = A.at(0, 0);
  B.at(2, 2) = 0.0;


  return B;
}

?

代碼3：計算手眼矩陣Tm

?

#include??//頭文件
#include 
#include 
#include 
using namespace std;
using namespace cv; //包含cv命名空間
int?main()
{
    double a1[4][4] = { -0.9976,0.0676, - 0.0173, - 146.8929,
      -0.0697 ,- 0.9488 ,   0.3082 ,- 43.6165,
      0.0044 ,    0.3087,    0.9512 ,  10.3295,
      0 ,   0  ,   0 ,  1.0000 };
    double a2[4][4] = { 0.0535, - 0.7980,    0.6003 ,-165.7422,
      0.9483 , 0.2289, 0.2197, 44.2528,
      -0.3127,0.5575,0.7690, 21.0485,
      0, 0, 0, 1 };
  
    double b1[4][4] = { -0.9975,  0.0711, - 0.0029 ,- 11.6622,
      -0.0663, - 0.9443, - 0.3223,  104.2345,
      -0.0257, - 0.3213,    0.9466 ,  21.3907,
      0, 0, 0, 1 };
    double b2[4][4] = { 0.0544, - 0.7855, - 0.6164,  177.7842,
      0.9515,    0.2280 ,- 0.2067,   65.4536,
      0.3029, - 0.5753,    0.7598 ,  84.5619,
      0, 0, 0, 1 };
  
    Mat A1(4, 4, CV_64FC1, a1);
    Mat A2(4, 4, CV_64FC1, a2);
    Mat B1(4, 4, CV_64FC1, b1);
    Mat B2(4, 4, CV_64FC1, b2);
  
    vector Hgij;
    vector Hcij;
  
    Hgij.push_back(A1);
    Hgij.push_back(A2);
    Hcij.push_back(B1);
    Hcij.push_back(B2);
  
    Mat Hcg1(4, 4, CV_64FC1);
    Tsai_HandEye(Hcg1, Hgij, Hcij);
}

?

輸出結(jié)果如下：

?

　　審核編輯：湯梓紅