TCP坐标标定

TCP（Tool Center Point）工具座标系标定
在机器人工具上建立一个工具坐标系，其原点即为工具中心点（Tool Center Point，TCP）。单臂：法兰到工具坐标系的标定

举例说明：https://support.pickit3d.com/article/43-defining-the-tool-center-point-tcp-on-a-universal-robot

」四点法「实施方法：

在机器人的工作空间内放置一个固定点
通过控制机器人的姿态，使TCP与空间内的固定点重合
重复上述步骤3次，改变机器人的姿态使 TCP 移动到同一点
以四次TCP点在世界坐标系中坐标相等为条件来建立方程组并求解，从而实现工具坐标系位置的标定。

let $g_{bt}$ is base to tool, $g_{be}$ is base to end-effector, $g_{et}$ is end-effector to tool.

then $g_{bt}$ = $g_{be}cdot g_{et}$

i.e./that is (to say)：

$left[ egin{matrix} R_{bt}&p_{bt}\ 0&1\ end{matrix} ight] =left[ egin{matrix} R_{be}&p_{be}\ 0&1\ end{matrix} ight] cdot left[ egin{matrix} R_{et}&p_{et}\ 0&1\ end{matrix} ight] = left[ egin{matrix} R_{be}cdot R_{et}&R_{be}cdot p_{bt}+p_{be}\ 0&1\ end{matrix} ight]$

so,

$p_{bt}=R_{be}cdot p_{bt}+p_{be}$

其中， $R_{et}=e^{hat{w}0}=I+hat{w}sin0+hat{w}^2(1-cos0)=I$ ,位置矢量 $p_{et}$ 未知，为所求。

$R_{be}、p_{be}$ 为已知，因为通过示教，触碰一个目标点，（Q: 触碰一个目标点，在机器上是直接可以得到前向运动学的解的吗？还是说可以看到各个关节的theta值）/A: 其它paper是说，，从机器人控制器读取当前的关节转角为，也可以在下图拿到位置信息：

$p_{bt}=R^1_{be}cdot p_{et}+p^1_{be}$

通过多种位姿触碰同一个目标点：

$p_{bt}=R^2_{be}cdot p_{et}+p^2_{be}$

$p_{bt}=R^3_{be}cdot p_{et}+p^3_{be}$

i.e.

$R^1_{be}cdot p_{bt}+p^1_{be}=R^2_{be}cdot p_{bt}+p^2_{be}$

i.e.

$(R^1_{be}-R^2_{be})cdot p_{et}=p^2_{be}-p^1_{be}$

Similarly,

$(R^1_{be}-R^3_{be})cdot p_{et}=p^3_{be}-p^1_{be}$

$(R^1_{be}-R^4_{be})cdot p_{et}=p^4_{be}-p^1_{be}$

因为 $p_{et}$ 为3*3 matrix

so，

$left[ egin{matrix} (R^1_{be}-R^2_{be})\ (R^1_{be}-R^3_{be})\ (R^1_{be}-R^4_{be})\ end{matrix} ight] cdot p_{et} = left[ egin{matrix} p^2_{be}-p^1_{be}\ p^3_{be}-p^1_{be}\ p^4_{be}-p^1_{be} end{matrix} ight]$