标定
模型:眼在手外
目的:确定相机在机器人基座的坐标位置,便于对后续观测点位置进行确定。搭建整体的视觉抓取平台。
ROS标定方法见苏大头师兄记录
#Easy Handeye配置_u012894550的博客-CSDN博客blog.csdn.net
ROS下通过如下图得到图片,rviz获取位姿数据
棋盘格_matlab方法
- 得到棋盘格并通过机械臂末端固定棋盘格
I = checkerboard(200,10,7);
I = I(1:size(I,1)/2,1:size(I,2)/2);
imsave(imshow(I));
2. 移动机械臂确保整个棋盘格都可以被看到
每个动作在变化中一定要有移动的旋转,否则会出现无法计算的问题
3. 保存相机获取的棋盘格图像,通过示教器获取机械臂位姿信息,重复>=10次
图片
位姿
4. 运行matlab代码
%-------------------------------------------------------
clear
clc
% Define images to process
imageFileNames = {'E:\抓取\10\image1.png',...
'E:\抓取\10\image2.png',...
'E:\抓取\10\image3.png',...
'E:\抓取\10\image4.png',...
'E:\抓取\10\image5.png',...
'E:\抓取\10\image6.png',...
'E:\抓取\10\image7.png',...
'E:\抓取\10\image8.png',...
'E:\抓取\10\image9.png',...
'E:\抓取\10\image10.png',...
};
% Detect checkerboards in images
[imagePoints, boardSize, imagesUsed] = detectCheckerboardPoints(imageFileNames);
imageFileNames = imageFileNames(imagesUsed);
% Generate world coordinates of the corners of the squares
squareSize = 17.5; % in units of 'mm'
worldPoints = generateCheckerboardPoints(boardSize, squareSize);
% Calibrate the camera
[cameraParams, imagesUsed, estimationErrors] = estimateCameraParameters(imagePoints, worldPoints, ...
'EstimateSkew', false, 'EstimateTangentialDistortion', false, ...
'NumRadialDistortionCoefficients', 2, 'WorldUnits', 'mm', ...
'InitialIntrinsicMatrix', [], 'InitialRadialDistortion', []);
% View reprojection errors
h1=figure; showReprojectionErrors(cameraParams, 'BarGraph');
% Visualize pattern locations
h2=figure; showExtrinsics(cameraParams, 'CameraCentric');
% Display parameter estimation errors
displayErrors(estimationErrors, cameraParams);
Trans = cameraParams.TranslationVectors;
Rot = cameraParams.RotationMatrices;
for i=1:size(Trans,1)
tempc = [Rot(:,:,i),Trans(i,:)';0 0 0 1];
TRMarker2Camera(:,:,i) = tempc; %存储每一个棋盘格在相机坐标系下的旋转矩阵
end
%读取机械臂末端到base的姿态变换即为Tre2rb 这个地方可能有问题需要
load endpose
for i=1:size(Trans,1)
rpy = endpose(:,4:6,i);
r=[rpy(1),rpy(2),rpy(3)];
theta = sqrt(rpy(1) * rpy(1) + rpy(2) * rpy(2) + rpy(3) * rpy(3));
kx = rpy(1)/theta;
ky = rpy(2)/theta;
kz = rpy(3)/theta;
r1 = kx * kx * (1-cos(theta))+ cos(theta);
r2 = kx * ky * (1-cos(theta))- kz * sin(theta);
r3 = kx * kz * (1-cos(theta))+ ky * sin(theta);
r4 = kx * ky * (1-cos(theta))+ kz * sin(theta);
r5 = ky * ky * (1-cos(theta))+ cos(theta);
r6 = ky * kz * (1-cos(theta))- kx * sin(theta);
r7 = kx * kz * (1-cos(theta))- ky * sin(theta);
r8 = ky * kz * (1-cos(theta))+ kx * sin(theta);
r9 = kz * kz * (1-cos(theta))+ cos(theta);
dcm = [r1 r2 r3 ; r4 r5 r6 ; r7 r8 r9];
Tre2rb(:,:,i) = [dcm,endpose(:,1:3,i)';0 0 0 1];
TRrb2re(:,:,i)=inv(Tre2rb(:,:,i));
end
n=size(TRMarker2Camera,3);
A=[];
B=[];
C=[];
d=[];
M=zeros(3,3);
l=0;
for i=[1:size(Trans,1)-1]
l=l+1;
B(:,:,l)=(TRMarker2Camera(:,:,i+1))/TRMarker2Camera(:,:,i);
Tb(:,:,l)=B(1:3,4,l); % translation matrix of B
A(:,:,l)= TRrb2re(:,:,i+1)\(TRrb2re(:,:,i)); % multiplication A = inv(Deri+1) * Deri
Ra(:,:,l)=A(1:3,1:3,l); % tranlation and rotation matrix of A
Ta(:,:,l)=A(1:3,4,l);
alpha=logMatrix(A(:,:,l)); %matrix logarithms
beta=logMatrix(B(:,:,l));
M=M+beta*alpha';
end
Rx=(M'*M)^(-1/2)*M';
for i=1:l
C=[C; eye(3)-Ra(:,:,i)];
d=[d; Ta(:,:,i)-Rx*Tb(:,:,i)];
end
Tx=(C'*C)\(C'*d);
TRCamera2base=[Rx Tx; 0 0 0 1];
Rerr=0;
Terr=0;
for i=1:l
ZUOCE(:,:,i)=A(:,:,i)*TRCamera2base;
zuocezuobiao(1:3,i)=ZUOCE(1:3,4,i);
YOUCE(:,:,i)=TRCamera2base*B(:,:,i);
yuocezuobiao(1:3,i)=YOUCE(1:3,4,i);
AXsubtractXB(:,:,i)=A(:,:,i)*TRCamera2base-TRCamera2base*B(:,:,i);
Rerr=Rerr+sqrt((sum(AXsubtractXB(1:3,1,i).^2)+sum(AXsubtractXB(1:3,2,i).^2)+sum(AXsubtractXB(1:3,3,i).^2))/9);
RERR(1,i)=sqrt((sum(AXsubtractXB(1:3,1,i).^2)+sum(AXsubtractXB(1:3,2,i).^2)+sum(AXsubtractXB(1:3,3,i).^2))/9);
Terr=Terr+sqrt(sum(AXsubtractXB(1:3,4,i).^2)/3);
TERR(1,i)=sqrt(sum(AXsubtractXB(1:3,4,i).^2)/3);
end
subplot(2,1,1)
plot(RERR(1,:));
set(gca,'xtick',0:1:l)
subplot(2,1,2)
plot(TERR(1,:));
set(gca,'xtick',0:1:l)
% figure
% plot3(zuocezuobiao(1,:),zuocezuobiao(2,:),zuocezuobiao(3,:),'r',yuocezuobiao(1,:),yuocezuobiao(2,:),yuocezuobiao(3,:),'b')
ERR0=[Rerr/l,Terr/l]
TRCamera2base(1:3,4)=TRCamera2base(1:3,4)/1000
TRMarkertoHololens=inv(TRCamera2base);