scara机器人用户坐标系标定与工具坐标(TCP)标定

一、scara机器人用户坐标系标定

  如上图，坐标系 { x 0 O y 0 } \{x_0Oy_0\} { x0​Oy0​}为机器人基坐标系，坐标系 { x 1 P y 1 } \{x_1Py_1\} { x1​Py1​}为需要建立的用户坐标系，这里假设$z$轴垂直于纸面向外。用户坐标系标定也即确定机器人基坐标系与用户坐标系之间的关系。采用两点法便可标定scara机器人的用户坐标系：
  (1)机器人末端标定针对准点$P$，记录此时机器人末端坐标为$(x_P,y_P)$；
  (2)机器人末端标定针对准点$Q$(用户坐标系$x$轴上任意一点，$|PQ|$不能太短以免导致计算误差较大)，记录此时机器人末端坐标为$(x_Q,y_Q)$；
  (3)用户坐标系到机器人基坐标系的齐次变换矩阵为：
$$T=\left[\begin{array}{ccc}cos\theta & -sin\theta & x_P\\ sin\theta & cos\theta & y_P\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(1)}$$
  其中：
$$\theta =atan2(y_Q-y_P,x_Q-x_P)\text{\hspace{1em}(2)}$$
  关于绕任意轴(这里是绕$z$轴)的旋转矩阵的求解问题，详见另外一篇博文：三维空间任意一点绕任意轴线旋转。

二、scara机器人工具坐标(TCP)标定

1、工具固定在小臂

  如上图，scara机器人大臂为$L_1=|OA|$，小臂为$L_2=|AB|$，固定在小臂的工具为$CD$，${\theta }_1,{\theta }_2$为关节角度，坐标系 { x 0 O y 0 } \{x_0Oy_0\} { x0​Oy0​}为机器人基坐标系，坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}为机器人小臂末端坐标系。工具固定在小臂的$TCP$即为点$D$在坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}的坐标，可以利用两点法求得。
  将坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}沿向量$\overrightarrow{BA}$平移至点$A$，建立辅助坐标系 { x 1 A y 1 } \{x_1Ay_1\} { x1​Ay1​}。将点$D$对准机器人工作空间里任意一点，此时机器人两关节角度分别为${\theta }_1^{(1)},{\theta }_2^{(1)}$，将$|AD|$视为机器人的小臂长，根据运动学正解得：(详见博文：scara机器人运动学正逆解)
$$\{\begin{array}{c}{x}_D=L_{1}cos{\theta }_1^{(1)}+|AD|cos({\theta }_1^{(1)}+{\theta }_2^{(1)}+\alpha )\\ y_D=L_{1}sin{\theta }_1^{(1)}+|AD|sin({\theta }_1^{(1)}+{\theta }_2^{(1)}+\alpha )\end{array}\text{\hspace{1em}(3)}$$
  上式展开得：
$$\{\begin{array}{c}{x}_D=L_{1}cos{\theta }_1^{(1)}+|AD|[cos({\theta }_1^{(1)}+{\theta }_2^{(1)})cos\alpha -sin({\theta }_1^{(1)}+{\theta }_2^{(1)})sin\alpha ]\\ y_D=L_{1}sin{\theta }_1^{(1)}+|AD|[sin({\theta }_1^{(1)}+{\theta }_2^{(1)})cos\alpha +cos({\theta }_1^{(1)}+{\theta }_2^{(1)})sin\alpha ]\end{array}\text{\hspace{1em}(4)}$$
  设点$D$在坐标系 { x 1 A y 1 } \{x_1Ay_1\} { x1​Ay1​}的坐标为$(x_D^{\prime },y_D^{\prime })$，则$x_D^{\prime }=|AD|cos\alpha ,y_D^{\prime }=|AD|sin\alpha$，上式化简为：
$$\{\begin{array}{c}{x}_D=L_{1}cos{\theta }_1^{(1)}+cos({\theta }_1^{(1)}+{\theta }_2^{(1)})x_D^{\prime }-sin({\theta }_1^{(1)}+{\theta }_2^{(1)})y_D^{\prime }\\ y_D=L_{1}sin{\theta }_1^{(1)}+sin({\theta }_1^{(1)}+{\theta }_2^{(1)})x_D^{\prime }+cos({\theta }_1^{(1)}+{\theta }_2^{(1)})y_D^{\prime }\end{array}\text{\hspace{1em}(5)}$$

  在另一手系下，将点$D$对准机器人工作空间同一点，此时机器人两关节角度分别为${\theta }_1^{(2)},{\theta }_2^{(2)}$，同理可得：
$$\{\begin{array}{c}{x}_D=L_{1}cos{\theta }_1^{(2)}+cos({\theta }_1^{(2)}+{\theta }_2^{(2)})x_D^{\prime }-sin({\theta }_1^{(2)}+{\theta }_2^{(2)})y_D^{\prime }\\ y_D=L_{1}sin{\theta }_1^{(2)}+sin({\theta }_1^{(2)}+{\theta }_2^{(2)})x_D^{\prime }+cos({\theta }_1^{(2)}+{\theta }_2^{(2)})y_D^{\prime }\end{array}\text{\hspace{1em}(6)}$$
  令：
$$\{\begin{array}{l}{a}_{11}=cos({\theta }_1^{(2)}+{\theta }_2^{(2)})-cos({\theta }_1^{(1)}+{\theta }_2^{(1)})\\ a_{12}=-sin({\theta }_1^{(2)}+{\theta }_2^{(2)})+sin({\theta }_1^{(1)}+{\theta }_2^{(1)})\\ b_1=L_1(-cos{\theta }_1^{(2)}+cos{\theta }_1^{(1)})\\ a_{21}=sin({\theta }_1^{(2)}+{\theta }_2^{(2)})-sin({\theta }_1^{(1)}+{\theta }_2^{(1)})\\ a_{22}=cos({\theta }_1^{(2)}+{\theta }_2^{(2)})+cos({\theta }_1^{(1)}+{\theta }_2^{(1)})\\ b_2=L_1(-sin{\theta }_1^{(2)}+sin{\theta }_1^{(1)})\end{array}\text{\hspace{1em}(7)}$$

  联立式(5)和式(6)，解得：
$$\{\begin{array}{l}{x}_D^{\prime }=-(a_{12}{b}_2-a_{22}{b}_1)/(a_{11}{a}_{22}-a_{12}{a}_{21})\\ y_D^{\prime }=(a_{11}{b}_2-a_{21}{b}_1)/(a_{11}{a}_{22}-a_{12}{a}_{21})\end{array}\text{\hspace{1em}(8)}$$
  工具$TCP$为：
$$\{\begin{array}{l}{t}_{xD}=x_D^{\prime }-L_2\\ t_{yD}=y_D^{\prime }\end{array}\text{\hspace{1em}(9)}$$

2、工具固定在末端

  如上图，scara机器人大臂为$L_1=|OA|$，小臂为$L_2=|AB|$，固定在末端的工具为$BD$，其长度为$R$，${\theta }_1,{\theta }_2,{\theta }_4$为关节角度，坐标系 { x 0 O y 0 } \{x_0Oy_0\} { x0​Oy0​}为机器人基坐标系，坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}为机器人末端坐标系。工具固定在末端的$TCP$即为点$D$在坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}的坐标，可以利用两点法求得。
  将点$D$对准机器人工作空间里任意一点，此时机器人末端位姿为$(x_B^{(1)},y_B^{(1)},c_B^{(1)})$，根据运动学正解得:
$$\{\begin{array}{c}{x}_D=x_B^{(1)}+Rcos(c_B^{(1)}+\alpha )\\ y_D=y_B^{(1)}+Rsin(c_B^{(1)}+\alpha )\end{array}\text{\hspace{1em}(10)}$$
  上式展开得：
$$\{\begin{array}{c}{x}_D=x_B^{(1)}+R[cos(c_B^{(1)})cos\alpha -sin(c_B^{(1)})sin\alpha ]\\ y_D=y_B^{(1)}+R[sin(c_B^{(1)})cos\alpha +cos(c_B^{(1)})sin\alpha ]\end{array}\text{\hspace{1em}(11)}$$
  设点$D$在坐标系 { x e B y e } \{x_eBy_e\} { xe​Bye​}的坐标为$(t_{xD},t_{yD})$，则$t_{xD}=Rcos\alpha ,t_{yD}=Rsin\alpha$，上式化简为：
$$\{\begin{array}{c}{x}_D=x_B^{(1)}+cos(c_B^{(1)})t_{xD}-sin(c_B^{(1)})t_{yD}\\ y_D=y_B^{(1)}+sin(c_B^{(1)})t_{xD}+cos(c_B^{(1)})t_{yD}\end{array}\text{\hspace{1em}(12)}$$

  在另一位姿下，将点$D$对准机器人工作空间同一点，此时机器人末端位姿为$(x_B^{(2)},y_B^{(2)},c_B^{(2)})$，同理可得：
$$\{\begin{array}{c}{x}_D=x_B^{(2)}+cos(c_B^{(2)})t_{xD}-sin(c_B^{(2)})t_{yD}\\ y_D=y_B^{(2)}+sin(c_B^{(2)})t_{xD}+cos(c_B^{(2)})t_{yD}\end{array}\text{\hspace{1em}(13)}$$

  令：
$$\{\begin{array}{l}{a}_{11}=cos(c_B^{(2)})-cos(c_B^{(1)})\\ a_{12}=-sin(c_B^{(2)})+sin(c_B^{(1)})\\ b_1=-x_B^{(2)}+x_B^{(1)}\\ a_{21}=sin(c_B^{(2)})-sin(c_B^{(1)})\\ a_{22}=cos(c_B^{(2)})+cos(c_B^{(1)})\\ b_2=-y_B^{(2)}+y_B^{(1)}\end{array}\text{\hspace{1em}(14)}$$

  联立式(11)和式(12)，解得工具$TCP$为：
$$\{\begin{array}{l}{t}_{xD}=-(a_{12}{b}_2-a_{22}{b}_1)/(a_{11}{a}_{22}-a_{12}{a}_{21})\\ t_{yD}=(a_{11}{b}_2-a_{21}{b}_1)/(a_{11}{a}_{22}-a_{12}{a}_{21})\end{array}\text{\hspace{1em}(15)}$$