机器人手眼标定Matlab程序高精度

最近做科研用到手眼系统标定，在网上搜索方法无果，于是自己亲自编写手眼标定程序，经验证，具有较高精度。

clearvars -except  cameraParams  estimationerrors  REs ;  clc;
%% 
img_num = 18;
RobotEffectorPose;      %机器人末端位姿
%% 
TE = estimationerrors.ExtrinsicsErrors.TranslationVectorsError;
Tex = mean（TE（:1））;   Tey = mean（TE（:2））;  Tez = mean（TE（:3））;
errorT = [TexTeyTez];
%% 求末端和摄像机转轴kl kr.
kl=[]; kr=[]; theta_r=[]; theta_l=[];
for i = 1:img_num-2
    %% 求摄像机转轴Kr
    Rr1 = cameraParams.RotationMatrices（::i）;
    Tr1 = cameraParams.TranslationVectors（i:）+errorT;
    Rr2 = cameraParams.RotationMatrices（::i+1）;
    Tr2 = cameraParams.TranslationVectors（i+1:）+errorT;
    Rr3 = cameraParams.RotationMatrices（::i+2）;
    Tr3 = cameraParams.TranslationVectors（i+2:）+errorT;
    Rrt1 = [Rr1‘ （Tr1/1000）‘;0001]/[Rr2‘ （Tr2/1000）‘;0001];
    Rrt2 = [Rr2‘ （Tr2/1000）‘;0001]/[Rr3‘ （Tr3/1000）‘;0001];
    [fr1 thetar1] = InvRot（Rrt1）;
    [fr2 thetar2] = InvRot（Rrt2）;
    kr = [krfr1fr2cross（fr1fr2）];  theta_r = [theta_r;thetar1;thetar2];
    %% 求末端转轴Kl
    Re1 = REs（4*i-3:4*i:）;
    Re2 = REs（4*（i+1）-3:4*（i+1）:）; 
    Re3 = REs（4*（i+2）-3:4*（i+2）:）; 
    Ret1 = Re1\Re2;
    Ret2 = Re2\Re3;
    Rlt1 = Ret1（1:31:3）;
    Rlt2 = Ret2（1:31:3）;
    [fl1 thetal1] = InvRot（Rlt1）;
    [fl2 thetal2] = InvRot（Rlt2）;
    kl = [klfl1fl2cross（fl1fl2）];  theta_l = [theta_l;thetal1;thetal2];
end
%% 求手眼矩阵旋转关系
Rm = kl*pinv（kr）;      %-----------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 求手眼关系平移向量
Prs=[]; Pls=[];
for i = 1:img_num-1
%% （E-Rli）*Pm = Pli-Rm*Pri ----------构造右端    
    %% 末端平移向量
    TRl1 = REs（4*i-3:4*i:）; 
    TRl2 = REs（4*（i+1）-3:4*（i+1）:）;   
    Rl = TRl1\TRl2;
    Pli = Rl（1:34）;            %--------------Pli
    %% 相机外参平移向量
    TRr1 = cameraParams.RotationMatrices（::i）;
    TTr1 = cameraParams.TranslationVectors（i:）+errorT;
    TRr2 = cameraParams.RotationMatrices（::i+1）;
    TTr2 = cameraParams.TranslationVectors（i+1:）+errorT;
    TRlt = [TRr1‘ （TTr1/1000）‘;0001]/[TRr2‘ （TTr2/1000）‘;0001];
    Pri = TRlt（1:34）;
    Pr = Pli - Rm*Pri;
    Prs = [Prs;Pr];    %--------------存储
%% （E-Rli）*Pm = Pli-Rm*Pri ----------构造左端 
    E=[100;010;001];
    Rli = Rl（1:31:3）;
    Pl = E - Rli;
    Pls = [Pls;Pl];   %--------------存储
end
%% 求手眼矩阵平移向量
Pm = pinv（Pls）*Prs;  %-----------------------------------------
H_E = [RmPm;0001]
%clearvars -except  cameraParams  estimationerrors  H_E ;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2645  2018-11-30 14:38  HandEyeCalib-Matlab.m

     文件        169  2018-12-05 18:23  说明.txt

----------- ---------  ---------- -----  ----

                 2814                    2