基于四元數(shù)EKF算法的小型無人機姿態(tài)估計

宋 宇,翁新武,郭昕剛

(長春工業(yè)大學 計算機科學與工程學院,長春 130012)

?

基于四元數(shù)EKF算法的小型無人機姿態(tài)估計

宋 宇,翁新武,郭昕剛

(長春工業(yè)大學 計算機科學與工程學院,長春 130012)

針對小型無人機設計的姿態(tài)測量系統(tǒng),提出一種用于小型無人機姿態(tài)估計的四元數(shù)擴展Kalman濾波算法.該算法通過建立四元數(shù)姿態(tài)運動模型和航姿傳感器測量模型,構(gòu)建了以四元數(shù)和陀螺儀隨機漂移為狀態(tài)向量、以加速度計測量值和磁阻儀解算的航向角為觀測向量的擴展Kalman濾波器,并設計了自適應測量噪聲協(xié)方差矩陣修正法.實驗結(jié)果表明,該算法不但解決了微機電系統(tǒng)慣性器件用于載體姿態(tài)測量時精度低、易發(fā)散、易被干擾的問題,而且顯著減小了陀螺儀隨機漂移對姿態(tài)估計的影響,有效提高了姿態(tài)估計的精度.

四元數(shù)；擴展Kalman濾波；姿態(tài)估計；微機電系統(tǒng)；小型無人機

實時、穩(wěn)定、精準的姿態(tài)信息是無人機實現(xiàn)自主飛行和執(zhí)行各種任務的基礎[1].受載荷和尺寸的限制,小型無人機(small UAV)通常不適合搭載高性能、體積大、價格高的傳統(tǒng)姿態(tài)測量系統(tǒng).微機電系統(tǒng)(micro electro mechanical systems,MEMS)傳感器和嵌入式處理器由于其體積小、重量輕、功耗低等優(yōu)點,在小型無人機慣性導航上應用廣泛.但MEMS慣性器件用于姿態(tài)估計時存在如下問題：1)陀螺儀存在漂移,單獨使用陀螺儀估計無人機姿態(tài)存在積累誤差；2)加速度計受線性加速度和振動影響較大,單獨利用加速度計估計載體姿態(tài)角,會出現(xiàn)較大誤差,動態(tài)時可信度較低；3)磁阻儀易受周圍環(huán)境(如高壓線、鐵礦廠等)所產(chǎn)生的磁場干擾,從而影響航向角的輸出.由于此類誤差是隨機的,無法預先消除,因此不能單獨通過上述方法獲取準確的姿態(tài)角,必須采用合理的數(shù)據(jù)融合方法以補償各類誤差帶來的影響,才能獲得穩(wěn)定可靠的姿態(tài)信息[2-3].

描述飛行器姿態(tài)常用的方法有Euler角法、四元數(shù)法、方向余弦法和等效旋轉(zhuǎn)矢量法等.Euler角法描述簡單、無冗余參數(shù),但存在奇異值,計算量大；方向余弦法可全姿態(tài)工作,但存在9個參數(shù),求解計算量大；等效旋轉(zhuǎn)矢量法可消除計算的方向余弦矩陣或四元數(shù)中的不可交換性誤差,但存在大量參數(shù)冗余；四元數(shù)法因為計算量小、非奇異性、無超越函數(shù)和可全姿態(tài)工作等優(yōu)點得到廣泛應用.文獻[4]提出了應用于便攜式移動機器人的四元數(shù)姿態(tài)解算方法,但未考慮傳感器的測量誤差會影響姿態(tài)估計;文獻[5]將MEMS傳感器數(shù)據(jù)通過四元數(shù)互補濾波器實現(xiàn)三自由度的姿態(tài)測量,有效減小了測量誤差,提高了測量精度;文獻[6-7]采用基于四元數(shù)的Kalman濾波器(Kalman filter,KF),進一步提高了姿態(tài)測量精度.但在實際應用中,飛行器姿態(tài)測量系統(tǒng)一般都是非線性系統(tǒng),則基于系統(tǒng)線性Gauss分布的Kalman濾波器不能給出最優(yōu)解,而擴展Kalman濾波器(extended Kalman filter,EKF)利用Taylor級數(shù)展開的方法將非線性系統(tǒng)線性化,進而采用Kalman濾波的基本步驟進行狀態(tài)估計更新[8].

本文在總結(jié)、分析常見姿態(tài)測量方法的基礎上,提出一種基于四元數(shù)擴展Kalman濾波器的小型無人機姿態(tài)測量方案.通過建立四元數(shù)姿態(tài)描述模型和MARG(magnetic,angular rate,and gravity)傳感器測量模型,構(gòu)建了以四元數(shù)、陀螺儀零漂誤差為狀態(tài)向量、以加速度計和磁阻儀解算的航向角為觀測向量的擴展Kalman濾波器,為減小載體機動時和外界磁干擾對姿態(tài)測量的影響,設計了自適應的測量噪聲協(xié)方差矩陣,提高了姿態(tài)測量精度,實現(xiàn)了三自由度的姿態(tài)測量.最后通過實驗對上述算法進行了有效評估.

1 飛行器姿態(tài)運動模型

1.1參考坐標系

根據(jù)捷聯(lián)慣性導航原理[9],飛行器的姿態(tài)角就是載體坐標系b相對于導航坐標系n的方位關(guān)系.本文定義載體坐標系為OXbYbZb,該坐標系與機體固聯(lián),原點選為飛行器質(zhì)心,Xb軸沿機體橫軸指向右,Yb軸沿機體縱軸指向前,Zb軸垂直指向機體上方,且構(gòu)成右手坐標系.定義地理坐標系OXnYnZn作為導航坐標系,該坐標系的原點在飛行器的質(zhì)心,Xn,Yn和Zn軸分別從原點出發(fā)指向東、北和天向,簡稱ENU系.

1.2坐標系間的轉(zhuǎn)換

(1)

為坐標轉(zhuǎn)換矩陣,也稱為姿態(tài)矩陣.

(2)

由式(1),(2)可得載體的姿態(tài)角為

(3)

其中滾轉(zhuǎn)角φ和航向角ψ需要對主值進行相位判斷[10].

1.3四元數(shù)姿態(tài)運動模型

(4)

式中:ωx表示繞滾轉(zhuǎn)軸角速度;ωy表示繞俯仰軸角速度;ωz表示繞航向軸角速度.

采用一階Runge-Kutta法求解四元數(shù)微分方程[12],可得其離散時間模型為

其中:T表示系統(tǒng)采樣時間間隔;q(k-1)表示k-1時刻四元數(shù).設置初始四元數(shù)q(0),利用式(5)可遞推出第k時刻四元數(shù)q(k)的取值,進而更新姿態(tài)矩陣.

2 MARG傳感器誤差分析與校正

MARG傳感器由一個三軸MEMS陀螺儀、一個三軸MEMS加速度計和一個三軸MEMS磁阻儀組成,直接固聯(lián)在飛行器上,各傳感器內(nèi)部量測坐標軸相互正交,且與載體坐標系重合,因此各傳感器輸出為三維矢量,對應的輸出為角速度ωb、加速度ab、磁場向量mb.

2.1誤差分析

使用MARG傳感器求解飛行器姿態(tài)時主要存在以下幾種誤差：1)傳感器的安裝誤差和標度誤差；2)陀螺儀的漂移和加速度計的零位誤差；3)初始條件誤差,包括導航參數(shù)和姿態(tài)角的初始誤差；4)計算誤差,主要包括量化誤差、用四元數(shù)求解姿態(tài)角和濾波算法的計算誤差；5)載體機動導致的動態(tài)誤差.其中前兩項由MARG傳感器導致,而由于MEMS傳感器精度較低,其誤差還包括測量噪聲.MARG傳感器輸出誤差將影響姿態(tài)的精度,所以有必要對其進行誤差校準.

2.2誤差校正

由于飛行器在運動過程中其姿態(tài)并非一直保持水平,因此還需對磁場測量值進行傾斜補償,得到水平磁場分布,磁場的水平分量為

由式(6),解算得到航向角ψ=arctan(myn/mxn).

3 擴展Kalman濾波器的設計

3.1系統(tǒng)結(jié)構(gòu)

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 Block diagram of the system

姿態(tài)測量系統(tǒng)結(jié)構(gòu)如圖1所示.為保證系統(tǒng)輸出的姿態(tài)角可靠、穩(wěn)定,本文將陀螺儀、加速度計和磁阻儀測量值通過擴展Kalman濾波器進行多傳感器信息融合,求取姿態(tài)角、更新姿態(tài)矩陣,并設置自適應的測量噪聲協(xié)方差矩陣,提高姿態(tài)測量精度.

3.2系統(tǒng)模型

考慮非線性離散系統(tǒng)：

其中:x(k)∈n表示k時刻系統(tǒng)的狀態(tài)矢量;z(k)∈m表示k時刻系統(tǒng)的觀測矢量；f(·)表示實的n維矢量函數(shù),h(·)表示實的m維矢量函數(shù),f(·)和h(·)對其自變量都是非線性的；w(k)∈n和v(k)∈m是協(xié)方差分別為Q(k)和R(k)的過程噪聲矢量.

3.2.1 狀態(tài)方程 設系統(tǒng)狀態(tài)量為

(7)

其中:q0(k),q1(k),q2(k),q3(k)為姿態(tài)四元數(shù)；bωx(k),bωy(k),bωz(k)分別為繞滾轉(zhuǎn)軸、俯仰軸和航向軸的陀螺儀隨機漂移向量.則系統(tǒng)的狀態(tài)方程為

對f(x(k-1),k-1)求取Jacobi矩陣：

(9)

當飛行器靜止和勻速運動時,存在如下關(guān)系：

(10)

系統(tǒng)的觀測方程為

對h(x(k),k)求取Jacobi矩陣,可得系統(tǒng)量測矩陣為

3.3擴展Kalman濾波方程

根據(jù)擴展Kalman濾波理論,通過時間更新和量測更新的迭代求得系統(tǒng)狀態(tài)向量的最優(yōu)估計,系統(tǒng)的擴展Kalman濾波遞推過程如下：

1)初始估計.計算狀態(tài)初始值x(0),設定過程噪聲協(xié)方差Q(k)、量測噪聲協(xié)方差R(k)和誤差方差矩陣P(0).

2)時間更新.計算狀態(tài)轉(zhuǎn)移函數(shù)的Jacobi矩陣：

(12)

從而k時刻狀態(tài)向量估計值為

更新狀態(tài)誤差協(xié)方差陣為

3.4擴展Kalman濾波修正

ca和cm為設定的權(quán)重因子.當載體線性加速度和外界磁干擾越大,相應的協(xié)方差也越大,此時狀態(tài)增益矩陣將減小,以減緩Kalman預測校準.

4 實 驗

本文采用的慣性測量單元(IMU)包括三軸加速度計、三軸陀螺儀(MPU-6050)、三軸磁阻儀(HMC5583L)和一個高性能FPGA微處理器(EP2C35F672C6).用微處理器對MARG傳感器測量的原始數(shù)據(jù)以1 000 Hz的采樣頻率進行采樣,在未濾波的情況下將原始數(shù)據(jù)通過串口發(fā)送至PC機,在PC機上利用MATLAB設計的擴展Kalman濾波器對數(shù)據(jù)進行處理,以獲取準確的姿態(tài)信息,最后將實驗結(jié)果用曲線描繪的方式顯示,并對解算結(jié)果進行比較分析.

為驗證本文姿態(tài)解算算法的有效性,分別進行3組實驗:1)IMU置于水平桌面靜止不動,如圖2所示；2)IMU做無規(guī)律運動,且有外界磁干擾,如圖3所示；3)IMU做無規(guī)律運動,有外界磁干擾,且增加高頻振動,如圖4所示.每組實驗都設計3種姿態(tài)解算法：利用加速度計和磁阻儀測量值的姿態(tài)解算法(G-M)(圖中黑色實線)；只采用陀螺儀的四元數(shù)姿態(tài)解算法(AR-Q)(圖中藍色雙線)；以及利用四元數(shù)EKF濾波將陀螺儀、加速度計和磁阻儀測量數(shù)據(jù)融合的姿態(tài)解算法(Q-EKF)(圖中紅色虛線).通過3組實驗對比3種算法在不同情況下的性能.

圖2 靜態(tài)時仿真結(jié)果Fig.2 Simulation results at static state

圖3 動態(tài)時仿真結(jié)果Fig.3 Simulation results at movement state

圖4 振動干擾時仿真結(jié)果Fig.4 Simulation results under vibration disturbance

由圖2可見,靜態(tài)時沒有其他線性加速度及干擾磁場,G-M算法能準確獲得載體的姿態(tài)信息；AR-Q算法會產(chǎn)生姿態(tài)角發(fā)散問題；而Q-EKF算法能有效地修正陀螺儀的隨機漂移誤差,解決了角度誤差累積的問題,準確地獲取了載體的姿態(tài),且俯仰角和橫滾角都在0°附近,誤差控制在0.2°內(nèi).由圖3可見,動態(tài)時由于存在線性加速度及干擾磁場的影響,G-M算法獲得的載體姿態(tài)信息有時誤差會較大；AR-Q算法雖然不受線性加速度及干擾磁場的影響,但仍存在姿態(tài)角發(fā)散問題；而Q-EKF算法能有效減小載體線性加速度和干擾磁場的影響,且能有效地修正陀螺儀的隨機漂移誤差,動態(tài)性能較好.由圖4可見,當載體附加有高頻振動時,G-M算法得到姿態(tài)角有明顯的毛刺和突變分量,可能會導致載體的震蕩；AR-Q算法姿態(tài)角發(fā)散問題較嚴重；而Q-EKF算法有效地消除了干擾噪聲和突變分量,實現(xiàn)了姿態(tài)角的正確估計.

綜上所述,本文提出了一種適用于小型無人機姿態(tài)估計的四元數(shù)擴展Kalman濾波算法,將三軸陀螺儀、三軸加速度計和三軸磁阻儀的測量數(shù)據(jù)通過擴展Kalman濾波器進行信息融合,修正了陀螺儀的隨機漂移誤差,解決了角度誤差累積問題,能夠準確獲取載體的姿態(tài).并設計了自適應量測噪聲協(xié)方差矩陣修正法,能有效抑制線性加速度和干擾磁場對姿態(tài)估計的影響,提高了姿態(tài)測量精度.實驗表明,本文提出的算法在靜態(tài)、動態(tài)時都能提供實時、穩(wěn)定、精準的姿態(tài)信息.

[1] 房建成,張霄.小型無人機自動駕駛儀技術(shù) [J].中國慣性技術(shù)學報,2007,15(6):658-663.(FANG Jiancheng,ZHANG Xiao.Development of Autopilot Technology of Small UAV [J].Journal of Chinese Inertial Technology,2007,15(6):658-663.)

[2] Tayebi A,McGilvray S,Roberts A,et al.Attitude Estimation and Stabilization of a Rigid Body Using Low-Cost Sensors [C]//Proceeding of the 46th IEEE Conference on Decision and Control.Berlin:Springer,2007:6424-6429.

[3] 秦勇,臧希喆,王曉宇,等.基于MEMS慣性傳感器的機器人姿態(tài)檢測系統(tǒng)的研究 [J].傳感技術(shù)學報,2007,20(2):298-301.(QIN Yong,ZANG Xizhe,WANG Xiaoyu,et al.Research of Attitude Estimation System Using MEMS Inertial Sensors [J].Chinese Journal of Sensors and Actuators,2007,20(2):298-301.)

[4] 張思奇,趙憶文.便攜式移動機器人姿態(tài)解算方法 [J].微計算機信息,2007,23(10-2):188-190.(ZHANG Siqi,ZHAO Yiwen.Attitude Algorithm of Portable Mobile Robot [J].Microcomputer Information,2007,23(10-2):188-190.)

[5] YUN Xiaoping,Lizarraga M,Bachmann E,et al.An Improved Quaternion-Based Kalman Filter for Real-Time Tracking of Rigid Body Orientation [C]//Proceedings of the IEEE International Conference on Intelligent Robots and Systems.Piscataway,NJ:IEEE,2003:1074-1079.

[6] Harms H,Amft O,Winkler R,et al.ETHOS:Miniature Orientation Sensor for Wearable Human Motion Analysis [C]//Proceedings of IEEE Sensors Conference.Piscataway,NJ:IEEE,2010:1037-1042.

[7] Yun X,Bachmann E,McGhee R.A Simplified Quaternion Based Algorithm for Orientation Estimation from Earth Gravity and Magnetic Field Measurements [J].IEEE Trans Instrum Meas,2008,57(3):638-650.

[8] 王松,田波,戰(zhàn)榆莉,等.基于修正EKF的微小型飛行器姿態(tài)估計 [J].高技術(shù)通訊,2011,21(6):612-618.(WANG Song,TIAN Bo,ZHAN Yuli,et al.Flight Attitude Estimation for MAVs Based on Amended EKF [J].Chinese High Technology Leters,2011,21(6):612-618.)

[9] 陳哲.捷聯(lián)慣導系統(tǒng)原理 [M].北京:宇航出版社,1996:82-85.(CHEN Zhe.The Principle of Strap Down Inertial Navigation System [M].Beijing:Astronautics Press,1996:82-85.)

[10] 秦永元.慣性導航 [M].北京:科學出版社,2006:297.(QIN Yongyuan.Inertial Navigation [M].Beijing:Science Press,2006:297.)

[11] Oshman Y,Carmi A.Attitude Estimation from Vector Observations Using Genetic Algorithm-Embedded Quaternion Particle Filter [J].Journal of Guidance,Control,and Dynamics,2006,29(4):879-891.

[12] 王鵬,呂顯瑞,張伸煦.求解隨機微分方程的三級半隱式隨機龍格庫塔方法 [J].吉林大學學報:理學版,2008,46(2):219-223.(WANG Peng,Lü Xianrui,ZHANG Shenxu.Three-Stage Semi-implicit Stochastic Runge-Kutta Methods for Stochastic Differential Equations [J].Journal of Jilin University:Science Edition,2008,46(2):219-223.)

[13] 劉興川,張盛,李麗哲,等.基于四元數(shù)的MARG傳感器姿態(tài)測量算法 [J].清華大學學報:自然科學版,2012,52(5):627-631.(LIU Xingchuan,ZHANG Sheng,LI Lizhe,et al.Quaternion-Based Algorithm for Orientation Estimation from MARG Sensors [J].Tsinghua Univ:Sci &Tech,2012,52(5):627-631.)

[14] Suh Y S.Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter with Adaptive Estimation of External Acceleration [J].IEEE Trans Instrumentation and Measurement,2010,59(12):3296-3305.

[15] Bird J,Arden D.Indoor Navigation with Foot-Mounted Strap Down Inertial Navigation and Magnetic Sensors [J].IEEE Wireless Communcation,2011,18(2):28-35.

(責任編輯：韓 嘯)

SmallUAVAttitudeEstimationBasedontheAlgorithmofQuaternionExtendedKalmanFilter

SONG Yu,WENG Xinwu,GUO Xingang

(CollegeofComputerScienceandEngineering,ChangchunUniversityofTechnology,Changchun130012,China)

A new algorithm of quaternion extended Kalman filter was adopted to estimate the attitude of small UAV.After the establishment of the models of quaternion attitude movement and magnetic,angular rate,and gravity sensor measurement,a new designed extended Kalman filter was given with the quaternion and random drifting of gyro being the static vectors and the accelerometer measurements and the heading solved by the magnetometer being the observational vectors.What’s more,an innovation amendment method based on an adaptive approach to construct the measurement noise covariance matrix was designed.Experimental results show that the algorithm not only solves the problems of micro electro mechanical systems inertial sensors used for attitude measurement to show low accuracy and to be easy to diverge and to disturb,but also significantly reduces the effect of random drifting of gyro scope on attitude estimation,and proves to be effective at improving the accuracy of attitude estimation.

quaternion;extended Kalman filter;attitude estimation;micro electro mechanical systems (MEMS);small UAV

10.13413/j.cnki.jdxblxb.2015.03.31

2014-09-17.

宋 宇(1969—),男,漢族,碩士,副教授,從事嵌入式系統(tǒng)設計的研究,E-mail：songyu@mail.ccut.edu.cn.

教育部“春暉計劃”項目(批準號:Z2011139).

TP39

：A

：1671-5489(2015)03-0511-08