摘 要： 四旋翼飛行器的飛行控制效果取決于姿態信息獲取的準確性，所以姿態解算是飛行器研究的關鍵技術。針對目前飛行器高動態特性和復雜環境下的姿態解算誤差較大的問題，提出一種基于四元數的二階互補濾波算法，通過增加一個PI反饋控制環節，能更好地利用多傳感器進行姿態融合，使經典互補濾波算法中的低通阻帶衰減速度過慢、陀螺儀漂移等問題得以改善。同時文中自主設計了IMU系統，完成數據采集和姿態解算算法的實現，分別進行三軸轉臺實驗，改進后姿態解算的結果表明，無論是在靜態還是動態環境下，俯仰、橫滾角的誤差小于0.5°，驗證了該算法的可行性。

　　關鍵詞： 姿態解算; 四旋翼飛行器; 四元數; 互補濾波; 姿態融合; IMU系統設計

　　0 引 言

　　四旋翼飛行器集合了不同飛行器的優點，憑借其結構簡單、使用方便、成本低、便于操作等特點，已經被廣泛應用在各行各業。如今針對四旋翼飛行器的研究不止限于熟知的軍事、工業領域，更多的被應用到航拍和物流行業中，產生了巨大的商業價值，因此眾多的科研院校和商業國內公司都爭相投入到四旋翼的研發生產當中。

　　而在研究當中關鍵項目之一是四旋翼飛行器的姿態解算研究[1]。因為四旋翼飛行器在飛行的過程中，不僅會受到自身欠驅動系統的影響，也易受到噪聲、空氣阻力等復雜環境的干擾，導致姿態信息的獲取增加了難度，而姿態信息獲取的精準性是相應飛行器的獨立飛行最基本的前提，所以姿態解算研究的重要性不言而喻。

　　在姿態解算中，卡爾曼濾波器的應用較為廣泛，文獻[2]通過卡爾曼濾波(Kalman Filtering) 融合陀螺儀和加速度計數據，遞推計算出最優估計值來修正輸出誤差。但是飛行器系統要滿足非線性系統和高斯白噪聲的前提。因此擴展卡爾曼(EKF)提出，應用在非線性系統中[3]。但是EKF算法計算量大，在線性化假設無法成立的情況下，不滿足四旋翼飛行器高機動、實時性的要求。而互補濾波算法簡單有效，易實現傳感器測量精度。四元數的旋轉特性解決了飛行器姿態解算中的奇異性問題。

　　本文自主設計了以嵌入式MEMS控制器和慣性測量單元(Inertial Measurement Unit，IMU)組合的系統平臺，采用四元數方法描述系統模型，提出了一種基于四元數的改進型互補濾波算法，致力于降低計算的復雜性和難度，提高濾波的效果。通過三軸轉臺實驗和實物飛行測試，驗證本文提出的算法可以適用于四旋翼飛行器系統，能提高姿態解算的效率和精度。

　　1 四元數法的姿態解算

　　姿態被用來描述載體坐標系和參考(地理)坐標系之間的角位置關系，四旋翼飛行器姿態解算則是飛行器的載體坐標系相對地理坐標系的姿態矩陣問題求解。

　　1.1 坐標系定義

　　根據參考坐標系，才能確定物體的速度、姿態、位置等信息。在慣性導航中，常用的有5種坐標系，分別為地球、慣性、載體、地理、導航坐標系[4]。參考坐標系如圖1所示。

　　地球坐標系([O?xeyeze])：將地球中心作為坐標系的原點，坐標軸始終與地球維持一致。其中，[xe]和[ye]沿地球的赤道平面和格林尼治子午面的交線[5]，[ze]的方向平行于地球的極軸方向，并以地球自轉角速度相對于地心慣性坐標系旋轉。

　　導航坐標系([O?xnynzn])：即當地的地理坐標系，坐標的原點位于P點，坐標軸指向北、東和當地垂線方向(向下)。通常把導航坐標系相對于地球固連坐標系的旋轉角速率稱為轉移速率，速率的大小取決于P點相對于地球運動的快慢。

　　載體坐標系([O?xbybzb])：不同于導航坐標系的“東北天”，機體坐標系取載體的“前左上”，描述其姿態的角度分別為橫滾(Roll)角γ、俯仰(Pitch)軸θ和航向(Heading)角ψ。

　　1.2 四元數姿態表示方法

　　四元數由實數和復數組合構成：即一個實數單位和三個虛數單位組成的四維向量，可以用來表示剛體和坐標系在三維立體空間里的姿態方向。表達式如下：

　　式中：[q0]是標量部分的四元數系數，其大小等于坐標系旋轉角度一半的余弦值，是四元數的標量部分的系數;[q1]～[q3]表示矢量的部分;[θ] 是矢量在動坐標系上轉動的角度值[6]。

　　通過歸一化的四元數可以計算出坐標軸旋轉矩陣：

　　得旋轉矩陣為：

　　歐拉角由四元數方法表達為：

　　式中，四元數方法描述轉動、估計姿態都優于其他方法，因為始終可以避免奇異性，且僅需要4個參數進行計算，計算效率得以提升。載體系相對于地理系的角度求解，可以被看作是由地理系只經過一次旋轉形成載體系，四元數方法可以支持這種等效計算。

　　2 姿態算法實現

　　2.1 互補濾波

　　姿態解算需要采用加速度計、陀螺儀采集的數據進行計算，陀螺儀的測量值是載體坐標系的角加速度，加速度計的測量值則是其線加速度。各自都有缺點，特別是在飛行器載體因干擾產生抖動的時候，來自陀螺儀、加速度計會更容易產生高斯白噪聲;單個陀螺儀漂移誤差與工作時間產生正比的遞增，表現形式為誤差隨時間增長而累積，但是在較短的時間內可獲取較為精確的數據;加速度計與其相反，靜態特性好，但是短時間內的精度表現較差，無法滿足飛行器易抖動的特性。

　　總結為兩者在頻率上呈互補關系，互補濾波則是將加速度計和陀螺儀分別在不同頻率上的優勢融合。即利用加速度計測得短時間的瞬時值來補償陀螺儀長時間積累出現的漂移誤差[7]。其原理圖如圖2所示。

　　設[θ]為姿態角，融合后的姿態角估計值為：

　　式中：[ωg] 是陀螺儀測量的角速度值;[θα]則是角度值，來自加速度計測量;高通濾波器為;低通濾波器為。高通濾波器的作用是減小低頻下陀螺儀的漂移誤差，低通濾波器則是限制高頻下的加速度計噪聲[8]。式(5)可經過反拉普拉斯變換得：

　　觀察式(6)，互補濾波是通過調整[kP]值改變濾波器的截止頻率，達到對陀螺儀、加速度計所占比重的處理。

　　2.2 互補濾波改進設計

　　經過分析，第2.1節介紹的一階互補濾波可以在某個階段抑制陀螺儀積分產生漂移誤差、降低高頻噪聲。但是文獻[9?11]等人對經典互補濾波進行了研究，得到以下結論：由于其低通阻帶衰減緩慢，四旋翼飛行器具有高動態性，固定的調節參數設置無法在所有的情況下得到最優的估計值。所以一階互補濾波器不能隨時有效消除系統誤差。

　　因此在互補濾波原理的基礎上，針對傳統互補濾波應用在姿態解算上的不足，結合誤差來源，提出在互補濾波基礎上增加一個PI(比例積分)反饋控制環節，完成互補濾波的改進。理論上可以加快低通阻帶的衰減速度，可以在不同的情況下，動態地調整補償的系數：如果處于勻速狀態，增大加速度計的所占權重;高動態時，減小加速度及占的比重，增大陀螺儀比重。

　　整個流程即是先將加速度計解算出的角度與最終角度產生的誤差實現PI控制，促進誤差的修正。修正后和陀螺儀解算出的姿態角相加后再進行積分得到一個新姿態角。改進后的互補濾波算法如圖3所示。

　　算法的實現過程如下：

　　1) 首先對加速度計測量的三維向量進行規范化：

　　對載體坐標進行轉換到地理坐標系后，可得到載體坐標系下重力加速度的三維向量：

　　2) 加速度計的測量值向量[A]和陀螺儀積分后的姿態估計向量[A]之間的誤差向量利用向量間的叉積表示：

　　改進增加的比例積分環節可以求解得出陀螺儀的漂移補償向量：

　　如上所述可修正誤差。

　　3) 通過一階龍格庫塔法求解四元數微分方程，求解出的四元數的更新方程為：

　　4) 將漂移誤差向量代入到式(9)中，得到角速度微分四元數。

　　5) 對更新后的四元數積分規范化處理，由式(4)求解得出姿態角估計值。

　　推薦閱讀：科技核心投稿論文要求有基金嗎