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

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

对于向量-四元数的情况，有 $\xi\sim(t, \mathring{q})$，其中 $t\in\mathbb{R}^3$ 是坐标系原点相对于参考坐标系的笛卡儿位置， $\mathring{q}\in\mathbb{Q}$ 是坐标系相对于参考坐标系的姿态。其加法定义如下:
$\xi_1\oplus\xi_2=(t_1+\mathring{q}_1\cdot t_2,\mathring{q}_1\oplus\mathring{q}_2)$取负数为
$\ominus\xi=(-\mathring{q}^{-1}\cdot t,\mathring{q}^{-1})$一个点坐标向量通过下式在坐标系之间变换：
$^Xp={}^X\xi_Y\cdot{}^Yp=\mathring{q}\cdot{}^Yp+t$

齐次变换矩阵法

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

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

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