機器人學——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