盛廣潤 高國偉 張伯源

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

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

中圖分類號： TN967.6?34; V219? ? ? ? ? ? ? ? ? 文獻標識碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2020）14?0008?05

Research on quadrotor attitude calculation based on quaternion

SHENG Guangrun1， GAO Guowei1，2， ZHANG Boyuan1

（1. Key Laboratory of Sensors， Beijing Information Science and Technology University， Beijing 100192， China;

2. MOE Key Laboratory of Modern Measurement and Control Technology， Beijing Information Science and Technology University， Beijing 100101， China）

Abstract： As the flight control effect of the quadrotor depends on the accuracy of the attitude information acquisition， the attitude calculation is the key technology of aircraft research. In allusion to the large attitude calculation error in the complex environment and high dynamic feature of aircraft， a second?order complementary filtering algorithm based on quaternion is proposed. The multi?sensor attitude fusion can be better utilized by adding a PI feedback control link， and the low?pass stopband attenuation speed and gyroscope drift in the classical complementary filtering algorithm are improved. The IMU system is designed independently， and the data collection and attitude calculation algorithm is completed. The three?axle table experiment is conducted respectively， and the results of the improved attitude calculation show that pitch and roll angle errors are less than 0.5° in both static and dynamic environments. It proves the feasibility of the algorithm.

Keywords： attitude calculation; quadrotor; quaternion; complementary filter; attitude fusion; IMU system design

0? 引? 言

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

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

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

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

1? 四元數法的姿態(tài)解算

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

1.1? 坐標系定義

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

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

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

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

1.2? 四元數姿態(tài)表示方法

四元數由實數和復數組合構成：即一個實數單位和三個虛數單位組成的四維向量，可以用來表示剛體和坐標系在三維立體空間里的姿態(tài)方向。表達式如下：[qnb=q0q1q2q3? ? ?=cosθ2-γxsinθ2-γysinθ2-γzsinθ2] （1）

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

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

[Cbn=2q20-1+2q212（q1q2-q0q3）2（q1q3-q0q2）2（q1q2+q0q3）2q20-1+2q222（q2q3-q0q1）2（q1q2-q0q2）2（q2q3+q0q1）2q20-1+2q23]? ? ? ? ? ? ?（2）

得旋轉矩陣為：

[Cnb=（Cbn）-1=（Cbn）T] （3）

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

[ψ=arctan2（q1q2+q0q3）2q20-1+2q22θ=arcsin2（q2q3+q0q1）γ=arctan-2（q1q3+q0q2）2q20-1+2q23] （4）

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

2? 姿態(tài)算法實現

2.1? 互補濾波

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

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

設[θ]為姿態(tài)角，融合后的姿態(tài)角估計值為：

[θ=sωgs+kP·1s+kPs+kPθα]? （5）

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

[θ=kP（θα-θ）+ωg] （6）

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

2.2? 互補濾波改進設計

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

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

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

算法的實現過程如下：

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

[A=AA=αxαyαz]? （7）

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

[A=AA=αxαyαz]? ? ?（8）

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

[Eα=A×A=eαxeαyeαz]

改進增加的比例積分環(huán)節(jié)可以求解得出陀螺儀的漂移補償向量：

[Δ=δxδyδz=kPeαx+kI∫eαxkPeαy+kI∫eαykPeαz+kI∫eαz]

如上所述可修正誤差。

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

[q0q1q2q3t+Δt=q0q1q2q3t+12Δt-ωxq1-ωyq2-ωzq3ωxq0-ωyq3+ωzq2ωxq3+ωyq0-ωzq1-ωxq2+ωyq1+ωzq0]? ? ? ? ? （9）

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

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

3? 實驗驗證與分析

3.1? 試驗平臺

試驗平臺以自主開發(fā)的四旋翼飛行控制系統為主體，主要包含三軸加速度計、三軸陀螺儀、三軸磁力計、ARM處理器、通信模塊、電源模塊等部分。系統整體硬件框架如圖4所示。

試驗系統平臺利用處理器對姿態(tài)信息進行融合，通過串口輸出到上位機顯示姿態(tài)角。主控選用STM32F103處理器，性價比高，運算速度快;陀螺儀選用ADXRS450，高分辨率（0.015 （°）·s-1·g-1），通信接口選用SPI;加速度計選用ADXL355，高分辨率（256 000/g），SPI通信;磁力計傳感器采用MAG3110，分辨率高：

0.1 μT/LSB，通信接口為I2C。設置串口波特率為115 200 b/s、8位數據位、1位停止位、無奇偶校驗。利用上位機軟件接收姿態(tài)數據，自主研發(fā)IMU系統實物圖如圖5所示。

3.2? 測試試驗設計

本試驗將飛行器的IMU慣性測量單元固定在SGT320E三軸多功能轉臺進行測試，主要針對飛行器的俯仰角和滾轉角試驗。SGT320E三軸轉臺在飛行器研制領域中起到關鍵性的作用[12?13]，能夠提供精準的航姿變換軌跡、實時的姿態(tài)信息;復現飛行器運動的各種動力學特性。實驗室三軸轉臺裝置如圖6所示。

實驗過程：首先將四旋翼飛行器的IMU測量單元固定在轉臺上，保證其姿態(tài)不會發(fā)生改變，進行靜態(tài)測試;結束靜態(tài)測試階段進行動態(tài)測試，分別測試IMU測量系統的橫滾角、俯仰角。姿態(tài)傳感器在一定時間完成數據采集后，通過SPI通信方式與主控芯片進行傳輸[14]。對姿態(tài)數據進行解算后，通過RS 232串口上傳到上位機上進行姿態(tài)角數據的顯示[15]。

靜止狀態(tài)下：將三軸轉臺歸零，處于閑置狀態(tài)，運行程序，采取約45 s的數據，可以看出來圖7、圖8中，無論是俯仰角還是橫滾角，改進后的算法輸出的數據都在0°附近幾乎沒有發(fā)散，誤差相對較小，經足夠數據驗算取平均值。經典互補濾波下：橫滾角誤差、俯仰角誤差分別為0.352 1°和0.384 3°;改進型互補濾波橫滾角、俯仰角誤差為0.129 5°和0.087 5°。得出結論，在靜止狀態(tài)下，改進后互補濾波算法精度優(yōu)越，改善了陀螺儀的漂移誤差問題。

動態(tài)測試條件下，為了觀察兩種算法在姿態(tài)發(fā)生變化時的響應性能，通過對不同角度進行動態(tài)測試，上位機輸出不同算法解算的姿態(tài)角度數據，為了動態(tài)數據采集的完整性，程序運行后等待1～3 s開始轉動轉臺。首先設置轉臺進行橫滾角轉動，圖9和圖10分別是0°～40°，40°～0°（回零）數據波形。波形表示在轉動的瞬間，改進后互補濾波并沒有出現較大的超調量，在6～8 s時候波動的幅度要遠小于原始的互補濾波算法;在設置40～0°的動態(tài)測試中，為了觀察不同速度下姿態(tài)角的變化，加快了轉臺的轉速，結果發(fā)現改進型算法仍然具有較好的穩(wěn)定性能，滿足飛行器的高動態(tài)性能下的解算要求。

考慮到有負角度的情況，對俯仰角進行0°～-40°，

-40°～0°的對比姿態(tài)變化，如圖11和圖12所示。同上，可以看出改進后互補濾波算法的優(yōu)越性，動態(tài)測試中，數據波動相對平滑，角度變化的過程中沒有出現很大的數據波動和較大的動態(tài)誤差。

4? 結? 語

本文基于四元數的姿態(tài)表達方法，通過與姿態(tài)旋轉矩陣的關系獲得姿態(tài)角。針對四旋翼姿態(tài)解算誤差較大的問題，通過分析，提出一種改進型互補濾波對飛行器的姿態(tài)信息進行誤差修正，加快低通阻帶衰減速度。本文自主設計了基于多傳感器的IMU測量系統，對采集到的姿態(tài)數據進行融合，分別在靜態(tài)和動態(tài)的條件下進行三軸轉臺試驗，經過數據記錄和分析，結論是基于四元數的改進型互補濾波算法能夠抑制干擾誤差，有效地提升了無人機姿態(tài)融合的準確度，可以有效降低四旋翼的姿態(tài)解算上的誤差，滿足四旋翼飛行器實際飛行的姿態(tài)估計精度要求。

參考文獻

[1] 張謙，鄔依林.基于智能視覺的無人機導航技術研究[J].現代電子技術，2018，41（16）：132?134.

[2] 譚祖鋒.慣性導航技術的新進展及其發(fā)展趨勢[J].電子技術與軟件工程，2019（5）：76.

[3] 薄江輝，王茂鋒.GPS與慣性導航系統的組合應用研究[J].通訊世界，2019，26（6）：254?255.

[4] 史凱，劉馬寶.捷聯慣導四元數的四階龍格庫塔姿態(tài)算法[J].探測與控制學報，2019，41（3）：61?65.

[5] ZHANG Lihong， MASEK Vlastimil， SANATDOOST Nikoo?Naeemi. Structural optimization of z?axis tuning?fork MEMS gyroscopes for enhancing reliability and resolution [J]. Microsystem technologies， 2015， 21（6）： 1187?1201.

[6] 盧毅.小型航姿參考系統設計[J].傳感器與微系統，2019，38（4）：117?120.

[7] 秦永元.慣性導航[M].北京：科學出版社，2006.

[8] WANG D J， WANG G C， JIE W U. Fixed?interval smoothing post?processing algorithm for low?cost MEMS?based integrated navigation system [J]. Journal of Chinese inertial technology， 2017， 25（1）： 97?102.

[9] 周彬.MEMS?INS/GNSS組合導航系統研究[D].南京：南京航空航天大學，2017.

[10] LUIGI Palatella， FABIO Grasso. The EKF?AUS?NL algorithm implemented without the linear tangent model and in presence of parametric model error [J]. Software X， 2018， 7： 28?33.

[11] FENG Kaiqiang， LI Jie， ZHANG Xiaoming， et al. A new quaternion?based Kalman filter for real?time attitude estimation using the two?step geometrically?intuitive correction algorithm [J]. Sensors， 2017， 17（9）： 2146.

[12] 康躍然，肖本龍，傅亦源，等.三軸轉臺系統姿態(tài)變化下指向誤差建模與分析[J].制造業(yè)自動化，2018，40（12）：135?139.

[13] 陳傳俊，陳志高.基于振動臺的加速度傳感器靈敏度的標定[J].大地測量與地球動力學，2013，33（z1）：168?171.

[14] 孫佳，鄒靖，胡桐.基于24位置的MEMS慣性傳感器快速標定方法[J].壓電與聲光，2019，41（3）：440?444.

[15] 王日俊，曾志強，黨長營，等.一種基于STM32的機載慣性穩(wěn)定云臺的設計[J].現代電子技術，2018，41（12）：145?148.

[16] 朱新宇，陶庭葉，姜冬致.基于GPS/MEMS慣性傳感器的消防員室內定位研究[J].合肥工業(yè)大學學報（自然科學版），2018（7）：949?955.