基于改進(jìn)型顯性互補(bǔ)濾波的MENS姿態(tài)解算

付 雷，章 政，余 義

（武漢科技大學(xué) 信息科學(xué)與工程學(xué)院，武漢 430080）

對(duì)于移動(dòng)機(jī)器人、飛行器和衛(wèi)星通信等系統(tǒng)而言，實(shí)時(shí)、準(zhǔn)確的姿態(tài)估計(jì)是實(shí)現(xiàn)各種載體穩(wěn)定運(yùn)行的有效前提之一。

目前，由低成本、小型化的微機(jī)械陀螺儀、加速度計(jì)等慣性測量單元IMU（inertial measurement unit）構(gòu)成的微機(jī)電系統(tǒng)MEMS（micro-electro-mechanical system）姿態(tài)估計(jì)得到了廣泛的研究和應(yīng)用。然而在實(shí)際應(yīng)用過程中，單一的IMU易受到噪聲、溫度漂移及安裝誤差等多方面因素影響[1]。因此，將不同的傳感器進(jìn)行優(yōu)勢(shì)互補(bǔ)，形成多傳感器融合系統(tǒng)用于保證載體姿態(tài)的精度和實(shí)時(shí)性。

1 多傳感器融合系統(tǒng)的研究現(xiàn)狀

低成本STM32處理器的四旋翼飛行器試驗(yàn)測試平臺(tái)，將所設(shè)計(jì)的算法進(jìn)行靜態(tài)試驗(yàn)和動(dòng)態(tài)試驗(yàn)，以驗(yàn)證所設(shè)計(jì)系統(tǒng)的有效性。

在多傳感器融合系統(tǒng)中，通常是以卡爾曼濾波算法為基礎(chǔ)的擴(kuò)展和衍生算法或者互補(bǔ)濾波算法，將多種IMU傳感器的信息進(jìn)行融合，由此提高載體姿態(tài)的精度和抗干擾性。文獻(xiàn)[2]設(shè)計(jì)了一種卡爾曼濾波算法，將加速度及磁航向作為觀測量對(duì)姿態(tài)信息及陀螺儀的漂移進(jìn)行修正；文獻(xiàn)[3]提出了一種擴(kuò)展卡爾曼濾波算法，將測量噪聲和系統(tǒng)噪聲引入狀態(tài)方程，提高了姿態(tài)解算精度，但在觀測方程線性的時(shí)候引入了誤差；文獻(xiàn)[4-5]提出了一種無跡卡爾曼濾波算法，解決了線性化帶來的誤差，相比擴(kuò)展卡爾曼精度更高；文獻(xiàn)[6]設(shè)計(jì)了一種互補(bǔ)濾波算法，無需對(duì)噪聲進(jìn)行精確建模，從頻域的角度上消除誤差并進(jìn)行數(shù)據(jù)融合。在此基礎(chǔ)上，文獻(xiàn)[7-8]提出了一種顯性互補(bǔ)濾波ECF（explicit complementary filter）算法，在互補(bǔ)濾波算法的補(bǔ)償環(huán)節(jié)中加入了積分器，直接對(duì)IMU所測的矢量數(shù)據(jù)進(jìn)行融合。文獻(xiàn)[9]提出一種具有增益調(diào)節(jié)機(jī)制的顯性互補(bǔ)濾波器，使得基于重力加速度估計(jì)的互補(bǔ)濾波器，能獲得較為精確的姿態(tài)估計(jì)。文獻(xiàn)[10]給出了一種自適應(yīng)顯性互補(bǔ)濾波算法，根據(jù)加速計(jì)的輸出，判斷載體的運(yùn)動(dòng)情況，實(shí)時(shí)調(diào)整PI參數(shù)，以提高姿態(tài)估計(jì)的精度。

對(duì)于以卡爾曼濾波算法為基礎(chǔ)的信息融合方法，由于載體姿態(tài)變量的非線性、觀測方程的求解，以及系統(tǒng)噪聲和測量噪聲序列方差陣的計(jì)算，其繁瑣的計(jì)算過程對(duì)處理器的運(yùn)算速度和要求都較高，對(duì)于MEMS的實(shí)際工程應(yīng)用難度較大。

相對(duì)而言，以互補(bǔ)濾波為基礎(chǔ)的相關(guān)算法既可解決單一傳感器無法準(zhǔn)確估計(jì)姿態(tài)的問題，也無需對(duì)姿態(tài)估計(jì)進(jìn)行重構(gòu)，更適用于嵌入式硬件平臺(tái)。

鑒于此，文中設(shè)計(jì)了一種加速度計(jì)/陀螺儀組合導(dǎo)航的改進(jìn)型顯性互補(bǔ)濾波姿態(tài)解算算法。首先，對(duì)加速度計(jì)量測數(shù)據(jù)的白噪聲進(jìn)行處理，再將其與陀螺儀數(shù)據(jù)融合進(jìn)行姿態(tài)解算，由此減小白噪聲對(duì)姿態(tài)解算精度的影響。然后，為了解決濾波后加速度數(shù)據(jù)對(duì)姿態(tài)解算的實(shí)時(shí)性造成的影響，設(shè)置緩存區(qū)保存更新后的四元數(shù)向量，在緩存區(qū)中取出與加速度對(duì)應(yīng)時(shí)刻的四元數(shù)向量進(jìn)行數(shù)據(jù)融合。最后，搭建了基于

2 系統(tǒng)姿態(tài)描述

在載體的運(yùn)動(dòng)狀態(tài)描述中，定義的導(dǎo)航坐標(biāo)系（參考坐標(biāo)系）n系和載體坐標(biāo)系b系如圖1所示。其中，導(dǎo)航坐標(biāo)系選取“東-北-天”建立坐標(biāo)系，坐標(biāo)原點(diǎn)為O點(diǎn)；載體坐標(biāo)系選取右手直角坐標(biāo)系，即向右為X正方向，向前為Y軸正方向，向上為Z軸正方向，其坐標(biāo)原點(diǎn)O′與導(dǎo)航坐標(biāo)系原點(diǎn)重合。

圖1 導(dǎo)航坐標(biāo)系和載體坐標(biāo)系Fig.1 Navigation coordinate system and carrier coordinate system

在此選用計(jì)算量小、算法簡單的四元數(shù)更新算法[11]實(shí)現(xiàn)載體的姿態(tài)估計(jì)。四元數(shù)定義為

式中：q0，q1，q2，q3為實(shí)數(shù)；i，j，k 為互相正交的單位向量，且為虛單位

載體的姿態(tài)分別用繞Z軸旋轉(zhuǎn)為偏航角（yaw）ψ，繞Y軸旋轉(zhuǎn)為橫滾角（roll）θ，繞X軸旋轉(zhuǎn)為俯仰角（pitch）φ來描述，且按照ψ→θ→φ的旋轉(zhuǎn)順序。將四元數(shù)轉(zhuǎn)化為歐拉角形式的方向余弦矩陣為

由式（2）（3）可得，四元數(shù)與歐拉角的關(guān)系為

3 基于改進(jìn)顯性互補(bǔ)濾波器姿態(tài)估計(jì)算法

3.1 陀螺儀姿態(tài)更新算法

四元數(shù)的運(yùn)動(dòng)學(xué)微分方程為

其中

由式（5），可得載體的四元數(shù)更新等式為

假設(shè)，四元數(shù)更新時(shí)間及采樣時(shí)間為T，在時(shí)間間隔（t，t+T）內(nèi)，ω 為常數(shù)，載體四元數(shù)的更新迭代公式為

式中：i=1，2，3；Δts為系統(tǒng)采樣時(shí)間。

根據(jù)更新后t時(shí)刻的四元數(shù)，可得到載體的旋轉(zhuǎn)矩陣，由式（4）得到t時(shí)刻載體的姿態(tài)角。

3.2 改進(jìn)顯性互補(bǔ)濾波器設(shè)計(jì)

顯性互補(bǔ)濾波器是在速度計(jì)/陀螺儀互補(bǔ)濾波算法的補(bǔ)償環(huán)節(jié)中加入積分環(huán)節(jié)，由此補(bǔ)償陀螺儀產(chǎn)生的漂移誤差。同時(shí)，為了減小加速度原始數(shù)據(jù)中白噪聲對(duì)姿態(tài)解算精度造成的干擾，在顯性互補(bǔ)濾波算法的基礎(chǔ)上，消除低通濾波器延時(shí)給姿態(tài)解算實(shí)時(shí)性帶來的影響，文中所設(shè)計(jì)的改進(jìn)顯性互補(bǔ)濾波算法如圖2所示。

利用該算法進(jìn)行四旋翼姿態(tài)解算的具體步驟如下：

初始化四元數(shù)，令

圖2 改進(jìn)顯性互補(bǔ)濾波器Fig.2 Improved explicit complementary filter

將測得的載體坐標(biāo)系下的加速度A′經(jīng)過巴特沃斯低通濾器后進(jìn)行標(biāo)準(zhǔn)化，得到載體坐標(biāo)系下三軸加速度的分量構(gòu)成的向量

式中：ax′，ay′，az′分別為三軸加速度分量。 但信號(hào)經(jīng)過巴特沃斯低通濾波器之后存在延時(shí)，這種特性稱為濾波器的群時(shí)延，其定義[12]為

式中：φ（f）為濾波器相位響應(yīng)；f為頻率。為了消除群時(shí)延對(duì)姿態(tài)解算實(shí)時(shí)性的影響，需求出延時(shí)時(shí)間。通過式（9）計(jì)算出加速度延時(shí)時(shí)間為Δt，在緩存

其中

為了對(duì)陀螺儀的輸出進(jìn)行預(yù)測矯正，誤差向量經(jīng)過PI環(huán)節(jié)得到的校正量δ為

顯示互補(bǔ)濾波器的效果與KP，KI的選擇有很大的關(guān)系，如果參數(shù)不合適，則姿態(tài)估計(jì)的誤差較大，甚至偏離實(shí)際值。KP為低通濾波器和高通濾波器的之間的轉(zhuǎn)接頻率，根據(jù)加速度計(jì)的輸出A′和陀螺儀的輸出ω的頻率特性來選??；KI為積分增益，其值為Kp的幾十或幾百分之一。

最后，將陀螺儀測得的角速度與式（11）中所求的校正量相加，得到補(bǔ)償后的三軸角速度為

將補(bǔ)償后的三軸角速度代入式（6），求得更新后的姿態(tài)四元數(shù)微分；利用式（7）對(duì)四元數(shù)微分進(jìn)積分，積分時(shí)間為采樣周期，根據(jù)更新后的四元數(shù)可得到對(duì)應(yīng)的旋轉(zhuǎn)矩陣，進(jìn)而通過式（4）得到四旋翼的姿態(tài)角 θ，ψ，φ。

4 試驗(yàn)測試平臺(tái)

文中所搭建的四旋翼飛行器試驗(yàn)測試平臺(tái)如圖3所示；建立了上位機(jī)、飛行器和遙控器通訊，系統(tǒng)結(jié)構(gòu)如圖4所示。

圖3 四旋翼飛行器實(shí)物Fig.3 Four-rotor aircraft object

圖4 飛控系統(tǒng)結(jié)構(gòu)Fig.4 Flight control system structure

圖4中，四旋翼飛行器的控制單元由主控模塊、慣性測量模塊、穩(wěn)壓模塊、電源模塊、電機(jī)驅(qū)動(dòng)模塊和無線通訊模塊構(gòu)成。其中，主控模塊選用ST公司的基于ARM-CortexM3內(nèi)核的STM32F103RCT6芯片。慣性測量模塊選用美國Inven Sense公司的三軸陀螺儀和三軸加速度計(jì)集成芯片MPU6050，與主控模塊通過IIC進(jìn)行通訊。無線通訊模塊選用富斯公司的10通道2.4 GHz遙控器與PPM接收器，以及2.4 GHz無線數(shù)傳模塊，PPM信號(hào)通過單個(gè)I/O口就能與主控通訊，無線傳輸距離可達(dá)700 m，無線數(shù)傳模塊通過串口與主控模塊通訊，發(fā)送實(shí)時(shí)姿態(tài)數(shù)據(jù)到上位機(jī)。電機(jī)驅(qū)動(dòng)模塊則利用PWM控制電子調(diào)速器的輸出來驅(qū)動(dòng)無刷電機(jī)。四旋翼飛行器的各項(xiàng)參數(shù)見表1。

表1 四旋翼飛行器參數(shù)Tab.1 Parameters of quadrotor

5 試驗(yàn)結(jié)果及分析

為了測得噪聲的頻率范圍，讓四旋翼飛行器處于懸停狀態(tài)，對(duì)加速度計(jì)的原始測量值進(jìn)行采集，將采集到的數(shù)據(jù)通過USART上傳到上位機(jī)，并進(jìn)行FFT變換。根據(jù)頻譜分析，可以確定截止頻率約為20 Hz，所以試驗(yàn)選用了截止頻率為20 Hz的二階巴特沃斯數(shù)字低通濾波器來進(jìn)行噪聲處理。采用MatLab仿真出幅頻和相頻特性曲線，如圖5所示。根據(jù)3.2的分析，由式（9）可求得延時(shí)時(shí)間約為64 ms。

圖5 低通濾波器幅頻和相頻特性Fig.5 Amplitude-frequency and phase-frequency characteristics of low-pass filters

為驗(yàn)證改進(jìn)型顯性互補(bǔ)濾波算法的實(shí)時(shí)性和準(zhǔn)確性，進(jìn)行了2組試驗(yàn)：第1組為靜態(tài)試驗(yàn)，測試算法的精確度和靜態(tài)性能；第2組為動(dòng)態(tài)試驗(yàn)，測試算法在不同角速度下的實(shí)時(shí)性和動(dòng)態(tài)性能。

當(dāng)試驗(yàn)平臺(tái)靜止時(shí)，分別用標(biāo)準(zhǔn)型顯性互補(bǔ)濾波算法和改進(jìn)型互補(bǔ)濾波ECF算法，解算出四旋翼飛行器的姿態(tài)，并記錄橫滾角和俯仰角（如圖6所示）。由圖可見，靜態(tài)測試標(biāo)準(zhǔn)ECF算法中橫滾角和俯仰角的波動(dòng)范圍為-0.1～0.1；改進(jìn)ECF算法中橫滾角和俯仰角的波動(dòng)范圍為-0.1～0.1。

圖6 標(biāo)準(zhǔn)ECF和改進(jìn)ECF靜態(tài)測試Fig.6 Standard ECF and improved ECF static test

表2給出了2種算法姿態(tài)角的均方根誤差、最大值及最小值。對(duì)比2組數(shù)據(jù)，可知2種算法在姿態(tài)解算的精度上差別不大，說明改進(jìn)ECF保證了姿態(tài)解算的準(zhǔn)確性。

表2 靜態(tài)測試下兩種算法姿態(tài)解算誤差Tab.2 Attitude estimation errors of two algorithms in static test

將試驗(yàn)平臺(tái)沿Y軸固定在碳纖維桿上，保證四旋翼只能繞單軸運(yùn)動(dòng)。使四旋翼以不同角速度做繞軸運(yùn)動(dòng)，同時(shí)將2種算法解算出的俯仰角發(fā)送至上位機(jī)進(jìn)行顯示，姿態(tài)采樣時(shí)間為4 ms；為了更容易觀察波形，選取發(fā)送周期為姿態(tài)采樣周期的3倍，如圖7所示共選取了1000個(gè)采樣點(diǎn)。

圖7 動(dòng)態(tài)測試姿態(tài)的整體對(duì)比Fig.7 Overall comparison of dynamic attitude testing

為了更清楚地反映不同角速度下2種算法姿態(tài)的效果，進(jìn)行了局部姿態(tài)的對(duì)比，如圖8所示。由圖可見，在慢速轉(zhuǎn)動(dòng)時(shí)，2種算法解算的姿態(tài)跟隨幾乎沒有差別；而在快速轉(zhuǎn)動(dòng)時(shí)，標(biāo)準(zhǔn)ECF相對(duì)于改進(jìn)ECF解算的姿態(tài)有明顯的滯后。

圖8 不同角速度運(yùn)動(dòng)的局部姿態(tài)對(duì)比Fig.8 Local attitude comparison of different angular velocities

為了進(jìn)一步確定標(biāo)準(zhǔn)ECF相對(duì)于改進(jìn)ECF的滯后時(shí)間，分別選取在50，150，250，350采樣點(diǎn)附近進(jìn)行比較，對(duì)比結(jié)果見表3。由表可算出平均滯后時(shí)間約為24 ms。

通過動(dòng)態(tài)繞軸試驗(yàn)，說明了改進(jìn)ECF姿態(tài)解算的實(shí)時(shí)性相對(duì)于標(biāo)準(zhǔn)ECF，有了明顯的提高。

表3 姿態(tài)估計(jì)滯后時(shí)間的對(duì)比Tab.3 Comparison of lag time in attitude estimation

6 結(jié)語

文中設(shè)計(jì)了一種改進(jìn)顯性互補(bǔ)濾波算法，在IMU數(shù)據(jù)融合之前對(duì)加速度原始數(shù)據(jù)進(jìn)行巴特沃斯低通濾波，同時(shí)設(shè)置了緩存區(qū)保存更新后的四元數(shù)向量，在緩存區(qū)中取出與加速度對(duì)應(yīng)時(shí)刻的四元數(shù)向量進(jìn)行數(shù)據(jù)融合，在保證了精確度的基礎(chǔ)上提高了姿態(tài)解算的實(shí)時(shí)性。為了驗(yàn)證文中所設(shè)計(jì)算法的有效性和實(shí)用性，搭建了四旋翼飛行器試驗(yàn)測試平臺(tái)，將文中所設(shè)計(jì)的算法與傳統(tǒng)的顯性互補(bǔ)濾波算法在試驗(yàn)平臺(tái)上進(jìn)行了靜態(tài)試驗(yàn)和動(dòng)態(tài)繞軸試驗(yàn)。試驗(yàn)結(jié)果表明，該算法能有效地減小加速度白噪聲對(duì)姿態(tài)解算精度的影響，改善了加速度低通濾波之后造成姿態(tài)解算滯后問題，保證了姿態(tài)解算的精確度，提高了姿態(tài)解算的實(shí)時(shí)性。