姿態解算進階之理解旋轉矩陣(也稱方向餘弦矩陣)

背景介紹：
Sugar 已寫完連續 5 篇的姿態解算入門系列，這個系列的特點是：特別簡單、起點特別低。
算法類推文比技能類推文難寫，Sugar 本來想姿態解算的算法系列在入門後就不再寫了，但有讀者下載了 Sugar 在 5 篇入門系列共享的 MATLAB 代碼之後表示對其中一些方法不理解，所以 Sugar 再開個進階系列。
注意：深入算法無終點，打算進階的讀者需要自行補一點數學基礎。

旋轉矩陣為何五花八門

關於旋轉矩陣的基本原理 Sugar 就不再重寫了，網上非常多。

有初學的讀者問 Sugar 這樣一個問題：剛學，基礎不太好，很勤奮的去網上找講基礎知識的文章，發現同類內容不同的文章講得不一樣，卻又都是看起來很對的樣子，究竟學哪個才是對的？
Sugar 回答是：寫出來的文章在其文章本身環境下基本不會出錯。能把握住核心的讀者會理解 Sugar 的回答，但對於初學者並不知道什麼是「核心」。下面 Sugar 就來舉個例子。
對於繞 Z 軸的旋轉，網上的文章裡有兩種表述：

兩種表述符號不同無傷大雅，但「負號不在同一位置」就讓很多初學者撓頭了（頭髮揪淨了都不知道哪個對，Sugar 稱此類問題為「揪淨問題」）。
上圖中都表示「繞 Z 軸旋轉的偏航角」，只是轉向不同。在 Sugar 之前的推文中一直是以「標準的笛卡爾右手系規則」來定義轉角正方向的，上面兩個截圖中的「第二種表述」是符合笛卡爾右手系規則的，而「第一種表述」與笛卡爾右手系規則正好相反。
對單軸的旋轉矩陣最終會影響到最終的姿態旋轉矩陣，這裡 Sugar 給出下面的 MATLAB 代碼，讀者要自己動手試一下這個影響究竟是什麼樣的：

clearclc% cr:cos(roll)  sr:sin(roll)% cp:cos(pitch) sp:sin(pitch)% cy:cos(yaw)   sy:sin(yaw)syms cr sr cp sp cy sy% RX:Rotate X axis% RY:Rotate Y axis% RZ:Rotate Z axissyms RX RY RZ
RX = [  1    0    0;        0    cr -sr;        0    sr  cr]RY = [  cp   0   sp;        0    1    0;       -sp   0   cp]RZ = [  cy  -sy   0;        sy   cy   0;        0    0    1]%--from_euler_xyz--% column : x y z% row    : a b c%         x                 y                 z% a [ cp*cy, cy*sp*sr - cr*sy, sr*sy + cr*cy*sp]% b [ cp*sy, cr*cy + sp*sr*sy, cr*sp*sy - cy*sr]% c [   -sp,            cp*sr,            cp*cr]from_euler_xyz = RZ*RY*RX
至此就不難理解 Sugar 說的「在其文章本身環境下」是什麼意思了，直白說就是：每個文章對旋轉正方向的定義不同，所以公式存在差異。Sugar 推薦一律採用笛卡爾右手系，這樣不容易出錯。有些文章是 X-Y 符合笛卡爾右手系規則，而 Z 與笛卡右手系規則相反，那麼之後所有的旋轉都要把 Z 單獨轉向才行，這樣不利於算法總體的統一。
跟 Sugar 一起看 ArduPilot 裡的旋轉矩陣
ArduPilot 是符合「標準笛卡爾右手系規則」的。下面 Sugar 對 ArduPilot 代碼結合 MATLAB 做一個圖形化解讀。
先看一下 Sugar 從 github 上截的 ArduPilot 旋轉矩陣代碼的圖：
不難看出 Sugar 上面 MATLAB 代碼裡的注釋就是對這段代碼寫的。這就是「先繞 X 軸旋轉、再繞 Y 軸旋轉最後繞 Z 軸旋轉」的旋轉矩陣（網上也有叫「方向餘弦矩陣」的）。下面 Sugar 解釋一下旋轉矩陣的用法和注意點。
一、列向量左乘旋轉矩陣
在笛卡爾三維直角坐標下，一個有方向的線段用三維向量表示。為了能夠使用上面的旋轉矩陣對向量進行旋轉，這個表示有向線段的三維向量要以「列向量」形式存在。所以：
對一個三維列向量施加一個三維旋轉的方法就是：將這個三維列向量左乘旋轉矩陣。
下面看下 Sugar 用 MATLAB 做的旋轉效果：
二、不同的旋轉順序得到不同的結果
Sugar 習慣使用的旋轉順序是：X-Y-Z 順序。就是上面說到的：先繞 X 軸旋轉、再繞 Y 軸旋轉最後繞 Z 軸旋轉。下面 Sugar 以「橫滾 30 度、俯仰 30 度、偏航 90 度」為例，看看「X-Y-Z 順序」和「Z-Y-X 順序」有多大差別。
事實說明：同樣的旋轉角度、不同的旋轉順序，效果可以是天差地別。
三、gimbal lock 萬向鎖
關於萬向鎖網上能找到的解釋很多，Sugar 在這裡不再贅述。下面 Sugar 通過分步 Z-Y-X 順序旋轉為例，演示一下 gimbal lock 現象在姿態解算上引起的烏龍事件（也說明一下上面 Z-Y-X 順序的圖為什麼轉成了那個樣子）。
按上面的角度，首先繞 Z 軸轉 90 度，如下：
上圖就是下面烏龍事件的起因，下面我們該繞 Y 軸轉出 30 度的俯仰角了，結果卻：
因為第一步繞 Z 轉遇到了 gimbal lock，所以導致第二步繞 Y 轉俯仰角的效果變成了轉出橫滾的效果。
這裡要注意的是：第二步繞 Y 軸轉指的是「第一步旋轉前的 Y 軸」（是上圖中與紅色虛線重合的綠色實線，而不是那個綠色虛線）。
下面還剩一步，繞紅色實線 X 軸轉出 30 度的橫滾角。當然，因為第一步的 gimbal lock 導致變成了轉俯仰角的效果，如下:
完整的無人機動力系統測試解決方案