機器人運動學：空間描述和變換

表達空間中的工具、零件或結構的位姿數學工具，這個工具必須建立實物與數的聯系，将問題轉換到“數”，借助數字的表達能力，降低表述難度。

笛卡爾坐标系就是一種非常有效的工具，根據具體情況選擇合适的坐标系能夠減少計算量和複雜度。建立一個世界坐标系，所有在上面的物體和事情都可以很好的描述，是我們學習的重點。

下面的内容都是三維笛卡爾坐标系範圍内的讨論。

一、坐标系的描述

一個坐标系是由三個兩兩正交的機關向量來表示的，這個機關向量又叫做坐标系的主軸。需要了解常見記号的含義：

• 坐标系。 { A } \{A\} {A}表示坐标系 A A A，它是由大括号包圍的大寫字母表示的就是坐标系名稱；
• 坐标系主軸。 X ^ B \hat X_B X^B​ 表示坐标系 { B } \{B\} {B}的 X X X主軸，上面的帽子表示這是一個主軸，字母 X X X表示主軸的具體類型，右下标表示坐标系名稱 { B } \{B\} {B}；
• 坐标系下的點、向量。 A P ^AP AP表示坐标軸 { A } \{A\} {A}下的點 P P P，同時也表示坐标 { A } \{A\} {A}原點作為起點終點為 P P P的向量；

1.1 坐标系原點描述

一旦坐标系建立，空間中任意一點既可用向量表達、也可以用坐标表達。

點的坐标是對應向量向參考坐标的三個主軸投影得到的，投影按順序寫成一列代表這個點：

A P = [ p x p y p z ] (1) ^AP=\begin{bmatrix}p_x\\p_y\\pz\end{bmatrix}{\tag{1}} AP=⎣⎡​px​py​pz​⎦⎤​(1)

• 第一個元素代表向量向 X ^ A \hat X_A X^A​的投影；
• 第二個元素代表向量向 Y ^ A \hat Y_A Y^A​的投影；
• 第三個元素代表向量向 Z ^ A \hat Z_A Z^A​的投影；

1.2 坐标系姿态描述

光有位置描述可不夠，還需要加上姿态描述。

為了研究機械手末端，我們會在上面固定一個直角坐标系 { B } \{B\} {B}，其上的三個主軸記為 X ^ B \hat X_B X^B​ Y ^ B \hat Y_B Y^B​ Z ^ B \hat Z_B Z^B​ 。坐标系之間的關系就是主軸之間的關系，主軸是一個向量，将這個向量分别往目标坐标系投影即可得到一個坐标值，如前面向量一樣，向量結果将會被寫成一個列向量。

将我們要描述的三個坐标軸 { B } \{B\} {B}上的主軸往 { A } \{A\} {A}投影就可以确定其唯一姿态，具體做法是：

• 首先，取 { B } \{B\} {B}坐标系中的第一個主軸 X ^ B \hat X_B X^B​，往 { A } \{A\} {A}的 X ^ A \hat X_A X^A​軸 Y ^ A \hat Y_A Y^A​軸 Z ^ A \hat Z_A Z^A​軸投影三次；
• 投影的結果按順序寫成第一列，記為 A X ^ B ^A{\hat X_B} AX^B​；
• 然後，再取 { B } \{B\} {B}坐标系中的第二個主軸 Y ^ B \hat Y_B Y^B​，往 { A } \{A\} {A}的 X ^ A \hat X_A X^A​軸 Y ^ A \hat Y_A Y^A​軸 Z ^ A \hat Z_A Z^A​軸投影三次；
• 投影的結果按順序寫成第二列，記為 A Y ^ B ^A{\hat Y_B} AY^B​；
• 最後，再取 { B } \{B\} {B}坐标系中的最後一個主軸 Z ^ B \hat Z_B Z^B​，往 { A } \{A\} {A}的 X ^ A \hat X_A X^A​軸 Y ^ A \hat Y_A Y^A​軸 Z ^ A \hat Z_A Z^A​投影三次；
• 投影的結果按順序寫成第三列，記為 A Z ^ B ^A{\hat Z_B} AZ^B​。

最後我們将會得到一個 3 × 3 3\times3 3×3的矩陣，在機器人學上被稱為旋轉矩陣。特别要注意這些符号的表達：

• A X ^ B ^A{\hat X_B} AX^B​： { B } \{B\} {B}系 X X X主軸向 { A } \{A\} {A}系的三個投影列向量，因為主軸是一個向量；
• B A R ^A_BR BA​R：将坐标系為 { A } \{A\} {A}作為參考坐标系表達的 { B } \{B\} {B}坐标系姿态，是一個 3 × 3 3\times 3 3×3的矩陣；

那麼将 { B } \{B\} {B}的所有軸都投影至 { A } \{A\} {A}的三個主軸 X ^ A \hat X_A X^A​ Y ^ A \hat Y_A Y^A​ Z ^ A \hat Z_A Z^A​，那麼總計有九個投影，相同軸向 { A } \{A\} {A}的三個軸投影放在一列：

B A R = [ A X ^ B A Y ^ B A Z ^ B ] = [ X ^ B ⋅ X ^ A Y ^ B ⋅ X ^ A Z ^ B ⋅ X ^ A X ^ B ⋅ Y ^ A Y ^ B ⋅ Y ^ A Z ^ B ⋅ Y ^ A X ^ B ⋅ Z ^ A Y ^ B ⋅ Z ^ A Z ^ B ⋅ Z ^ A ] (2) \begin{aligned} _B^A{R}&=\begin{bmatrix} ^A{\hat X_B}& ^A\hat Y_B &^A\hat Z_B \end{bmatrix}\\&= \begin{bmatrix} \hat X_B\cdot \hat X_A& \hat Y_B\cdot \hat X_A& \hat Z_B\cdot \hat X_A\\ \hat X_B\cdot \hat Y_A& \hat Y_B\cdot \hat Y_A& \hat Z_B\cdot \hat Y_A\\ \hat X_B\cdot \hat Z_A& \hat Y_B\cdot \hat Z_A& \hat Z_B\cdot \hat Z_A\\ \end{bmatrix}{\tag 2} \end{aligned} BA​R​=[AX^B​​AY^B​​AZ^B​​]=⎣⎡​X^B​⋅X^A​X^B​⋅Y^A​X^B​⋅Z^A​​Y^B​⋅X^A​Y^B​⋅Y^A​Y^B​⋅Z^A​​Z^B​⋅X^A​Z^B​⋅Y^A​Z^B​⋅Z^A​​⎦⎤​​(2)

旋轉矩陣的逆等于其轉置這是一個非常有用的性質。

根據點積的定義與主軸是機關向量的事實，可以得知：旋轉矩陣每一個元素值的大小都代表着主軸之間的夾角餘弦值。根據坐标的定義，坐标值其實就是這個點對應向量在坐标軸的投影數值大小，那麼要得到 A P ^AP AP，隻需要将在 { B } \{B\} {B}上的投影繼續投影至 { A } \{A\} {A}主軸上即可。

兩個主軸夾角的餘弦值，這個性質意味着，所有與主軸共線的向量長度都可以通過乘以旋轉矩陣上的元素得到在另一個坐标系主軸上的長度大小。

向量 a a a在 b b b的投影可以轉換成向量 a a a與向量 b b b的點積，記做 a ⋅ b a \cdot b a⋅b，如果 a a a b b b都是機關向量，投影的數值等于兩個方向的方向餘弦。 X ^ B ⋅ X ^ A \hat X_B\cdot \hat X_A X^B​⋅X^A​= X ^ B ⋅ X ^ A \hat X_B\cdot \hat X_A X^B​⋅X^A​這是向量點積 的性質。

1.3 統一的坐标系的描述

坐标系描述=位置描述+姿态描述

從向量角度來說，總共需要四個向量來表示一個坐标系，其中姿态三個，位置一個；從矩陣角度來說，則需要一個位置列向量和 3 × 3 3 \times3 3×3的姿态矩陣來表示：

{ B } = { B A R , A P B O R G } (3) \{B\}=\{_B^A{R},^AP_{BORG}\}{\tag{3}} {B}={BA​R,APBORG​}(3)

A P B O R G ^AP_{BORG} APBORG​是一個表示相對于坐标系 { B } \{B\} {B}原點的位置列向量。

二、映射：不同坐标系點坐标變換

映射是了解換算的關鍵，最早接觸映射還是在中學學習函數這一知識點的時候，函數就是一種映射，給我一個自變量，經過映射計算後将給出一個應變量。

坐标系之間的描述可以用一個原點向量和三個主軸投影組成的。同一個實體點在不同坐标系下的點的坐标值一定不同。這一節，我們将會給出一個坐标系下的點與另一個坐标系下點的映射關系：

2.1 平行坐标系的映射

如下圖所示，兩個坐标系的描述是純平移，記從 { A } \{A\} {A}下 { B } \{B\} {B}描述為 A P B O R G ^AP_{BORG} APBORG​。已知空間中的同一點 P P P在 { B } \{B\} {B}系下的表示： B P ^BP BP，試着求取 P P P在 { A } \{A\} {A}系下的表達 A P ^AP AP。

兩個坐标系之間的關系為 A P B O R G ^AP_{BORG} APBORG​已知，在這種情況下，隻需要對應坐标相加即，這是因為 P P P在投影的主軸平行，表現的僅僅是對應分量的增加和減少。

A P = B P + A P B O R G (4) ^AP=^BP+^AP_{BORG}\tag{4} AP=BP+APBORG​(4)

P P P仍然是同一個點，隻是描述它的坐标系改變了。

小結：坐标系間平行的映射，輸入一個坐标系下的點，輸出是另一個坐标系下的點。公式如下：

A P = B P + A P B O R G (5) ^AP=^BP+^AP_{BORG}{\tag{5}} AP=BP+APBORG​(5)

2.2 旋轉坐标系的映射

如下圖所示，坐标系之間的描述為已知為純旋轉關系，記 B A R ^A_BR BA​R為 { B } \{B\} {B}坐标系在 { A } \{A\} {A}坐标系姿态描述。已知空間中的同一點 P P P在 { B } \{B\} {B}系下的表示： B P ^BP BP，試着求取 P P P在 { A } \{A\} {A}系下的表達 A P ^AP AP。

轉換關系如下：

A P = B A R B P (6) ^AP=^A_BR^BP\tag{6} AP=BA​RBP(6)

已知一個坐标系下的點坐标，隻需要左乘一個将原坐标系向目标坐标系的旋轉矩陣即可。如上， B P ^BP BP已知，又有 B A R 。 ^A_BR。 BA​R。

{ B } \{B\} {B}系下點 B P ^BP BP坐标已知，也就是該點對應向量被分解成 { B } \{B\} {B}主軸的三個分向量：

B P = [ x b y b z b ] A P = [ x a y a z a ] ^BP=\begin{bmatrix}x_b\\y_b\\z_b\end{bmatrix}\quad^AP=\begin{bmatrix}x_a\\y_a\\z_a\end{bmatrix} BP=⎣⎡​xb​yb​zb​​⎦⎤​AP=⎣⎡​xa​ya​za​​⎦⎤​

x b x_b xb​為 P P P點在 X ^ B \hat X_B X^B​上的投影，将這個投影乘以旋轉矩陣第一個元素，即 x b x_b xb​所在主軸與 X ^ A \hat X_A X^A​主軸的夾角為 r 11 r_{11} r11​得到 x b x_b xb​在新的主軸上的投影大小， y b y_b yb​是 P P P點在 Y ^ B \hat Y_B Y^B​上的投影，乘以 r 12 r_{12} r12​得到 y b y_b yb​在 X ^ A \hat X_A X^A​上的投影大小， y b y_b yb​是 P P P點在 Z ^ B \hat Z_B Z^B​上的投影，乘以 r 13 r_{13} r13​得到 z b z_b zb​在 Z ^ A \hat Z_A Z^A​上的投影大小，将三個和加起來即為 B P ^BP BP在 { A } \{A\} {A}下的 X ^ A \hat X_A X^A​表示，即 x a x_a xa​。

A P x = B X ^ A ⋅ B P A P y = B Y ^ A ⋅ B P A P z = B Z ^ A ⋅ B P \begin{aligned} ^AP_x=^B\hat X_A \cdot ^BP\\ ^AP_y=^B\hat Y_A \cdot ^BP\\ ^AP_z=^B\hat Z_A \cdot ^BP \end{aligned} APx​=BX^A​⋅BPAPy​=BY^A​⋅BPAPz​=BZ^A​⋅BP​

寫成矩陣形式：

[ x a y a z a ] = [ B X ^ A B Y ^ A B Z ^ A ] [ x b y b z b ] = [ B X ^ A B Y ^ A B Z ^ A ] T [ x b y b z b ] = ( A B R ) T B P = B A R B P = A P \begin{bmatrix}x_a\\y_a\\z_a\end{bmatrix}=\begin{bmatrix}^B\hat X_A \\^B\hat Y_A\\^B\hat Z_A \end{bmatrix}\begin{bmatrix}x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}^B\hat X_A &^B\hat Y_A&^B\hat Z_A \end{bmatrix}^T\begin{bmatrix}x_b\\y_b\\z_b\end{bmatrix}=(^B_AR)^T{^BP}=^A_BR^BP=^AP ⎣⎡​xa​ya​za​​⎦⎤​=⎣⎡​BX^A​BY^A​BZ^A​​⎦⎤​⎣⎡​xb​yb​zb​​⎦⎤​=[BX^A​​BY^A​​BZ^A​​]T⎣⎡​xb​yb​zb​​⎦⎤​=(AB​R)TBP=BA​RBP=AP

小結：坐标系間僅有旋轉的映射，輸入一個坐标系下的點，輸出是另一個坐标系下的點。公式如下：

A P = B A R B P (7) ^AP=^A_BR^BP{\tag{7}} AP=BA​RBP(7)

2.3 一般坐标系的映射

兩個坐标系可能是上述兩種關系的混合，如下：

已知空間中的同一點 P P P在 { B } \{B\} {B}系下的表示： B P ^BP BP，試着求取 P P P在 { A } \{A\} {A}系下的表達 A P ^AP AP。做法如下，首先将 B P ^BP BP經過左乘 B A R ^A_BR BA​R得到旋轉至中間坐标系（與 { A } \{A\} {A}同姿态），然後再經過加法，求得 A P {^AP} AP，整合後：

A P = B A R ⋅ B P + A P B O R G (8) ^AP=^A_BR\cdot^BP+^AP_{BORG}{\tag{8}} AP=BA​R⋅BP+APBORG​(8)

經過上述計算就完成了坐标切換。寫成矩陣形式：

[ A P 1 ] = [ B A R A P B O R G 0 0 0 1 ] \begin{bmatrix} ^AP\\1 \end{bmatrix}=\begin{bmatrix} ^A_BR&^AP_{BORG}\\ 0 \quad0\quad 0&1 \end{bmatrix} [AP1​]=[BA​R000​APBORG​1​]

那麼 T = [ B A R A P B O R G 0 0 0 1 ] T=\begin{bmatrix} ^A_BR&^AP_{BORG}\\ 0 \quad0\quad 0&1 \end{bmatrix} T=[BA​R000​APBORG​1​]則是實作轉換的一個左乘矩陣。

小結：更加通用的映射，坐标系間既有旋轉又有平移的映射，輸入一個坐标系下的點，輸出是另一個坐标系下的點。公式如下：

A P = B A R ⋅ B P + A P B O R G (9) ^AP=^A_BR\cdot^BP+^AP_{BORG}{\tag{9}} AP=BA​R⋅BP+APBORG​(9)

通用映射的矩陣形式：

A P = B A T B P (10) ^AP=^A_BT^BP{\tag{10}} AP=BA​TBP(10)

三、算子：同個坐标系下點的變換

前面的映射是為了解決不同坐标系下的點的變換，這一節的映射則是為了解決同一坐标系下點的變換。

3.1 平移算子

平移将空間中的一個點沿着一個已知的矢量方向移動一定距離。空間中點的平移與此點向另一個坐标系的映射有相同數學表達式，因為你可以将平移看作是坐标系的移動。

A P 2 = A P 1 + A Q (11) ^AP_2=^AP_1+^AQ{\tag{11}} AP2​=AP1​+AQ(11)

矩陣形式：

A P 2 = D Q ( q ) A P 1 ^AP_2=D_Q(q)^AP_1 AP2​=DQ​(q)AP1​

其中 q q q是沿着矢量 Q ^ \hat Q Q^​方向的數量。 D Q D_Q DQ​的具體形式如下：

D Q ( q ) = [ 1 0 0 q x 0 1 0 q y 0 0 1 q z 0 0 0 1 ] D_Q(q)=\begin{bmatrix} 1&0&0&q_x\\ 0&1&0&q_y\\ 0&0&1&q_z\\ 0&0&0&1 \end{bmatrix} DQ​(q)=⎣⎢⎢⎡​1000​0100​0010​qx​qy​qz​1​⎦⎥⎥⎤​

3.2 旋轉算子

同理，有：

A P 2 = R A P 1 ^AP_2=R^AP_1 AP2​=RAP1​

數學表達式完全相同，不過 R R R含義變成了繞廣義軸旋轉的旋轉矩陣。用矩陣表示：

A P 2 = R K ( θ ) A P 1 ^AP_2=R_K(\theta)^AP_1 AP2​=RK​(θ)AP1​

K K K為 Z Z Z軸旋轉的 R K ( θ ) R_K(\theta) RK​(θ)具體形式如下:

R K ( θ ) = [ cos ⁡ θ − sin ⁡ θ 0 0 sin ⁡ θ cos ⁡ θ 0 0 0 0 1 0 0 0 0 1 ] R_K(\theta)=\begin{bmatrix} \cos\theta &-\sin\theta& 0&0\\ \sin\theta&\cos\theta&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix} RK​(θ)=⎣⎢⎢⎡​cosθsinθ00​−sinθcosθ00​0010​0001​⎦⎥⎥⎤​

3.3 變換算子

同理，變換算子 T T T也是其次矩陣形式。

四、總結

一個齊次矩陣完成了三個方面的描述：

• 坐标系描述
• 變換映射
• 變換算子