经典手眼标定方法汇总（matlab程序）

%% %eye to hand 手眼标定验证程序，以仿真验证代替物理机器验证。 %libing,2020.4.23 %% %建立机器人模型，获得试验数据，%验证算法理论的正确性，再用于验证C++程序的正确性。 clc clear clc; close('all'); %随意建立一个机器人 PM_PI=3.1415926; deg=PM_PI/180; % theta d a alpha offset ML1 = Link([ 0, 0.4967, 0, 0, 0], 'modified'); ML2 = Link([ 0, -0.18804, 0.2, 3*PM_PI/2, 0], 'modified'); ML3 = Link([ 0, 0.17248, 0.79876, 0 , 0], 'modified'); ML4 = Link([ 0, 0.98557, 0.25126, 3*PM_PI/2, 0], 'modified'); ML5 = Link([ 0, 0, 0, PM_PI/2 , 0], 'modified'); ML6 = Link([ 0, 0, 0, PM_PI/2 , 0], 'modified'); robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','test\_robot'); robot.teach(); %可以自由拖动的关节角度 hold on; %基坐标系 H_base=transl(0,0,0); trplot(H_base,'frame','b','color','b'); hold on; %axis([-2,2,-2,2,-1,3]); %末端坐标系 T_b_t=zeros(4,4); theta=[0,0,0,0,0,0]; T_b_t(:,1:4)=robot.fkine(theta); fig_tool=trplot(T_b_t,'frame','t','color','b'); %设置相机坐标系（随意设置，与标定结果对比） H_b_c=zeros(4,4); t=transl(1,1.5,2); H_b_c=t*trotx(15)*troty(30)*trotz(45); fprintf("设置相机位姿为\n"); disp(H_b_c); trplot(H_b_c,'frame','cam','color','g'); %设置机器人末端坐标系到标定板坐标系的变换（随意设置） H_t_cal=zeros(4,4); t=transl(0.1,0.2,0.1); H_t_cal=t*trotx(15)*troty(30)*trotz(45); T_b_cal=T_b_t*H_t_cal; fig_cal=trplot(T_b_cal,'frame','cal','color','g'); %采集手眼标定所需的数据数据(10个位姿) fp1=fopen('testq_T_b_t.txt','w'); fp2=fopen('testq_T_c_cal.txt','w'); for i=1:10 J1=-6*i*deg; J2=-4.5*i*deg; J3=-4.5*i*deg; J4=3.0*i*deg; J5=4.5*i*deg; J6=3.0*i*deg; theta=[J1,J2,J3,J4,J5,J6]; %采集手眼标定数据数据 T_b_t(:,4*i-3:4*i)=robot.fkine(theta); %基座坐标系到机器人末端坐标系的变换 T_b_cal=T_b_t(:,4*i-3:4*i)*H_t_cal; %由T_b_cal=T_b_c*T_c_cal导出T_c_cal T_c_cal(:,4*i-3:4*i)=inv(H_b_c)*T_b_cal;%相机坐标系到标定板坐标系的变换 q1(1:3)= T_b_t(1:3,4*i);%写入，用于C++程序测试 q1(4:7)=rot2q(T_b_t(1:3,4*i-3:4*i-1)); q2(1:3)= T_c_cal(1:3,4*i);%写入，用于C++程序测试 q2(4:7)=rot2q(T_c_cal(1:3,4*i-3:4*i-1)); fprintf(fp1,'%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n',... q1(1),q1(2),q1(3),q1(4),q1(5),q1(6),q1(7)); fprintf(fp2,'%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n',... q2(1),q2(2),q2(3),q2(4),q2(5),q2(6),q2(7)); pause(0.2); delete(fig_tool); delete(fig_cal); fig_tool=trplot(T_b_t(:,4*i-3:4*i),'frame','t','color','r');%绘制末端坐标系 fig_cal=trplot(T_b_cal,'frame','cal','color','g');%绘制标定板坐标系 robot.plot(theta); end fclose(fp1); fclose(fp2); %利用采集的数据构造方程的系数矩阵A,B H_b_t=T_b_t; H_c_cal=T_c_cal; [m,n]=size(H_b_t); m=n/4; fp1=fopen('testA.txt','w'); fp2=fopen('testB.txt','w'); n=1; for i=1:m-1 j=i+1; A(:,4*i-3:4*i)=H_b_t(:,4*j-3:4*j)*inv(H_b_t(:,4*i-3:4*i)); B(:,4*i-3:4*i)=H_c_cal(:,4*j-3:4*j)*inv(H_c_cal(:,4*i-3:4*i)); testA(4*i-3:4*i,:)=A(:,4*i-3:4*i);%用于C++程序测试 testB(4*i-3:4*i,:)=B(:,4*i-3:4*i);%用于C++程序测试 for k=1:4 fprintf(fp1,'%.6f %.6f %.6f %.6f\n',... testA(4*i-4+k,1),testA(4*i-4+k,2),... testA(4*i-4+k,3),testA(4*i-4+k,4)); fprintf(fp2,'%.6f %.6f %.6f %.6f\n',... testB(4*i-4+k,1),testB(4*i-4+k,2),... testB(4*i-4+k,3),testB(4*i-4+k,4)); end end fclose(fp1); fclose(fp2); %求AX=XB解方程 fprintf("标定结果\n"); X1=solve_hand_eye_equation(A,B); %文献的其他方法 X2=tsai(A,B) X3=shiu(A,B) X4=park(A,B) X5=lu8(A,B) X6=liang(A,B)