通过DH建模,可计算出笛卡尔空间中,从机器人基坐标原点到末端法兰中心点的位姿矩阵,记为$_{E}^{B}T$。
从末端法兰中心点到工具尖端点,也可以通过一个位姿矩阵来描述,记为 $_{T}^{E}T$。由于该两点间并没有自由度变量,所以对于某一确定工具的确定尖端, $_{T}^{E}T$总是定值常量。
根据机器人位姿变换规则,从机器人基坐标原点到工具尖端点的位姿矩阵,可由矩阵$_{E}^{B}T$右乘矩阵$_{T}^{E}T$得到,记为$_{T}^{B}T$。
$${}_E^BT*{}_T^ET = {}_T^BT$$
工具标定算法的实质,就是要求出矩阵$_{T}^{E}T$。对于4x4的矩阵$_{T}^{E}T$,可分块表示为:
$${}_T^ET = \left[
\begin{matrix}
{}_T^ER & {}_T^EP \\\
0 & 1
\end{matrix}
\right]$$
其中,$_{T}^{E}R$为3x3矩阵,表示姿态偏移转换;$_{T}^{E}P$为3x1矩阵,表示位置偏移转换。
进行6点工具标定时,前3点位置(XYZ)相同,需满足工具目标尖端与某一固定点刚好碰上;姿态(ABC)任意,需满足尽可能姿态差别明显。后3点姿态(ABC)相同,位置(XYZ)不同,若第4点垂直接触于固定点;第5点在第4点的基础上,另工具尖端的位置偏移到期望工具坐标系的X+方向;第6点在第4点的基础上,另工具尖端的位置偏移到期望工具坐标系的Z+方向。
$$\begin{matrix}
\because
{}_E^B{T_i}*{}_T^ET = {}_T^B{T_i}\\\
\therefore \left[ {\begin{matrix}
{{}_E^B{R_i}}&{{}_E^B{P_i}}\\\
0&1
\end{matrix}} \right]*\left[ {\begin{matrix}
{{}_T^ER}&{{}_T^EP}\\\
0&1
\end{matrix}} \right] = \left[ {\begin{matrix}
{{}_T^B{R_i}}&{{}_T^B{P_i}}\\\
0&1
\end{matrix}} \right]\\\
\therefore {}_E^B{R_i}*{}_T^EP + {}_E^B{P_i} = {}_T^B{P_i}
\end{matrix}$$
由于前4点的位置(XYZ)相同:
$$\begin{matrix}
{}_T^B{P_{\left( {i = 1..4} \right)}} = {}_E^B{R_1}*{}_T^EP + {}_E^B{P_1}\\\
= {}_E^B{R_2}*{}_T^EP + {}_E^B{P_2}\\\
= {}_E^B{R_3}*{}_T^EP + {}_E^B{P_3}\\\
= {}_E^B{R_4}*{}_T^EP + {}_E^B{P_4}
\end{matrix}$$
移项可得:
$$\left\{ {\begin{matrix}
{{}_E^B{P_1} - {}_E^B{P_2} = \left( {{}_E^B{R_2} - {}_E^B{R_1}} \right)*{}_T^EP}\\\
{{}_E^B{P_2} - {}_E^B{P_3} = \left( {{}_E^B{R_3} - {}_E^B{R_2}} \right)*{}_T^EP}\\\
{{}_E^B{P_3} - {}_E^B{P_4} = \left( {{}_E^B{R_4} - {}_E^B{R_3}} \right)*{}_T^EP}
\end{matrix}} \right.$$
改写成矩阵形式可得:
$${}_T^EP = {\left[ {\begin{matrix}
{{}_E^B{R_2} - {}_E^B{R_1}}\\\
{{}_E^B{R_3} - {}_E^B{R_2}}\\\
{{}_E^B{R_4} - {}_E^B{R_3}}
\end{matrix}} \right]^ + }*\left[ {\begin{matrix}
{{}_E^B{P_1} - {}_E^B{P_2}}\\\
{{}_E^B{P_2} - {}_E^B{P_3}}\\\
{{}_E^B{P_3} - {}_E^B{P_4}}
\end{matrix}} \right]\\$$
$$\therefore
{}_T^EP = {\left( {{{\left[ {\begin{matrix}
{{}_E^B{R_2} - {}_E^B{R_1}}\\\
{{}_E^B{R_3} - {}_E^B{R_2}}\\\
{{}_E^B{R_4} - {}_E^B{R_3}}
\end{matrix}} \right]}^T}*\left[ {\begin{matrix}
{{}_E^B{R_2} - {}_E^B{R_1}}\\\
{{}_E^B{R_3} - {}_E^B{R_2}}\\\
{{}_E^B{R_4} - {}_E^B{R_3}}
\end{matrix}} \right]} \right)^{ - 1}}*{\left[ {\begin{matrix}
{{}_E^B{R_2} - {}_E^B{R_1}}\\\
{{}_E^B{R_3} - {}_E^B{R_2}}\\\
{{}_E^B{R_4} - {}_E^B{R_3}}
\end{matrix}} \right]^T}*\left[ {\begin{matrix}
{{}_E^B{P_1} - {}_E^B{P_2}}\\\
{{}_E^B{P_2} - {}_E^B{P_3}}\\\
{{}_E^B{P_3} - {}_E^B{P_4}}
\end{matrix}} \right]$$
将前4点位姿带入上式,即可求得位置偏移$_{T}^{E}P$。
由第4点和第5点确定工具坐标系的X+方向向量$_{T}^{B}N$
由第4点和第6点确定工具坐标系的Z+方向向量$_{T}^{B}A$
$$\left\{ {\begin{matrix}
{{}_T^BN = \frac{{{}_T^B{P_5} - {}_T^B{P_4}}}{{\left| {{}_T^B{P_5} - {}_T^B{P_4}} \right|}}}\\\
{{}_T^BA = \frac{{{}_T^B{P_6} - {}_T^B{P_4}}}{{\left| {{}_T^B{P_6} - {}_T^B{P_4}} \right|}}}
\end{matrix}} \right.$$
再根据XYZ三个方向向量,两两正交,$_{T}^{B}A$与$_{T}^{B}N$进行矩阵叉乘,可求得Y+方向向量$_{T}^{B}O$。
$${}_T^BO = {}_T^BA \times {}_T^BN$$
$${}_T^BA = {}_T^BN \times {}_T^BO$$
因此,可求得从机器人基坐标原点到工具尖端点的位姿矩阵为:
$${}_T^BR = \left[ {\begin{matrix}
{{}_T^BN}&{{}_T^BO}&{{}_T^BA}
\end{matrix}} \right]$$
根据后3点位姿相同,求得从末端法兰中心点到工具尖端点的姿态偏移矩阵$_{T}^{E}R$:
$$\because {}_E^B{R_4} = {}_E^B{R_5} = {}_E^B{R_6}$$
$$\therefore {}_T^BR = {}_E^B{R_4}*{}_T^ER = {}_E^B{R_5}*{}_T^ER = {}_E^B{R_6}*{}_T^ER$$
$$\therefore {}_T^ER = {}_E^B{R_4}^{ - 1}*{}_T^BR$$
最终,可组合出从末端法兰中心点到工具尖端点的完整位姿偏移矩阵$_{T}^{E}T$:
$${}_T^ET = \left[
\begin{matrix}
{}_T^ER & {}_T^EP \\\
0 & 1
\end{matrix}
\right]$$
[1]熊烁,叶伯生,蒋明.机器人工具坐标系标定算法研究[J].机械与电子,2012(06):60-63.