02 三維空間的剛體運(yùn)動(dòng)

2.0 機(jī)器人位姿表述

（1）二維與三維空間中，機(jī)器人位姿的表述：

• 2D 的情況：橫縱坐標(biāo) + 旋轉(zhuǎn)角（機(jī)器人朝向），即 $[x,y,\theta ]$

• 3D 的情況：三維空間中的旋轉(zhuǎn)和平移

2.1 點(diǎn)和坐標(biāo)系

2.1.1 三維坐標(biāo)系有關(guān)表述

（1）基本運(yùn)算

• 加減法

• 內(nèi)積（點(diǎn)乘）：$\mathbf{a}?\mathbf{b}={\mathbf{a}}^T\mathbf{b}={\sum }_{i=1}^3{a}_i\times b_i=|\mathbf{a}||\mathbf{b}|cos<\mathbf{a},\mathbf{b}>$，且 $\mathbf{a}?\mathbf{b}=\mathbf{b}?\mathbf{a}$

• 外積（叉乘）：

$$\begin{array}{rl}\mathbf{a}\times \mathbf{b}=\Vert \begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{e}}_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert & =(a_2{b}_3?a_3{b}_2){\mathbf{e}}_1+(a_3{b}_1?a_1{b}_3){\mathbf{e}}_2+(a_1{b}_3?a_3{b}_1){\mathbf{e}}_3\\ & =\left[\begin{array}{c}{a}_2{b}_3?a_3{b}_2\\ a_3{b}_1?a_1{b}_3\\ a_1{b}_2?a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& ?a_3& a_2\\ a_3& 0& ?a_1\\ ?a_2& a_1& 0\end{array}\right]\mathbf{b}\stackrel{\text{?def?}}{=}{\mathbf{a}}^{\wedge }\mathbf{b}\end{array}\text{\hspace{1em}(2-1)}$$
也即$\mathbf{a}\times \mathbf{b}=|\mathbf{a}||\mathbf{b}|sin<\mathbf{a},\mathbf{b}>$，方向垂直于向量$\mathbf{a}$、$\mathbf{b}$ 組成的平面，遵循右手定則。

性質(zhì)：$\mathbf{a}\times \mathbf{b}=?\mathbf{b}\times \mathbf{a}$

為便于后續(xù)表達(dá)，記

$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& ?a_3& a_2\\ a_3& 0& ?a_1\\ ?a_2& a_1& 0\end{array}\right]\text{\hspace{1em}(2-2)}$$
這是一個(gè)反對(duì)稱矩陣，滿足$A^T=?A$。

2.1.2 坐標(biāo)系變換

（1）三維空間向量表示：

$$\mathbf{a}=\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{\mathbf{e}}_1+a_2{\mathbf{e}}_2+a_3{\mathbf{e}}_3\text{\hspace{1em}(2-3)}$$
其中，為基向量。

（2）兩個(gè)坐標(biāo)系之間的運(yùn)動(dòng)由一個(gè)旋轉(zhuǎn)加一個(gè)平移組成，這種運(yùn)動(dòng)稱為剛體運(yùn)動(dòng)。

（3）對(duì)于坐標(biāo)系旋轉(zhuǎn)變換，假設(shè)原坐標(biāo)系和現(xiàn)坐標(biāo)系單位正交基底分別為$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]$和$[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]$，

坐標(biāo)分別為$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$，由于向量本身的絕對(duì)位置沒(méi)有改變，則

$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(2-4)}$$

將上式兩端分別左乘$\left[\begin{array}{l}{\mathbf{e}}_1^{\mathbf{T}}\\ {\mathbf{e}}_2^T\\ {\mathbf{e}}_3^T\end{array}\right]$得到，

$$\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{lll}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\text{?def?}}{=}\mathbf{R}{\mathbf{a}}^{\prime }\text{\hspace{1em}(2-5)}$$

矩陣$\mathbf{R}$即為旋轉(zhuǎn)矩陣。它有如下性質(zhì)：

• $\mathbf{R}$ 行列式為 1;
• $\mathbf{R}$ 是一個(gè)正交矩陣，即滿足${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$。
• 坐標(biāo)系 1 到坐標(biāo)系 2 ，有${\mathbf{a}}_1={\mathbf{R}}_{12}{\mathbf{a}}_2$，反之有${\mathbf{a}}_2={\mathbf{R}}_{21}{\mathbf{a}}_1$，其中，
  ${\mathbf{R}}_{12}={{\mathbf{R}}_{21}}^{?1}={{\mathbf{R}}_{21}}^T$

將滿足此性質(zhì)的矩陣集合定義為特殊正交群：

$$\mathrm{SO}(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}\text{\hspace{1em}(2-6)}$$

（4）旋轉(zhuǎn)加平移

可以將三維空間的剛體運(yùn)動(dòng)分解為平移以及旋轉(zhuǎn)運(yùn)動(dòng)，滿足：

$${\mathbf{a}}^{\prime }=\mathbf{Ra}+\mathbf{t}\text{\hspace{1em}(2-7)}$$

（5）齊次坐標(biāo)與旋轉(zhuǎn)矩陣（參考機(jī)器人學(xué)中的坐標(biāo)變換）

為便于表達(dá)，寫(xiě)成齊次形式

$$\left[\begin{array}{l}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\stackrel{\text{?def?}}{=}\mathbf{T}\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\text{\hspace{1em}(2-8)}$$

其中，$\mathbf{T}$即為變換矩陣（注意左乘右乘區(qū)別）。

（繞靜坐標(biāo)系（世界坐標(biāo)系）旋轉(zhuǎn)即左乘，繞自身坐標(biāo)系旋轉(zhuǎn)即右乘）

這種形式的矩陣集合定義為特殊歐式群，即

$$\mathrm{SE}(3)=\left\{\mathbf{T}=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\text{\hspace{1em}(2-9)}$$

同樣地，逆方向的變換為：

$${\mathbf{T}}^{?1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& ?{\mathbf{R}}^{\mathrm{T}}\mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-10)}$$

2.2 旋轉(zhuǎn)向量和歐拉角

2.2.1 旋轉(zhuǎn)向量

（1）三維空間中剛體運(yùn)動(dòng)有六個(gè)自由度（旋轉(zhuǎn)和平移各三個(gè)）。顯然無(wú)論是上述的旋轉(zhuǎn)矩陣（9個(gè)量）還是變換矩陣（16個(gè)量），都有很大的冗余；并且，矩陣中的元素相互關(guān)聯(lián)，這不利于后續(xù)的非線性優(yōu)化計(jì)算。因此我們希望找到一種更為緊湊的表達(dá)方式。

（2）任意一個(gè)旋轉(zhuǎn)都可以用旋轉(zhuǎn)軸和旋轉(zhuǎn)角刻畫(huà)，因此可以用旋轉(zhuǎn)向量（也稱為角軸或軸角）來(lái)表達(dá)，即向量方向?yàn)樾D(zhuǎn)軸，其模長(zhǎng)為旋轉(zhuǎn)角。

（3）羅德里格斯公式描述了旋轉(zhuǎn)矩陣和旋轉(zhuǎn)向量之間的關(guān)系。

$$\mathbf{R}=\cos (\theta \mathbf{I})+\left(1?\cos \theta \right)\mathbf{n}{\mathbf{n}}^T+\sin (\theta {\mathbf{n}}^{\wedge })\text{\hspace{1em}(2-11)}$$

反之，有
$$\theta =arccos(\frac{tr(\mathbf{R})?1}{2})\text{\hspace{1em}(2-12)}$$
$tr(\mathbf{R})$表示求跡，即矩陣對(duì)角線元素之和。

由于旋轉(zhuǎn)軸在旋轉(zhuǎn)過(guò)程中是不動(dòng)的，則有
$$\mathbf{R}\mathbf{n}=\mathbf{n}\text{\hspace{1em}(2-13)}$$
這說(shuō)明$\mathbf{n}$是矩陣$\mathbf{R}$特征值為 1 對(duì)應(yīng)的特征向量，由此可以求出向量$\mathbf{n}$的值。

2.2.2 歐拉角

（1）將旋轉(zhuǎn)分解為 X、Y、Z 三個(gè)方向上的轉(zhuǎn)動(dòng)，常用的是 ZYX 順序的旋轉(zhuǎn)：

• 繞 Z 軸旋轉(zhuǎn)，得到偏航角 yaw；
• 繞旋轉(zhuǎn)后的 Y 軸旋轉(zhuǎn)，得到俯仰角 pitch；
• 繞旋轉(zhuǎn)后的 X 軸旋轉(zhuǎn)，得到滾轉(zhuǎn)角 roll。

此時(shí)，使用$[y,p,r{]}^T$這樣一個(gè)三維向量便可以描述任意旋轉(zhuǎn)。

（2）存在萬(wàn)向鎖問(wèn)題：即當(dāng)俯仰角為 ±90° 時(shí)，第三次旋轉(zhuǎn)軸和第一次旋轉(zhuǎn)軸重合，使系統(tǒng)丟失了一個(gè)自由度，這被稱為奇異性問(wèn)題。因此歐拉角在 SLAM 中較少使用。

（3）旋轉(zhuǎn)向量也存在奇異性，當(dāng) $\theta$超過(guò) $2\pi$時(shí)會(huì)產(chǎn)生周期性，這時(shí)將其限定在$2\pi$范圍內(nèi)便可以避免。

2.3 四元數(shù)

2.3.1 四元數(shù)的定義

（1）2D 情況下，可用單位復(fù)數(shù)表達(dá)旋轉(zhuǎn)，在復(fù)數(shù)平面中，結(jié)合歐拉公式，如下圖，向量$\mathbf{z}$旋轉(zhuǎn) 90° ，相當(dāng)于乘上 $i$。

（2）類(lèi)似的，在三維空間中，采用四元數(shù)描述旋轉(zhuǎn)。四元數(shù)有一個(gè)實(shí)部，三個(gè)虛部（分別對(duì)應(yīng) x、y、z軸）。

$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k\text{\hspace{1em}(2-14)}$$

寫(xiě)成向量形式

$$\mathbf{q}=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3\text{\hspace{1em}(2-15)}$$

并且，虛部之間滿足

$$?\begin{array}{c}{i}^2=j^2=k^2=?1\\ ij=k,ji=?k\\ jk=i,kj=?i\\ ki=j,ik=?j\end{array}\text{\hspace{1em}(2-16)}$$

（3）當(dāng)四元數(shù)的實(shí)部為 0 時(shí)，稱為虛四元數(shù)（分別對(duì)應(yīng)三維坐標(biāo)），此時(shí)，可以用來(lái)表示三維空間中的點(diǎn)。

2.3.2 四元數(shù)的計(jì)算

• 加法：對(duì)應(yīng)部分相加

• 乘法：每一項(xiàng)相乘，最后相加

• 模長(zhǎng)：各項(xiàng)系數(shù)平方和再開(kāi)方；并且兩個(gè)四元數(shù)乘積的模等于各自模的乘積，即$||{\mathbf{q}}_1{\mathbf{q}}_2||=||{\mathbf{q}}_1||?||{\mathbf{q}}_1||$。

• 共軛：實(shí)部相等，虛部互為相反數(shù)：${\mathbf{q}}_{\mathbf{a}}^{?}=[s_a,?{\mathbf{v}}_{\mathbf{a}}{]}^T$。并且，${\mathbf{q}}^{?}\mathbf{q}=\mathbf{q}{\mathbf{q}}^{?}=[s^2+{\mathbf{v}}^{\mathbf{T}}\mathbf{v},0{]}^T$

• 逆：${\mathbf{q}}^{?1}={\mathbf{q}}^{?}/{||\mathbf{q}||}^2$，則${\mathbf{q}}^{?1}\mathbf{q}=\mathbf{q}{\mathbf{q}}^{?1}=1$，同時(shí)，$({\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}{)}^{?1}={{\mathbf{q}}_{\mathbf{a}}}^{?1}{{\mathbf{q}}_{\mathbf{b}}}^{?1}$

• 數(shù)乘：$k\mathbf{q}=[ks,k\mathbf{v}{]}^T$

2.3.3 四元數(shù)表示旋轉(zhuǎn)

（1）三維空間中任意點(diǎn)均可用一個(gè)純虛四元數(shù)表示即$\mathbf{p}=[0,\mathbf{v}{]}^T$，經(jīng)一個(gè)單位四元數(shù)$\mathbf{q}$的旋轉(zhuǎn)后，得到${\mathbf{p}}^{\prime }$，則

$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{?1}\text{\hspace{1em}(2-17)}$$

最終${\mathbf{p}}^{\prime }$的虛部即為旋轉(zhuǎn)后點(diǎn)的坐標(biāo)。

2.3.4 四元數(shù)與其他旋轉(zhuǎn)表示法的轉(zhuǎn)換

• 角軸到四元數(shù)：

$$\mathbf{q}=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]\text{\hspace{1em}(2-18)}$$

• 四元數(shù)到角軸
  $$?\begin{array}{c}\theta =2arccos{q}_0\\ \\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}\text{\hspace{1em}(2-19)}$$

• 四元數(shù)到旋轉(zhuǎn)矩陣

• 旋轉(zhuǎn)矩陣到四元數(shù)

2.4 相似、仿射、射影變換

歐式變換不改變向量本身，只是進(jìn)行旋轉(zhuǎn)或平移。

（1）相似變換

相似變換比歐式變換多了一個(gè)自由度，即相當(dāng)于在旋轉(zhuǎn)或平移后，各坐標(biāo)再進(jìn)行等比例縮放，表達(dá)式為

$${\mathbf{T}}_s=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-20)}$$
$s$為縮放因子。

（2）仿射變換

表達(dá)式如下：
$${\mathbf{T}}_A=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-21)}$$

這里不要求$\mathbf{A}$為正交矩陣，因此，變換后，正方形就不是方的了，但仍是平行四邊形。

（3）射影變換

$${\mathbf{T}}_P=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^{\mathrm{T}}& v\end{array}\right]\text{\hspace{1em}(2-22)}$$

射影變換是最一般的變換，左上角$\mathbf{A}$為可逆矩陣，右上角$\mathbf{t}$為平移，左下角為縮放${\mathbf{a}}^{\mathbf{T}}$。從真實(shí)世界到相機(jī)照片的變換可以看做是射影變換：原本正方形的地磚，在照片中將不再是方形，由于近大遠(yuǎn)小，甚至可能是不規(guī)則的四邊形。