熟悉無人機的朋友一定常常聽到姿態，這兩個字，無人機工程師一定覺得這實在太熟悉了，方位余弦，四元數，可能有些朋友也用過指數表示方法。Ok，這些內容確實不難，本文也會直接給到大家相關公式，可以拿來在程序中直接使用。但MR.城堡不知道各位有沒有過濾過這些問題：無人機為什么需要姿態描述？旋轉為什么有三個呢？為什么耦合不直接用自由度表示出來呢？為什么要12個狀態（說15個狀態的朋友，線性加速度也放了進去，后文將告訴你這是不必要的），未來有沒有可能需要更多狀態？這些姿態表示方法有什么區別？有沒有其它姿態表示方法呢？什么叫做萬向節鎖死呢？為什么鎖死就控制不了？等等
這些問題有些工程師可以憑直覺得到答案，有些比如未來是否需要更多狀態？為什么選擇某一種姿態描述方式？萬向節鎖死的本質解釋，以及不能控制的原因？等等，可能就會比較吃力，然而這些卻很重要，當我們把這些理論與無人機系統實物聯系起來時，不但可以用更低的成本，做出更好的產品，更能豐富我們對無人機的理解，形成對技術和趨勢的預判。
通過前面的專欄文章：城堡里學無人機：無人機導航之玩轉坐標系，城堡里學無人機：如何處理GPS數據獲取無人機狀態信息？，我們知道無人機如何獲取位置和速度狀態，並轉換到NED坐標平面內。但這並不足以讓我們全面了解無人機的運動情況，換句話說，相同的位置中，無人機可能有不同的運動姿態。舉例來說，一架多旋翼無人機在不同時間經過位置![P=[X,Y,Z]](/image/aHR0cDovL3d3dy56aGlodS5jb20vZXF1YXRpb24_dGV4PVAlM0QlNUJYJTJDWSUyQ1olNUQ=.png)
時，前一次為水平飛行，后一次為垂直飛行。雖然位置坐標相同，但運動方式卻如此不同。這時我們需要一些額外的信息才能夠精確描述無人機的飛行狀態，這也就是本文所涉及的內容：無人機姿態。
上面是用一個簡單的例子說明一下，但不具有普遍性，下面從理論上淺析一下為什么外環信息出來了還要內環姿態。
多旋翼無人機的姿態可以通過機體坐標系和NED坐標系之間的關系來進行定義。在多旋翼機型介紹中，大家已經了解了多旋翼無人機的姿態控制方式。很容易看出多旋翼無人機具備六個自由度（Degree of Freedom, DOF），沿機體坐標系三軸的線性運動以及繞機體坐標系三軸進行的旋轉運動。
然而很容易看出來的內容，想搞清楚why卻往往並不容易。
關於自由度，在結構力學或者分析力學中，它的定義是：確定一個質點的位置所需要的獨立坐標的個數。物理有的時候比較抽象，大家可以直接通過線性代數里的線性空間來理解，線性空間中我們說到一個子空間，它的維度（Dim）就是張成（Span）這個空間的一組基（Bases）中相互獨立向量的個數，我們可以在這里將dimension和DOF等同起來。因此，力學中的“獨立坐標”，可以理解為“相互獨立的基向量”。有興趣的朋友可以結合流形，微分同胚來理解（當然，對於工程師而言，還是可以直接轉換到矩陣變換的范疇內）
在無人機中有6個自由度，意味着我們需要六組基坐標來描述它的完整位置，這六組基就是無人機所在的
空間中的三個坐標軸，以及圍繞三個坐標軸的姿態角（請注意，這里不是說在中的一組基能有6個相互獨立向量，因為研究對象已經不再是質點，而是剛體，剛體旋轉位置描述也不能只用）。從位置到運動，就是導數了，這時6*2=12個狀態。我們並不需要更多的向量來描述無人機位置，因此也不需要更多狀態來描述無人機的運動，雖然有時候為了方便實現某些功能，或者擴展我們對於“運動”的理解，我們想要控制它的受力狀態，我們會增加一些控制行為，比如針對加速度的控制，但這些變量不是描述無人機運動所必須的。
姿態的英文詞匯有兩個“attitude”或者“orientation”后者的意義更為明顯，可以理解為“取向”“指向”，常用於衛星姿態描述中。
有多種方法對無人機旋轉運動進行描述，最常見的是方向余弦和四元數及指數表現形式。但無論是哪一種，我們都可以理解為是按照某種表現方式提供一組基向量，來描述無人機旋轉位置的三個自由度。但是它們卻因為算法的不同，具有不同性質，這些性質將決定我們對它們的選擇。
1)歐拉角
無人機圍繞機體系三軸的運動可以分解為：繞
軸的橫滾運動，繞
軸的俯仰運動以及繞
軸的偏航運動。歐拉角直觀地將無人機旋轉姿態轉化為機體坐標系和NED坐標系之間的夾角關系。
一說到角度，一定是同一個平面內線和線的關系或者相交平面之間的關系。在無人機坐標系中需要大家建立起六個面的抽象圖形，機體坐標系下三個平面：
以及NED坐標系下三平面：
當我們確立了這六個平面之后，就可以描述無人機姿態了。我們定義橫滾角,俯仰角,偏航角來分別描述無人機的橫滾運動，俯仰運動，偏航運動。
2)方向余弦
實現從NED坐標系到機體坐標系的數據變換有什么意義呢？無人機的旋轉運動相對於無人機自身是沒有任何意義的，因為它相對於自己永遠是沒有運動的，只有相對於地面（參考坐標系）才有意義。因此我們要用NED和機體坐標系間的關系來表述無人機旋轉運動，無疑，矩陣就是描述旋轉最好的工具了。
我們可以把無人機任意一次旋轉按次序拆分為三個部分：繞軸的偏航運動、繞軸的俯仰運動、繞軸的橫滾運動。每一次運動用一個旋轉矩陣來描述就成為了下面的形式：
大家一定注意到這三個矩陣都是斜對稱陣，喜歡矩陣的朋友可以自行思考一下，肯定可以挖掘出很多有趣的內容。容易得到從NED到機體坐標系的坐標變換矩陣（方向余弦矩陣）：
從機體坐標系到NED坐標系的方向余弦矩陣很容得到：
如此一來，我們就很容易實現兩個坐標系下信息的相互轉換。
3)不唯一與奇異性
在任何系統中，控制工程師首先關注的是系統方程解的唯一性和存在性。
歐拉角非常直觀，但也存在兩個問題：1.不唯一2.奇異性。這兩者分別對應了解的唯一性和存在性。這兩個問題從直觀上比較難以描述，但從數學上借助方向余弦可以很清楚的解釋。
不唯一性：舉一個例子，令
這時：
很容易看出，這種不唯一性是由三角函數的周期性引起的，不唯一性可能會出現系統運動不按照我們期望的方式進行。
奇異性（局部）：如果我們令：
這會帶來什么問題呢？在后面介紹無人機數學模型時，三個歐拉角與無人機本身的旋轉角速度是通過一個矩陣進行關聯的：
此時，零出現在分母上，這時控制器就沒有辦法處理被控對象的數學模型了，也就是我們所說的解不存在的情況，也將因此無法實現有效控制。這就是常說的“萬向節鎖死”。數學描述是不是非常直觀明了呢？
上面的情況導致歐拉角的姿態描述方式在某些姿態情況下不可使用，這對於無人機控制而言是會帶來一定風險的。因此我們需要在另外一種描述方法可以對無人機所有的姿態進行描述。
4)四元數
四元數是一種全局的姿態描述方法，換句話說，當使用四元數描述姿態的時候，縱使
再次出現，描述方法依然有解。我們 直接驗證一下就可以了，首先看一看如何從歐拉角獲取四元數：
然后再對四元數進行歸一化處理：
q1,q2,q3的歸一化處理與q0相同。
請大家注意，此時，縱然出現90°的姿態角，由於沒有出現分母為零的情況，因此依然有解。這只是很直觀的驗證，結合指數姿態表示方法：
而四元數的不唯一性可以通過下式看出：
因此，帶入
時，R矩陣式一樣的。
很容易獲得從四元數到歐拉角的轉換公式：
5)程序實現
從歐拉角到四元數的程序實現非常直接，並不需要想一般算法那樣進行離散化，因為這里的轉換公式並不對應時域函數，而是直接對應變量值，所以只要我們通過傳感器獲得可靠的姿態數據值后就可以直接帶入公式求得對應姿態：
quat[0] = chphi0 * chtheta0 * chpsi0 + shphi0 * shtheta0 * shpsi0;
quat[1] = -chphi0 * shtheta0 * shpsi0 + shphi0 * chtheta0 * chpsi0;
quat[2] = chphi0 * shtheta0 * chpsi0 + shphi0 * chtheta0 * shpsi0;
quat[3] = chphi0 * chtheta0 * shpsi0 - shphi0 * shtheta0 * chpsi0;
如是而已。
6）指數表現形式
指數表現形式在實際使用中比較少，但是對於理解另外兩種旋轉表現方式很有助益。它是既不是全局的，也具有奇異性。有興趣的朋友可自行了解，推薦《A Mathematical Introduction to Robotic Manipulation》這本書。
相信大家通過本文應該可以直接獲取無人機姿態數據的轉換公式，並根據不同旋轉運動描述方法的特點結合自己的需求進行數據轉換（基坐標變換？不知這樣的詞匯合適么）。
有時我們面對的研究對象是可以碰觸的物理實體，比如無人機，這當然會給我們帶來很多好處，但過分陷入到對直觀感受的依賴中，很有可能將自己引入主觀的誤區，就像一場狩獵游戲，好狐狸總是想方設法用獵人自己的思維來迷惑他。而數學卻能簡潔，工整，清晰的將我們研究對象的本質揭露出來，很多情況下這是我們憑自己的直觀感受難以挖掘的，這也正如好的偵探，最后得出的結論也許很難在感情上接受它，卻無法否認它是更加接近真相的推斷。在無人機研究中暢快的運用數學就猶如身臨一場狩獵或者探案冒險，抽絲剝繭，能得到一點點結論都會讓自己心情久久暢快，有時會讓自己驚訝的一天都沉迷其中，這種快樂也可以算作研究無人機的副產品吧：）。
轉載自：無人機中的城堡