飛行模擬-3.剛體六自由度方程、變換矩陣與四元素

--閱讀本文大概需要15分鐘

1.剛體六自由度方程

飛行模擬歸根結底是通過飛行力學分析建立數學模型模擬分析飛機的各項性能。這裡的數學建模依然採用質點假設，採用經典力學中的牛頓第二運動定律，即物體加速度的大小跟作用力成正比，跟物體的質量成反比，為F=ma的形式，力矩也同理。三維空間中描述剛體一共有六個自由度，即分別沿三個軸方向的平動和轉動。如果已知當前合力和合力距就可以解算出下一時刻的加速度和角加速度量。按照微積分的定義，位移的變化量（微分）即為速度，速度的變化量（微分）即為加速度，如此便可通過列出微分方程積分得到速度、位移、姿態角、操縱性能值等狀態量，最終將這些狀態量通過虛擬儀錶顯示出來即完成一次飛行模擬的解算。

飛行模擬有一套特有的符號表示狀態量，力採用F（Force）表示，位移用P（Position）表示，機體坐標系下三個軸方向的速度（Velocity）採用Vb表示，角加速度採用ωb表示，寫成行向量的形式如下圖。機體系下三個歐拉角即滾轉角為Φ，偏航角為Ψ，俯仰角θ，個人感覺這三個符號與中國的象形文字相似，專用於表示飛機姿態很容易識別。

推薦採用線性代數表示比較簡潔，實際使用中也常採用三維列向量的形式，這在計算機編程求解中很方便索引。

有了合力與合力距，根據牛頓第二定律就可以列出機體質心的六自由度運動方程。

平動方程：

角運動方程：

參考Dreier M E.Introduction to Helicopterand Tiltrotor Flight Simulation[M].AIAA(American Institute of Aeronautics & Ast, 2007.一書還可以寫成更緊湊的矩陣形式，此不列出。

關於六自由度方程求解，想說明的是一定要明確未知和已知，比如平動方程中，我們首先已知的是左側的合力，未知的是右側的速度導數，因此通過這個方程解算出來的實際是加速度。三個方程三個未知數的求解並沒有太大難度。

2.變換矩陣與四元素

設慣性坐標系下的歐拉角速度為：

機體坐標系下的角速度為：

慣性坐標系下的歐拉角速度與機體坐標系下的角速度還有如下圖的轉換關係：

為了從機體坐標系速度得到地球坐標系下的歐拉角速度，就必須求變換矩陣的逆矩陣。但是的逆矩陣不是其轉置矩陣，且其逆矩陣在俯仰角為±90°時為奇異矩陣，即：

針對這一情況，雖然許多實例和文獻認為直升機的歐拉角幾乎不可能到±90°，可以放心使用該方法，但實際只是迴避並沒有解決根本問題，比如應用到固定翼飛機或遊戲開發中就無可迴避了，當俯仰角達到90°就會出現除0現象，無法計算，計算機程序界稱之為『溢出』。這一問題的核心解決方法就是採用更普適的四元素法。四元素在飛行飛行模擬中得到了廣泛的應用，它的變換矩陣不存在奇異點。其思路是，先將上一時刻的歐拉角（前提不是上述奇異點）轉化為四元素形式的表示形式，進而將變換矩陣也可換成四元素的形式參與微分方程解算，待解算完成後再將四元素轉化為歐拉角。

四元素的起源

四元素的起源於尋找複數的三維對應物，複數可以表達一個二維矢量，當處理不共面的多個矢量時，需要用新的數來表達一個三維矢量。1843年哈密頓（Hamilton）發明了四元素，這是一種形如A=a0+a1i+a2j+a3k的數。歷史上矢量是從四元素中分離出來的，並由此發展為廣泛使用的矢量代數和矢量分析，但矢量的點乘和叉乘不是可結合的，也不是可除的。現在反過來把矢量代數和矢量分析的內容納入到四元素的體系中，這使得矢量仍可用四元素的方法來處理。同時四元素還具有處理旋轉的優勢，是一個封閉的數系。現已廣泛應用於力學、磁學、晶體學。（參考https://www.cnblogs.com/gary-guo/p/5097008.html）

將四元素用行向量來表示：

將歐拉角轉換為四元素的公式為：

利用該轉換式可以實現任意歐拉角與四元素的變換，一般用來初始化四元素。但實際，動力學運動方程解算出來的是機體角速度p、q、r，因此還需要其微分形式用於連續求解。而四元素角速度與歐拉角速度又存在如下關係：

其中c為誤差修正係數，根據Dreier M E.Introduction to Helicopterand Tiltrotor Flight Simulation[M]. AIAA(American Institute of Aeronautics & Ast, 2007.一書，c取32為最佳修正係數。

為了說明四元素到歐拉角的變換，有必要先說明常用的變換矩陣。在任何一本計算機圖形學書籍中都會有這方面介紹，因為比較常用，在此簡述。

常用的變換矩陣

(1)基元旋轉矩陣

基元旋轉矩陣是旋轉變換的基本矩陣，複雜多變的矩陣都可以由這些基元旋轉變換矩陣組合而來。所有旋轉變換矩陣都滿足右手定則。

繞X軸的旋轉變換矩陣為：

繞Y軸的旋轉變換矩陣設為：

繞Z軸的旋轉變換矩陣為：

觀察可以發現，基元旋轉矩陣比較簡單，主要有如下識記規律：

首先，主對角線上的1的位子決定了變換的軸；

其次，主對角線除了1之外都是餘弦項，正弦項對角線與餘弦項對角線交叉垂直，正弦項的正負按X-Y-Z有「下-上-下」的關係；

最後，其餘元素填充為0，這些矩陣皆為正交陣，其轉置矩陣就等於其逆矩陣，很方便使用。

(2)地面坐標繫到機體坐標系的變換矩陣Tbe（earth to body）

按照NASA的約定，變換矩陣的是有固定變換順序的，順序為先繞Z軸的偏航角，再繞Y軸的俯仰角，最後繞X軸的滾轉角。實際矩陣相乘的順序為左乘，即排在最右端的首先被執行，所以Tbe=Rx.Ry.Rz。為化繁為簡，以C代表正弦項，S代表餘弦項，Tbe得到的具體項如下圖，地面慣性坐標繫到機體坐標系的變換矩陣也為正交陣，其轉置矩陣就等於其逆矩陣。矩陣論中關於逆的實際工程意義正在於此，例如已知Tbe（大地坐標繫到機體坐標系的變換矩陣）,要計算Teb（機體坐標繫到大地坐標系的變換矩陣），則求Tbe的逆即可，而Tbe轉置矩陣就等於其逆矩陣，極其方便。

(3)四元素形式的地面坐標繫到機體坐標系的變換矩陣Tbe(earth to body)

為了充分利用四元素，在匯總計算中，從大地坐標系（機場相對坐標系）到機體坐標系的變換矩陣也可以直接寫成四元素的形式，與原形式是等價的：

採用四元素完成解算後，由於四元素形式的Tbe與直接採用歐拉角是等效的，按對應項等效列方程就可以實現四元素到歐拉角的變換，再次轉換成我們熟悉的歐拉角。

四元素轉換為歐拉角的公式為（其中sign為數學中使用的符號函數）：

註：本文採用了大量線性代數、矩陣論、計算機圖形學、飛行模擬等數學定義，如果想進一步了解更多的細節您可參考Dreier M E.Introduction to Helicopterand Tiltrotor Flight Simulation[M]. AIAA(American Institute of Aeronautics & Ast, 2007

或其它更專業的書籍了解基本概念。

喜歡這篇文章嗎？立刻分享出去讓更多人知道吧！