位姿變換與李羣、李代數

　　本博客是在學習高翔《視覺SLAM十四講》過程中對位姿變換與李羣李代數相關知識點做的總結，不涉及公式的證明與推導。

一、位姿變換

1、旋轉矩陣與變換矩陣

旋轉矩陣是描述剛體旋轉最常見的一種形式，而變換矩陣通常是指旋轉矩陣與平移向量組成的齊次變換矩陣。對於三維空間的位姿變換，有旋轉矩陣：

R=[r1r2r3]=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥ $$R=\left[\begin{array}{ccc}{\mathbf{r}}_{\mathbf{1}}& {\mathbf{r}}_{\mathbf{2}}& {\mathbf{r}}_{\mathbf{3}}\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

平移向量：

t=[t1t2t3]T $$\mathbf{t}={\left[\begin{array}{ccc}{t}_1& t_2& t_3\end{array}\right]}^T$$

則旋轉矩陣 R $R$與平移向量 t $t$組成的齊次變換矩陣爲：

T=[R0Tt1]=⎡⎣⎢⎢⎢⎢r11r21r310r12r22r320r13r23r330t1t2t31⎤⎦⎥⎥⎥⎥ $$T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\\ 0& 0& 0& 1\end{array}\right]$$

旋轉矩陣 R $R$是一種單位正交陣，它具有單位正交陣所有的性質：

{r1T⋅r2=r1T⋅r3=r2T⋅r3=0∥r1∥=∥r2∥=∥r3∥=det(R)=1 $$\{\begin{array}{l}{{\mathbf{r}}_{\mathbf{1}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{2}}={{\mathbf{r}}_{\mathbf{1}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{3}}={{\mathbf{r}}_{\mathbf{2}}}^{\text{T}}\cdot {\mathbf{r}}_{\mathbf{3}}=0\\ \Vert {\mathbf{r}}_{\mathbf{1}}\Vert =\Vert {\mathbf{r}}_{\mathbf{2}}\Vert =\Vert {\mathbf{r}}_{\mathbf{3}}\Vert =\mathbf{d}\mathbf{e}\mathbf{t}(R)=1\end{array}$$

重要的是，旋轉矩陣的逆等於旋轉矩陣的轉置，即 R−1=RT $R^{-1}=R^T$，從而有 RR−1=RRT=I $R{R}^{-1}=R{R}^T=I$。齊次變換矩陣 T $T$的逆爲：

T−1=[R−10T−R−1t1]=[RT0T−RTt1] $$T^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathbf{-}\mathbf{1}}& \mathbf{-}{\mathbf{R}}^{\mathbf{-}\mathbf{1}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}^{\text{T}}& {\mathbf{-}\mathbf{R}}^{\text{T}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]$$

假設我們由座標系A經過變換矩陣 TABT=[R0Tt1] ${\phantom{T}}_B^{A}T=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]$得到了座標系B，那麼座標系B下的點 PBP ${\phantom{P}}_{}^B{P}_{}^{}$可在座標系A中表示爲：

PAP=TABTPBP=[R0Tt1][PBP1]=RPBP+t $${\phantom{P}}_{}^A{P}_{}^{}={\phantom{T}}_B^{A}T{\phantom{P}}_{}^B{P}_{}^{}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{\phantom{P}}_{}^B{P}_{}^{}\\ 1\end{array}\right]=R{\phantom{P}}_{}^B{P}_{}^{}+\mathbf{t}$$

同樣，座標系A下的點 PAP ${\phantom{P}}_{}^A{P}_{}^{}$可在座標系B中表示爲：

PBP=TBATPAP=TABT−1PAP=[RT0T−RTt1][PAP1]=RTPAP−RTt=RT(PAP−t) $${\phantom{P}}_{}^B{P}_{}^{}={\phantom{T}}_A^{B}T{\phantom{P}}_{}^A{P}_{}^{}={\phantom{T}}_B^A{T}_{}^{-1}{\phantom{P}}_{}^A{P}_{}^{}=\left[\begin{array}{cc}{\mathbf{R}}^{\text{T}}& {\mathbf{-}\mathbf{R}}^{\text{T}}\mathbf{t}\\ {\mathbf{0}}^{\text{T}}& 1\end{array}\right]\left[\begin{array}{c}{\phantom{P}}_{}^A{P}_{}^{}\\ 1\end{array}\right]=R^T{\phantom{P}}_{}^A{P}_{}^{}-R^T\mathbf{t}=R^T({\phantom{P}}_{}^A{P}_{}^{}-\mathbf{t})$$

2、歐拉角與旋轉矩陣

歐拉角是描述剛體旋轉最直觀的一種形式，它往往只用於可視化的人機交互，而無法直接參與運算。

對於三維空間座標系，我們通常這樣規定：

⎧⎩⎨繞 X 軸旋轉的角度稱爲橫滾角，Roll繞 Y 軸旋轉的角度稱爲俯仰角，Pitch繞 Z 軸旋轉的角度稱爲偏航角，Yaw $$\{\begin{array}{l}\text{繞 X 軸旋轉的角度稱爲橫滾角，Roll}\\ \text{繞 Y 軸旋轉的角度稱爲俯仰角，Pitch}\\ \text{繞 Z 軸旋轉的角度稱爲偏航角，Yaw}\end{array}$$

所有的旋轉，沿着旋轉軸方向順時針旋轉的角度爲正，逆時針旋轉的角度爲負。對於平移，沿着該軸正方向平移的距離爲正，負方向平移的距離爲負。

設剛體繞着 Z $Z$軸旋轉 γ $\gamma$角度，那麼剛體上所有點的 z $z$座標值不變， x $x$與 y $y$的座標值分別變爲：

x′=ρcos(θ+γ)=ρ(cosθ⋅cosγ−sinθ⋅sinγ)=x⋅cosγ−y⋅sinγy′=ρsin(θ+γ)=ρ(cosθ⋅sinγ+sinθ⋅cosγ)=x⋅sinγ+y⋅cosγ $$\begin{gathered} x^{\prime }=\rho cos(\theta +\gamma )=\rho (cos\theta \cdot cos\gamma -sin\theta \cdot sin\gamma )=x\cdot cos\gamma -y\cdot sin\gamma \\ {y}^{\prime }=\rho sin(\theta +\gamma )=\rho (cos\theta \cdot sin\gamma +sin\theta \cdot cos\gamma )=x\cdot sin\gamma +y\cdot cos\gamma \end{gathered}$$

寫成矩陣的形式：

⎡⎣⎢x′y′z′⎤⎦⎥=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥⎡⎣⎢xyz⎤⎦⎥=RZ(γ)⎡⎣⎢xyz⎤⎦⎥ $$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=R_Z(\gamma )\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$

從而，我們總結出歐拉角與旋轉矩陣的關係：

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Roll：RX(α)=rotx(α)=⎡⎣⎢1000cosαsinα0−sinαcosα⎤⎦⎥Pitch：RY(β)=roty(β)=⎡⎣⎢cosβ0−sinβ010sinβ0cosβ⎤⎦⎥Yaw：RZ(γ)=rotz(γ)=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥ $$\{\begin{array}{l}\text{Roll：}{R}_{\text{X}}(\alpha )=\text{rotx}(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]\\ \text{Pitch：}{R}_{\text{Y}}(\beta )=\text{roty}(\beta )=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]\\ \text{Yaw：}{R}_{\text{Z}}(\gamma )=\text{rotz}(\gamma )=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

有了歐拉角與旋轉矩陣之間的轉換關係，下面舉一個簡單的例子驗證一下：

如上圖所示，有位於立方體頂點上的三個座標系 O1 $O_1$、 O2 $O_2$、 O3 $O_3$，以及位於立方體中心的點 P $P$，立方體的邊長分別爲6、8、10。下面先進行三個座標系之間的變換：

• O1→O2 $O_1\to O_2$：

  相對變換： O1 $O_1$先沿着座標軸 O1X1 $O_1{X}_1$的正方向平移10個單位到達 O2 $O_2$，然後繞着當前位置的 O1Z1 $O_1{Z}_1$軸順時針旋轉90°，此時新得到的 O1 $O_1$與 O2 $O_2$座標系完全重合，即完成了 O1 $O_1$到 O2 $O_2$的座標系變換。以上過程可以在MATLAB中用變換矩陣表述爲：
  

  T12T=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥ $${\phantom{T}}_2^{1}T=\text{transl(10,0,0)*trotz(pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

  絕對變換： O1 $O_1$先繞着座標軸 O1Z1 $O_1{Z}_1$順時針旋轉90°，然後沿着原來 O1X1 $O_1{X}_1$軸的正方向平移10個單位到達 O2 $O_2$，此時新得到的 O1 $O_1$與 O2 $O_2$座標系完全重合，即完成了 O1 $O_1$到 O2 $O_2$的座標系變換。以上過程可以在MATLAB中用變換矩陣表述爲：
  

  T12T′=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=T12T $${{\phantom{T}}_2^{1}T}^{\prime }=\text{transl(10,0,0)*trotz(pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_2^{1}T$$

  你可能會問，兩次不同的操作爲什麼表達式是一樣的？原因是這裏牽涉到相對變換與絕對變換的區別：
  

  ⎧⎩⎨相對變換：每一步都以新得的坐標系爲參考，每一步得到的變換矩陣依次右乘；絕對變換：始終以最初的坐標系爲參考，通常是先旋轉再平移，每一步得到的變換矩陣依次左乘；相對變換與絕對變換不可同時使用！ $$\{\begin{array}{l}\text{相對變換：每一步都以新得的座標系爲參考，每一步得到的變換矩陣依次右乘；}\\ \text{絕對變換：始終以最初的座標系爲參考，通常是先旋轉再平移，每一步得到的變換矩陣依次左乘；}\\ \text{相對變換與絕對變換不可同時使用！}\end{array}$$

  當然， O1→O2 $O_1\to O_2$的相對變換還可以有很多種操作順序，比如：
  

  T12T′′=trotz(pi/2)*transl(0,-10,0)=⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢1000010000100−1001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=T12T $${{\phantom{T}}_2^{1}T}^{″}=\text{trotz(pi/2)*transl(0,-10,0)}=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_2^{1}T$$

  等等，這裏不再一一列舉。

• O2→O1 $O_2\to O_1$：

  相對變換：
  

  T21T=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=trotz(-pi/2)*transl(-10,0,0)=⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢100001000010−10001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥ $$\begin{gathered} {\phantom{T}}_1^{2}T=\text{transl(0,10,0)*trotz(-pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \\ =\text{trotz(-pi/2)*transl(-10,0,0)}=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& -10\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \end{gathered}$$

  絕對變換：
  

  T21T′=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=T21T $${{\phantom{T}}_1^{2}T}^{\prime }=\text{transl(0,10,0)*trotz(-pi/2)}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]={\phantom{T}}_1^{2}T$$

  相信這些表達式的意義都很容易理解，接下來驗證一下 T12T ${\phantom{T}}_2^{1}T$與 T21T ${\phantom{T}}_1^{2}T$是不是互逆：
  

  T12TT21T=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=I $${\phantom{T}}_2^{1}T{\phantom{T}}_1^{2}T=\left[\begin{array}{cccc}0& -1& 0& 10\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=I$$

  顯然， O1→O2 $O_1\to O_2$與 O2→O1 $O_2\to O_1$互爲逆變換， T12T ${\phantom{T}}_2^{1}T$與 T21T ${\phantom{T}}_1^{2}T$互逆。其實，由 O1 $O_1$到 O2 $O_2$的變換矩陣就是對座標系 O2 $O_2$在座標系 O1 $O_1$中位姿的一種描述，即 T12T ${\phantom{T}}_2^{1}T$描述了 O2 $O_2$在 O1 $O_1$中的位置和姿態。同理， T21T ${\phantom{T}}_1^{2}T$描述了 O1 $O_1$在 O2 $O_2$中的位置和姿態。那麼，我們已經知道了點 P $P$在 O1 $O_1$座標系下的座標爲 P1P=(5,4,3)T ${\phantom{P}}_{}^1{P}_{}^{}=(5,4,3{)}^T$，座標系 O1 $O_1$在座標系 O2 $O_2$下的位姿爲 T21T ${\phantom{T}}_1^{2}T$，順理成章地，我們就可以求得點 P $P$在 O2 $O_2$座標系下的座標爲：
  

  P2P=T21TP1P=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥ $${\phantom{P}}_{}^2{P}_{}^{}={\phantom{T}}_1^{2}T{\phantom{P}}_{}^1{P}_{}^{}=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}5\\ 4\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right]$$

  很顯然，結果跟事實是一致的。

• O2↔O3 $O_2\leftrightarrow O_3$：
  

  T23T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥T32T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥T23TT32T=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥2=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=IP3P=T32TP2P=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥ $$\begin{gathered} {\phantom{T}}_3^{2}T=\text{transl(8,0,6)*troty(pi)}=\text{troty(pi)*transl(-8,0,-6)}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{T}}_2^{3}T=\text{transl(8,0,6)*troty(pi)}=\text{troty(pi)*transl(-8,0,-6)}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{T}}_3^{2}T{\phantom{T}}_2^{3}T={\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]}^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=I \\ {\phantom{P}}_{}^3{P}_{}^{}={\phantom{T}}_2^{3}T{\phantom{P}}_{}^2{P}_{}^{}=\left[\begin{array}{cccc}-1& 0& 0& 8\\ 0& 1& 0& 0\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right] \end{gathered}$$

• O1→O3 $O_1\to O_3$：
  

  T13T=transl(10,8,6)*trotz(pi/2)*troty(pi)=trotz(-pi/2)*trotx(pi)*transl(-8,-10,-6)=T12TT23T=⎡⎣⎢⎢⎢0−100−100000−1010861⎤⎦⎥⎥⎥P3P=T31TP1P=T13T−1P1P=⎡⎣⎢⎢⎢0−100−100000−1081061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥ $$\begin{gathered} {\phantom{T}}_3^{1}T=\text{transl(10,8,6)*trotz(pi/2)*troty(pi)}=\text{trotz(-pi/2)*trotx(pi)*transl(-8,-10,-6)}={\phantom{T}}_2^{1}T{\phantom{T}}_3^{2}T=\left[\begin{array}{cccc}0& -1& 0& 10\\ -1& 0& 0& 8\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right] \\ {\phantom{P}}_{}^3{P}_{}^{}={\phantom{T}}_1^{3}T{\phantom{P}}_{}^1{P}_{}^{}={\phantom{T}}_3^1{T}^{-1}{\phantom{P}}_{}^1{P}_{}^{}=\left[\begin{array}{cccc}0& -1& 0& 8\\ -1& 0& 0& 10\\ 0& 0& -1& 6\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}5\\ 4\\ 3\\ 1\end{array}\right]=\left[\begin{array}{c}4\\ 5\\ 3\\ 1\end{array}\right] \end{gathered}$$

3、旋轉向量與旋轉矩陣

旋轉向量（或稱軸角，Axis-Angle），是一種定義在 R3 ${\mathbb{R}}^3$上的三維向量（對於三維空間的旋轉來說），它可以描述剛體在三維空間中繞任意旋轉軸發生的旋轉。旋轉向量的方向代表旋轉軸的方向，它的模代表旋轉的角度。對於一個旋轉軸爲 n $\mathbf{n}$（ ∥n∥=1 $\Vert \mathbf{n}\Vert =1$），轉角爲 θ $\theta$的旋轉向量 θn $\theta \mathbf{n}$（ θ $\theta$單位爲弧度），它與旋轉矩陣 R $R$的關係爲：

• 旋轉向量到旋轉矩陣
  

  R=I+sinθn∧+(1−cosθ)n∧2=cosθI+sinθn∧+(1−cosθ)nnT(羅德裏格斯公式) $$\begin{array}{}\text{(羅德里格斯公式)}& \begin{array}{rl}R& =I+sin\theta {\mathbf{n}}^{\wedge }+(1-cos\theta ){{\mathbf{n}}^{\wedge }}^2\\ & =cos\theta I+sin\theta {\mathbf{n}}^{\wedge }+(1-cos\theta ){\mathbf{n}\mathbf{n}}^T\end{array}\end{array}$$

  這裏的 n∧ ${\mathbf{n}}^{\wedge }$爲 n=[nxnynz]T $\mathbf{n}={\left[\begin{array}{ccc}{n}_x& n_y& n_z\end{array}\right]}^T$所對應的反對稱矩陣：
  

  n∧=⎡⎣⎢0nz−ny−nz0nxny−nx0⎤⎦⎥ $$$$ $${\mathbf{n}}^{\wedge }=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$$

  且 n∧2=n∧n∧=nnT−I n ∧