机器人仿真中三种坐标系下运动的实现原理：关节坐标系、世界坐标系、工具坐标系

1、示教器控制（世界坐标、关节坐标、工具坐标系下机器人运动的实现思路）

1）情况说明：

机器人坐标系统基本上就只有两种：

第一种为关节坐标系；

第二种为世界坐标系；

2）机器人的运动方式概括为三种：

第一种为在世界坐标系下运动，即机器人末端TCP的运动沿着固定坐标系作变换（左乘，从右往左）；

第二种为关节坐标系下运动，即关节的运动；

第三种为工具坐标系下运动，即机器人末端TCP的运动沿着自身坐标系作变换（右乘，从左往右）；

无论哪种运动方式，最后呈现给用户的都是世界坐标和关节坐标；

3）世界坐标系下运动思路实现：

分为两块实现：

一是平移，沿着世界坐标的XYZ轴平移；

二是旋转，绕着世界坐标系的ZYX轴旋转；

a）关于平移：

即tcp（末端点位姿，世界坐标），当前的tcp(10,20, 40, 90, 0, 0)（ZYX欧拉角）沿着世界坐标的XYZ轴平移10个单位，以沿着X轴举例：

最简单的思路，tcp的X值加10，即x = x+10实现；

基于矩阵变换的思想分析：先构建平移的变换矩阵$T_{tran{s}_x}=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$,再构建tcp的变换矩阵$T_{tcp}=T_{trans}\ast R_z\ast R_y\ast R_x=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]$

最终的变换矩阵为（左乘，固轴，因为只有平移，所以右乘也一样）$T=T_{tran{s}_x}\ast T_{tcp}=\left[\begin{array}{cccc}0& -1& 0& 20\\ 1& 0& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]$，最终世界坐标为（20，20，40，90，0，0）；

b) 关于旋转：

即TCP绕着世界坐标轴的Z轴旋转90度：

只能基于矩阵变换分析：

$T=T_{ro{t}_z}\ast T_{tcp}=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}-1& 0& 0& -20\\ 0& -1& 0& 10\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]$

4）工具坐标系下的运动思路实现

a）关于平移

即TCP在自身坐标系下的平移，以沿着自身坐标系X轴平移10举例：

$T=T_{tcp}\ast T_{tran{s}_x}=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 30\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]$

b）关于旋转

即TCP绕着自身坐标系Z轴旋转90度：

$T=T_{tcp}\ast T_{ro{t}_z}=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}-1& 0& 0& 10\\ 0& -1& 0& 20\\ 0& 0& 1& 40\\ 0& 0& 0& 1\end{array}\right]$