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

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

　　0 引 言

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

　　而在研究當(dāng)中關(guān)鍵項(xiàng)目之一是四旋翼飛行器的姿態(tài)解算研究[1]。因?yàn)樗男盹w行器在飛行的過(guò)程中，不僅會(huì)受到自身欠驅(qū)動(dòng)系統(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]通過(guò)卡爾曼濾波(Kalman Filtering) 融合陀螺儀和加速度計(jì)數(shù)據(jù)，遞推計(jì)算出最優(yōu)估計(jì)值來(lái)修正輸出誤差。但是飛行器系統(tǒng)要滿(mǎn)足非線(xiàn)性系統(tǒng)和高斯白噪聲的前提。因此擴(kuò)展卡爾曼(EKF)提出，應(yīng)用在非線(xiàn)性系統(tǒng)中[3]。但是EKF算法計(jì)算量大，在線(xiàn)性化假設(shè)無(wú)法成立的情況下，不滿(mǎn)足四旋翼飛行器高機(jī)動(dòng)、實(shí)時(shí)性的要求。而互補(bǔ)濾波算法簡(jiǎn)單有效，易實(shí)現(xiàn)傳感器測(cè)量精度。四元數(shù)的旋轉(zhuǎn)特性解決了飛行器姿態(tài)解算中的奇異性問(wèn)題。

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

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

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

　　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]沿地球的赤道平面和格林尼治子午面的交線(xiàn)[5]，[ze]的方向平行于地球的極軸方向，并以地球自轉(zhuǎn)角速度相對(duì)于地心慣性坐標(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ù)卮咕€(xiàn)方向(向下)。通常把導(dǎo)航坐標(biāo)系相對(duì)于地球固連坐標(biāo)系的旋轉(zhuǎn)角速率稱(chēng)為轉(zhuǎn)移速率，速率的大小取決于P點(diǎn)相對(duì)于地球運(yùn)動(dòng)的快慢。

　　載體坐標(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)成：即一個(gè)實(shí)數(shù)單位和三個(gè)虛數(shù)單位組成的四維向量，可以用來(lái)表示剛體和坐標(biāo)系在三維立體空間里的姿態(tài)方向。表達(dá)式如下：

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

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

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

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

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

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

　　2.1 互補(bǔ)濾波

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　　如上所述可修正誤差。

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

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

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

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