手眼标定Tsai方法的Matlab仿真分析

大家好，又见面了，我是你们的朋友全栈君。

手眼标定方程推导
手眼标定求解：Tsai方法
基于上面两篇手眼标定的博文，相信有很多朋友在实验过程中发现精度不是那么的如意，毕业工作第一年就开始接触手眼标定，刚开始也是标定效果不好不知道问题出在哪里，后来从最基础的理论知识入手进行一些实验，记得刚开始做实验用的是UR5机械臂，根据手眼标定结果进行物体抓取，抓取效果还是很准确的，后来公司开发自己的机械臂进行同样的实验(机械臂连杆是3D打打印件，精度必定是比较差的)，标定效果却是非常不理想。使用Tsai方法求解标定方程文章中根据作者论文对误差影响做了一些分析，下面使用Tsai求解方法进行一些Matlab仿真分析。

1. 构造仿真数据
   若读者手上没有实体机械臂也可以进行算法分析，方法是假设已知手眼转换矩阵，手眼方程为AX=XB，X就是已知的手眼矩阵，假设我们给定A，那我们就可以得到 B = inv( X ) * A* X，A就是每组运动中机械臂末端姿态变换矩阵，根据A和B使用Tsai方法再求解手眼转换矩阵X1，然后比较X和X1间的误差，然后我们在A矩阵中加入一些噪声，再求解手眼矩阵X2，然后比较X2与X间的误差。现在我们构造一些用于仿真分析的数据，矩阵A我们使用欧拉角转换而来，这样我们可以想象一下机械臂进行一次运动的运动轨迹，构造数据Matalb代码如下：

根据欧拉角计算旋转矩阵：

function M = EulerTomatrix(roll, pich, yaw)
%zyx
    m1 = [ 1.0,    0.0,            0.0;
               0.0,    cos(roll),    -sin(roll);
               0.0,    sin(roll),     cos(roll)];

    m2 = [ cos(pich),   0.0,   sin(pich);
               0.0,            1.0,      0.0;
              -sin(pich),     0.0,    cos(pich);];

    m3 = [ cos(yaw),   -sin(yaw),     0.0;
               sin(yaw),    cos(yaw),     0.0;
               0.0,                 0.0,         1.0];
    M = m3*m2*m1;
end

Skew.m

function S = Skew( P )
    S = zeros(3,3);
    S(1,2) = -P(3);
    S(1,3) = P(2);
    
    S(2,1) = P(3);
    S(2,3) = -P(1);
    
     S(3,1) = -P(2);
    S(3,2) = P(1);
end

产生4组用于标定的数据 LoadData.m：

clear all
close all

%假设已知的手眼转换矩阵为Hx
Hx = eye(4,4);
Hx(1:3, 1:3) = EulerTomatrix(pi/2, 0.3, 0.5);
Hx(1:3, 4) = [0.01, 0.02, 0.03]'

HA = zeros(4,4, 4); %构造4组用于标定的A矩阵
EHA = zeros(4,4, 4);%在A矩阵中加入噪声的矩阵

for i = 1:size(HA,3)
    HA(:, :, i) = eye(4,4);
end

%第一组：机械臂绕x轴旋转0.8*pi，平移量为[0.0, 0.01, 0.01]
HA(1:3, 1:3, 1) = EulerTomatrix(0.8*pi, 0.0, 0.0);
HA(1:3, 4, 1) = [0.0, 0.01, 0.01]';

%在第一组矩阵A中旋转角度加入0.02rad的噪声，下面的EHA同理
EHA(1:3, 1:3, 1) = EulerTomatrix(0.8*pi+0.02, 0.0, 0.0);
EHA(1:3, 4, 1) = [0.0, 0.01+0.001, 0.01]';

%第二组：机械臂绕y轴旋转0.5*pi，平移量为[0.01, 0.0, 0.0]
HA(1:3, 1:3, 2) = EulerTomatrix(0.0, 0.5*pi, 0.0);
HA(1:3, 4, 2) = [0.01, 0.0, 0.0]';

EHA(1:3, 1:3, 2) = EulerTomatrix(0.0, 0.5*pi+ 0.05, 0.0);
EHA(1:3, 4, 2) = [0.01+0.001, 0.0, 0.0]';

%第三组：机械臂绕z轴旋转0.7*pi，平移量为[0.0, 0.0, 0.02]
HA(1:3, 1:3, 3) = EulerTomatrix(0.0, 0.0, 0.7*pi);
HA(1:3, 4, 3) = [0.0, 0.0, 0.02]';

EHA(1:3, 1:3, 3) = EulerTomatrix(0.0, 0.0, 0.7*pi+0.01);
EHA(1:3, 4, 3) = [0.0, 0.0, 0.02+0.001]';

%第四组：机械臂绕x轴旋转pi/2，绕y轴旋转0.3，平移量为[0.1, 0.0001, 0.25]
HA(1:3, 1:3, 4) = EulerTomatrix(pi/2, 0.3, 0.0);
HA(1:3, 4, 4) = [0.1, 0.0001, 0.25]';

EHA(1:3, 1:3, 4) = EulerTomatrix(pi/2+0.06, 0.3+0.05, 0.0);
EHA(1:3, 4, 4) = [0.1, 0.0001, 0.25]';

%根据无噪声的矩阵A和已知的手眼矩阵计算矩阵B，然后就得到了用于仿真的数据
%敢兴趣的朋友根据类似的方法生产更多的数据
HB = zeros( size(HA) );
for i = 1:size(HA,3)
    HB(:, :, i) = inv(Hx)*  HA(:, :, i) * Hx;
end

2. Tsai手眼标定方法Matlab实现

function Hx = HandEyeTsai(HA, HB, num)
%num为使用输入数据中多少组数据进行标定

%Hx用于返回手眼标定结果
Hx = eye(4,4);

%根据Tsai方法生成最小二乘法求解的方程Ax = B，求解手眼矩阵的旋转矩阵
A = zeros(num*3, 3);
B = zeros(num*3, 1);

for i = 1:num
   %使用罗德里格斯计算旋转矩阵对应的旋转轴
    rgij = rodrigues( HA(1:3, 1:3, i) );        
    rcij = rodrigues( HB(1:3, 1:3, i) ); 
    
    theta_gij = norm(rgij);
    theta_cij = norm(rcij);                        
    rngij = rgij/theta_gij;
    rncij = rcij/theta_cij;
    
    %Tsai方法里定义的旋转轴
    Pgij = 2*sin(theta_gij/2)*rngij;
    Pcij = 2*sin(theta_cij/2)*rncij; 

   %构造求解手眼旋转矩阵对应旋转轴的求解方程
    S = Skew( Pgij + Pcij );
    b = Pcij - Pgij;
    
    A(3*i-2 : 3*i, 1:3) = S;
    B(3*i-2 : 3*i, 1) = b;
end
 
 %最小二乘法求解旋转矩阵对应的旋转轴，然后根据Tsai的定义计算旋转矩阵
  Pce1 = inv(A'*A)*A'*B;
  Pce   =  2*Pce1/sqrt( 1 + norm(Pce1) * norm(Pce1) ) ;
  I = eye(3,3);
  np2 = norm(Pce) * norm(Pce);
  %得到手眼关系的旋转矩阵
  Rx = ( 1 - np2*0.5 )*I + 0.5*( Pce*Pce' + sqrt(4 - np2)* Skew(Pce) );
  
  %构造求解手眼关系平移量的方程Ax = B
A = zeros(num*3, 3);
B = zeros(num*3, 1);
  for i = 1:num
   RA =  HA(1:3, 1:3, i);
   TA =  HA(1:3, 4, i);
   
   TB =  HB(1:3, 4,i);
   
   A(3*i-2 : 3*i, 1:3) = RA - I;
   B(3*i-2 : 3*i, 1) = Rx * TB - TA;
  end
  %最小二乘法求解手眼关系的平移量
Tx = inv(A'*A)*A'*B;
Hx(1:3,1:3) = Rx;
Hx(1:3, 4) = Tx;

3. 简单实验分析

close all
clear all

%构造数据
LoadData

%使用无噪声的数据求解手眼矩阵
Hce = HandEyeTsai(HA, HB,4)

%使用有噪声的数据求解手眼矩阵
Hce2 = HandEyeTsai(EHA, HB,4)

%以下是我计算得到的数据

%假设已知的手眼关系
Hx =

    0.8384    0.2593    0.4794    0.0100
    0.4580    0.1417   -0.8776    0.0200
   -0.2955    0.9553    0.0000    0.0300
         0         0         0    1.0000

%无噪声的数据求解得到的手眼矩阵
Hce =

    0.8384    0.2593    0.4794    0.0100
    0.4580    0.1417   -0.8776    0.0200
   -0.2955    0.9553   -0.0000    0.0300
         0         0         0    1.0000

%添加噪声的数据求解得到的手眼矩阵
Hce2 =

    0.8339    0.2661    0.4834    0.0098
    0.4612    0.1450   -0.8754    0.0203
   -0.3031    0.9530   -0.0018    0.0291
         0         0         0    1.0000

从以上计算结果可以看到，使用无噪声的数据求解得到的手眼矩阵与给定的手眼矩阵之间是无误差的，当在矩阵A中添加一定的噪声的时候可以看到有噪声的数据得到的手眼矩阵与给定的手眼矩阵之间是有一定误差的，矩阵A中添加的噪声就相当于机械臂末端位姿存在误差然后对手眼标定结果的影响，篇幅有限，感兴趣的读者可以根据类似的方法进行更多的分析。
rodrigues.m