最近在学习机器人学，在操作机器人利用四点法和默认方向建立机器人工具坐标系时，对其原理并不理解，所以就查阅了一下论文，用matlab编程实现了一个。
1. 工具坐标系
在使用工业机器人的时候，为了方便编程，常常会建立各种坐标系，常见的有极坐标系，工具坐标系，工件坐标系。
机器人工具坐标系标定就是确定工具坐标系相对于末端连杆坐标系的变换矩阵。研究了一种机器人工具坐标系标定算法。其中工具中心点（ＴＣＰ）位置标定采用最小二乘法进行拟合；工具坐标系（ＴＣＦ）姿态标定采用坐标变换进行计算。
工业机器人在使用的过程中，根据实际工作对象，会设计安装不同的末端执行机构，例如焊枪、抓具、吸盘等. 对于ABB机器人，无论是在手动示教编程还是离线编程，系统都要求操作者指定机器人工作的工具坐标，工业机器人在出厂时有一个默认的工具坐标系tool0,该工具坐标系位于机器人第六轴法兰盘的中心，是由基坐标系通过机器人正解得到的一个旋转偏移矩阵. 实际工作中，默认的工具坐标系tool0并不能有效满足实际工作要求，所以要根据末端执行机构的种类和特点，重新建立一个或多个工具坐标系. 为机器人建立新的工具坐标系，可以方便操作者灵活地调整机器人末端执行机构的姿态，更加精确地控制机器人的运动轨迹.
2. 四点法建立机器人Tcp
下图左是Abb机器人建立工具坐标系的截图，右图是实际操作示意图
四点法标定TCP步骤和原理. 如图所示，在机器人运动空间选取一固定参考点XT，移动机器人，尽可能选取关节差异较大的4种的位姿（得到的数据更加准确），使机器人的TCP与点XT重合，得到tool0相对于Base坐标系的旋转平移矩阵T1 、T2 、T3、 T4，在此条件下，四个矩阵中的数据不同，但XT相对于机器人Base坐标系的位置是不变的，所以可以得到
将（2）写成分块矩阵的形式：
令等式两边第４列对应相等，则得：
式（4）的右边相等(XT在基坐标系中的位置不变)
写成方程的形式：
即：
利用最小二乘法计算：
3. matlab实现
根据式（8）用选用六组数据（最少三组）matlab编程得到：
clc
close all;
clear all;
%%
xMat=[1028.29,1118.69,1129.45,983.98, 605.47, 983.98 ];
yMat=[233.29,-231.99, 25, 0.39,0.39,0.39 ];
zMat=[1432.98,1478.85,1564,1502.31,1502.31,1122.24 ];
alfaMat=[-156.2, 153.6,-175.1,179.8,179.8,179.8 ]; %按照Z-Y-X方式旋转
betaMat=[44.8,19, 29.4,54.3, 54.3, 54.3 ];
gammaMat=[-126.6,136.1,-175,180,180,180 ]; %tool0 的坐标和欧拉角
n=6;
RotateMat_EtoB =num2cell(1,n); %旋转矩阵
P_tool0_To_B =num2cell(1,n); % 平移矩阵
for i=1:n
RotateMat_EtoB{1,i}=RotateMat(alfaMat(i),betaMat(i),gammaMat(i));
P_tool0_To_B{1,i} =[xMat(i);yMat(i);zMat(i)];
end
%%
A = num2cell(n-1,1);
B = num2cell(n-1,1);
for i=1:n-1
A{i,1} =RotateMat_EtoB{1,i}-RotateMat_EtoB{1,i+1};
B{i,1} =P_tool0_To_B{1,i+1 } - P_tool0_To_B{1,i};
end
L =zeros(3,3);
R= zeros(3,1);
for i =1:n-1
L=L+ A{i,1}*A{i,1};
R =R +A{i,1}*B{i,1};
end
E = inv(L)*R
旋转矩阵函数
function [ OutMat ] = RotateMat( alfa ,beta,gamma)
%UNTITLED 输入zyx角
% 输出旋转矩阵
alfa =alfa*pi/180;beta=pi*beta/180; gamma=pi*gamma/180;
a00=cos(alfa)*cos(beta) ; a01 = cos(alfa)*sin(beta)*sin(gamma)-sin(alfa)*cos(gamma);
a02= cos(alfa)*sin(beta)*cos(gamma)+sin(alfa)*sin(gamma);
a10= sin(alfa)*cos(beta);a11 =sin(alfa)*sin(beta)*sin(gamma)-cos(alfa)*cos(gamma);
a12 =sin(alfa)*sin(beta)*cos(gamma)-cos(alfa)*sin(gamma);
a20= -sin(beta);a21=cos(beta)*sin(gamma); a22=cos(beta)*cos(gamma);
OutMat = [ a00 a01 a02 ; a10 a11 a12; a20 a21 a22];
end
如果安装了 matlab Robotics Toolbox
可以直接使用
rotz(alfa)*roty(beta)*rotx(gamma)
运行结果
实际测量得到的Tcp坐标为
对比来看，误差还算满意。
对于机器人工具坐标姿态（TCF），笔者在使用时使用的是默认方向，即与tool0 相同。
参考文献 [机器人工具坐标系标定算法研究] [http://kns.cnki.net/kcms/detail/knetsearch.aspx?dbcode=CJFQ&sfield=au&skey=叶伯生&code=25929018;07601371;07587638]