一、開篇

? ? ? ? 很久沒更新blog了， 最近研究的東西比較雜亂，也整理了很多東西，沒有來的及更新，最近發現很多小伙伴都開始寫blog了，在不更新就要“被落后了”。兄弟們，等等我啊~~~

? ? ? ? 轉了這么久又回到了姿態解算部分，最近研究了現有的幾種關于姿態解算的實現算法，理論大都相通，請各位耐心看完。 希望閱讀本部分的讀者對姿態解算算法有一定基礎的認識，該部分涉及了常用的三種姿態解算算法：Explicit Complementary Filter(ECF)、Extended Kalman Filter(EKF)、Gradient Descent Filter(GD)。本篇只做大致介紹，不結合代碼分析，后續會結合代碼分析算法單獨出文章。

? ? ? ? 現有的開源的姿態解算算法的代碼實現在如下地址都可以找到，里面涉及了很多的姿態解算算法，慢慢的一個個的扣吧。

? ? ? ? 鏈接：https://developer.mbed.org/users/gke/code/UAVXArm-GKE/file/90292f8bd179/attitude.c

三、實驗平臺

Software?Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、正文

? ? ? ? 經過近一段時間的研究，主要是關于多旋翼無人機姿態解算算法的研究，開源PX4源代碼里面默認使用的是基于mahony的互補濾波算法（explicit complement filter）進行姿態解算的；還有一套備用姿態解算算法是基于kalman的EKF（extended kalman filter）；然后呢，其實還有一套PX4源代碼中尚未涉及的基于madgwick的梯度下降算法（gradient descent）。

1.1、三套不同的算法各有優勢可有優缺點。

? ? ? ? 三套算法都可以使用，很多公司偏向于EKF，所以對EKF的要求還是比較高的，慢慢擼EKF吧，不然工作就愁了。小四軸用的比較多的還是互補濾波算法，尤其是大學生參加比賽也好，個人愛好也好，基于STM32自己制作小四軸的大多基于這套（我見過的很多都是用的CF，可能也有很多用的別的算法吧，不用太糾結這個）。梯度下降用的屬于最少的了吧。最后出場的就是大頭了：EKF；啥都別說了，好好學習它沒錯。

1.1.1、mahony算法

? ? ? ? Mahony算法即所謂的互補濾波算法，通過PID反饋控制器把誤差量反饋補償修正陀螺儀的誤差。Mahony的互補濾波算法是基于IMU系統的，即陀螺儀和加速度計，過程中不涉及磁力計的修正。而我們使用過程中一般都是AHRS系統（即MARG：Magnetic、Angular Rate、Gravity），這個就需要尋求另外的算法或者基于mahony算法進行改進。PX4源代碼中就是使用的改進的mahony算法，即加入了磁力計數據和GPS數據進行姿態解算。該算法是最簡單和最容易實現的算法，PX4姿態解算默認的就是使用的這個。但是濾波器截止頻率不好確定且與采樣頻率有關，Kp大截止頻率就大。

? ? ? ? 關于這一塊，現在研究的比較多就是如何實現自適應調參。固定的參數不能獲得所有情況下的最優運動姿態角，可以設計參數可調的自適應算法在不同運動狀態下進行調節參數的大小。其參數調節規則為：正常運動狀態情況下，Kp和Ki值取為系統初始化值；當運動體具有較大運動加速度或姿態變化劇烈時，應選擇較小的Kp值（可取其初始化值的0.1倍），而Ki值應在同一數量級內適當取大一點。具體取值需根據實際應用系統選取。

? ? ? ? Mahony論文：《AComplementary Filter for Attitude Estimation of a Fixed-Wing UAV》和《NonlinearComplementary Filters on the Special Orthogonal Group》

? ? ? ? Mahony團隊發表了太多的論文了，有興趣的可以到researchgate查看：

? ? ? ? 鏈接：https://www.researchgate.net/profile/Robert_Mahony/publications/3

Fig.1 Explicit Complementary Filter

1.1.2 EKF算法

? ? ? ? EKF俗稱擴展卡爾曼濾波算法，優點不言而喻。補充一句：Kalman還活著，還活著呢~~~

? ? ? ? EKF是出了名的計算量大，因為算法實現過程在不范是各種矩陣運算，對處理器的運算速度和精度要求很高對MCU的主頻要求比較大，再加上系統的非線性，難以建立穩定可靠的狀態方程以及合適的過程噪聲協方差陣Q以及測量噪聲協方差陣R。一言以蔽之，就是CPU計算負荷大。但是理解了KF以后，對整合濾波過程還是比較清晰的，很多公司都要求必須會EKF算法，優勢就是在于處理過程中的預測和修正，其中一點就是可以估計陀螺儀的bias；在算法的計算處理過程中每一次都在不停的修正K和bias，相當于動態自適應算法了，主要就是調好Q、R。所以得把EKF搞懂，雖然比較難，但是必須知難而上。后續會根據直接KF求歐拉角和EKF求取四元數來更新blog，kalman filter相當有研究價值。

? ? ? ? 相關論文：《A Double-Stage Kalman Filter for Orientation TrackingWith an Integrated Processor in 9-D IMU》

Fig.2 Extended Kalman Filter

1.1.3、梯度下降法

? ? ? ? The Gradient Descent是madgwick在2010搞出來的一套算法，其核心思想是將用加速度計和磁力計通過梯度下降法得到的姿態四元數與由陀螺儀積分得到的姿態，進行線性融合，得到最優的姿態。在算法實現過程中需要了解目標函數的建立，梯度下降法是根據目標函數求取誤差向量的最小值的，這一點在madgwick的論文中尚未提及，應該是覺得默認大家都懂這個吧。

? ? ? ? 該算法首先是只用陀螺儀數據根據一階畢卡更新四元數，然后才是梯度下降法對加速度數據和磁力計數據進行處理以便求出最小的誤差四元數，最后根據權重值（線性）把兩者融合到一起得到最終的姿態四元數。使用梯度下降法就是對加速度數據和磁力計數據和相應參考向量求取誤差量，這一部分和mahony_base算法非常類似，mahony_base是利用向量的叉乘求取量測值和參考向量的誤差的，而梯度下降法是根據反梯度方向迭代求取最小誤差的（理論基礎就是構造目標函數使用梯度下降法求取誤差向量的最小值）。過程中最重要就是參數β需要通過實驗進行確定gyro的測量噪聲（measurement error）。

? ? ? ? 該算法適用于IMU和AHRS系統，其明顯特點就是計算量小（computationally inexpensive），低頻有效性（比如10hz）。Madgwick說該算法最后的精度比kalman_base算法要好，我也不知道，沒實際測試過；基于梯度下降的數據融合算法能夠顯著降低對處理器速度的要求。

? ? ? ? Madgwick論文：《An efficient orientation filter for inertial and inertial magneticsensor arrays》和《Estimation of IMU and MARG orientation using agradient descent algorithm》。梯度wiki：https://en.wikipedia.org/wiki/Gradient_descent

? ? ? ? 算法代碼試下：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

Fig.3 Complete Orientation Filter

五、結論

? ? ? ? 上述三種算法都是以陀螺儀采集的角速度數據位主要控制量，通過不同的算法處理加速度數據和磁力計數據對由陀螺儀估計的姿態進行修正補償。比如mahony_base算法是通過構造PID反饋控制器實現對由陀螺儀估計的的姿態修正的，主要過程是基于重力或者重力和磁力參考向量修正陀螺儀數據，最后根據一階畢卡（或多階）更新姿態四元數；EKF是通過量測方程實現對由陀螺儀估計的姿態角修正的；Gradient descent是通過兩種算法的融合來進行姿態解算的，首先是只用陀螺儀數據根據一階畢卡更新四元數，然后才是梯度下降法對加速度數據和磁力計數據進行處理以便求出最小的誤差四元數，最后根據權重值把兩者融合到一起得到最終的姿態四元數。如果kalman算法是自適應的，那解算精度就比梯度下降算法強了。

? ? ? ? 綜上所述，姿態解算不過就是利用陀螺儀的高動態性能估計實時的姿態，但是由于陀螺儀固有的缺陷問題，導致隨著時間的漂移實時的姿態估計的越來越不準確，所以呢，找一個也能估計姿態的sensor來修正一下，但是呢，總不能用兩個陀螺儀吧，可以要找一個可以和陀螺儀固有缺陷相互補的sensor，那就是加速計了，無漂移，低頻特性好。那么為何不直接只用加速度計進行姿態估計呢？原因就在于它的高頻特性太爛，響應時間久，比如想實現一個3°角度變化量，多旋翼飛行速度很快，加速計響應時間來個上百ms的話，飛機早掉地上上升天了。這個也是為什么PX4里面對姿態控制部分分為兩級PID控制的原因所在吧，內環控制角速度響應速度極快，外環控制角度響應速度相對較慢，但是有內環在接近實時的PID反饋修正，所以就不怕飛機升天了。

六、附加題

? ? ? ? 經常關注群內的技術討論有利于快速提升自我水平。所以，在群里水還是有很大好處的~~~好吧，就是找個借口而已~~~

? ? ? ? 不知道如何添加附件，只能留網盤了，自己去下載吧。

? ? ? ? 360云盤鏈接：https://yunpan.cn/cReQVk9qMKTGT?（提取碼：e25f）

總結

以上是生活随笔為你收集整理的Pixhawk之姿态解算篇（5）_ECF/EKF/GD介绍的全部內容，希望文章能夠幫你解決所遇到的問題。

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