机器人基础知识（1.2.2）平移与旋转组合

现在我们回到三维空间中相对位姿的表示，即两个坐标系之间位置和姿态的变化，如下图所示。在本文将介绍使用$4\times 4$齐次变换矩阵的方法表示旋转和平移。
使用一个齐次变换矩阵来表示旋转和转换，推导的方法与二维的类似在此基础上增加了$z$轴，可写为：
$$\left(\begin{array}{c}{}^{A}x\\ {}^{A}y\\ {}^{A}z\\ 1\end{array}\right)=\left(\begin{array}{cc}{}^A{R}_B& t\\ 0_{1\times 3}& 1\end{array}\right)\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\\ {}^{B}z\\ 1\end{array}\right)$$
坐标系原点之间的笛卡尔平移向量是$t$，姿态的变化由一个$3\times 3$正交子矩阵$R$表示，其余向量都表示为齐次形式，可以写成
$${}^A\tilde{p}=\left(\begin{array}{cc}{}^A{R}_B& t\\ 0_{1\times 3}& 1\end{array}\right){}^B\tilde{p}={}^A{T}_B{}^B\tilde{p}$$
${}^A{T}_B$是一个$4\times 4$阶齐次变换矩阵。

>>T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)
>T=    1.0000         0         0    1.0000
         0    0.0000   -1.0000    0.0000
         0    1.0000    0.0000    1.0000
         0         0         0    1.0000

函数$transl$创建了一个有平移但无旋转的相对位姿，而函数$trotx$则返回一个绕$x$轴旋转$pi/2$的$4\times 4$齐次变换矩阵：旋转部分与$rotx(pi/2)$相同，平移部分为零。可以将以上函数组合对应的坐标系变化描述如下：首先沿着$x$轴方向前进一个单位长度，然后绕$x$轴旋转$90^{\circ }$,接着沿新的$y$轴，也就是原来的$z$轴前进一个单位长度。所得矩阵的最后一列，表示了沿原坐标系的$x$轴和$z$轴各平移一个单位长度的结果。从最终位姿矩阵的姿态部分，则可以看出是绕$x$轴旋转的结果。可以用以下的函数绘制相应的坐标系：

>>trplot(T)    #建立的坐标系
>>t2r(T)    #旋转部分
>>transl(T)'    #平移部分

所建立的坐标系如下图所示：

前面几节讲述了二维和三维世界中表示点和位姿，以及用正交旋转矩阵表示姿态，并用它的延伸——齐次变换矩阵表示姿态和平移。