标签：机器人 标定 TCP tag bmatrix TCF 工具 坐标系
一、基本步骤
(1)在机器人动作范围内找一个非常精确的固定点作为参考点；
(2)在工具上确定一个参考点(最好是工具中心点Tool Center Point, TCP);
(3)手动操纵机器人的方法移动TCP，以四种不同的工具姿态与固定点刚好碰上。
??前三个点任意姿态，第四点是用工具的参考点垂直于固定点，第五点是工具参考点从固定点向将要设定的TCP的x方向移动，第六点是工具参考点从固定点向将要设定的TCP的在z方向移动，如下图所示：
(4)通过前4个点的位置数据即可计算出TCP的位置，通过后2个点即可确定TCP的姿态
二、标定过程
1、TCP位置标定
??假设取1、2、3、4四个标定点之间相差90°且不在同一平面上，如下图所示：
??给定如下坐标系定义：
【1】基坐标系(0坐标系)：B
【2】末端坐标系：E
【3】工具坐标系：T
??给定如下变换矩阵定义：
【1】末端坐标系 E 相对于基坐标系 B的变换关系 ：BETEBT
【2】工具坐标系T 相对于末端坐标系 E的变换关系 ：ETTTET
【3】工具坐标系T 相对于基坐标系 B的变换关系 ：BTTTBT
??显然可以知道：
BET⋅ETT=BTT(1)
(1)EBT·TET=TBT
??对于选定位置点 i = 1、2、3、4，有：
??【1】BETEBT不等，设：
\[^{B}_ {E}T =
\begin{bmatrix}
\pmb{^{B}_ {E}R_{i}} & \pmb{^{B}P_ {Ei}}\ 0 & 1 \ \end{bmatrix}
\tag{2}
\]
??【2】ETTTET不等，但其位置EPTEPT相等，设：
\[^{E}_ {T}T =
\begin{bmatrix}
^{E}_ {T}R_ {i} & \pmb{^{E}P_ {T}} \ 0 & \pmb{1} \ \end{bmatrix}
\tag{3}
\]
??【3】BTTTBT 不等，但其位置EPTEPT相等，设：
\[ ^{B}_ {T}T =
\begin{bmatrix}
^{B}_ {T}R_ {i} & \pmb{^{B}P_ {T}}\ 0 & 1 \ \end{bmatrix}
\tag{4}
\]
??关注公式(2)、(3)、(4)中加粗符号，有：
\[ \pmb{^{B}_ {E}R_ {i}} · \pmb{^{E}P_ {T}} + \pmb{^{B}P_ {Ei}} = \pmb{^{B}P_ {T}} \tag{5}
\]
??在实际中，BETEBT 由机器人正解方程可以直接测得，因此，我们直接读取四个位置点的姿态BERiEBRi和位置BPEi=1,2,3,4BPEi=1,2,3,4。假设：
\[^{B}_ {E}R_ {1}· ^{E}P_ {T} + ^{B}P_ {E1} = ^{B}P_ {T} \tag{6}
\]
\[^{B}_ {E}R_ {2}· ^{E}P_ {T} + ^{B}P_ {E2} = ^{B}P_ {T} \tag{7}
\]
??则，(6) - (7)得：
\[(^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2} )· ^{E}P_ {T} = ^{B}P_ {E2} - ^{B}P_ {E1} \tag{8}
\]
??同理可得：
\[(^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3} )· ^{E}P_ {T} = ^{B}P_ {E3} - ^{B}P_ {E2} \tag{9}
\]
\[(^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4} )· ^{E}P_ {T} = ^{B}P_ {E4} - ^{B}P_ {E3} \tag{10}
\]
??由(8)、(9)、(10)可得：
\[\begin{bmatrix}
^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2}\^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3}\^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4}
\end{bmatrix}·
^{E}P_ {T} =
\begin{bmatrix}
^{B}P_ {E2} - ^{B}P_ {E1}\^{B}P_ {E3} - ^{B}P_ {E2}\^{B}P_ {E4} - ^{B}P_ {E3}
\end{bmatrix}
\tag{11}
\]
??由于EPTEPT为 3x1 列向量，而等式右边为 9x3的矩阵，因此方程(11)为不相容方程组，不可直接用非齐次线性方程组求解的方法或者solve求解。采用最小二乘法的矩阵形式，因其系数矩阵不是方阵，不可直接求逆， 因此使用广义逆。采用高斯消元法得到：
\[^{E}P_ {T} =
\begin{bmatrix}
^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2}\^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3}\^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4}
\end{bmatrix}\verb|\|
\begin{bmatrix}
^{B}P_ {E2} - ^{B}P_ {E1}\^{B}P_ {E3} - ^{B}P_ {E2}\^{B}P_ {E4} - ^{B}P_ {E3}
\end{bmatrix}
\tag{12}
\]
??则式(12)所求得的EPTEPT即为TCP的位置向量。
2、TCF姿态标定
??在第1部分已经得到工具坐标系(TCF)的位置，而计算TCP姿态采用z/x方向标定。
??此过程中TCF的姿态保持不变(如第一节 -- 基本步骤中图所示)。取第一个姿态标定点为位置点4(下图记作标定点1)；机器人从位置点4出发，沿+x方向移动一定距离得到位置点5(下图记作标定点2)；机器人从位置点4出发，沿+z方向移动一定距离得到位置点6(下图记作标定点3)。如下图所示：
??由于3个标定点中的TCF姿态不变，故BERi=4,5,6EBRi=4,5,6均相等，且由(12)得 EPTEPT 保持不变，故可得到工具坐标系 T 的 x 轴轴向向量 XX ，且：
\[X = ^{B}P_ {E5} - ^{B}P_ {E4} \tag{13}
\]
??同理，可得到工具坐标系 T 的 z 轴轴向向量 ZZ ，且：
\[Z = ^{B}P_ {E6} - ^{B}P_ {E4} \tag{14}
\]
??进而由右手定则得工具坐标系 T 的 y 轴轴向向量 YY ：
\[Y = Z \times X\tag{15}
\]
??为进一步保证坐标系矢量的正交性，重新计算ZZ ：
\[Z = X \times Y \tag{16}
\]
??将(13)、(15)、(16)单位化得到X‘,Y‘,Z‘X‘,Y‘,Z‘，得到工具坐标 T 相对于基坐标 B 的姿态BTRTBR，且
\[^{B}_ {T}R = \begin{bmatrix}X^{‘} Y^{‘} Z^{‘} \end{bmatrix} \tag{17}
\]
??又末端坐标系 E 旋转矩阵为BEREBR，且：
\[^{B}_ {E}R\ ^{E}_ {T}R=^{B}_ {T}R \tag{18}
\]
??故由(17)、(18)即可得到工具坐标系的旋转矩阵ETRTER，即：
\[^{E}_ {T}R = ^{B}_ {E}R^{-1}\ ^{B}_ {T}R \tag{19}
\]
3、TCF标定结果
??由(12)和(19)，可得到工具坐标系 ETTTET 标定为：
\[^{E}_ {T}T =
\begin{bmatrix}
^{E}_ {T}R & ^{E}P_ {T} \0 & 1
\end{bmatrix} \tag{20}
\]
??显然，TCF六点法标定的最小条件是能够获取到6个位置点的位姿BETi=1,2,3,4,5,6EBTi=1,2,3,4,5,6，且为使式(12)能求解，应保证位置点1，2，3，4不在同一平面上。
三、参考文献
【1】康存锋，王红伟，张鹏飞等.焊接机器人工具坐标系标定的研究与实现[j].北京工业大学学报 2016, 42(1).
【2】兰虎等.《工业机器人技术及应用》.
————————————————
版权声明：本文为CSDN博主「高山垚」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_33816175/article/details/113716996
标签：机器人,标定,TCP,tag,bmatrix,TCF,工具,坐标系 来源： https://www.cnblogs.com/mrliu0515/p/16209339.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。