機器人運動學基礎——旋轉矩陣

旋轉矩陣是機器人運動學的基礎，但有時候經常會被各種旋轉求解問題搞暈，比如旋轉向量、旋轉座標系、同一空間點在不同座標系下的座標求解等等。正好當前需要對機械臂進行行爲規劃，所以又重新學習並理解了一下旋轉矩陣，特此記錄一下此次重溫的結果。

1. 本質理解

從本質上來理解，不管是座標系中的向量旋轉，還是同一空間點在不同座標系的座標求解等等，其最本質的原因都是座標系之間的變換。所謂萬變不離其宗，只要抓住了其本質屬性，就不會再隨便地被各種換着說法的求解問題搞暈。

因此在以下的介紹中會在不同的問題中尋找其共同點，總結出本質所在。

2. 基礎概念

涉及到機器人運動學，就繞不開剛體的位姿這一概念，即剛體的位置和姿態。教材中的說明示意圖如下圖所示：

2.1 位置

剛體可以由其在空間中相對於參考座標系的位置和方向進行完整的描述。在上圖中假設 $O-xyz$ 爲標準正交參考座標系，其中由藍色箭頭代表的$\boldsymbol{ x,y,z}$爲座標軸的單位向量。

則剛體上的點$O'$相對於座標系$O-xyz$的位置可以表示爲：$\boldsymbol o'=o'_x\boldsymbol x+o'_y\boldsymbol y+o'_z\boldsymbol z$ 其中$o'_x, o'_y, o'_z$表示向量$\boldsymbol o'$在參考座標系的座標軸上的分量。而$\boldsymbol o'$的位置寫爲(3X1)的向量爲：$\boldsymbol o' = \begin{bmatrix} o'_x \\ o'_y \\ o'_z \end{bmatrix}$而這個向量的完整描述可以表述爲基與座標的乘積：$\boldsymbol o' = \begin{bmatrix} \boldsymbol{x\, y\, z} \end{bmatrix} \begin{bmatrix} o'_x \\ o'_y \\ o'_z \end{bmatrix}$

2.2 姿態

除了位置之外，還需要對剛體的指向，即姿態進行描述。而這也是旋轉矩陣最本質的來源，即來源於 座標系的旋轉，這一部分理解不好或是理解不透徹，會導致此後面對各種形式的旋轉時出現混亂，所以公式將進行詳細的表述。

描述姿態的方式就是爲剛體建立一個固連於剛體的標準正交座標系，並由 其相對於參考座標系的 單位向量 在參考座標系中的描述 來表示。如上圖所示，剛體的局部固連參考系爲 $O-x'y'z'$ ，其局部固連參考系座標系的單位向量爲$\boldsymbol{ x',y',z'}$，而這局部座標系的單位向量在參考座標系 $O-xyz$ 中的表示爲：$\boldsymbol x' = a_1\boldsymbol x + a_2\boldsymbol y + a_3\boldsymbol z \\ \boldsymbol y' = b_1\boldsymbol x + b_2\boldsymbol y + b_3\boldsymbol z \\ \boldsymbol z' = c_1\boldsymbol x + c_2\boldsymbol y + c_3\boldsymbol z$其中$a_1, a_2, a_3$分別表示局部參考座標系的$x'$軸的基向量$\boldsymbol x'$在參考座標系 $O-xyz$ 三個軸上的分量。其它的兩軸同理。對於上式，可以進行進一步的整理，即上式等價於：$\boldsymbol{[x'\; y'\; z']} = \begin{bmatrix} \boldsymbol{x\; y\; z} \end{bmatrix} \begin{bmatrix} a_1\; b_1\; c_1 \\ a_2\; b_2\; c_2 \\ a_3\; b_3\; c_3 \end{bmatrix}$最後一個矩陣就是兩個座標系基之間的關係，即兩個座標系之間的轉換關係，將這個矩陣定義爲旋轉矩陣$\boldsymbol R$。$\boldsymbol R = \begin{bmatrix} a_1\; b_1\; c_1 \\ a_2\; b_2\; c_2 \\ a_3\; b_3\; c_3 \end{bmatrix}$

通過計算過程也可以得出，當前這一旋轉矩陣，是 $O-x'y'z'$ 座標系相對於 $O-xyz$ 的，同時需要注意的也有：旋轉矩陣的每一列表示的是對應的剛體座標系的基在原參考座標系下的表示，所以每一列之間是相互正交的，且模長爲1，所以旋轉矩陣是一個正交矩陣，並且符合以下性質：$\boldsymbol{R^TR=I} \\ \boldsymbol{R^T=R^{-1}}$第二個性質說明，旋轉矩陣的轉置與其逆矩陣相等。

3. 特例分析

3.1 向量的表示

向量的表示問題其實就是同一空間點在不同座標系下的求解問題，爲了理解其幾何意義，暫時不考慮座標系之間的位移，即假設所有相關座標系的原點重合。考慮一個座標系（稱之爲新座標系）通過參考座標系（稱之爲舊座標系）相對於某一旋轉軸旋轉得到。如果相對旋轉軸作逆時針方向旋轉，則旋轉方向爲正，反之則爲負。如下圖所示：

相對於舊座標系 $O-xyz$，空間中的一點 $\boldsymbol P$ 可以表示爲$\boldsymbol p = \begin{bmatrix} \boldsymbol{x\; y\; z} \end{bmatrix} \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix}$而相對於新座標系 $O'-x'y'z'$,同一點可以表示爲$\boldsymbol p' = \begin{bmatrix} \boldsymbol{x'\; y'\; z'} \end{bmatrix} \begin{bmatrix} p'_x \\ p'_y \\ p'_z \end{bmatrix}$而以上兩個方程式之間的唯一聯繫就是新舊座標系之間的聯繫，即代表座標系的基之間的聯繫。所以將上式與之前推導的座標系之間的關係聯立可以得到：$\boldsymbol p' = \begin{bmatrix} \boldsymbol{x'\; y'\; z'} \end{bmatrix} \begin{bmatrix} p'_x \\ p'_y \\ p'_z \end{bmatrix} = \begin{bmatrix} \boldsymbol{x\; y\; z} \end{bmatrix} \begin{bmatrix} a_1\; b_1\; c_1 \\ a_2\; b_2\; c_2 \\ a_3\; b_3\; c_3 \end{bmatrix} \begin{bmatrix} p'_x \\ p'_y \\ p'_z \end{bmatrix}$因爲$\boldsymbol{p, p'}$在空間中是同一個點，當然$\boldsymbol{p, p'}$的數值並不一樣，但因爲是空間中同一個點，所以如果將它們的座標同時轉換到同一個參考座標系下，則數值必然是相等的，所以通過在同一基下的$\boldsymbol{p, p'}$的表示式，可以得到純座標的關係爲：$\begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} = \begin{bmatrix} a_1\; b_1\; c_1 \\ a_2\; b_2\; c_2 \\ a_3\; b_3\; c_3 \end{bmatrix} \begin{bmatrix} p'_x \\ p'_y \\ p'_z \end{bmatrix}$ 即新座標系中的點左乘新座標系相對於舊座標系的旋轉矩陣就等於當前點在舊座標系中的位置。 從矩陣關係上可以很容易得出同一個點在不同座標系下的座標轉換關係。但正如之前在第一部分所講到的，這所有的旋轉形式，不管是同一空間點在不同座標系下的座標關係還是向量的旋轉求解，其本質都是座標系之間的變換。那麼如何從幾何上理解同一點在不同座標系下的不同表示這一問題呢？

如果想清楚其中的關鍵，其實很簡單，甚至順便把向量旋轉的問題也解決了，那麼其中的關鍵點是什麼？用下圖輔助說明：

假設存在一個 $\boldsymbol p_0$ 點，位於舊座標系中，其在舊座標系中的座標在數值上與 $\boldsymbol p$ 點在新座標系中的座標完全相同。那麼舊座標系經過新座標系相對於舊座標系的旋轉關係旋轉之後， $\boldsymbol p_0$ 點與 $\boldsymbol p$ 點必然是重合的。所以同一空間點在不同座標系下的座標求解問題，本質上是新舊座標系之間的旋轉變換問題，是舊座標系帶着與新座標系中的點具有相同座標數據的屬於舊座標系的點進行了新座標系相對於舊座標系旋轉關係的旋轉變換之後，所得到的點在舊座標系中的座標表示問題。 而這個問題，同時也是向量旋轉問題。通過以上分析，可得出同一空間點在不同座標系下的座標求解問題的本質，是座標系間的相對變換關係。想清楚了這一部分的幾何解釋，再來看向量的旋轉問題。

3.2 向量的旋轉

如下圖所示，教科書中講：旋轉矩陣也可視爲使某一向量繞空間中任一軸旋轉給定角度的矩陣算子。從圖中也可以看出，向量$\boldsymbol p'$ 繞 $z$ 軸旋轉 $\alpha$ 弧度之後，生成新的向量 $\boldsymbol p$。計算算式爲 $\boldsymbol p = R_z(\alpha)\boldsymbol p'$。

那從幾何來說是爲什麼呢？其實這個例子比一個更直觀，因爲從幾何上理解更容易一點，就是舊座標系帶着隸屬於舊座標系的向量$\boldsymbol p'$繞着舊座標系的 $z$ 旋轉了 $\alpha$ 弧度之後，將向量$\boldsymbol p'$繞到了現在向量 $\boldsymbol p$ 的位置，所以向量$\boldsymbol p$ 的新座標中的座標數值就是向量$\boldsymbol p'$在舊座標系中的座標數值。現在求解的就是這個隨着舊座標系到達新位置的向量$\boldsymbol p'$在之前的舊座標中的位置。

所以在計算上，當然是 新座標系中的點左乘新座標系相對於舊座標系的旋轉矩陣就等於當前點在舊座標系中的位置。

4. 感性總結

經過這一次對旋轉矩陣的重新學習與理解，試着將旋轉相關的問題都解釋爲座標系間的變換，將各種具有不同表象的問題統統歸因於座標系間的相對變換，返本歸源。理解上又透徹了一分。

5. 參考文獻

[1] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo. Robotics:Modeling,Planning and Control[M]. Springer-Verlag London, 2009.