描述

使用matlab求解AX=XB的问题

可以正确求出X的值，我已经成功验证了

至于说X具体代表什么，要结合你自己推导出的公式AX=XB之中，A、X、B分别代表什么

代码

main.m

包含我做手眼标定时的14组A和B
然后使用X= tsai(A,B)一句话就可以求解出手眼矩阵X

clc;
clear;
num = 14;
A=zeros(4,4,num);
B=zeros(4,4,num);
A(:,:,1)=[0.9997980101273221, -0.01607111192618671, -0.01206935570872313, -0.003056556362190066;
 0.01674895934272706, 0.9981564074674805, 0.05833747642946587, 0.04142996722424749;
 0.01110954509912888, -0.0585278414516273, 0.9982239653950075, 0.009770159468120519;
 0, 0, 0, 1];
B(:,:,1)=[1, -0.011, -0.007, 0.001496;
 0.011, 0.998, 0.055, -0.042174;
 0.006, -0.055, 0.998, -0.012222;
 0, 0, 0, 1];
A(:,:,2)=[0.9980680730327043, 0.0007536742026831328, 0.06212542640675847, 0.01423670063468297;
 -0.006760465497263591, 0.9953066584308737, 0.09653467238220965, 0.01882418647028945;
 -0.06176110098209641, -0.09676817546987918, 0.9933888980914715, 0.01173102823510962;
 0, 0, 0, 1];
B(:,:,2)=[0.998, 0.004, 0.06900000000000001, -0.012265;
 -0.01, 0.996, 0.09, -0.011046;
 -0.068, -0.091, 0.994, 0.003755;
 0, 0, 0, 1];
A(:,:,3)= [0.9961645965748482, -0.01927828468436444, 0.08534896601555519, 0.02670399624248279;
 0.009180953127219364, 0.9930713156194926, 0.1171540252797969, 0.02821974684234077;
 -0.08701614734992202, -0.1159211187333192, 0.9894394867308542, 0.03910466661604729;
 0, 0, 0, 1];
B(:,:,3)= [0.996, -0.015, 0.093, -0.022542;
 0.005, 0.994, 0.108, -0.020474;
 -0.094, -0.107, 0.99, -0.01874;
 0, 0, 0, 1];
A(:,:,4) = [0.9983135366773674, 0.05794501125950243, -0.003531135651603939, 0.0004571480459416591;
 -0.05794274671098512, 0.9983196309130306, 0.0007399841851459972, 0.001622949927601588;
 0.003568083345779068, -0.0005341359364298359, 0.9999934944605708, 0.001438857641607883;
 0, 0, 0, 1];
B(:,:,4) = [0.998, 0.064, -0.001, -0.0009090000000000001;
 -0.064, 0.998, 0.001, 0.011387;
 0.001, -0.001, 1, -0.001757;
 0, 0, 0, 1];
A(:,:,5) = [0.9915062741597328, -0.1300111837225313, -0.003522119356921797, 0.0004499521578996357;
 0.1300145141115178, 0.9915118150071343, 0.000732801097784591, 0.001616023647135735;
 0.003396948061207789, -0.001184514470142244, 0.9999935303277178, 0.001444427330547771;
 0, 0, 0, 1];
B(:,:,5) = [0.992, -0.128, 0.001, 1.3e-05;
 0.128, 0.992, 0.001, -0.02897;
 -0.001, -0.001, 1, -0.001337;
 0, 0, 0, 1];
A(:,:,6)= [0.7796705063973758, 0.6177659298534316, 0.102367727270737, 0.06459299889437092;
 -0.619431191453391, 0.7369254849768153, 0.2706396639522871, 0.1932374132341246;
 0.09175457749995197, -0.2744195409075993, 0.9572225559978212, -0.02365370727335698;
 0, 0, 0, 1];
B(:,:,6)= [0.775, 0.623, 0.106, -0.017595;
 -0.625, 0.733, 0.27, -0.048337;
 0.091, -0.275, 0.957, 0.00014;
 0, 0, 0, 1];
A(:,:,7)= [0.9032752089508844, 0.4142955461852235, 0.1115934411020693, 0.04616501891956003;
 -0.4196000653234244, 0.9072743594850097, 0.02808948526733768, 0.1213523510544282;
 -0.08960853026262187, -0.07219715765503833, 0.9933568767972825, 0.0129242778821884;
 0, 0, 0, 1];
B(:,:,7)= [0.989, -0.127, 0.06900000000000001, -0.012234;
 0.122, 0.989, 0.08, -0.024133;
 -0.078, -0.07000000000000001, 0.994, -0.000615;
 0, 0, 0, 1];
A(:,:,8)= [0.9997213543278972, -0.004796289479486885, 0.02311311293903669, -0.006273615478196445;
 0.004098452810363369, 0.9995371158007407, 0.03014550252795428, -0.02731756318985714;
 -0.0232470108716518, -0.0300423743548195, 0.9992782599283017, -0.008738587559791436;
 0, 0, 0, 1];
B(:,:,8)= [1, -0.005, 0.03, 0.005644000000000001;
 0.005, 1, 0.028, 0.027656;
 -0.03, -0.028, 0.999, 0.016522;
 0, 0, 0, 1];
A(:,:,9)= [0.9571131547973167, -0.2664570819176147, 0.1137323238007965, 0.03788466525548986;
 0.2561176601861805, 0.9616917362725793, 0.09773815153711633, -0.02465358121547014;
 -0.1354184685304765, -0.06441761199791894, 0.9886921746692146, 0.03311648869810188;
 0, 0, 0, 1];
B(:,:,9)= [0.956, -0.263, 0.128, -0.024772;
 0.253, 0.963, 0.08699999999999999, -0.020774;
 -0.146, -0.051, 0.988, -0.000945;
 0, 0, 0, 1];
A(:,:,10)= [0.8101226888988626, -0.5702527704166932, 0.1360624881988172, 0.04536203556901543;
 0.5787306958514427, 0.8149590341450765, -0.0302083171664643, -0.1122168944349427;
 -0.09365898477523348, 0.1032159788271309, 0.9902396005720315, 0.003448933325671426;
 0, 0, 0, 1];
B(:,:,10)= [0.8080000000000001, -0.569, 0.149, 0.001221;
 0.58, 0.8129999999999999, -0.04, -0.008701;
 -0.098, 0.119, 0.988, 0.019394;
 0, 0, 0, 1];

A(:,:,11) = [-0.5849001985816084, 0.797759402396539, 0.1465322674879359, 0.349538332588254;
 -0.7744962173563898, -0.602973722279076, 0.1912545376627483, 0.2104526735999798;
 0.2409302191069788, -0.0016238669154268, 0.9705410877923556, -0.04350441785238145;
 0, 0, 0, 1];
B(:,:,11) = [-0.6077232302200001, 0.7847301930100001, 0.125, -0.008952999999999999;
 -0.75718227244, -0.61917364823, 0.207, -0.0369;
 0.23978123884, 0.03087001008000002, 0.97, -0.020702;
 0, 0, 0, 1];
A(:,:,12) = [-0.797092949601237, 0.5827565157001222, 0.1582330249511362, 0.3986689757978814;
 -0.5664946075618164, -0.8123867995094362, 0.1382444949967465, 0.1859198622468514;
 0.2091093074042133, 0.02055556232456227, 0.9776762096058501, -0.06105708485927139;
 0, 0, 0, 1];
B(:,:,12) = [-0.78482604808, 0.60623763874, 0.125, -0.011664;
 -0.58627835595, -0.79304961365, 0.168, -0.056815;
 0.20059576409, 0.05833814743, 0.978, 0.00421;
 0, 0, 0, 1];
A(:,:,13) = [0.9198004278848106, -0.3671078089002355, 0.1385605882366377, 0.0408386377677557;
 0.3497268669691551, 0.927110715787682, 0.1347472479038771, -0.03696367605603731;
 -0.1779277753633862, -0.07548221848766874, 0.9811443008754227, 0.02580593622351347;
 0, 0, 0, 1];
B(:,:,13) = [0.92, -0.364, 0.143, -0.014118;
 0.351, 0.93, 0.113, -0.022443;
 -0.174, -0.054, 0.983, 0.011453;
 0, 0, 0, 1];
A(:,:,14) = [-0.4414521651027964, 0.8817551781832361, 0.1662161482121915, 0.3023485424774342;
 -0.882839153220496, -0.4599336869135009, 0.09516315351470551, 0.2369591083970457;
 0.1603590169968309, -0.1047321446927015, 0.9814867161376279, -0.03719682838587067;
 0, 0, 0, 1];
B(:,:,14) = [-0.4513495456200001, 0.8834837791600001, 0.126, 0.009211;
 -0.8814102394400001, -0.46382754358, 0.089, -0.039305;
 0.13653720861, -0.07123616128, 0.988, -0.000394;
 0, 0, 0, 1];

X= tsai(A,B)

tsai.m

tasi手眼标定求解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 = S\v;
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 = C\d;

%Put everything together to form X
X = [R t;0 0 0 1];