本篇繼續參照高翔老師《視覺SLAM十四講從理論到實踐》，講解四元數表示變換，博文將原第三講分為四部分來講解：1、旋轉矩陣和變換矩陣；2、旋轉向量與羅德里格斯公式；3、歐拉角與萬向（節）鎖；4-1、四元數表示變換；4-2、四元數差值，本文相對于原文會適當精簡，同時為便于理解，會加入一些注解和補充知識點，本篇為第四部分：四元數表示變換，另外三部分請參照博主的其他博文，
本篇文章，博主猶豫了很久，一是四元數東西較多，怕寫不好；二是已經有些人寫的非常好，基本疑問都解決了，后來者難以超越，博主在附錄會做推薦；三是作取舍真的太難，寫的太多篇幅太長，寫的少又怕涵蓋不全，然而自己挖的坑哭著也要填上，博主會盡量釋疑，做一些修補作業，也只求沒有錯誤紕漏，另外寫的不好的地方還請見諒，也歡迎多提意見，博主會盡量完善，

1. 四元數的定義

1.1 為什么使用四元數

旋轉矩陣用9個量描述3自由度的旋轉，具有冗余性；歐拉角和旋轉向量是緊湊的，但具有奇異性，事實上，我們找不到不帶奇異性的三維向量描述方式，
回憶之前學習過的復數，我們用復數集 C \mathbb{C} C表示復平面上的向量，可以表示為 z = a + b i z=a+bi z=a+bi的形式，其中 a , b ∈ R a,b\in R a,b∈R而且 i 2 = ? 1 i^{2}=-1 i2=?1，而復數的乘法則表示復平面上的旋轉：例如，乘上復數 i i i相當于逆時針把一個復向量旋轉 9 0 ° 90^{\circ } 90°，類似的，在表達三維空間旋轉時，也有一種類似于復數的代數：四元數（Quaternion），四元數是Hamilton找到的一種擴展的復數，它既是緊湊的，也沒有奇異性，如果說缺點，四元數不夠直觀，其運算稍復雜些，

1.2 復數與四元數

把四元數與復數類比可以幫助你更快地理解四元數，例如，當我們想要將復平面的向量旋轉 θ \theta θ角時，可以給這個復向量乘以 e i θ e^{i\theta} eiθ，這是極坐標表示的復數，它也可以寫成普通的形式，只要用歐拉公式即可： e i θ = c o s θ + s i n θ . e^{i\theta} = cos\theta + sin\theta. eiθ=cosθ+sinθ.
歐拉公式將指數函式的定義域擴大到了復數域，建立和三角函式和指數函式的關系，被譽為“數學中的天橋”，歐拉公式的簡單推導如下， e x e^{x} ex的泰勒展開式為：
e x = 1 + x + 1 2 ! x 2 + 1 3 ! x 3 + ? ? ? e^{x} = 1+x+\frac{1}{2!}x^{2}+\frac{1}{3!}x^{3}+\cdot \cdot \cdot ex=1+x+2!1?x2+3!1?x3+???將 x x x替換為 i θ i\theta iθ：
e i θ = 1 + i θ + 1 2 ! ( i θ ) 2 + 1 3 ! ( i θ ) 3 + 1 4 ! ( i θ ) 4 + 1 5 ! ( i θ ) 5 + 1 6 ! ( i θ ) 6 + 1 7 ! ( i θ ) 7 + 1 8 ! ( i θ ) 8 + ? ? ? = 1 + i θ ? 1 2 ! θ 2 ? 1 3 ! i θ 3 + 1 4 ! θ 4 + 1 5 ! i θ 5 ? 1 6 ! θ 6 ? 1 7 ! i θ 7 + 1 8 ! θ 8 + ? ? ? = ( 1 ? θ 2 2 ! + θ 4 4 ! ? θ 6 6 ! + θ 8 8 ! ? ? ? ? ) + i ( θ ? θ 3 3 ! + θ 5 5 ! ? θ 7 7 ! + ? ? ? ) = c o s θ + s i n θ . \begin{aligned} e^{i\theta} &= 1+i\theta+\frac{1}{2!}(i\theta)^{2}+\frac{1}{3!}(i\theta)^{3}+\frac{1}{4!}(i\theta)^{4}+\frac{1}{5!}(i\theta)^{5}+\frac{1}{6!}(i\theta)^{6}+\frac{1}{7!}(i\theta)^{7}+\frac{1}{8!}(i\theta)^{8}+\cdot \cdot \cdot \\ &= 1+i\theta-\frac{1}{2!}\theta^{2}-\frac{1}{3!}i\theta^{3}+\frac{1}{4!}\theta^{4}+\frac{1}{5!}i\theta^{5}-\frac{1}{6!}\theta^{6}-\frac{1}{7!}i\theta^{7}+\frac{1}{8!}\theta^{8}+\cdot \cdot \cdot \\ &= (1-\frac{\theta^{2}}{2!}+\frac{\theta^{4}}{4!}-\frac{\theta^{6}}{6!}+\frac{\theta^{8}}{8!}-\cdot \cdot \cdot)+i(\theta-\frac{\theta^{3}}{3!}+\frac{\theta^{5}}{5!}-\frac{\theta^{7}}{7!}+\cdot \cdot \cdot ) \\ &= cos\theta + sin\theta. \end{aligned} eiθ?=1+iθ+2!1?(iθ)2+3!1?(iθ)3+4!1?(iθ)4+5!1?(iθ)5+6!1?(iθ)6+7!1?(iθ)7+8!1?(iθ)8+???=1+iθ?2!1?θ2?3!1?iθ3+4!1?θ4+5!1?iθ5?6!1?θ6?7!1?iθ7+8!1?θ8+???=(1?2!θ2?+4!θ4??6!θ6?+8!θ8?????)+i(θ?3!θ3?+5!θ5??7!θ7?+???)=cosθ+sinθ.?當 θ = π \theta=\pi θ=π時，帶入歐拉公式得到： e i π = c o s π + i s i n π = ? 1 ? e i π + 1 = 0 e^{i\pi} = cos\pi + isin\pi = -1 \Rightarrow e^{i\pi} + 1 = 0 eiπ=cosπ+isinπ=?1?eiπ+1=0其中 e i π + 1 = 0 e^{i\pi} + 1 = 0 eiπ+1=0就是歐拉恒等式，它被譽為上帝公式，因為 e 、 π 、 i e 、 \pi 、 i e、π、i、乘法單位元1、加法單位元0，這五個重要的數學元素全部被包含在內，在數學愛好者眼里，仿佛一行詩道盡了數學的美好，歐拉公式的詳細說明可參見《歐拉公式之美》，
歐拉公式的右側正是一個單位長度的復數，所以在二維情況下，旋轉可以有單位復數來描述，類似的，可以看到，三維旋轉可以有單位四元數來描述，

1.3 四元數的形式

四元數的定義和復數非常類似，唯一的區別就是四元數有三個虛部，而復數只有一個，所有的四元數 q ∈ H q \in \mathbb{H} q∈H（ H \mathbb{H} H代表四元數的發現者William Rowan Hamilton）都可以寫成如下形式： q = q 0 + q 1 i + q 2 j + q 3 k . \mathbf{q}=q_{0}+q_{1}i+q_{2}j+q_{3}k. q=q0?+q1?i+q2?j+q3?k.其中 i , j , k i,j,k i,j,k為四元數的三個虛部，這三個虛部滿足以下關系式： { i 2 = j 2 = k 2 = ? 1 i j = k , j i = ? k j k = i , k j = ? i k i = j , i k = ? j . \left\{\begin{matrix} i^{2}=j^{2}=k^{2}=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j \end{matrix}\right.. ????????i2=j2=k2=?1ij=k,ji=?kjk=i,kj=?iki=j,ik=?j?.
如果把 i , j , k i,j,k i,j,k看成三個坐標軸，那么它們與自己的乘法和復數一樣，相互之間的乘法和外積一樣，有時，人們也用一個標量和一個向量來表達四元數： q = [ s , v ] T , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 . \mathbf{q}= [s, v]^{T}, s=q_{0} \in \mathbb{R},v=[q_{1},q_{2},q_{3}]^{T} \in \mathbb{R}^{3}. q=[s,v]T,s=q0?∈R,v=[q1?,q2?,q3?]T∈R3.這里， s s s稱為四元數的實部，而 v v v稱為它的虛部，如果一個四元數的虛部為0，則稱為實四元數，反之，若實部為0，則稱為虛四元數或純四元數，
如果 ∥ q ∥ = 1 \left \| q \right \|=1 ∥q∥=1，那么 q q q是單位四元數，可以用單位四元數表示三維空間中任意一個旋轉，不過這種表達方式和復數有著微妙的不同，在復數中，乘以 i i i意味著旋轉 9 0 ° 90^{\circ } 90°，而在四元數中，乘以 i i i對應著旋轉 18 0 ° 180^{\circ } 180°，這樣才能保證$ i j = k ij=k ij=k的性質，而 i 2 = ? 1 i^{2}=-1 i2=?1，意味著繞 i i i軸旋轉 36 0 ° 360^{\circ } 360°后得到一個相反的東西，也就是要旋轉兩周才會和它原先的樣子相等，
下面我們看一下四元數之間的運演算法則，

2. 四元數的運算

四元數和復數一樣，可以進行一系列的運算，常見的有四則運算、共軛、求逆、數乘等，下面分別介紹，
現有兩個四元數 q a ， q b q_{a}，q_{b} qa?，qb?，它們的向量表示為 [ s a , v a ] T [s_{a}, v_{a}]^{T} [sa?,va?]T， [ s b , v b ] T [s_{b}, v_{b}]^{T} [sb?,vb?]T，其中 v a = x a i + y a j + z a k ， v b = x b i + y b j + z b k v_{a}=x_{a}i+y_{a}j+z_{a}k，v_{b}=x_{b}i+y_{b}j+z_{b}k va?=xa?i+ya?j+za?k，vb?=xb?i+yb?j+zb?k，或者原始四元數表示為： q a = s a + x a i + y a j + z a k ， q b = s b + x b i + y b j + z b k . q_{a}=s_{a}+x_{a}i+y_{a}j+z_{a}k，q_{b}=s_{b}+x_{b}i+y_{b}j+z_{b}k. qa?=sa?+xa?i+ya?j+za?k，qb?=sb?+xb?i+yb?j+zb?k.那么，其運算可表示如下：

1. 加法和減法 q a ± q b = [ s a ± s b , v a ± v b ] q_{a}\pm q_{b}=[s_{a}\pm s_{b},v_{a}\pm v_{b}] qa?±qb?=[sa?±sb?,va?±vb?]

2. 乘法
   乘法是把 q a q_{a} qa?的每一項與 q b q_{b} qb?的每一項相乘，最后相加，整理得： q a q b = s a s b ? x a x b ? y a y b ? z a z b + ( s a x b + x a s b + y a z b ? z a y b ) i + ( s a y b ? x a z b + y a s b + z a x b ) j + ( s a z b + x a y b ? y a x b ? z a s b ) k . \begin{aligned} q_{a}q_{b} &=s_{a}s_{b}-x_{a}x_{b}-y_{a}y_{b}-z_{a}z_{b} \\ &+ (s_{a}x_{b}+x_{a}s_{b}+y_{a}z_{b}-z_{a}y_{b})i \\ &+ (s_{a}y_{b}-x_{a}z_{b}+y_{a}s_{b}+z_{a}x_{b})j \\ &+ (s_{a}z_{b}+x_{a}y_{b}-y_{a}x_{b}-z_{a}s_{b})k. \end{aligned} qa?qb??=sa?sb??xa?xb??ya?yb??za?zb?+(sa?xb?+xa?sb?+ya?zb??za?yb?)i+(sa?yb??xa?zb?+ya?sb?+za?xb?)j+(sa?zb?+xa?yb??ya?xb??za?sb?)k.?雖然稍微復雜，但形式上還是整齊有序的，如果寫成向量形式并利用內外積運算，該表達會更加簡潔： q a q b = [ s a s b ? v a T v b , s a v b + s b v a + v a × v b ] q_{a}q_{b}=[s_{a}s_{b}-v_{a}^{T}v_{b},s_{a}v_{b}+s_{b}v_{a}+v_{a}\times v_{b}] qa?qb?=[sa?sb??vaT?vb?,sa?vb?+sb?va?+va?×vb?]這個結果也稱為 G r a B m a n n GraBmann GraBmann積，它是四元數與旋轉聯系起來的關鍵，
   另外，由于最后一項外積的存在，四元數乘法通常是不可交換的，除非 v a v_{a} va?和 v b v_{b} vb?在 R \mathbb{R} R中共線，此時外項積為零，

3. 模長
   四元數的模長定義為： ∥ q a ∥ = s a 2 + x a 2 + y a 2 + z a 2 \left \| q_{a} \right \|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}} ∥qa?∥=sa2?+xa2?+ya2?+za2? ?可以驗證，兩個四元數乘積的模即模的乘積，這使得單位四元數相乘后仍是單位四元數： ∥ q a q b ∥ = ∥ q a ∥ ∥ q b ∥ \left \| q_{a}q_{b} \right \|=\left \| q_{a} \right \|\left \| q_{b} \right \| ∥qa?qb?∥=∥qa?∥∥qb?∥

4. 共軛
   四元數的共軛是把虛部取成相反數： q a ? = s a ? x a i ? y a j ? z a k = [ s a , ? v a ] . q_{a}^{*}=s_{a}-x_{a}i-y_{a}j-z_{a}k=[s_{a}, -v_{a}]. qa??=sa??xa?i?ya?j?za?k=[sa?,?va?].四元數共軛與其本身相乘，會得到一個實四元數，其實部為模長的平方： q ? q = q q ? = [ s 2 + v T v , 0 ] T q^{*}q=qq^{*}=[s^{2}+v^{T}v,0]^{T} q?q=qq?=[s2+vTv,0]T

5. 逆
   一個四元數的逆為： q ? 1 = q ? / ∥ q ∥ q^{-1}=q^{*}/\left \| q \right\| q?1=q?/∥q∥按此定義，四元數和自己的逆的乘積為實四元數 1 1 1： q q ? 1 = q ? 1 q = q q ? / ∥ q ∥ 2 = 1 qq^{-1}=q^{-1}q=qq^{*}/\left \| q \right\|^{2}=1 qq?1=q?1q=qq?/∥q∥2=1如果 q \mathbf{q} q為單位四元數，其逆和共軛就是同一個量，同時，乘積的逆具有和矩陣相似的性質： ( q a q b ) ? 1 = q b ? 1 q a ? 1 (q_{a}q_{b})^{-1}=q_{b}^{-1}q_{a}^{-1} (qa?qb?)?1=qb?1?qa?1?

6. 數乘
   又稱標量乘法，四元數可以與數相乘： k q = [ k s , k v ] T k\mathbf{q}=[ks,kv]^{T} kq=[ks,kv]T

3. 用四元數表示旋轉

3.1 四元數與旋轉關系

我們可以用四元數表達對一個點的旋轉，假設有一個空間三維點 p = [ x , y , z ] ∈ R 3 p=[x,y,z] \in \mathbb{R}^{3} p=[x,y,z]∈R3，以及一個由單位四元數 q q q指定的旋轉，三維點 p p p經過 q q q的旋轉后變為 p ′ p^{'} p′，如果使用矩陣描述，那么有 p ′ = R p p^{'}=Rp p′=Rp，而如何用四元數描述旋轉呢？
首先，把三維空間點用一個虛四元數來描述： w = [ 0 , x , y , z ] T = [ 0 , v ] T w=[0,x,y,z]^{T}=[0,v]^{T} w=[0,x,y,z]T=[0,v]T相當于把四元數的3個虛部和空間中的3個軸相對應，那么，旋轉后的點 w ′ w^{'} w′可表示為這樣的乘積： w ′ = q w q ? 1 w^{'}=qwq^{-1} w′=qwq?1這里的乘法均為四元數乘法，結果也是四元數，最后把 w ′ w^{'} w′的虛部取出，即得旋轉之后點的坐標，并且可以驗證，計算結果的實部為0，即為虛四元數，
下面從幾何的角度進一步講解四元數與3D旋轉之間的關聯，如果只是簡單應用，不需要了解幾何證明程序，則本小節就足夠了，

3.2 四元數與3D旋轉幾何證明

回憶一下《旋轉向量與羅德里格斯公式》討論的內容：如果我們需要將一個向量 v v v沿著一個用單位向量所定義的旋轉軸 u u u旋轉 θ \theta θ度，那么可以將其拆分為正交于旋轉軸的 v ⊥ v_{\perp } v⊥?以及平行于旋轉軸的 v ∥ v_{\parallel } v∥?，進行旋轉后獲得 v ⊥ ′ v_{\perp }^{'} v⊥′?和 v ∥ ′ v_{\parallel }^{'} v∥′?，相加后得到旋轉后的結果 v ′ = v ⊥ ′ + v ∥ ′ v^{'}=v_{\perp }^{'}+v_{\parallel }^{'} v′=v⊥′?+v∥′?，
將這些向量定義為純四元數，下表 q q q代表對應四元數：
w = [ 0 , v ] w ′ = [ 0 , v ′ ] w ⊥ = [ 0 , v ⊥ ] w ⊥ ′ = [ 0 , v ⊥ ′ ] w ∥ = [ 0 , v ∥ ] w ∥ ′ = [ 0 , v ∥ ′ ] u q = [ 0 , u ] \begin{aligned} & w=[0,v] && w^{'}=[0,v^{'}] \\ & w_{\perp }=[0,v_{\perp }] && w^{'}_{\perp }=[0,v^{'}_{\perp }] \\ & w_{\parallel }=[0,v_{\parallel }] && w^{'}_{\parallel }=[0,v^{'}_{\parallel }] \\ & u_{q}=[0,u]\end{aligned} ?w=[0,v]w⊥?=[0,v⊥?]w∥?=[0,v∥?]uq?=[0,u]??w′=[0,v′]w⊥′?=[0,v⊥′?]w∥′?=[0,v∥′?]?那么我們就能得到： w = w ⊥ + w ∥ w ′ = w ⊥ ′ + w ∥ ′ \begin{aligned} w=w_{\perp }+w_{\parallel } && w^{'}=w^{'}_{\perp }+w^{'}_{\parallel }\end{aligned} w=w⊥?+w∥???w′=w⊥′?+w∥′??和之前一樣，這里也分開討論 w ⊥ w_{\perp } w⊥?和 w ∥ w_{\parallel } w∥?的情況，

3.2.1 w ⊥ w_{\perp} w⊥?的旋轉

之前推導過，如果一個向量 v ⊥ v_{\perp } v⊥?正交于旋轉軸 u u u，那么 v ⊥ ′ = c o s ( θ ) v ⊥ + s i n ( θ ) ( u × v ⊥ ) v^{'}_{\perp }=cos(\theta)v_{\perp }+sin(\theta)(u\times v_{\perp }) v⊥′?=cos(θ)v⊥?+sin(θ)(u×v⊥?)將三維向量替換為對應的四元數， v ⊥ ′ v^{'}_{\perp } v⊥′?和 v ⊥ v_{\perp } v⊥?可以直接替換，而對于 u × v ⊥ u\times v_{\perp } u×v⊥?，由于 w w ⊥ = [ ? u ? v ⊥ , u × v ⊥ ] = [ 0 , u × v ⊥ ] = u × v ⊥ \begin{aligned}ww_{\perp} &=[-u\cdot v_{\perp},u\times v_{\perp}] \\ &=[0,u\times v_{\perp}] \\ &= u\times v_{\perp}\end{aligned} ww⊥??=[?u?v⊥?,u×v⊥?]=[0,u×v⊥?]=u×v⊥??將之前定義的四元數帶入，就能得到 w ⊥ ′ = c o s ( θ ) w ⊥ + s i n ( θ ) ( u q w ⊥ ) w^{'}_{\perp }=cos(\theta)w_{\perp }+sin(\theta)(u_{q}w_{\perp}) w⊥′?=cos(θ)w⊥?+sin(θ)(uq?w⊥?)因為四元數遵守分配率，可以繼續變換這個等式： w ⊥ ′ = ( c o s ( θ ) + s i n ( θ ) u q ) w ⊥ w^{'}_{\perp }=(cos(\theta)+sin(\theta)u_{q})w_{\perp} w⊥′?=(cos(θ)+sin(θ)uq?)w⊥?此時，可以將 c o s ( θ ) + s i n ( θ ) u q cos(\theta)+sin(\theta)u_{q} cos(θ)+sin(θ)uq?看作一個四元數，記為 q q q，且 ∥ q ∥ = 1 \left \| q \right \|=1 ∥q∥=1（讀者可以自證，用到 ∥ u ∥ = 1 \left \| u \right \|=1 ∥u∥=1），這樣我們就能將旋轉寫成四元數的乘積了，到此為止，我們已經將旋轉與四元數的積聯系起來了，由此得到 w ⊥ ′ = q w ⊥ w^{'}_{\perp }=qw_{\perp} w⊥′?=qw⊥?也就是說，如果旋轉軸 u u u的坐標為 u = [ u x u y u z ] u=\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix} u=???ux?uy?uz?????，旋轉角為 θ \theta θ，那么完成這一旋轉所需要的四元數 q q q可以構造為： q = c o s ( θ ) + s i n ( θ ) u q = [ c o s ( θ ) , 0 ] + [ 0 , s i n ( θ ) u ] = [ c o s ( θ ) , s i n ( θ ) u ] = c o s ( θ ) + s i n ( θ ) u x i + s i n ( θ ) u y j + s i n ( θ ) u z k \begin{aligned}q &= cos(\theta)+sin(\theta)u_{q} \\ &= [cos(\theta),0]+[0, sin(\theta)u] \\ &= [cos(\theta), sin(\theta)u] \\ &= cos(\theta)+sin(\theta)u_{x}i+sin(\theta)u_{y}j+sin(\theta)u_{z}k\end{aligned} q?=cos(θ)+sin(θ)uq?=[cos(θ),0]+[0,sin(θ)u]=[cos(θ),sin(θ)u]=cos(θ)+sin(θ)ux?i+sin(θ)uy?j+sin(θ)uz?k?總結一下，得到定理3D旋轉公式（正交四元數型）：當 v ⊥ v_{\perp} v⊥?正交于旋轉軸 u u u時，旋轉 θ \theta θ角度之后的 v ⊥ ′ v^{'}_{\perp} v⊥′?可以使用四元數乘法來獲得，令 w ⊥ = [ 0 , v ⊥ ] w_{\perp}=[0,v_{\perp}] w⊥?=[0,v⊥?]， q = [ c o s ( θ ) , s i n ( θ ) u ] q=[cos(\theta), sin(\theta)u] q=[cos(θ),sin(θ)u]，那么： w ⊥ ′ = q w ⊥ w^{'}_{\perp }=qw_{\perp} w⊥′?=qw⊥?

3.2.2 w ∥ w_{\parallel} w∥?的旋轉

接下來是平行于旋轉軸的 w ∥ w_{\parallel} w∥?，之前已經討論過，如果一個向量 v ∥ v_{\parallel} v∥?平行于 u u u，那么旋轉 θ \theta θ后對它不會做出任何變換，由此得到定理3D旋轉公式（平行四元數型）：當 v ∥ v_{\parallel} v∥?平行于旋轉軸 u u u時，旋轉 θ \theta θ角度之后的 v ∥ ′ v^{'}_{\parallel} v∥′?用四元數可以寫為： w ∥ ′ = w ∥ w^{'}_{\parallel}=w_{\parallel} w∥′?=w∥?

3.2.3 w w w的旋轉

有了前面的結論，我們可以獲得一般情況下 w ′ w^{'} w′的結果了： w ′ = w ∥ ′ + w ⊥ ′ = w ∥ + q w ⊥ \begin{aligned} w^{'} &= w^{'}_{\parallel}+w^{'}_{\perp} \\ &= w_{\parallel}+qw_{\perp}\end{aligned} w′?=w∥′?+w⊥′?=w∥?+qw⊥??在進一步化簡之前，引入三個引理：

1. 如果 q = [ c o s ( θ ) , s i n ( θ ) u ] q = [cos(\theta), sin(\theta)u] q=[cos(θ),sin(θ)u]，而且 u u u為單位向量，那么 q 2 = q q = [ c o s ( 2 θ ) , s i n ( 2 θ ) u ] q^{2}=qq=[cos(2\theta),sin(2\theta)u] q2=qq=[cos(2θ),sin(2θ)u]，
2. 假設 w ∥ = [ 0 , v ∥ ] w_{\parallel}=[0,v_{\parallel}] w∥?=[0,v∥?]是一個純四元數，而 q = [ α , β u ] q=[\alpha,\beta u] q=[α,βu]，其中 u u u是一個單位向量， α , β ∈ R \alpha, \beta \in \mathbb{R} α,β∈R，在這種條件下，如果 v ∥ v_{\parallel} v∥?平行于 u u u，那么 q v ∥ = v ∥ q qv_{\parallel}=v_{\parallel}q qv∥?=v∥?q，
3. 假設 w ⊥ = [ 0 , v ⊥ ] w_{\perp}=[0,v_{\perp}] w⊥?=[0,v⊥?]是一個純四元數，而 q = [ α , β u ] q=[\alpha,\beta u] q=[α,βu]，其中 u u u是一個單位向量， α , β ∈ R \alpha, \beta \in \mathbb{R} α,β∈R，在這種條件下，如果 v ⊥ v_{\perp} v⊥?平行于 u u u，那么 q v ⊥ = v ⊥ q ? qv_{\perp}=v_{\perp}q^{*} qv⊥?=v⊥?q?，

關于引理的證明，讀者可以參考文獻2，這里不再擴展，
為方便證明，再引入一個新的四元數 p = [ c o s ( 1 2 θ ) , s i n ( 1 2 θ ) u ] p=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u] p=[cos(21?θ),sin(21?θ)u]， q = p 2 q=p^{2} q=p2， ∥ p ∥ = 1 \left \| p \right \|=1 ∥p∥=1，現在，我們就能對原本的旋轉公式進行變形了： w ′ = w ∥ + q w ⊥ = p p ? 1 w ∥ + p p w ⊥ = p p ? w ∥ + p p w ⊥ = p w ∥ p ? + p w ⊥ p ? = p ( w ∥ + w ⊥ ) p ? = p w p ? \begin{aligned} w^{'} &= w_{\parallel}+qw_{\perp} \\ &= pp^{-1}w_{\parallel}+ppw_{\perp} \\ &= pp^{*}w_{\parallel}+ppw_{\perp} \\ &= pw_{\parallel}p^{*}+pw_{\perp}p^{*} \\ &=p(w_{\parallel}+w_{\perp})p^{*} \\ &= pw_{}p^{*}\end{aligned} w′?=w∥?+qw⊥?=pp?1w∥?+ppw⊥?=pp?w∥?+ppw⊥?=pw∥?p?+pw⊥?p?=p(w∥?+w⊥?)p?=pw?p??至此，我們就解開了四元數與3D旋轉之間的謎題，3D空間中任意一個旋轉都能夠用一個三個四元數相乘的形式表達出來，
綜上，我們可以總結出一個定理3D旋轉公式（一般情況四元數型）：任意向量 v v v沿著以單位向量定義的旋轉軸 u u u旋轉 θ \theta θ度之后的 v ′ v^{'} v′，可以使用四元數乘法來獲得，令 w = [ 0 , v ] , q = [ c o s ( 1 2 θ ) , s i n ( 1 2 θ ) u ] w=[0,v],q=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u] w=[0,v],q=[cos(21?θ),sin(21?θ)u]，那么： w ′ = q w q ? = q w q ? 1 w^{'}=qwq^{*}=qwq^{-1} w′=qwq?=qwq?1這里用的是 1 2 θ \frac{1}{2}\theta 21?θ而不是 θ \theta θ，這是因為 q q q做的就是一個 1 2 θ \frac{1}{2}\theta 21?θ的旋轉，而 q ? q^{*} q?或 q ? 1 q^{-1} q?1也做了一個 1 2 θ \frac{1}{2}\theta 21?θ的旋轉，我們進行了兩次旋轉，而不是一次，這兩次旋轉的結果是一個旋轉角為 θ \theta θ的旋轉，下圖或可解釋旋轉的程序：
對 w ′ = q w q ? = q w q ? 1 w^{'}=qwq^{*}=qwq^{-1} w′=qwq?=qwq?1，我的理解是：對向量 v v v轉化成的純四元數 w w w，經過 q q q的旋轉 q w qw qw后，得到實部不為0的普通四元數，這時沒辦法映射到三維超平面（總不能轉著圈就轉到了四維空間吧），結果與最初的點不在同一三維空間，為了解決這個問題，先對第四維（角度）旋轉一半，再用逆或共軛旋轉回來，這時正好將產生的第四維變為0，重新回到初始的三維超平面空間，這樣不僅解決了奇點問題，也不會產生冗余資料，不知道 H a m i l t o n Hamilton Hamilton怎么想到的，太牛了，

4. 四元數到其它旋轉表示的相互轉換

任意單位四元數描述了一個旋轉，該旋轉也可用旋轉向量、旋轉矩陣或歐拉角描述，現在來考察四元數與旋轉向量、旋轉矩陣或歐拉角的相互轉換關系，

4.1 旋轉向量

本節介紹旋轉向量與四元數之間的轉換關系，
繞坐標軸的多次旋轉可以等效為繞某一轉軸旋轉一定的角度，假設已知等效旋轉軸方向單位旋轉向量 u u u的坐標為 u = [ u x u y u z ] u=\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix} u=???ux?uy?uz?????，旋轉角為 θ \theta θ，根據3.2.3推導的定理3D旋轉公式（一般情況四元數型），設其等效的單位四元數為 q = [ q 0 , q 1 , q 2 , q 3 ] q=[q_{0},q_{1},q_{2},q_{3}] q=[q0?,q1?,q2?,q3?]，則有： { q 0 = c o s ( 1 2 θ ) q 1 = u x s i n ( 1 2 θ ) q 2 = u y s i n ( 1 2 θ ) q 3 = u z s i n ( 1 2 θ ) (4.1) \left\{\begin{matrix} q_{0}=cos(\frac{1}{2}\theta )\\ q_{1}=u_{x}sin(\frac{1}{2}\theta )\\ q_{2}=u_{y}sin(\frac{1}{2}\theta )\\ q_{3}=u_{z}sin(\frac{1}{2}\theta ) \end{matrix}\right.\tag{4.1} ????????q0?=cos(21?θ)q1?=ux?sin(21?θ)q2?=uy?sin(21?θ)q3?=uz?sin(21?θ)?(4.1)
同理，當已知單位四元數 q = [ q 0 , q 1 , q 2 , q 3 ] q=[q_{0},q_{1},q_{2},q_{3}] q=[q0?,q1?,q2?,q3?]，其對應的旋轉角 θ \theta θ和旋轉向量 u u u為： { θ = 2 arccos ? q 0 [ u x , u y , u z ] T = [ q 1 , q 2 , q 3 ] T / sin ? ( 2 θ ) (4.2) \left\{\begin{matrix} \theta=2\arccos q_{0}\\ [u_{x},u_{y},u_{z}]^{T}=[q_{1},q_{2},q_{3}]^{T}/\sin(\frac{2}{\theta }) \end{matrix}\right.\tag{4.2} {θ=2arccosq0?[ux?,uy?,uz?]T=[q1?,q2?,q3?]T/sin(θ2?)?(4.2)

4.2 旋轉矩陣

已知單位四元數 q q q，根據博文《三維空間剛體運動2：旋轉向量與羅德里格斯公式》中的羅德里格斯公式展開式（2.3）及本文公式（4.2），將單位旋轉向量 u u u及旋轉角 θ \theta θ替換為單位四元數 q q q，省去計算程序，得到單位四元數到旋轉矩陣的轉換公式為： R = [ 1 ? 2 q 2 2 ? 2 q 3 2 2 ( q 1 q 2 ? q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) 1 ? 2 q 1 2 ? 2 q 3 2 2 ( q 2 q 3 ? q 0 q 1 ) 2 ( q 1 q 3 ? q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 1 ? 2 q 1 2 ? 2 q 2 2 ] (4.3) R=\begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} & 2(q_{1}q_{2}-q_{0}q_{3}) & 2(q_{1}q_{3}+q_{0}q_{2})\\ 2(q_{1}q_{2}+q_{0}q_{3}) & 1-2q_{1}^{2}-2q_{3}^{2} & 2(q_{2}q_{3}-q_{0}q_{1})\\ 2(q_{1}q_{3}-q_{0}q_{2}) & 2(q_{2}q_{3}+q_{0}q_{1}) & 1-2q_{1}^{2}-2q_{2}^{2} \end{bmatrix}\tag{4.3} R=???1?2q22??2q32?2(q1?q2?+q0?q3?)2(q1?q3??q0?q2?)?2(q1?q2??q0?q3?)1?2q12??2q32?2(q2?q3?+q0?q1?)?2(q1?q3?+q0?q2?)2(q2?q3??q0?q1?)1?2q12??2q22?????(4.3)
假設已知變換矩陣： R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix} R=???r11?r21?r31??r12?r22?r32??r13?r23?r33?????根據公式（4.3），推導對應的單位四元數為： { q 0 = 1 2 1 + r 11 + r 22 + r 33 q 1 = r 32 ? r 23 4 q 0 q 2 = r 13 ? r 31 4 q 0 q 3 = r 21 ? r 12 4 q 0 (4.4) \left\{\begin{matrix} q_{0}=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\\ q_{1}=\frac{r_{32}-r_{23}}{4q_{0}}\\ q_{2}=\frac{r_{13}-r_{31}}{4q_{0}}\\ q_{3}=\frac{r_{21}-r_{12}}{4q_{0}} \end{matrix}\right.\tag{4.4} ????????q0?=21?1+r11?+r22?+r33? ?q1?=4q0?r32??r23??q2?=4q0?r13??r31??q3?=4q0?r21??r12???(4.4)

4.3 歐拉角

那么將Z-Y-X歐拉角（或RPY角：繞固定坐標系的X-Y-Z依次旋轉α,β,γ角）轉換為四元數： q = [ cos ? γ 2 0 0 sin ? γ 2 ] [ cos ? β 2 0 sin ? β 2 0 ] [ cos ? α 2 sin ? α 2 0 0 ] = [ cos ? α 2 cos ? β 2 cos ? γ 2 + sin ? α 2 sin ? β 2 sin ? γ 2 sin ? α 2 cos ? β 2 cos ? γ 2 ? cos ? α 2 sin ? β 2 sin ? γ 2 cos ? α 2 sin ? β 2 cos ? γ 2 + sin ? α 2 cos ? β 2 sin ? γ 2 cos ? α 2 cos ? β 2 sin ? γ 2 ? sin ? α 2 sin ? β 2 cos ? γ 2 ] q=\begin{bmatrix} \cos\frac{\gamma}{2}\\ 0\\ 0\\ \sin\frac{\gamma}{2} \end{bmatrix} \begin{bmatrix} \cos\frac{\beta}{2}\\ 0\\ \sin\frac{\beta}{2}\\ 0 \end{bmatrix} \begin{bmatrix} \cos\frac{\alpha}{2}\\ \sin\frac{\alpha}{2}\\ 0\\ 0 \end{bmatrix} =\begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix} q=?????cos2γ?00sin2γ????????????cos2β?0sin2β?0???????????cos2α?sin2α?00??????=?????cos2α?cos2β?cos2γ?+sin2α?sin2β?sin2γ?sin2α?cos2β?cos2γ??cos2α?sin2β?sin2γ?cos2α?sin2β?cos2γ?+sin2α?cos2β?sin2γ?cos2α?cos2β?sin2γ??sin2α?sin2β?cos2γ???????
根據上面的公式可以求出逆解，即由四元數 q = ( q 0 , q 1 , q 2 , q 3 ) q=(q_{0},q_{1},q_{2},q_{3}) q=(q0?,q1?,q2?,q3?)到歐拉角的轉換為： [ α β γ ] = [ a t a n 2 ( 2 ( q 0 q 1 + q 2 q 3 ) , 1 ? 2 ( q 1 2 + q 2 2 ) ) arcsin ? ( 2 ( q 0 q 2 ? q 1 q 3 ) a t a n 2 ( 2 ( q 0 q 3 + q 1 q 2 ) , 1 ? 2 ( q 2 2 + q 3 2 ) ) ] \begin{bmatrix} \alpha\\ \beta\\ \gamma \end{bmatrix} =\begin{bmatrix} atan2(2(q_{0}q_{1}+q_{2}q_{3}),1-2(q^{2}_{1}+q^{2}_{2}))\\ \arcsin (2(q_{0}q_{2}-q_{1}q_{3})\\ atan2(2(q_{0}q_{3}+q_{1}q_{2}),1-2(q^{2}_{2}+q^{2}_{3})) \end{bmatrix} ???αβγ????=???atan2(2(q0?q1?+q2?q3?),1?2(q12?+q22?))arcsin(2(q0?q2??q1?q3?)atan2(2(q0?q3?+q1?q2?),1?2(q22?+q32?))????這里使用了 a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)而不是 a r c t a n ( y x ) arctan(\frac{y}{x}) arctan(xy?)，因為 a r c t a n ( y x ) arctan(\frac{y}{x}) arctan(xy?)的取值范圍為 [ ? π 2 , π 2 ] [-\frac{\pi}{2},\frac{\pi}{2}] [?2π?,2π?]，只有 180 ° 180° 180°，而繞某個軸旋轉時范圍是 360 ° 360° 360°， a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)正好滿足需求， a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)是一個函式，在C語言里回傳的是指方位角，函式原型為 d o u b l e a t a n 2 ( d o u b l e y , d o u b l e x ) double \space atan2(double y, double x) double atan2(doubley,doublex) ，回傳以弧度表示的 y / x y/x y/x 的反正切，y 和 x 的值的符號決定了正確的象限，也可以理解為計算復數 x + y i x+yi x+yi 的輻角，計算時atan2 比 atan 穩定，
另外需要注意的就是奇異性問題，即萬向鎖，下面分析這種情況，當剛體繞Y軸旋轉了 90 ° 90° 90°（俯仰角pitch=90）時，如何計算橫滾角roll和偏航角yaw？這時會發生自由度丟失的情況，即Yaw和Roll會變為一個自由度，此時再使用上面的公式根據四元數計算歐拉角會出現問題： arcsin ? ( 2 ( q 0 q 2 ? q 1 q 3 ) \arcsin (2(q_{0}q_{2}-q_{1}q_{3}) arcsin(2(q0?q2??q1?q3?)的定義域為 [ ? 1 , 1 ] [-1,1] [?1,1]，當 2 ( q 0 q 2 ? q 1 q 3 ) = ± 0.5 2(q_{0}q_{2}-q_{1}q_{3})=\pm 0.5 2(q0?q2??q1?q3?)=±0.5時（在程式中浮點數不能直接進行等于判斷，要使用合理的閾值），俯仰角 β \beta β為 ± 90 ° \pm 90° ±90°，將其帶入正向公式計算出四元數 ( q 0 , q 1 , q 2 , q 3 ) (q_{0},q_{1},q_{2},q_{3}) (q0?,q1?,q2?,q3?)，然后可以發現逆向公式中atan2函式中的引數全部為0，即出現了 0 0 \frac{0}{0} 00?的情況！無法計算，
當 β = 90 ° \beta=90° β=90°時， sin ? β 2 = cos ? β 2 = 0.707 \sin \frac{\beta}{2}=\cos \frac{\beta}{2}=0.707

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/81251.html

標籤：其他

上一篇：2020云棲大會編程限時搶答賽 - 早中晚3場題解

下一篇：Git三大核心問題總說：版本、分支、遠程