【Robotics toolbox】（二）平移变换transl | 旋转坐标变换trot| t2t | r2t

关注微信公众号`二进制人工智能`，回复`robot`获取机器人工具箱

1 坐标变换

在三维空间中,任一坐标系均可作为描述一个物体位姿的参考坐标系。在机器人学的许多问题中,常常需要在不同的参考坐标系中表示同一个物体的位姿。例如,图1-8的应用例子中，需要在相机的指导下﹐使用机器人的手臂去夹取一个目标物体。因此，需要获取世界坐标系、固定相机的坐标系.机器人的坐标系.机器人的相机坐标系、目标物体的坐标系的位置与姿态，使得机器人的末端执行器能够达到夹取目标物体所需要的位姿,执行夹取任务。

1.1 平移坐标变换

如图1-9所示，坐标系$A$没有经过旋转，只经过平移得到坐标系 { B } \{B\} {B}。$P$是$B$坐标系中的一点，用矢量${}^{B}P$表示它在坐标系 { B } \{B\} {B}中的位置；${}^A{P}_{BORG}$表示$A$坐标系平移到$B$坐标系的距离。当$P$用坐标系表示时，用矢量${}^{A}P$表示。因为矢量${}^{A}P$和${}^{B}P$具有相同的姿态，所以可以使用矢量相加的方法表示${}^{A}P$：

用$4\times 4$的齐次矩阵表示平移变换矩阵$T$，则

其中，$p_x，p_y和p_z$ ,是平移向量$Р$相对于参考坐标系$X$轴、$Y$轴和$Z$轴的3个分量。与矢量和旋转矩阵一样,坐标系还可以用上述变换算子表示。

对于平移坐标变换,机器人工具箱提供了函数transl()计算一段平移相对应的平移变换矩阵。

例如，空间中的一个坐标系 { A } \{A\} {A}，它可以表示为：

如果将这个坐标系沿着参考坐标系的$Y$轴移动10个单位，然后再沿着$Z$轴移动5个单位得到坐标系 { B } \{B\} {B}，求坐标系 { B } \{B\} {B}的表示。

A=[0.527,-0.574,0.628,5;0.369,0.819,0.439,3;-0.766,0,0.643,8;0,0,0,1];
T=transl(0,10,5)
B=T*A

1.2 旋转平移矩阵

如图1-10所示，当坐标系 { A } \{A\} {A}没有经过平移，只经过旋转时（旋转矩阵为${}_B^{A}R$），得到坐标系 { B } \{B\} {B}。同一个点$P$在坐标系 { A } \{A\} {A}和 { B } \{B\} {B}中的表示分别为${}^{A}P$和${}^{B}P$，这两个矢量有这样的变换关系：

机器人工具箱分别提供了trotx(),troty(),trotz()三个函数计算旋转变换矩阵，分别对应于$X$轴，$Y$轴，$Z$轴旋转一定的角度，得到的是一个$4\times 4$的矩阵。

例如：使用上面的坐标系$A$，绕着$X$轴旋转30°，求旋转后的坐标系 { C } \{C\} {C}：

A=[0.527,-0.574,0.628,5;0.369,0.819,0.439,3;-0.766,0,0.643,8;0,0,0,1];
T=trotx(pi/6)
C=T*A

1.3 齐次坐标变换

如图1-11所示，当坐标系 { A } \{A\} {A}经过平移（距离为${}^A{P}_{BORG}$）于旋转（旋转矩阵为^A_BR）得到坐标系 { B } \{B\} {B}。同一个点$P$在坐标系 { A } \{A\} {A}和 { B } \{B\} {B}中的表示分别为${}^{A}P$和${}^{B}P$，这两个矢量有如下的变换关系：

从上式可以得出在不同坐标系中的位姿${}^{A}P$和${}^{B}P$之间的关系，但在上式中${}^{A}P$和${}^{B}P$并不是齐次变换的关系，结果只能简易计算相邻坐标系中位姿之间的关系，当不是相邻甚至需要得到末端执行器和基座坐标系上位姿的关系时,计算量太大,太复杂,所以应该将这个等式写成齐次变换的形式。因为在三维矩阵计算不能充分描述齐次变换,所以增加了矩阵的维数,将等式改写为四维矩阵的形式，即:

将转换因子用齐次变换矩阵${}_B^{A}T$表示，即：

因此上式可以表示为：

对于旋转矩阵${}_B^{A}R$，根据正交矩阵的性质可以得到：

而对于旋转矩阵，其逆矩阵不等于原矩阵，经过推导，可得到：

因此，得到了相邻坐标系间矢量（旋转矢量和移动矢量）的齐次变换。

下面用例子说明如何用机器人工具箱解决齐次变换的问题。

解：
（1）旋转矩阵${}_B^{A}R=R(y，60°)$，平移矢量为${}^A{p}_B=[403{]}^T$

输入命令，得到${}_B^{A}T$：

T=transl(4,0,3)*troty(pi/3)

点$p_1$在坐标系 { A } \{A\} {A}中的描述为${}^{A}P{=}_B^A{T}^{B}P$，输入命令：

p=T*[2;4;3;1]

得到${}^{A}P$

（2）由${}^{A}P{=}_B^A{T}^{B}P$可得到${}^{B}P{=}_B^A{T}^{-1}{\cdot }^{A}P$,输入命令：

p2=inv(T)*[2;4;3;1]

运行结果：

画出坐标系A和B：

T=transl(4,0,3)*troty(pi/3);
T0=transl(0,0,0);
trplot(T0,'frame','A','color','b');
hold on;
trplot(T,'frame','B','color','r');
axis([0,5,0,5,0,5]);

此外，关于齐次变换矩阵${}_B^{A}T$，机器人工具箱提供了函数t2r()可以提取旋转矩阵分量：

R=t2r(T)

而函数r2t()则可以将旋转矩阵转换成对应的齐次变换矩阵。如通过运行指令r2t(R)可以得到上面的矩阵T。

T1=r2t(R)

通过以下的命令可以提取平移变换分量：

p=transl(T)

2 相关函数详细用法

2.1平移变换transl

（1）使用transl()创建平移变换矩阵。

T= transl(x，y，z):表示能够获取一个分别沿着工$x，y，z$轴平移一段距离得到的4×4齐次变换矩阵。

T = transl(p):表示由经过矩阵(或向量)p =[r，y，z]的平移得到的齐次变换矩阵。
如果p为$(M\times 3)$的矩阵,则$T$为一组齐次变换矩阵$(4\times 4\times M)$，其中T(:,: ,i)对应于p的第i行。
（2）使用transl提取一个矩阵中的平移变换分量。

[x, y,z]= transl(T): $x，y，z$是齐次变换矩阵中的三个分量，是一个$1\times M$的向量。

p =transl(T):p是齐次变换矩阵中T的平移部分，是一个$3\times M$的矩阵。

2.2 旋转坐标变换

（1）T=trotx(0):表示围绕X轴旋转((弧度)得到的齐次变换矩阵(4×4)。

（2）T=troty(0):表示围绕Y轴旋转0(弧度)得到的齐次变换矩阵(4×4)。

（3）T=trotz(0):表示围绕Z轴旋转0(弧度)得到的齐次变换矩阵(4×4)。

以上3个函数中,可选参数为deg,表示角度值单位为度(degree)，如totx(180,'deg')。

2.3 t2r()

R= t2r(T)获取齐次变换矩阵T中正交旋转矩阵分量。如果T是一个4×4的矩阵，则R是一个3×3的矩阵﹔如果T是一个3×3的矩阵,则R是一个2×2的矩阵。

2.4 r2t()

函数r2t()可将旋转矩阵转换为齐次变换矩阵。T=r2t(R)获取一个正交旋转矩阵R等价的具有零平移分量的齐次变换矩阵。如果R是一个3×3的矩阵,则T是一个4×4的矩阵;如果R是一个2×2的矩阵,则T是一个3×3的矩阵。

《机器人仿真与编程技术》 杨辰光 李智军 许扬