盛廣潤 高國偉 張伯源

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

關(guān)鍵詞： 姿態(tài)解算; 四旋翼飛行器; 四元數(shù); 互補(bǔ)濾波; 姿態(tài)融合; IMU系統(tǒng)設(shè)計(jì)

中圖分類號： TN967.6?34; V219? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼： 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)點(diǎn)，憑借其結(jié)構(gòu)簡單、使用方便、成本低、便于操作等特點(diǎn)，已經(jīng)被廣泛應(yīng)用在各行各業(yè)。如今針對四旋翼飛行器的研究不止限于熟知的軍事、工業(yè)領(lǐng)域，更多的被應(yīng)用到航拍和物流行業(yè)中，產(chǎn)生了巨大的商業(yè)價值，因此眾多的科研院校和商業(yè)國內(nèi)公司都爭相投入到四旋翼的研發(fā)生產(chǎn)當(dāng)中。

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

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

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

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

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

1.1? 坐標(biāo)系定義

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

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

導(dǎo)航坐標(biāo)系（[O?xnynzn]）：即當(dāng)?shù)氐牡乩碜鴺?biāo)系，坐標(biāo)的原點(diǎn)位于P點(diǎn)，坐標(biāo)軸指向北、東和當(dāng)?shù)卮咕€方向（向下）。通常把導(dǎo)航坐標(biāo)系相對于地球固連坐標(biāo)系的旋轉(zhuǎn)角速率稱為轉(zhuǎn)移速率，速率的大小取決于P點(diǎn)相對于地球運(yùn)動的快慢。

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

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

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

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

通過歸一化的四元數(shù)可以計(jì)算出坐標(biāo)軸旋轉(zhuǎn)矩陣：

[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）

得旋轉(zhuǎn)矩陣為：

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

歐拉角由四元數(shù)方法表達(dá)為：

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

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

2? 姿態(tài)算法實(shí)現(xiàn)

2.1? 互補(bǔ)濾波

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

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

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

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

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

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

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

2.2? 互補(bǔ)濾波改進(jìn)設(shè)計(jì)

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

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

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

算法的實(shí)現(xiàn)過程如下：

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

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

對載體坐標(biāo)進(jìn)行轉(zhuǎn)換到地理坐標(biāo)系后，可得到載體坐標(biāo)系下重力加速度的三維向量：

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

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

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

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

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

如上所述可修正誤差。

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

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

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

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

3? 實(shí)驗(yàn)驗(yàn)證與分析

3.1? 試驗(yàn)平臺

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

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

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

3.2? 測試試驗(yàn)設(shè)計(jì)

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

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

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

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

考慮到有負(fù)角度的情況，對俯仰角進(jìn)行0°～-40°，

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

4? 結(jié)? 語

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

參考文獻(xiàn)

[1] 張謙，鄔依林.基于智能視覺的無人機(jī)導(dǎo)航技術(shù)研究[J].現(xiàn)代電子技術(shù)，2018，41（16）：132?134.

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

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

[4] 史凱，劉馬寶.捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法[J].探測與控制學(xué)報，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] 盧毅.小型航姿參考系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng)，2019，38（4）：117?120.

[7] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，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ǎo)航系統(tǒng)研究[D].南京：南京航空航天大學(xué)，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] 康躍然，肖本龍，傅亦源，等.三軸轉(zhuǎn)臺系統(tǒng)姿態(tài)變化下指向誤差建模與分析[J].制造業(yè)自動化，2018，40（12）：135?139.

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

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

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

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