机械臂手眼标定原理与实现

空白不对照

已于 2024-05-02 19:58:35 修改

阅读量1.1k

点赞数 9

文章标签：机器人 matlab 算法

于 2024-05-02 19:50:52 首次发布

本文链接：https://blog.csdn.net/qq_44903985/article/details/138394862

版权

机械臂手眼标定的主要目的是将相机对目标的位姿识别结果转换为机械臂坐标系下的位姿坐标，从而使得机械臂能够借助视觉信息执行相应的任务。根据相机的位置可以分为眼在手上以及眼在手外。这两种在原理推导上有些许差异，但是求解方法是一样的。

一、手眼标定原理

在眼在手外的场景示意图如下所示，将标定板固定在机械手臂末端，并多次移动机械臂，过程中保持标定板和机械手臂末端的位置关系不变，机械臂底座和相机之间的相对位置不变。一般采集数据为17组。

定义机械臂底座坐标系为base，机械臂末端坐标系为end，相机坐标系为cam，标定板坐标系为cal。由于机械臂移动过程中始终保持机械臂末端与标定板之间的相对位姿不变，因此任意选取移动过程中的两组观测数据可以得到：

$H_{end1}^{base}\cdot H_{base}^{cam}\cdot H_{cam}^{cal1}=H_{end2}^{base}\cdot H_{base}^{cam}\cdot H_{cam}^{cal2}$

其中， $H_a^b$ 表示a坐标系到b坐标系的齐次转换矩阵。通过移项可以得到

$AX=XB$

其中， $A=(H_{end2}^{base})^{-1}H_{end1}^{base}$ ， $X=H_{base}^{cam}$ ， $B=H_{cam}^{cal2}(H_{cam}^{cal1})^{-1}$

而对于眼在手上的情况，可以利用base坐标系与标定板cal之间相对位姿不变的关系进行构造求解机械臂末端end到相机坐标系cam的位姿转换关系。

$H_{base}^{end1}\cdot H_{end1}^{cam}\cdot H_{cam}^{cal1}=H_{base}^{end2}\cdot H_{end2}^{cam}\cdot H_{cam}^{cal2}$

二、基于Tsai的AX=XB的求解方法

$AX=XB$ 中，A、X、B均为齐次矩阵，可以写成

$A=\begin{pmatrix} R_A & T_A\\ 0& 1 \end{pmatrix}$ ， $X=\begin{pmatrix} R_X & T_X\\ 0& 1 \end{pmatrix}$ ， $B=\begin{pmatrix} R_B & T_B\\ 0& 1 \end{pmatrix}$ ，

可将AX=XB拆解为

$R_A\cdot R_X=R_X\cdot R_B$

$(R_A-I)\cdot T_X=R_X\cdot T_B-T_A$

在Tsai-Lenz论文中使用旋转轴+旋转角的方式来表示旋转。作者使用了修正的罗德里格斯参数表示旋转变换。 R表示一个旋转矩阵，R的特征向量和特征值一定是它的旋转轴和1。我们可以定义R的旋转轴为Pr(旋转向量)，则有：R *P r = 1 * Pr。使用修正的罗德里格斯变换重新定义Pr：

$P_r=2sin(\frac{\theta}{2})[n_1,n_2,n_3]^T,0\leqslant \theta\leqslant \pi$

Tsai在论文中利用代数方法证明了

$P_{cg}\perp (P_c-P_g)$

并进一步证明了

$skew(P_c+P_g)P_{cg}^{'}=P_c-P_g$

那么，

$\theta=2atan(|P_{cg}^{'}|)$

$P_{cg}=\frac{2P_{cg}^{'}}{\sqrt{1+|P_{cg}^{'}|^{2}}}$

计算旋转矩阵R

$R_X=cos\theta\cdot I+sin\theta\cdot skew(n)+(1-cos\theta)n\cdot n^{T}$

计算平移矩阵 $T_X$

$(R_A-I)\cdot T_X=R_X\cdot T_B-T_A$

三、基于matlab的仿真实现

设计了眼在手外的应用场景进行仿真分析，选取了三个理想位姿作为观测数据进行仿真，

 clc;clear all;
%%%%%%%%%%位姿1的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose1=[1141.243,-15.261,-97.721,178.91,0.47,92.37];
Px = Pose1(1);
Py = Pose1(2);
Pz = Pose1(3);
rota = Pose1(4)*pi/180;
rotb = Pose1(5)*pi/180;
rotc = Pose1(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R1 = Rz*Ry*Rx;
T1= [Px Py Pz]';
%%%%%%%%%%位姿2的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose2=[1103.946,-163.910,-107.673,-160.90,-0.14,-91.62];
Px = Pose2(1);
Py = Pose2(2);
Pz = Pose2(3);
rota = Pose2(4)*pi/180;
rotb = Pose2(5)*pi/180;
rotc = Pose2(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R2 = Rz*Ry*Rx;
T2= [Px Py Pz]';
%%%%%%%%%%位姿3的时候机器人末端相对于机器人基坐标系下变换矩阵
Pose3=[1073.714,2.669,-142.448,-142.86,0.84,-178.55];
Px = Pose3(1);
Py = Pose3(2);
Pz = Pose3(3);
rota = Pose3(4)*pi/180;
rotb = Pose3(5)*pi/180;
rotc = Pose3(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
R3 = Rz*Ry*Rx;
T3= [Px Py Pz]';
%%%%%%%%%位姿1,2,3时候机器人末端相对于机器人基坐标系下变换矩阵
T61=[R1 T1;0 0 0 1]   ;      
T62=[R2 T2;0 0 0 1];
T63=[R3 T3;0 0 0 1];
%%%%%%%%%定义camera相对于base的位姿转换矩阵
Pose4=[100,100,100,0,0,0];%camera相对于base的位姿
Px = Pose4(1);
Py = Pose4(2);
Pz = Pose4(3);
rota = Pose4(4)*pi/180;
rotb = Pose4(5)*pi/180;
rotc = Pose4(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
Rc = Rz*Ry*Rx;
Tc= [Px Py Pz]';
TC1=inv([Rc Tc;0 0 0 1]);
%%%%%%%%%定义标定板marker相对于机械臂末端end的位姿转换矩阵
Pose5=[10,10,10,0,0,0];%marker相对于end的位姿
Px = Pose5(1);
Py = Pose5(2);
Pz = Pose5(3);
rota = Pose5(4)*pi/180;
rotb = Pose5(5)*pi/180;
rotc = Pose5(6)*pi/180;
Rx = [1 0 0; 0 cos(rota) -sin(rota); 0 sin(rota) cos(rota)];
Ry = [cos(rotb) 0 sin(rotb); 0 1 0; -sin(rotb) 0 cos(rotb)];
Rz = [cos(rotc) -sin(rotc) 0; sin(rotc) cos(rotc) 0; 0 0 1];
Re = Rz*Ry*Rx;
Te= [Px Py Pz]';
Tend1=([Re Te;0 0 0 1]);
Tca2m1=TC1*T61*Tend1;
Tca2m2=TC1*T62*Tend1;
Tca2m3=TC1*T63*Tend1;
T_base_end=[inv(T61),inv(T62),inv(T63)];
T_marker_camera=[(Tca2m1),(Tca2m2),(Tca2m3)];
[m,n]=size(T_end_base);
for i=1:(n/4-1)
    A(:,(4*i-3):4*i)=inv(T_base_end(:,4*i-3:4*i))*(T_base_end(:,4*i+1:4*i+4));
    B(:,(4*i-3):4*i)=T_marker_camera(:,4*i-3:4*i)*inv(T_marker_camera(:,4*i+1:4*i+4));
end

X=tsai(A,B);


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];
end

function Sk = skew( x )
%SKEW 此处显示有关此函数的摘要
%   此处显示详细说明
   Sk = [0,-x(3),x(2);x(3),0,-x(1);-x(2),x(1),0];
end

最终结果表明该程序没有问题

参考链接与论文：

[1]一分钟详解机器人手眼标定MATLAB及C++实现

[2]A new technique for fully autonomous and efficient 3D robotics hand/eye calibration

[3]相机标定（一）：机器人手眼标定 - 知乎 (zhihu.com)

空白不对照

关注

9
点赞
踩
18

收藏

觉得还不错? 一键收藏
1
评论
机械臂手眼标定原理与实现

机械臂手眼标定的主要目的是将相机对目标的位姿识别结果转换为机械臂坐标系下的位姿坐标，从而使得机械臂能够借助视觉信息执行相应的任务。根据相机的位置可以分为眼在手上以及眼在手外。这两种在原理推导上有些许差异，但是求解方法是一样的。
复制链接

扫一扫