UR机器人之一：坐标系及位姿表示方法

一、UR机器人坐标系

默认情况下UR机器人的基坐标系和TCP如下图所示：

注意这里的TCP位置和坐标系都是在默认TCP配置的情况下，默认的TCP配置如下图所示：

如果用户想要自己设置TCP的位置和坐标系就可以在这个基础上进行配置，上图中的X,Y,Z,RX,RY,RZ表示用户自定义TCP与机器人默认TCP之间的关系（注意不是相对于基坐标系的关系）。X,Y,Z表示位置关系，RX,RY,RZ是使用的旋转矢量法表示的姿态关系，旋转矢量法见下一节。
当UR机器人回零后，机器人默认的TCP相对于基座标系的位姿如下图：

注意这里是实际值，与理论值相比有一些误差，比如理论值X=0，而实际可能不是0。这里的姿态描述方式使用的是旋转矢量法，见下一节。

二、UR机器人Pose的表示方式

UR机器人的Pose表示的是TCP坐标系相对于基座标系的位置和姿态。一个坐标系相对于另一个坐标系的姿态的表示方法有很多种，比如：X-Y-Z固定角，Z-Y-X欧拉角，RPY角，四元数等。而UR机器人里面使用的是旋转矢量法。
在介绍旋转矢量法之前先看一下等效轴角坐标系表示法。在《机器人学导论》中对等效轴角法有详细讲解，如下图所示，这种方法是用一个单位向量
$$\hat{K}=\left[\begin{array}{ccc}kx& ky& kz\end{array}\right]$$
和一个转动角度θ来表示两个坐标系之间的旋转关系。具体介绍如下图所示：

而旋转矢量法用一个旋转矢量：
$$R=\left[\begin{array}{ccc}Rx& Ry& Rz\end{array}\right]$$
来表示两个坐标系之间的旋转关系，其中旋转矢量法与等效轴角坐标系之间的关系是：Rx=θkx，Ry=θky，Rz=θkz。
由旋转矢量求旋转矩阵的方法可以按照如下公式：

使用上述公式之前需要先求出θ和kx,ky,kz，求法如下：
$$\theta =\sqrt{R{x}^2+R{y}^2+R{z}^2}$$
$$kx=Rx/\theta ,ky=Ry/\theta ,kz=Rz/\theta$$
可以看出如果θ=0°或180°时，旋转轴根本无法确定，上式也将无解。这也是旋转矢量法的一个明显缺点。
反过来，如果已知旋转矩阵，可以通过如下公式求解等效轴角表示法，进而求出旋转矢量表示法：

注意2-82中的r33应改为r23，这是书中的一处错误。

由以上叙述可以知道，对于同一姿态，旋转矩阵的表示不唯一。所以在UR机器人中，通过面板观察到的TCP Pose可能与通过脚本命令get_actual_tcp_pose()所得到的不一样，这是正常现象。如果不相信可以将它们都转换成RPY角就会发现，它们表示的是同一个姿态。特别是在零点位置时比较明显，如下面两个图所示，可以看出在面板上看到的Rx,Ry,Rz与通过脚本函数get_actual_tcp_pose()获得的TCP姿态不一样，但其实它们表示的是同种TCP姿态。


另外，UR提供了一个脚本函数rotvec2rpy(rotation_vector)，可以将旋转矢量表示的姿态转换成rpy角表示的姿态，但是这里传递的参数是一个list：[Rx,Ry,Rz]，而怎么把pose里的RX,RY,RZ分离出来传递个这个函数呢？很简单：
UR里面的pose是一种数据类型，假设pose=p[px,py,pz,Rx,Ry,Rz]，那么list=[pose[3],pose[4],pose[5]]，这样就可以把pose里表示姿态的[Rx,Ry,Rz]提取出来给到一个list里面。
还有一个pose_trans(p_from,p_from_to)函数，p_from表示坐标系B相对于A的pose，p_from_to表示坐标系C相对于B的pose，函数的返回值表示坐标系C相对于A的pose。pose与变换矩阵有一一对应的关系，用矩阵的方式理解这个函数就是：
$$T_C^A=T_B^A{T}_C^B$$