机器人学——1.9-三维空间平移与旋转的组合

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

对于向量-四元数的情况，有 $xisim(t, mathring{q})$，其中 $tinmathbb{R}^3$ 是坐标系原点相对于参考坐标系的笛卡儿位置，$mathring{q}inmathbb{Q}$ 是坐标系相对于参考坐标系的姿态。其加法定义如下:
$xi_1oplusxi_2=(t_1+mathring{q}_1cdot t_2,mathring{q}_1oplusmathring{q}_2)$取负数为
$ominusxi=(-mathring{q}^{-1}cdot t,mathring{q}^{-1})$一个点坐标向量通过下式在坐标系之间变换：
$^Xp={}^Xxi_Ycdot{}^Yp=mathring{q}cdot{}^Yp+t$

齐次变换矩阵法

另一种办法是可以使用一个齐次变换矩阵来表示旋转和转换。推导方法与二维空间位姿描述描
述的情况类似，但因为增加了 $z$ 轴，所以进行了扩展：
$left(egin{array}{c}^A ext{x}\^A ext{y}\^A ext{z}\1end{array} ight) = left(egin{array}{cc}{}^AR_B & t \ 0_{1 imes 3} & 1end{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}\^B ext{z}\1end{array} ight)$
坐标系原点之间的笛卡儿平移向量是 $t$，姿态的变化由一个 $3 imes 3$ 正交子矩阵 $R$ 表示，其余向量都表示成齐次形式，这样可以写成
$egin{array}{rl} ^A ilde{p}&=left(egin{array}{cc}{}^AR_B & t \ 0_{1 imes 3} & 1end{array} ight){}^B ilde{p}\[1.5em] &={}^AT_B{}^B ilde{p} end{array}$
$^AT_B$是一个 $4 imes4$ 阶齐次变换。这个矩阵有一个非常特殊的结构，属于特殊的三维欧几里得群，记作 $Tin SE(3)subset mathbb{R}^{4 imes4}$。
三维空间的位姿代数和二维空间有相似性，这里略过。

机器人工具箱中的齐次变换

>> 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 imes4$ 齐次变换矩阵：旋转部分与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