姿態解算是飛控的一個基礎、重要部分，估計出來的姿態會發布給姿態控制器，控制飛行平穩，是飛行穩定的最重要保障。有關姿態解算的基礎知識，這里筆者不會細細描述，有關這方面的資料，網上已經有很多。主要是先掌握坐標系的概念、姿態角的幾種描述方法（歐拉角、四元數、旋轉矩陣）等。

姿態解算的難點主要在于，消費級飛控上一般所選用的慣性傳感器，都是mems器件，精度相對較差，同時陀螺儀、加速度計、地磁計單個傳感器無法得到滿意的姿態角信息，所以需要一些融合算法，進行姿態估計。

首先，如何測量姿態解算的精度？

很多人都會有這個困擾，就是沒辦法確定所得到的角度的精確程度。一般用外部參考的方法測量，比如飛機上同時掛載自己的飛控以及高精度的IMU設備（比如xsens、sbg等），飛行完成后，對比自己飛控所解算的角度和外部設備的誤差；又或者，在室內裝vicon設備來給出外部參考。

筆者手上沒有精確的外部設備，所以本文不能得出結論哪種算法的好壞，只是提供大概思路。

采用開源PX4飛控，進行姿態模式飛行，取出飛行數據，注意，驗證算法的時候，最好還是用實際飛行的數據，否則加速度噪聲對算法的影響無法驗證，同時，這里的角度參考僅是px4飛控所解算得到的角度，不能算高精度參考，數據曲線：

1.看一下單獨的陀螺儀積分角度與加速度計算出的角度各有什么問題？

陀螺儀角度：就是對角速度數據進行積分；

加速度角度：

在matlab中編寫m代碼進行驗證對比。

如上圖所示，藍色的陀螺積分角度隨著時間會有漂移，而加速度計得到的角度則噪聲很大，都無法使用。

2.互補濾波

由于單一傳感器獲得角度都有缺點，一個動態精度好，一個靜態精度好，很自然聯想到互補濾波，即通過調整一個加權系數，如果陀螺儀所占的權重大些，則解算的角度則跟陀螺角度近似，加速度類似。

filtered(i) = (filtered(i-1) +IMU_GyroX(i)*IMUPer)*a + (1-a)*angle_acc(i);

a為0.95的曲線如下：

a為0.3，曲線如下：

綜上所看，固定增益a，無論如何調整，性能總是不盡理想，陀螺儀的權重大了，動態性能還可以，大致能跟上角度，但是不能靜態保持，當沒有角速度時，角度也收斂了；加速度權重大了，噪音大，另外動態性能差，100s左右的來回打桿，所得的角度基本沒有變化。

3.自適應互補濾波

如果權重參數可變呢？也就是如果角度變化大時，更相信陀螺，否則則更相信加速度的角度。

如圖所示，雖然動態精度看上去好了，但是當靜態的時候，還是不能達到滿意效果。另外，圖中可以看出，加速度的角度所保證的靜態精度是有延遲的。

4.mahony&PX4SO3算法

PX4飛控早期的姿態解算有一個SO3版本，其算法其實就是網上常見的mahony算法。算法原理：根據加速度計和地磁計的數據，轉換到地理坐標系后，與對應參考的重力向量和地磁向量進行求誤差，這個誤差用來校正陀螺儀的輸出，然后用陀螺儀數據進行四元數更新，再轉換到歐拉角。

具體步驟如下：

該算法能得到大概的角度。至于哪個更準確，這里無法判斷。不過俯仰角，在40s左右的一段，這時候飛機姿態是回平的，但是解算有誤。

5.卡爾曼濾波

卡爾曼濾波是非常常用的狀態估計算法，相關的理論介紹有很多，這里不贅述了。有一點要注意，就是你的狀態方程和量測方程的模型不一樣，效果肯定也是不一樣的，卡爾曼濾波只是理論，具體怎么用還得看具體情況，效果也是看所建的模型。

模型一：

狀態量分別是俯仰角、滾轉角以及對應的角速度偏移。

模型如下：

模型二：

參考書目

Nonami K, Kendoul F, Suzuki S, et al.Autonomous Flying Robots: Unmanned Aerial Vehicles and Micro AerialVehicles[M]. Springer Publishing Company, Incorporated, 2010.

第10章

結果如圖：

兩者曲線差不多，認為可以使用。

模型三：

PX4飛控早期的attitude_estimator_ekf版本。

算法的具體步驟，可以參考：

幾個小點：

1、怎么用卡爾曼進行融合？模型哪里來？

建模一般則是根據物理規律進行定的，運用現成的數學理論，如果不是很了解，可以參考一些論文。

2、卡爾曼如何調參數？

簡單來講，就是重點調R矩陣參數，一般取數據，用matlab看曲線。

3、數據的坐標系？

很多人用同樣的代碼，輸入數據后，運算的結果不對。一般主要原因就是正負號不對應，要對好數據的坐標系，就是正負號，不知道的話，本辦法就是試湊。

4、數據質量對解算的影響？

最后那個數據曲線就是手晃的數據，不是直接飛行的，加速度噪音小。數據質量一般關心的是加速度的數據，起飛后，加速度噪音比較大，可能你在地面測試，覺得姿態角算的挺準的，但是一起飛就不行了，這時候需要回過頭來處理加速度數據，怎么處理？涉及到濾波方面的知識，后續會簡單講講。

5、卡爾曼濾波好在哪里？

主要是因為一般的互補濾波，權重參數是固定的，而卡爾曼本質也是調整權重參數，但是是時變的，不過計算量相對較大。

6、動加速度的影響？

動加速度對姿態解算影響較大，因為你的陀螺是靠重力加速度向量去校正的，一旦運動的加速度大了，則會影響參考，自然也算不準了。普遍的方法是會融合GPS的速度算加速度。

這篇文章只是簡單描述了一些常用的姿態解算算法，并不直接點明好壞，沒有參考數據，不太好分析。有的一些沒有給出推導過程，讀者可以自己推導。一些理論也請自行查閱相關文獻。后續也會講解下卡爾曼濾波等內容。

總結

以上是生活随笔為你收集整理的详解几种飞控的姿态解算算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。