四元數指數映射旋轉參數化的實際應用

四元數指數映射旋轉參數化的實際應用

哪吒三太子 2016/3/26 於上海盧灣

下面爲本文使用術語表,表中全部詞條大多直接採用英文術語,請各位讀者自行伸縮去取,筆者在此不作所謂"直譯".

• DOF(degree-of-freedom) 旋轉自由度
• ODE(ordinary differential equation) 常微分方程
• transformation matrix 變換矩陣(變換一詞包含:平移,旋轉,放縮,甚至仿射)
• S3 單位四元數組成的球形表面空間
• SO3 四維空間中的三維子空間集合
• normalize 標準化,一般變模爲單位長度1
• end-effector 機械臂末端-與環境交互部分
• singularity 奇點
• hierarchies 層級結構

摘要:

  三維旋轉自由的參數化一直不竟如人意,雖如此現有的圖形應用方法除了要求咱們擁有計算連續位移和物體連接旋轉的能力以外,大多數還須要咱們可以自行處理微積分方程,DOF的優化,和旋轉插值.
(注意:在這裏的旋轉爲 orientation 統稱,但不具體指定參數化方法,e.g.歐拉角表示法,四元數表示法,亦或者本文介紹的指數映射法)但普遍使用的歐拉角或四元數參數法由於其自身數學限制,都不能很好的適應上述這些需求.
  指數映射法使用旋轉軸和旋轉量來描述R³中的向量. 一些圖形研究員已經把這種方法應用在了旋轉插值問題中,但其餘卻未涉及. 本文就是爲了補缺這一片應用方法的空白,詳細展示了DOF旋轉問題中的計算,微分,積分方法供讀者參考使用. 而且這種方法在計算機精度方面呈現出數值穩定,在映射中存在的奇點(singularity)也能夠簡單的動態重複參數化來解決. 本文將會演示如何使用指數映射法來解決相似萬向自由體和DOF空間鉸鏈問題. e.g. 精確地數學建模人體的肩膀,胯等關節的鏈接運動.
  在實驗結果中指數映射法相比較於歐拉角和四元數法,有如下優點:更強的魯棒性,小向量能夠被更好的表達,無需明確過多的限制條件,更好的建模能力,ODE簡化和更好的插值性能.

1 介紹

  

爲何咱們要使用四元數指數映射？

  在現有的旋轉參數化方法中主要有歐拉角表示法和四元數表示法兩種. 在多剛體鏈接系統中動力學模擬 必須使用 hierarchies (層次結構)來描述剛體連接. 而歐拉角(歐幾里德空間參數)會遇到萬向節死鎖問題(因爲遇到數學上的奇點子空間,致使降維), 四元數方法則必須限制其範數(一般稱爲"模")爲單位長度1.
  在R ³中每一個非零向量都由方向和長度組成. 而旋轉是由物體沿着一個旋轉軸的旋轉量組成的. 若是咱們擴展零向量爲0模長,單位旋轉的話,整個指數映射就成爲了連續空間的映射. 與四元數參數化不一樣的是,因爲指數映射參照歐幾里德空間,因此也存在萬向節死鎖的奇點空間,但這個奇點的座標區間很是遠離咱們工做區間,而且參數化結果包含了大多數四元數參數結果的特性,而且無需擔憂會掉進非歐幾里德流形之中. 最後本文也會絕不避諱的討論指數映射方法的缺點,以供讀者在解決特定問題中參考選擇. 在 第二節中咱們討論現有參數法的優缺利弊. 第三節討論指數映射爲何把旋轉映射到四元數而非歐拉角組成的旋轉矩陣,而後介紹瞭如何在這基礎上進行微分.  第四節中展現了指數映射法很是適用於微分控制和前向動力學模擬,而且也討論在插值和時空優化中的限制. 第五節中咱們進一步拓展該方法使其適用於帶限制條件(鉸鏈)的DOF旋轉問題.  最後在 第六節中總結方法的健壯度並給出C實現的僞代碼供讀者參考.

2 經常使用參數法的評估

  在圖形應用中主要有5種基本方法來描述並控制物體的旋轉:
   1. forward kinematics
   2. inverse kinematics
   3. forward dynamics
   4. inverse dynamics
   5. spacetime optimization
   其餘方法則基於這五種基本方法之上.
  基於這些方法的應用除了在軟件大小和複雜度上有差別以外,都必須具備如下能力:
  1. 能計算物體(物體的組合)上每一個點的旋轉和座標, 這是forward kinematics方法所須要的基礎能力.
  2. 能計算每一個點旋轉和座標的導數,這是inverse kinematics, dynamics,spacetime所須要的基礎能力.
  3. 能對ODE進行積分運算,這是inverse kinematics, dynamics,spacetime所需能力.
  4. 能對兩個關鍵幀進行平滑插值.
  5. 能組合多個旋轉成爲一個操做.
  6. 能計算對應旋轉的參數逆,找出對應參數. i.e. 上述能力都是以參數計算旋轉,而此能力是以旋轉計算參數. 故稱爲逆操做.

上述中3.4.5是參數空間與生俱來的特性. 1,2則須要把旋轉表示爲4x4的 transformation 矩陣. 而且由於 transformation 具備諸多線性空間變換能力,因此咱們能夠在方法重穿插使用多種參數方法 e.g.使用旋轉矩陣計算偏微分獲得點旋轉和座標導數.
\[R(v) 和 \frac{\partial R}{\partial v}\]
其中R爲4x4矩陣, \(\partial R / \partial v\) 爲4x4xn張量, n維向量中每一個元素爲4x4矩陣. 至此咱們就獲得了點和旋轉的 Jacobi矩陣.

2.1 3x3旋轉矩陣

既然旋轉矩陣能很好的表示一個旋轉,而且它也有乘法進行旋轉組合的功能,更進一步可使用優化代碼來對矩陣進行快速運算,那咱們爲何不用它來對旋轉進行參數化表達?
緣由是咱們必須強加6個非線性的限制條件在旋轉矩陣的9個參數上,3個條件限制每一個矩陣列向量的模爲1,3個條件使他們互相保持正交).因此每一步的運算以前都首先保證矩陣爲正交矩陣,這個條件很苛刻.

2.2 歐拉角

歐拉角由一個指定座標系的 x,y,z 軸,決定一個向量在對應座標軸上的對應旋轉. 一般咱們能夠看到它被表示成矩陣的形式並用 sin cos 正餘弦函數來表徵對應座標旋轉,雖然對旋轉角的參數化是非線性的(sin,cos),但他們的導數很是容易被計算出來.
歐拉角的壞處和好處一樣明顯,它會遇到一個致命的問題 --- 萬向節死鎖. 討論死鎖問題超出了本文範疇,有興趣的讀者可參考https://en.wikipedia.org/wiki/Gimbal_lock.
雖然歐拉角有致命缺陷,但咱們不能忽視它在 2DOFs 旋轉問題中表現良好,並在不遇到死鎖問題下很是容易使用.

2.3 四元數

四元數的歷史很是離奇,它是在愛爾蘭數學家 William Rowan Hamilton和妻子散步時偶然發現的. 它用一個四維向量表示三維旋轉,對四元數的詳細討論一樣也超出了本文的範疇,請移步參考http://baike.baidu.com/view/319754.htm.
在這裏咱們只說如下四元數的優缺點. 四元數因爲定義及其精妙(其實四元數在羣論中表達爲一個S3球表面的數域,對乘法封閉(筆者實際上是羣論的瘋狂腦殘粉 :P)),用它來表示旋轉問題比矩陣運算更快速,比歐拉角而言沒有死鎖問題,並且很是易於插值計算 e.g.動畫的計算.
它的缺點是因爲四元數表示四維空間中的向量,而現實世界只有三個維的自由度,因此在進行積分運算的時候,每一步運算都須要把它進行 normalize 以壓縮掉一個維度,對積分運算形成了很大的麻煩. 在實際應用中積分步長很難控制,過短則會致使性能降低問題,太長則離開S3球表面,偏差就會很大. 還有一種方法由Michael Gleicher 提出,它解決了 normalize 問題,但會致使 Jacobi 矩陣秩缺失,這意味着在四維空間中有一個"子空間"中全部四元數都對應同一個三維旋轉 i.e.四元數在這個"奇特的"空間中任意改變並不會致使三維空間中旋轉出現變化.(一般來講就是掉進了一個高維流形陷阱之中了). 可能遇到的一個實際問題就是:咱們再也沒法求出物體的轉動慣量了.
總結來講使用四元數確實能夠解決不少問題,但當軟件須要計算導數來控制和優化系統的時候,事情就會變得很麻煩. 最後毋庸置疑的它是計算3DOF旋轉插值的最佳方法.

3 四元數指數映射

引入指數映射的緣由是四元數只用到了全部四元數集中一個一個子空間,也就是S³球表面. 因此咱們但願有一種方法能讓R³空間中的點一一對應S³球表面,而不會超出這個球表面空間,一般咱們用強加給四元數單位長度的限制來解決這個問題(上面也說明了每步須要 normalize 的開銷和積分時候的麻煩).因此咱們的需求總結以下:

1. 方法必須沒有萬向節死鎖
2. 旋轉插值要很容易實現
3. 組合旋轉的能力
4. 容易進行導數與積分運算

Anyway,知足上述全部條件這是不可能的:( 已經有數學家進行過嚴密的證實R³至SO³(四元數的全部三維子空間的統稱,固然也包含球空間)是不可能消除奇點的,也就是說必定會遇到死鎖問題,可是咱們能夠垂手可得地躲開這個不可避免的問題.

下面咱們就用數學語言來描述它:
四元數: \[q = [{q}_{w},{q}_{v1},{q}_{v2},{q}_{v3}] = [{q}_{w},v] \equiv w+ai+bj+ck\]
咱們定義當v=0時 \[{e}^{{[0,0,0]}^{T}} = {[0,0,0,1]}^{T}\]
當\(v \neq 0\) \[ {e}^{v} = {[sin(\frac{1}{2}\theta) \hat{v}, cos(\frac{1}{2}\theta)]}^{T}\]
四元數的指數形式方程推導參見https://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power

其中\(\hat{v}=v/|v|\) \(\theta = |v|\),從方程上看出咱們把四維向量包裝成三維的了,其中 v = ai+bj+ck 而常量 w 則用 v 的模來表示. 上述方程式在數學上須要推導在零處函數的極限,可是在計算機中當|v|接近於 0 時計算結果溢出(由於計算機 float 最多表達1e-38 小數,小於這個數則會棧溢出),會致使計算溢出,那麼咱們稍許改變一下方程式\(\theta = |v|\). 這樣式子就變成 \(sin(1/2\theta)\frac{v}{\theta}\) 而 \(\frac{sin(1/2\theta)}{1/2\theta} = sinc(1/2\theta)\) 爲辛格函數 在零處有定義,因此方程式在計算機計算中變得穩定. 而使人沮喪的是標準C庫沒有辛格函數的定義,因此咱們用泰勒展開表達(題外話:其實計算機計算三角函數都是用泰勒展開計算的,由於泰勒展開在三角函數的\([-\pi ,\pi]\)收斂且拉格朗日餘項偏差能夠任意小)
\[\frac{sin(\frac{1}{2}\theta))}{\theta} = \frac{1}{\theta}TaylerExpansion(sin(\frac{1}{2}\theta))\]
\[ = \frac{1}{\theta}(\frac{\theta}{2}-\frac{{(\theta/2)}^{3}}{3!}+\frac{{(\theta/2)}^{5}}{5!}+...)\]
\[ = \frac{1}{2}-\frac{{\theta}^{2}}{48}+\frac{{\theta}^{4}}{{2}^{5}\cdot 5!} \]

學過《高等數學》的讀者必定知道一旦函數的泰勒展開收斂,則當估計項爲 n 時,餘項偏差不會超過第 n+1 項的值,咱們只要讓 n+1 項爲計算機最小精度便可,這樣咱們就消除了計算機計算偏差了. 事實上咱們只需前兩項便可消偏差:
\[\frac{sin(\frac{1}{2}\theta)}{\theta} = \frac{1}{2} - \frac{{\theta}^{2}}{48}\]
至此咱們有了四元數的指數映射來完整表達一個旋轉,且消除了四元數的四維特徵使其徹底落在S3球上而不用強加 normalize 的限制. Great,下面咱們就來討論它的實際用途:導數微分,積分,插值 etc.

3.1 導數

由於四元數的指數是連續函數,因此可使用鏈式法則進行求導:
\[\frac{\partial R}{\partial v} = \frac{\partial R}{\partial q}\frac{\partial q}{\partial v}\]
計算的具體過程推導過程詳見http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf

3.2 缺陷

####3.2.1 奇點問題
以前咱們一直說該方法是不能避免遇到奇點問題的,而這個問題是能夠被躲避的,緣由在於該函數的奇點在 \(2n\pi\) 上. 衆所周知當取到 \(2\pi\) 時,旋轉實際上是繞到了原處,咱們只要把取值範圍變成 \([0,2\pi)\)便可,當 \(\theta\) 接近\(2\pi\)時咱們用 \(2\pi - \theta\) 也就是-v來代替便可.
由於模擬時間步長內旋轉不可能超過\(\pi\),因此每一步模擬咱們都檢測一下 |v| 若是它接近\(\pi\),那麼咱們就用 \((1-2\pi/|v|)v\)來代替 v,在試驗中上面等價的方程更利於倒數的計算.這個動態的重複計算參數理論上來講能徹底避免函數掉入奇點之中.固然這也使咱們不能在超出規定範圍內進行動畫插值(e.g. 有兩個rotation狀態假設沿着x軸正方向旋轉,r1=30度,r2=430度,咱們不能簡單地在這兩個狀態中插值,由於後者超過了咱們規定的範圍,因此必須被分割成30-180, 180-360, 360-430 三個部分進行插值計算,這無疑增長了系統的複雜度)
####3.2.2 旋轉合併
由於四元數的乘法 \(\equiv\) 旋轉矩陣的乘法.可是咱們把四元數常量 w 整合進了 v 之中致使這個性質被破壞,因此在合併旋轉的時候咱們必須先把四元數的指數還原成標準四元數(保證模爲單位長度1),而後進行乘法合併,最後再映射到四元數的指數之中.
幸運的是咱們一般不須要合併指數形式的旋轉,由於物體的旋轉一般由它的導數來驅動,或者直接修改四元數部分的參數. 當他們轉換成 transformation matrix(變換矩陣)並施加在物體上時，它們就以矩陣的形式被合併了. 故除非特殊須要,不然沒有額外的運算開銷.

4 應用

磨刀霍霍,咱們討論了那麼久就是爲了在這裏說明它是如何被用來簡化實際應用的.請你相信,指數映射在控制,模擬方面都具備很是好的性能.但在插值和優化方面比其餘方法複雜.

微分控制和動力學模擬

研究這項工做的主要動機是由於咱們想要一個較好的微分控制器,它能夠直接控制物體的旋轉,進行 inverse kinematics研究和實時的機械臂控制. 爲了控制物體或者機械臂的末端(end-effector)(這裏使用機械臂泛指用關節連接的物體運動-其實也就是多剛體系統運動),微分控制器只要求\(\partial R/\partial v\)是連續的,而且不存在萬向節死鎖這兩個條件便可. 由於計算機控制發生在離散時間內,而咱們以前介紹的動態重複參數化方法能夠有效躲過萬向節死鎖,因此這兩項條件老是能夠被知足的.
在動力學模擬中,咱們不只要追蹤物體某一時刻的位置和姿態,一樣須要追蹤它的線速度和角速度.
不要忘了電腦模擬是離散時間的,爲了正確的計算物體的位移和旋轉,咱們須要計算導數來進行函數的數值逼近,對一個四元數表示的旋轉而言,它對時間的導數爲以下公式 (數學推導在這裏）:
\[\dot{q} = \frac{1}{2}\tilde{\omega}\circ q\]
其中\(\tilde{\omega}\)是末尾加一個 0 的四元數對應的角速度. 那麼對四元數指數映射的導數一樣能夠求出：
\[v=log(q)=\frac{2cos^{-1}(q_w)}{|q_v|}q_v\] 所以
\[\dot{v} = \frac{\partial log(q)}{\partial q}\dot{q} = \frac{\partial log(q)}{\partial q}\frac{1}{2}\tilde{\omega}\circ q\]
其中 \(q_v\)是四元數的虛部. 在上述方程中若是咱們用 v 徹底替代 q 的話那將使公式更簡潔,而且無需四元數的參與.咱們來看:
當\(\theta >> 0\)時:
咱們定義: \[p=\omega\times v \gamma=\theta cos(\frac{1}{2}\theta) \eta=\frac{\omega\cdot v}{\theta}(cot(\frac{1}{2}\theta)-\frac{2}{\theta})\] 那麼將有
\[\dot{v}=\frac{1}{2}(\gamma\omega+p-\eta v)\]
當\(\theta -> 0\)時,注意 \(cot(\frac{1}{2}\theta)=cos(\frac{1}{2}\theta)/sin(\frac{1}{2}\theta)\)趨向無窮,須要計算在零點處函數的極限,一樣的咱們用其泰勒展開代替 cot 三角函數便獲得最終公式以下:
\[p=\omega\times v \gamma_0=\frac{12-\theta^2}{6} \eta_0=\omega\cdot v\frac{60+\theta^2}{360}\]而後公式便成了\[\dot{v}=\frac{1}{2}(\gamma_0\omega+p-\eta_0 v)\]
總結下來指數映射很是適用於微分控制和模擬,除了咱們要採起一些小手段避免其陷入奇點以外,它比四元數的優點有如下三點

1. 沒有 2.3節所說的 Jacobi 秩缺失
2. 不用在每步積分前先 normalize ,使其不脫離 S3球面
3. 系統使用三維向量而非四維,簡化了計算的複雜度

5 鉸鏈和關節

上面咱們已經介紹了全自由度下四元數指數映射的使用,如今咱們用一個例子來演示帶限制條件的鉸鏈關節.
假設如今咱們要對手腕進行數學建模,手腕在兩個自由度上有角度限制並在"直線"方向上不能扭曲. 咱們設直線方向上爲 z座標軸,任意兩個互相垂直且垂直於 z軸的座標軸爲 x,y 那麼咱們就有如下公式: \[v = \alpha\hat{x}+\beta\hat{y}\]
其中\(\hat{x}\) 代表 x 軸向量長度爲單位長度. \(\alpha \beta\) 各自爲 x,y 軸上的旋轉角度而且有最大最小值.
更進一步咱們能夠用一個不等式方程表示：\[(\frac{\alpha}{a})^2+\frac{\beta}{b})^2 \leq 1\] 上述方程在平面幾何中表示一個軸長爲 a , b 的實心橢圓. 能夠看出使用指數映射大大方便了各類帶限制空間鉸鏈的數學表達,讀者能夠嘗試一下使用歐拉角來建模手腕關節,你將發現歐拉角在三維上建模不但比四元數指數映射更復雜,且在超過 90度時必將發生萬向節死鎖問題. 須要指出的是歐拉角在一維,二維自由度狀況下表現仍是很良好的,若是你的需求只是在一維自由度 e.g. 膝關節 ,那麼你能夠考慮使用歐拉角來簡化問題. 最後這個手腕關節的求導數就做爲讀者的課後做業,提示一樣使用鏈式法則. 讀者甚至能夠更進一步本身組合一個機械臂來計算 end-effector 的位置和姿態,我就再也不這裏過多累述了. 本文在這裏收官, 願讀者在讀完本文後有所收穫.