互補濾波和卡爾曼濾波的融合姿態解算方法

張 棟， 焦嵩鳴， 劉延泉

(華北電力大學 自動化系，河北 保定 071003)

互補濾波和卡爾曼濾波的融合姿態解算方法

張 棟， 焦嵩鳴， 劉延泉

(華北電力大學 自動化系，河北 保定 071003)

針對捷聯慣性測量單元(IMU)噪聲大、精度低的缺點和常規的姿態解算算法精度不高等問題，提出了一種互補濾波和卡爾曼濾波相結合的融合算法。該算法基于姿態角微分方程建立系統的狀態方程模型，利用互補濾波后的姿態角作為系統的觀測量，再應用擴展卡爾曼濾波(EKF)算法融合了陀螺儀、加速度計和電子羅盤的測量數據。為驗證該算法有效性，用帶有傳感器的開發板依次進行靜態和動態測試，實驗結果表明：結合了互補濾波和卡爾曼濾波的融合算法，在靜態時能夠抑制姿態角漂移和濾出噪聲，在動態時能夠快速跟蹤姿態的變化，提高了姿態角的解算精度。

卡爾曼濾波； 互補濾波； 姿態估計； 數據融合； 慣性測量單元

0 引 言

隨著微機電系統(MEMS)技術和計算機技術的不斷發展，低成本的捷聯慣性測量單元(inertial measurement unit,IMU)被廣泛應用在小型飛行器中，捷聯慣性測量單元由陀螺儀、加速度計和電子羅盤組成。陀螺儀受溫漂的影響，長時間運行漂移嚴重，并且積分運算會產生累積誤差；加速度計易受載體震動和運動加速度的影響；電子羅盤容易受外部磁場的影響[1]。因此,這三種傳感器都不能單獨用于飛行器姿態的測量，需要進行這三種傳感器數據的融合。

被應用在飛行器姿態角上的數據融合算法有互補濾波[1～3]、共軛梯度法濾波[4]、擴展卡爾曼濾波[5～7](EKF)和無跡卡爾曼濾波[8～11](UKF)等。互補濾波原理簡單，能夠濾去噪聲和抑制漂移，由于不容易確定合適的高通和低通截止頻率，因此精度不高；文獻[4]介紹了共軛梯度法濾波和互補濾波相結合在姿態解算中的應用，存在迭代計算量和精度的平衡問題；EKF和UKF是卡爾曼濾波對非線性系統的濾波方法，EKF存在線性化誤差和截斷誤差，對一般的非線性系統影響不大；UKF在收斂速度和估計精度中要高于EKF，但其計算量要比EKF大，并且它們的系統噪聲和觀測噪聲不容易確定，解算的姿態角容易受到噪聲干擾。

本文提出的互補濾波與卡爾曼濾波相結合的融合算法，首先利用互補濾波把加速度計、電子羅盤和陀螺儀分別

解算的姿態角進行初步融合，濾去傳感器噪聲并抑制姿態角漂移，然后用互補濾波后的姿態角建立系統的狀態方程和觀測方程模型。因為模型的非線性特性，采用擴展卡爾曼濾波來提高姿態角的解算精度。

1 常規姿態解算方法

由于傳感器不能直接測量飛行器的姿態角，姿態角需要捷聯IMU測量的數據解算而來，姿態角既可由陀螺儀單獨解算，也可以由加速度計、電子羅盤聯合解算。

1.1 姿態描述

(1)

1.2 基于加速度計和電子羅盤的聯合解算方法

加速度計和電子羅盤相互結合可以計算出三個姿態角，易知加速度計在導航坐標系n系下的矢量輸出為[0 0 -g]T，由于

(2)

得到俯仰角和橫滾角分別為

(3)

φ=tan-1(-ayb,-azb),φ∈(-π,π)

(4)

式中 [axbaybazb]T為加速度計在載體坐標系下的測量值。

偏航角需要借助上面計算出的俯仰角和橫滾角來進一步計算得出。若電子羅盤在載體坐標系下的測量值為[mxb,myb,mzb]T，不考慮磁偏角影響，即將磁北作為真北，由

(5)

則地磁場在偏航角ψ、俯仰角θ和橫滾角φ為0°的載體坐標系下的水平分量為

mx=mxbcosθ+mybsinθsinφ+mzbsinθcosφ

my=mybcosφ-mabsinφ

(6)

由于偏航角ψ北偏東為正，北偏西為負，所以,可以得到偏航角的解

ψ=-tan-1(my,mx),ψ∈(-π,π)

(7)

1.3 基于陀螺儀的歐拉角法

(8)

圖1 補償結構圖

2 互補濾波和卡爾曼濾波的融合

加速度計和電子羅盤動態響應速度較慢，它們配合計算出的姿態角不能真實反映飛行器的姿態；陀螺儀解算的姿態角短時精度高，長期由于漂移和積分運算，計算的姿態角會產生累積誤差。因此,需要將這兩種姿態角進行融合得到精確的姿態角。

2.1 互補濾波

互補濾波就是用加速度計和電子羅盤解算的姿態角對陀螺儀解算的姿態角進行修正[3]，基本原理結構如圖2，公式為式(9)

圖2 互補濾波結構框圖

(9)

式中 [φgθgψg]和[φamθamψam]分別為陀螺儀和加速度計、電子羅盤解算的姿態角，[φθψ]為融合后的姿態角。

k值的大小是基于合力加速度與重力加速度比值大小而定。比值過大或過小，說明加速度計計算的姿態角誤差大，取消對陀螺儀計算的姿態角進行修正。比值處于規定區間，說明計算的姿態角是可靠的，可以修正

(10)

2.2 卡爾曼濾波

卡爾曼濾波算法是一種線性最小方差估計，是一種離散線性濾波的遞推算法，它被應用在飛行器的姿態融合中，具有很高的姿態解算精度。飛行器中的卡爾曼濾波是利用傳統姿態解算方法建立濾波模型，然后通過預測更新過程得到下一時刻的姿態角。

2.2.1 卡爾曼濾波狀態方程和觀測方程

為了實現卡爾曼濾波，應首先建立系統的狀態方程和

觀測方程。一般可選取三個姿態角參數作為狀態空間模型的狀態變量Xk=[φkθkψk]T，若系統的計算步距為T，由歐拉角法一階離散更新方程得到卡爾曼濾波狀態方程[13]為

Xk=f(Xk-1)+wk-1=

(11)

式中 [wφ(k-1)wθ(k-1)wψ(k-1)]T為系統噪聲向量。

選取加速度計、電子羅盤聯合解算的姿態角作為觀測量Zk=[φamkθamkψamk]T，觀測方程為

(12)

由于飛行器姿態濾波模型中的狀態方程是非線性的，卡爾曼濾波基本方程不能直接解決非線性系統的濾波問題，可選擇EKF解決系統的非線性。EKF是將模型的狀態方程的非線性部分泰勒級數展開并取一階近似，得到近似的線性化模型，然后再應用卡爾曼線性濾波。狀態方程線性化求得的雅可比矩陣為

(13)

2.2.2 卡爾曼濾波算法流程

EKF的預測更新過程[6]如框圖3。

圖3 卡爾曼濾波流程圖

1)狀態一步預測

(14)

2)狀態一步預測協方差矩陣

(15)

式中 P(k/k-1)為狀態協方差矩陣P(k-1) 的估計，Q為系統噪聲協方差矩陣。

3)卡爾曼增益更新

(16)

式中R為觀測噪聲協方差矩陣。

4)狀態更新

(17)

5)狀態協方差矩陣更新

P(k)=(I-K(k)Hk)P(k/k-1)

(18)

2.3 互補濾波和卡爾曼濾波的融合

觀測量的選擇對卡爾曼濾波很重要，選擇觀測量的誤差大小影響卡爾曼濾波的效果，加速度計、電子羅盤直接解算的姿態角可以直接作為觀測量，但是機體震動和外力加速度會給加速度計解算的姿態角很大的誤差，因此，選擇互補濾波融合后的姿態角作為模型的觀測變量。兩種算法融合的結構框圖如圖4。

圖4 互補濾波和卡爾曼濾波結合的原理圖

3 實驗與結果分析

為了檢驗上述算法的可行性和有效性，選擇了集成有處理器、陀螺儀、加速度計和電子羅盤的開發板，利用處理器采集傳感器中的測量數據，通過串口將數據傳送到上位機，利用Matlab軟件用上述算法對其進行編程分析，比較濾波效果。

3.1 靜態實驗

將開發板放置在水平臺面上，俯仰角和橫滾角的理想值為0°，圖5(a),(b),(c)分別為靜態下分別用互補濾波、EKF、兩者結合得到的橫滾角，從圖中可以看出，互補濾波后的姿態角有稍微的漂移，由于不能夠準確估計觀測量的噪聲，導致EKF后的姿態角受到噪聲干擾，兩者結合濾波得到的姿態角能避免噪聲干擾和抑制漂移。

圖5 靜態下的橫滾角

3.2 動態實驗

讓開發板姿態變化時，用上述濾波算法分別濾波得到姿態角，圖6(a),(b),(c)分別給出了互補濾波、EKF和兩者相結合得到的俯仰角曲線。從圖中可以看出：互補濾波得到的姿態角曲線能夠跟蹤姿態角的變化，但有一定的超調量，EKF濾波的姿態角動態響應慢，具有明顯的滯后性，兩者結合的濾波算法即能夠濾除噪聲，使姿態曲線更加平滑，又能在姿態變化較快時能夠快速跟蹤，且精度有所提高。

圖6 動態下的俯仰角

4 結 論

經實驗測試表明：兩者結合的濾波算法對姿態角的漂移和噪聲具有明顯的抑制作用，在姿態角快速變化時能夠實現快速跟蹤，與互補濾波和卡爾曼濾波相比，能夠明顯提高姿態的解算精度。

[1] 梁延德.基于互補濾波器的四旋翼飛行器姿態解算[J].傳感器與微系統,2011,30(11):56-58,61.

[2] 陳孟元.基于四元數改進型互補濾波的MEMS姿態解算[J].電子測量與儀器學報,2015,29(9):1391-1397.

[3] 張廣玉,張洪濤,李隆球,等.四旋翼微型飛行器設計[J].哈爾濱理工大學學報,2012,17(3):110-114.

[4] 孫金秋. 基于共軛梯度法和互補濾波相結合的姿態解算算法[J]. 傳感技術學報, 2014, 27(4): 524-528.

[5] 吳友強. 基于卡爾曼濾波的四旋翼飛行器懸停控制研究[J].自動化與儀表,2013(11):1-5.

[6] 汪 芳,朱少華,雷宏杰.基于卡爾曼濾波器的數字式捷聯航姿系統算法設計[J].中國慣性技術學報,2008,16(2):208-211.

[7] 汪紹華.基于卡爾曼濾波的四旋翼飛行器姿態估計和控制算法研究[J].控制理論與應用,2013,30(9):1109-1115.

[8] 喬相偉.用四元數狀態切換無跡卡爾曼曼濾濾波器估計的飛行器姿態[J].控制理論與控制應用,2012,29(1):97-103.

[9] 朱豐超.基于低成本陀螺和傾角儀的姿態估計[J].宇航學報,2011,32(8):1728-1733.

[10] Zhao Lin,Nie Qi,Guo Qiufen.Unscented Kalman filtering for SINS attitude estimation[C]∥IEEE International Conference on Control and Automation,2007:228-232.

[11] Pan Yue.Attitude estimation of miniature unmanned helicopter using unscented Kalman filter[C]∥International Conference on Transportation Mechanical and Electrical Engineering(TMEE),2011:1548-1551.

[12] 張 欣.多旋翼無人機的姿態與導航信息融合算法研究[D].長春:中國科學院研究生院,2015:43-44,63-65.

Fused attitude estimation algorithm based on complementary filtering and Kalman filtering

ZHANG Dong, JIAO Song-ming, LIU Yan-quan

(Department of Automation,North China Electric Power University,Baoding 071003,China)

Aiming at problem of high noise,low precision of inertial measurement unit(IMU)and low precision of classical attitude solution algorithm,a fused algorithm with complementary filtering and Kalman filtering is proposed.The algorithm establish the state equation model based on differential equation of attitude angle and choose attitude angle after compensation filtering as the observation of system.Use EKF algorithm fused measured data of gyro,accelerometer and electronic compass.To verify that the algorithm is effective, use development board with inertial sensors to test in static and dynamic condition.The results of experiments show that the algorithm fused with complementary filtering and Kalman filtering can constraint drift and noise of attitude angle in static condition and track change of attitude angle quickly in dynamic condition,thus precision of attitude angle estimation is improved.

Kalman filtering; complementary filtering; attitude estimation; data fusion; inertial measurement unit(IMU)

10.13873/J.1000—9787(2017)03—0062—04

2016—04—27

V 249

A

1000—9787(2017)03—0062—04

張 棟(1991-)，男，碩士研究生，主要研究方向為微型四旋翼飛行器的姿態和位置控制。