前面曾经讨论了几种不同的旋转姿态表示法，我们需要将它们与平移变换相结合，创造出一个完整的相对位姿表示方法。两种最实用的表示方法是：四元数向量对和 4×4 齐次变换矩阵。

对于向量-四元数的情况，有 是坐标系原点相对于参考坐标系的笛卡儿位置，q˚∈Q 是坐标系相对于参考坐标系的姿态。其加法定义如下:

一个点坐标向量通过下式在坐标系之间变换：

齐次变换矩阵法

另一种办法是可以使用一个齐次变换矩阵来表示旋转和转换。推导方法与二维空间位姿描述描
述的情况类似，但因为增加了 z  轴，所以进行了扩展：

坐标系原点之间的笛卡儿平移向量是 t ，姿态的变化由一个3×3 正交子矩阵 R  表示，其余向量都表示成齐次形式，这样可以写成

A T B 是一个4×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  轴旋转 π/2 的4×4 齐次变换矩阵：旋转部分与rotx(pi/2)相同，平移部分为零。我们可以将以
上函数组合对应的坐标系变化描述如下：首先沿着 x  轴方向前进一个单位长度，然后绕x轴旋转 90°，接着再沿新的 y  轴，也就是原来的 z  轴前进一个单位长度。所得矩阵的最后一列，表示了沿原坐标系的 x 轴和 z  轴各平移一个单位长度的结果。从最终位姿矩阵的姿态部分，则可以看出是绕 x  轴旋转的效果。我们可以用如下函数绘制出相应的坐标系：

>> trplot(T)

在这里插入图片描述
要提取矩阵T中的旋转矩阵部分，可用

>> t2r(T)
ans =
    1.0000         0         0
         0    0.0000   -1.0000
         0    1.0000    0.0000

平移部分是一个向量，可用以下函数获得：

>> transl(T)
ans =
    1.0000
    0.0000
    1.0000