1. 引言

上一篇文章我們講到了齊次變換矩陣，表面上看我們得到了一個十分簡潔的表達式。如果你認爲到這裏關於齊次變換的問題已經十分明瞭那就大錯特錯了。相反，我認爲這恰恰是混亂的開始。

舉個例子，首先一個座標系{A}繞其軸旋轉 $\alpha$ 得到座標系{B}，根據前面講過的知識你很容易明白座標系{A}和{B}之間的關係爲 $T_{B}^{A}=rot_{x}\left ( \alpha \right )$ 。可是如果座標系{B}再繞自身的軸旋轉 $\beta$ 得到座標系{C}，那麼座標系{A}和座標系{C}之間是什麼關係呢？還有如果座標系{B}繞座標系{A}的軸旋轉 $\beta$ 得到座標系{D}，那麼座標系{A}和座標系{D}之間是什麼關係呢？

你可能沒有答案。課堂上這個問題也總是被匆匆略過，而我認爲這裏纔是應該花大量的精力去講解的地方。只告訴我當繞原座標系旋轉時變換矩陣要倒着乘，當繞新座標系旋轉時變換矩陣要正着乘我是無法接受的。

例子中的問題其實涉及到了齊次變換矩陣的幾種解讀，從這幾種解讀方式出發，我們很容易理解矩陣相乘的順序到底是怎樣的以及爲什麼是這樣的。

2. 齊次變換矩陣的三種解讀

斯坦福大學公開課---機器人學，這門課對這三種解讀給出了很恰當的總結：座標系表示（coordinate representation）、座標系變換（coordinate transformation）、點的操作算子（point operator）。接下來我們分別對它們進行解釋。

2.1 座標系表示

所謂座標系表示指的是，齊次變換矩陣可以用來表示一個座標系。我們來看如下圖兩個座標系{A}和{B}。通過上一篇文章機器人正運動學---齊次變換矩陣的介紹我們知道座標系{A}和座標系{B}之間的關係爲：

$^{A}\textrm{T}_{B}=\begin{bmatrix} ^{A}\textrm{R}_{B} & p_{Borg}^{A}\\ 0 & 1 \end{bmatrix}$

在文章機器人正運動學---理解變換矩陣中我們介紹了旋轉矩陣 $^{A}\textrm{R}_{B}$ （{A}和{B}的姿態關係）代表的是座標系{B}三個軸的單位矢量在座標系{A}中的投影（即在座標系{A}中座標系{B}的三軸單位矢量的座標），在文章機器人正運動學---齊次變換矩陣中我們也看到 $p_{Borg}^{A}$ （{A}和{B}的位置關係）代表的是座標系{B}的原點在座標系{A}中的座標。

我們知道，要想唯一確定一個座標系需要兩個要素，一個是原點，一個是軸矢量。這兩個要素在上面的齊次變換矩陣中已經全部包含了，因此我們說齊次變換矩陣可以用來表示一個座標系，而 $^{A}\textrm{T}_{B}$ 就是座標系{B}在座標系{A}中的表示

2.2 座標系變換

所謂座標系變換指的是，齊次變換矩陣可以用來描述一個座標系經過怎樣的平移和旋轉能夠變換到另一個座標系。舉個例子，兩個座標系{A}和{B}之間的齊次變換矩陣可以表示爲：

$^{A}\textrm{T}_{B}=\begin{bmatrix} rot_{x}(\pi /4) & trans(1,2,3)\\ 0 & 1 \end{bmatrix}$

其中 $rot_{x}(\pi /4)$ 代表繞軸旋轉 $\pi/4$ 對應的旋轉矩陣，，代表在方向上平移1,2,3。這個時候我們就可以通過對座標系{A}進行一些平移和旋轉最終與座標系{B}重合。這個操作過程對應於下面這張圖。

我們把這個操作過程分爲兩個步驟。

第一步將座標系{A}沿着橘色的向量平移至與座標系{C}重合，我們很容易知道這個平移量是；

第二步將座標系{C}繞它的軸旋轉至與座標系{B}重合，同樣容易知道這個旋轉量是 $rot_{x}(\pi /4)$ 。因此我們說齊次變換矩陣可以用來描述座標系的變換關係，而 $^{A}\textrm{T}_{B}$ 就是座標系{A}經過平移和旋轉可以得到座標系{B}。我們可以形象地描述（不一定恰當）這個變換爲：

$\{B\}=\{A\} \star ^{A}\textrm{T}_{B}$

2.3 點的操作算子

所謂點的操作算子指的是，齊次變換矩陣可以用來對同一個座標系內的點進行平移和旋轉操作。注意這種理解和前面兩種大不一樣。前兩種都是在描述兩個座標系之間的關係。在討論齊次變換矩陣對點的操作時我們的討論僅限於一個座標系。

舉個例子在座標系{A}中有一個點 $P_{0}$ ，我們希望這個點繞座標系{A}的軸旋轉 $\pi/4$ 得到一個新的點 $P_{1}$ 。這個時候應該怎麼做呢？很簡單，我們寫出如下的變換矩陣：

$R=rot_{x}(\pi/4)=\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\pi/4) & -sin(\pi/4) \\ 0 & sin(\pi/4) & cos(\pi/4) \end{bmatrix}$

用這個變換矩陣去乘點 $P_{0}$ 就可以得到點 $P_{1}$ 啦。

如果你覺得理解這個有點困難，可以倒回到第一種理解來解釋對點的操作問題。這個時候我們假設有一個座標系{B}開始時與座標系{A}完全重合，而點 $P_{0}$ 又固定在座標系{B}中，這樣在把點 $P_{0}$ 繞座標系{A}的軸旋轉 $\pi/4$ 時，座標系{B}同樣是繞座標系{A}的軸旋轉 $\pi/4$ 。此時兩個座標系之間的變換矩陣即爲 $rot_{x}(\pi /4)$ 。由於點 $P_{0}$ 固定在座標系{B}中，因此其在座標系{B}中的座標不變，因此經過旋轉之後點 $P_{0}$ 在座標系{A}中的座標即爲 $rot_{x}(\pi /4)\cdot P_{0}$ 。我們可以看出這個點就是前面提到的在座標系{A}中的 $P_{1}$ 點。

光用文字來描述似乎有些凌亂，下面我們用圖把上面一段話再重新捋一遍，首先我們假設空間中一點 $P_{0}$ 在座標系{A}中的座標爲 $P_{0}(y_{0},z_{0})$ ，爲了便於看圖，座標系我們只畫出y，z兩個軸，所有點的x座標均爲0。與點 $P_{0}$ 固連的座標系稱爲{B}。座標系{B}最開始與座標系{A}重合，如下圖所示就是問題的初始狀態。

接下來開始旋轉操作。我們是希望點 $P_{0}$ 沿座標系{A}的軸旋轉 $\pi/4$ ，注意因爲座標系{B}和點 $P_{0}$ 是固連在一起的，所以座標系{B}也會繞着座標系{A}的軸旋轉 $\pi/4$ ，經過旋轉之後就得到如下的圖的狀態。點 $P_{1}$ 就是點 $P_{0}$ 旋轉之後對應的點。這裏有一點需要特別注意，由於座標系{B}與點 $P_{0}$ 固連，那麼經過旋轉後得到的 $P_{1}$ 點在座標系{B}中的座標數值實際上沒有發生任何變化（和最開始的時候點 $P_{0}$ 在座標系{A}中的座標值是一樣的），請仔細體會這句話。

下面我們把變換前後的兩種狀態合併起來得到下面的圖

至此我們可以得出這樣一個結論，點 $P_{0}$ 在座標系{A}中的座標值與點 $P_{1}$ 在{B}中的座標值是一致的。那麼如何求解點 $P_{1}$ 在座標系{A}中的座標值呢？仔細思考一下是不是這樣一種表達：

$P_{1}^{A}=R_{B}^{A}\cdot P_{1}^{B}$

解釋一下這個公式， $P_{1}^{A}$ 代表點 $P_{1}$ 在座標系{A}下的座標。 $P_{1}^{B}$ 代表 $P_{1}$ 在座標系{B}下的座標。還記得我們之前說的嗎， $P_{1}$ 在座標系{B}下的座標數值上與 $P_{0}$ 在座標系{A}中的座標數值一致！以上公式可以改寫爲

$P_{1}^{A}=R_{B}^{A}\cdot P_{0}^{A}$

至此我們看到 $R_{B}^{A}$ 實現了將點 $P_{0}$ 旋轉到 $P_{1}$ 點。你也可以清晰的看到上一個公式點的上標都是A，和我們之前說的同一個空間點在不同座標系下的表達還是有區別的。

3. 解決問題

3.1 齊次變換矩陣的逆

上一篇文章機器人正運動學---齊次變換矩陣中我們遺留了一個問題就是齊次變換矩陣的逆。通常而言求任意矩陣的逆是比較困難的。但是由於齊次變換矩陣的良好性質以及很強的幾何特徵，我們很容易直接寫出它的逆。

已知如下的齊次變換矩陣：

$^{A}\textrm{T}_{B}=\begin{bmatrix} ^{A}\textrm{R}_{B} & p_{Borg}^{A}\\ 0 & 1 \end{bmatrix}$

首先看旋轉部分，在文章機器人正運動學---理解變換矩陣中我們提到過 $^{A}\textrm{R}_{B}$ 的列代表座標系{B}的三軸矢量在座標系{A}中的投影，行代表座標系{A}的三軸矢量在座標系{B}中的投影，因此， $^{A}\textrm{R}_{B}$ 的轉置就是其逆。

其次來看平移部分， $p_{Borg}^{A}$ 代表座標系{B}的原點在座標系{A}中的座標，而我們需要的是座標系{A}的原點在座標系{B}中的座標，即 $p_{Aorg}^{B}$ ，從幾何的角度看，這兩個向量恰好是相反向量，因此我們首先把向量 $p_{Borg}^{A}$ 取反得到 $-p_{Borg}^{A}$ 。到這一步還沒有完，因爲目前爲止這個向量是在座標系{A}中描述的，因此我們要將它變換到座標系{B}中的描述，即爲 $-^{A}\textrm{R}_{B}^{T}\cdot p_{Borg}^{A}$ ，其中 $^{A}\textrm{R}_{B}^{T}$ 代表 $^{A}\textrm{R}_{B}$ 的轉置。

至此我們得到了座標系{A}相對於座標系{B}的姿態 $^{A}\textrm{R}_{B}^{T}$ ，以及座標系{B}中描述的座標系{A}原點座標 $p_{Aorg}^{B}=-^{A}\textrm{R}_{B}^{T}\cdot p_{Borg}^{A}$ 。因此變換矩陣的逆即爲：

$^{A}\textrm{T}_{B}^{-1}=\begin{bmatrix} ^{A}\textrm{R}_{B}^{T} & -^{A}\textrm{R}_{B}^{T}\cdot p_{Borg}^{A}\\ 0 & 1 \end{bmatrix}=^{B}\textrm{T}_{A}=\begin{bmatrix} ^{B}\textrm{R}_{A} & -^{B}\textrm{R}_{A}\cdot p_{Borg}^{A}\\ 0 & 1 \end{bmatrix}$

3.2 多重變換時的順序

在此我們解決文章開頭提出的多重變換時如何排列變換矩陣。其實就是兩種情況，第一種情況是所有的變換都相對於最開始的座標系；第二種情況是所有變換都相對於新得到的座標系。我們分情況討論。

首先來看第一種情況，座標系{A}繞其軸旋轉 $\alpha$ 得到座標系{B}，之後座標系{B}又繞座標系{A}的軸旋轉 $\beta$ 得到座標系{C}。此時我們用對齊次變換矩陣的第三種理解來求得座標系{C}和座標系{A}之間的關係。我們假設在座標系{A}中有一個點 $P_{0}$ ，在第一次變換時我們假設有一個座標系{T1}與座標系{A}重合，同時點 $P_{0}$ 固定在座標系{T1}中，經過第一次變換後得到在{A}中新的點 $P_{1}$ 的座標爲 $rot_{x}(\alpha )\cdot P_{0}$ （注意這是 $P_{1}$ 點在座標系{A}中的座標）。在第二次變換時我們假設又有一個座標系{T2}與座標系{A}重合，同時點 $P_{1}$ 又固定在座標系{T2}中，經過第二次變換後得到在{A}中新的點 $P_{2}$ 的座標爲 $rot_{y}(\beta )\cdot P_{1}=rot_{y}(\beta )\cdot rot_{x}(\alpha )\cdot P_{0}$ ，因此我們可以最終確定座標系{A}和{C}之間的變換關係爲：

$^{A}\textrm{R}_{C}=rot_{y}(\beta )\cdot rot_{x}(\alpha )$

再來看第二種情況，座標系{A}繞其軸旋轉 $\alpha$ 得到座標系{B}，之後座標系{B}又繞其軸旋轉 $\beta$ （注意這裏是繞座標系{B}的軸）。此時我們用齊次變換矩陣的第二種理解來求座標系{C}和座標系{A}之間的關係。設空間中有一個點，它在座標系{C}中的座標爲 $P_{C}$ ，我們首先看座標系{B}和座標系{C}之間的關係，由於{B}繞自身的軸旋轉 $\beta$ 就可以得到{C}，因此，同一個空間點在座標系{B}中的座標爲 $P_{B}=rot_{y}(\beta )\cdot P_{C}$ 。我們又知道座標系{A}繞其軸旋轉 $\alpha$ 得到座標系{B}，因此，同一個空間點在座標系{A}中的座標爲 $P_{A}=rot_{x}(\alpha )\cdot P_{B}=rot_{x}(\alpha )\cdot rot_{y}(\beta )\cdot P_{C}$ ，因此我們可以最終確定座標系{A}和{C}之間的變換關係爲：