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]$