頭條 | 一文詳解四元數、歐拉角、旋轉矩陣、軸角如何相互轉換

2021-01-14 計算機視覺之路


D3D和OpenGL不同，用的坐標系是Y軸豎直向上的左手系，所以歐拉角的順規是跟廣大blog、OpenGL不一樣的，那麼博客上、甚至維基百科[2]上的各種基於右手系xyz順規(分別對應roll, pitch,yaw)的看起來就不太能隨隨便便直接用了。

首先歐拉角旋轉序列(Euler Angle Rotational Sequence)一共有12種順規，6種繞三條軸的旋轉(也叫Tait-Bryan Angle，XYZ,XZY,YXZ,YZX,ZXY,ZYX)，另外6種只繞兩條軸的旋轉(也叫Proper Euler Angle，XYX,YXY,XZX,ZXZ,YZY,ZYZ)。如果相鄰兩次旋轉是繞同一條軸，例如XXY，那麼其實可以坍縮成XY。那麼只繞一條軸旋轉就根本不夠自由度就不需要說了。也就是說，一共有12種基礎旋轉的組合順序，它們可以旋轉出三維的所有旋轉狀態。所以一共是12種旋轉順規（可以表示所有旋轉的集合），DirectXMath庫採用的是ZXY順規，分別對應著Z-Roll，X-Pitch，Y-Yaw。


圖：歐拉旋轉與Yaw-Pitch-Roll的直觀意義（網圖魔改）


注意：那麼下文我們都採用ZXY順規來推導公式！採用列主向量(column major)！(但是注意DirectXMath API生成的矩陣其實是行主向量(row major)的)

參考一下維基百科的[3]Euler Angle、[4]Rotation Matrix

可以知道，歐拉角構造旋轉矩陣就直接把三個Elemental Rotation Matrix乘在一起就好了（LaTeX扣得真累orz）：上面的歐拉角--->矩陣的結果與維基百科Euler Angles[3]  給出的結果一致，那應該穩了：


圖：其實可以不用自己推的，維基百科把12種順規乘出來的矩陣都寫出來了


參考一篇NASA的關於姿態描述的技術報告[1]的Appendix-A6和[5]，我們可以用旋轉矩陣元素的相乘、相除、反三角函數等操作去「湊」出歐拉角。[5]給出了從XYZ順規提取歐拉角的方法、步驟、思路，[1]則給出了全部12種順規的歐拉角提取公式，但是沒有給一些細節注意事項。所以總結一下，根據[1]、[5]、[7]《Real Time Rendering 3rd Edition》4.2.2和自己的推導，從ZXY順規旋轉矩陣提取歐拉角的公式是（[1]原文下標似乎有點小問題）：注意到一點，注意到矩陣的每一個元素都是pitch angle  的函數…所以當  即  的時候，這時候其他的歐拉角提取表達式就涼涼了（分子分母都是0, arctan和atan2都沒有意義了）….其實pitch angle  恰好就是Gimbal Lock的位置。在Gimbal Lock的時候，旋轉矩陣會退化為：那麼要進一步處理萬向節死鎖的corner case就需要分兩種情況： ，此時 其中要給或者 其中一個歐拉角賦值，另外一個就按等式計算出來。 ，此時 同樣的，要給或者 其中一個歐拉角賦值，另外一個就按等式計算出來。從旋轉矩陣提取歐拉角的公式跟歐拉角順規的選取有關，因為旋轉矩陣的元素會略有不同，但是思路都是一樣的，就是根據旋轉矩陣的解析表達式+反三角函數湊出來。


眾所周知的是，歐拉旋轉是有萬向節死鎖(Gimbal Lock)的問題的。幸好我們有四元數(Quaternion)這種數學工具可以避免這個情況。一般來說，我們都會用單位四元數  來表示旋轉，其中  。那麼給定一個單位四元數，可以構造旋轉矩陣(column major)[1][4][8][14][15]：這個四元數構造的大概思路就是把四元數的旋轉操作寫成矩陣形式（註：給定一個用於旋轉的單位四元數  和被旋轉的三維向量  ，那麼要直接用四元數旋轉這個向量，則我們首先要構造一個純四元數  ，設旋轉後的向量為  ，旋轉後的向量構造的純四元數為  ，那麼  ）。因為是用四元數來構造矩陣的，所以這個矩陣構造公式就沒有歐拉角順規的說法了。


那第一步肯定是判斷3x3矩陣是一個正交矩陣啦（滿足  ）。那麼如果這個矩陣已經是一個合法的旋轉矩陣了，要從旋轉矩陣裡提取四元數，也是可以像提取歐拉角那樣，用參數化過的矩陣的表達式湊出來。參考[8]《Real Time Rendering 3rd edition》Chapter4的思路，我們觀察一下用四元數分量進行參數化的矩陣 ，然後經過一頓操作，我們發現：於是我們再湊出個實分量 ，就可以把四元數四個分量都用矩陣元素表示出來了。於是我們又機智地發現了一個等式：其中  是矩陣  的跡(trace)，也就是矩陣對角元素的和。因為這裡用的是3x3矩陣，跟其他資料裡面的表示有一點不同。所以我們可以把四元數的四個分量都用矩陣元素湊出來了：有一點《Real Time Rendering》提到的，  絕對值比較小的時候，可能會出現數值不穩定的情況，那麼想要數值穩定的話就得用一種不用除法的方式來湊，在這不展開了，可以看一下RTR。


參考維基百科[2]的思路，歐拉角構造四元數，跟歐拉角構造旋轉矩陣一樣，就是把三個基礎旋轉Elemental Rotation組合在一起。


那麼用於旋轉的四元數  的表達式是：這個我自己推導的結果跟[1]NASA Technical Report的Appendix A給出的結果對比過了，（[1]中四元數記號是  ），看起來沒什麼問題。


本來我以為，從四元數提取歐拉角的思路可以跟旋轉矩陣提取歐拉角類似，也是用四元數的元素運算和反三角函數湊出公式來。後來我發現這簡直就是一個極其硬核的任務，展開之後每一項都是六次多項式，畫面有一丟暴力且少兒不宜，直接強行湊的話畫風大概是這樣：這個結果跟歐拉角參數化的旋轉矩陣的  的表達式是吻合的。但這還只是最好湊的那一個，惹不起惹不起。所以舒服的思路還是四元數-->旋轉矩陣-->歐拉角，想一步到位的話，把四元數分量參數化的旋轉矩陣、歐拉角參數化的旋轉矩陣結合在一起，參考下旋轉矩陣轉歐拉角的方法，替換下元素就完事了。這裡就不把公式展開了，因為四元數直接轉歐拉角 跟 旋轉矩陣轉歐拉角一樣，依舊是要處理gimbal lock的corner case，還是那麼麻煩，所以這裡先鴿了


軸-角(Axis-Angle)顧名思義就是繞某條單位軸旋轉一定角度，從這個意義上看，它構造四元數是非常舒服的，畢竟直觀的幾何意義有一點點類似，繞單位軸  旋轉  的四元數是：


Axis Angle轉Rotation Matrix可以從[9]羅德裡格斯旋轉公式Rodrigues Rotation Formula開始推導。設  是我們要旋轉的單位向量，旋轉軸為  ，  繞  旋轉角度  ，那麼旋轉後的向量為：這個公式的推導思路是這樣子的，我們先對向量  進行正交分解，分解成投影到旋轉軸  的分量和垂直於  的分量：於是繞  旋轉向量  其實就是把上面正交投影后的向量分別旋轉之後再加起來。那麼很明顯的，投影到旋轉軸上的部分  都跟旋轉軸共享了，那麼自然旋轉之後的結果就沒有變化了，於是我們只需要旋轉和旋轉軸垂直的部分  。那麼這個旋轉後的表達式就是：然後我們不按wikipedia裡面坑爹的、不考慮下文的變形，自己推一波：這裡我們把旋轉後向量的表達式變形得只剩下叉積(cross product)，去掉點積(dot product)了，這樣子我們才可以把這個繞軸旋轉的表達式寫成矩陣形式。怎麼寫呢？首先叉積可以寫成矩陣形式：



[1]Henderson, D.M.. Euler angles, quaternions, and transformation matrices for space shuttle analysis[C]//NASA, Jun 09, 1977.[2] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles%23Euler_Angles_to_Quaternion_Conversion[3] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Euler_angles[4] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Rotation_matrix[5] Slabaugh G G. Computing Euler angles from a rotation matrix[J]. 1999.[6] Mike Day, Converting a Rotation Matrix to a Quaternion. https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf[7] Tomas K.M. , Eric H., Naty H.. Real Time Rendering 3rd Edition , p68-p69, 2008.[8] Tomas K.M. , Eric H., Naty H.. Real Time Rendering 3rd Edition , p76-p77, 2008.[9] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Rodrigues%2527_rotation_formula[10] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Cross_product%23Conversion_to_matrix_multiplication[11] https://link.zhihu.com/?target=http%3A//mathworld.wolfram.com/RodriguesRotationFormula.html[12] https://link.zhihu.com/?target=https%3A//zh.wikipedia.org/wiki/%25E5%259B%259B%25E5%2585%2583%25E6%2595%25B8[13] https://link.zhihu.com/?target=https%3A//blog.csdn.net/silangquan/article/details/39008903[14] Quaternion and Rotations, https://link.zhihu.com/?target=http%3A//run.usc.edu/cs520-s12/quaternions/quaternions-cs520.pdf[15] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

本文轉自知乎：雞哥

原文網址：https://zhuanlan.zhihu.com/p/45404840

未經同意請勿二次轉載

- END -

關注本公眾號，獲得更多的「自動駕駛」算法原創乾貨，幫你快速入行關注本公眾號，點擊「入群」，加入深度學習技術群，熱心大佬幫你答疑解惑


相關焦點

  • 一文詳解四元數、歐拉角、旋轉矩陣、軸角如何相互轉換
    那麼給定一個單位四元數，可以構造旋轉矩陣(column major)[1][4][8][14][15]：這個四元數構造的大概思路就是把四元數的旋轉操作寫成矩陣形式（註：給定一個用於旋轉的單位四元數  和被旋轉的三維向量  ，那麼要直接用四元數旋轉這個向量，則我們首先要構造一個純四元數  ，設旋轉後的向量為  ，旋轉後的向量構造的純四元數為  ，那麼  ）。
  • 三維旋轉:歐拉角、四元數、旋轉矩陣、軸角之間的轉換
    然後又發現，網上大部分資料的採用的歐拉角順規都是xyz，然後我基於D3D11的辣雞框架用了zxy，公式不太能直接套用，於是摸了兩三天魚，整理了一下幾種三維旋轉表示（歐拉角，四元數，旋轉矩陣，軸角）與他們之間的相互轉換的資料，並且加入了自己的一些推導，給出這些轉換公式的推導思路和細節，這樣子如果各位想使用其他歐拉角順規和定義的時候，自己動手算一算就好了。
  • 四元數與3D旋轉實例! Cocos Creator 3D Quternion !
    用幾個實用的例子帶你理解四元數!文末獲取完整項目！前言本文不會講太多四元數公式的推導過程，重點講講幾個接口的使用和個人理解。(矩陣基本沒有，歐拉角可以做插值，但可能遭遇萬向鎖的問題)當然四元數也有一些缺點：四元數可能不合法。（一般通過四元數標準化解決這個問題，確保四元數為單位四元數）對給定的方位的表達方式有兩種方法，它們相互為負。
  • (加餐)歐拉角及矩陣旋轉
    (2 本文適合有一定雷達及信號與系統知識基礎的讀者)鄉親們大家好，今天的這期內容就像有錢人的生活，比較枯燥，不過能完整讀下來也確能有所收穫，本文是分析微都卜勒的數學基礎，或者說研究微都卜勒的必備數學工具，為後續聊微都卜勒做鋪墊，也是就是（如何做好一款
  • 無人機理論(三):簡單搞定四元數
    ，同一種旋轉有無數種歐拉角表示形式，從而導致了歐拉角差值時出現問題，因為當你俯仰角接近90°時，兩組千差萬別的歐拉角表示可以是同一種旋轉。回到四元數上來，對於導航飛控的算法，我們需要對四元數有什麼了解，其實很簡單，我們要知道它的基本運算規律，要知道它以什麼樣的方式表徵姿態，要知道它跟其他兩個表徵姿態的歐拉角和旋轉矩陣方式如何互相轉換。除此之外，還有後續如何使用四元數進行建模和控制律設計，如何在導航算法中得到四元數的狀態，這些在以後的篇幅中會進行解釋，今天，我們就來看看上面幾個問題是怎麼解決的。
  • 姿態表示方法之四元數
    之前講了姿態表示方法之歐拉角和姿態表示方法之方向餘弦矩陣現在講一下對於四元數的簡單通俗理解
  • 剛體運動變換的描述——四元數
    一、（Eigen）四元數類型常用的四元數格式有Quaternionf(float)和Quaterniond(double)，模板類中的Scalar決定數據類型。四元數轉換Eigen::Quaterniond q7(1.0, 0.0, 0.0, 0.0);    q7.normalized();    // *注意，必須得是單位四元數才能表示旋轉矩陣    Eigen::Matrix3d R = q7.toRotationMatrix(); //由羅德裡格公式進行轉換    //Eigen::Matrix3d R = q7.
  • 組合導航科普(4):什麼是歐拉角?
    常見的描述姿態的方法有：歐拉（Euler）角、坐標轉換矩陣（coordinate transformation matrix）、四元數（quaternion）和旋轉矢量（rotation vector）等。其中，歐拉角將姿態描述分解為三個連續的轉動過程，每次旋轉所圍繞的軸與前後旋轉所圍繞的軸正交。
  • MEMS陀螺儀需組合其他傳感器使用 歐拉角和四元數表述飛行姿態
    MEMS陀螺儀需組合其他傳感器使用 歐拉角和四元數表述飛行姿態 　　至於MEMS陀螺儀是如何與其他傳感器配合使用實現平衡控制和輔助導航，金良詳細的向記者解釋道：「比如說，三軸加速度和三軸陀螺儀(角速度) 並發傳送給單片機或DSP處理，然後由單片機或DSP進行姿態解算(如四元數)，求解出當前飛機的pitch、roll及yaw三個角度值，再根據這三個角度經過PID控制運算，輸出四路PWM控制四個電機(對四軸無人機而言)的加減速從而達到無人機的平衡懸停狀態
  • 坐標系與相機旋轉表示
    ，可以看出其是行列式為1的正交矩陣(滿足AAT=I的矩陣，其行列式detA=±1)2.2旋轉向量與歐拉角歐拉角兩個參考系間的旋轉關係可以用一系列繞三個主軸的旋轉順序來表示2.3四元數四元數是一個4×1的列向量記為：其中ε是3×1的列向量，η是標量，我們定義四元數的左手形式的複合算子+和右手形式的複合算子⊕（這兩個運算可以看做四元數之間的左右乘法），滿足：四元數的逆定義為：
  • 「飛控」姿態誤差(一)-歐拉角做差
    我有幾個問題代表大家問一下，首先姿態本質是旋轉，姿態的誤差其實是旋轉之間的誤差，那必須還是個旋轉，那麼直接相減還能表示旋轉嗎？也就是說相減得到的角度轉換成旋轉矩陣還是個正交矩陣嗎？而且歐拉角是有隱藏條件的，就是旋轉順序，同樣是三個角 (10,20.30) ，x-y-z，y-x-z順序可是兩個不同的旋轉，如果我不告訴你它的旋轉順序，那其實它就是沒用的三個數，因為它根本就沒法還原成旋轉。但是我就沒見過有對控制器做先後順序處理的，roll,pitch,yaw的控制器都是同時啟動，沒有順序之分。所以，憑什麼姿態誤差可以用歐拉角相減！！！
  • 多體動力學裡的歐拉角解析
    歐拉角是在空間中描述從一個用於表示某個固定的參考系的、已知的方向，經過一系列基本旋轉得到、新的代表另一個參考系的方向的方式。歐拉角是描述各個坐標系間的轉換關係。下面三張動圖形象的表示了歐拉角的旋轉方式。第一張是繞x軸旋轉pitch，第二張繞y軸旋轉yaw，第三張是繞z軸旋轉roll。
  • GD32F103+MPU9150四旋翼飛行器第一步:姿態融合算法
    四旋翼飛行器的軟體核心包括兩大部分：姿態融合算法和控制算法;硬體核心便是MCU和傳感器。　　先介紹下姿態融合算法，姿態融合說白了就是將3軸加速度、3軸角速度和3軸磁場強度融合成四元數，再將四元數轉化為歐拉角，最後將歐拉角最為控制量輸送到所有電機以達控制飛行器姿態的目的。歐拉角包括偏航角Yaw、俯仰角Pitch和滾動角Roll。
  • [飛控]剛體運動學-歐拉角模型
    運動學模型，對於多旋翼來說主要關心的就是機體角速度與姿態變化率的關係。為什麼呢？因為，我們傳感器只能測量機體角速度，但是我們控制需要知道姿態變化率。如果有了機體角速度與姿態變化率的關係，我們就可以通過傳感器數據得到姿態。
  • 如何將傳感器坐標系下的數據轉換到自車坐標系?
    如何將傳感器坐標系下的數據轉換到自車坐標系？ 就意味著，只要把角雷達採集到的障礙物坐標值與上面這個矩陣進行矩陣乘法運算，即可完成沿Z軸的旋轉。在這裡我們把這個矩陣叫做Z軸旋轉矩陣RZ，那必然還有沿著X軸和Y軸的旋轉矩陣RX和RY。 角雷達目標的坐標依次右乘這三個矩陣，就完成了沿著Z軸，Y軸，X軸的旋轉，得到的結果就是自車坐標系下的坐標值了。
  • VEX-----quaternion 四元數
    對quaternion最大的誤解在於認為w表示旋轉角度，V表示旋轉軸。正確的理解應該是w與旋轉角度有關，v與旋轉軸有關。例如，要表示以向量N為軸，軸旋α度,相對的quaternion應該是：q =[ cos(α/ 2) , ( sina(α/ 2) Nx, sin(α/ 2)Ny, sin(α/ 2)Nz ) ]為了計算方便，一般要求N為單位矢量。
  • 四元數簡介
    旋轉，應該是三種坐標變換——縮放、旋轉和平移，中最複雜的一種了。大家應該都聽過，有一種旋轉的表示方法叫四元數。按照我們的習慣，我們更加熟悉的是另外兩種旋轉的表示方法——矩陣旋轉和歐拉旋轉。矩陣旋轉使用了一個4*4大小的矩陣來表示繞任意軸旋轉的變換矩陣，而歐拉選擇則是按照一定的坐標軸順序（例如先x、再y、最後z）、每個軸旋轉一定角度來變換坐標或向量，它實際上是一系列坐標軸旋轉的組合。那麼，四元數又是什麼呢？簡單來說，四元數本質上是一種高階複數（聽不懂了吧。。。），是一個四維空間，相對於複數的二維空間。