侯江寬,馬 珺*,賈華宇

(1.太原理工大學(xué)物理與光電工程學(xué)院,山西 晉中 030600;2.太原理工大學(xué)信息工程學(xué)院,山西 晉中 030600)

項目來源:山西省自然科學(xué)基金項目(2015011050)

2017-02-28修改日期2017-06-18

改進(jìn)型自適應(yīng)無跡卡爾曼姿態(tài)算法*

侯江寬1,馬 珺1*,賈華宇2

(1.太原理工大學(xué)物理與光電工程學(xué)院,山西 晉中 030600;2.太原理工大學(xué)信息工程學(xué)院,山西 晉中 030600)

提出了一種改進(jìn)型自適應(yīng)無跡卡爾曼濾波姿態(tài)算法,能夠有效的解決MEMS陀螺儀的漂移和噪聲問題,同時減小運動加速度對加速度計的影響。將改進(jìn)的自適應(yīng)Sage-Husa算法與無跡卡爾曼濾波器相結(jié)合,使量測噪聲統(tǒng)計特性在線更新,提高系統(tǒng)的抗干擾能力,避免擴(kuò)展卡爾曼濾波的線性化誤差,可以得到精確的全姿態(tài)角。每次迭代只更新3個歐拉角,提高了系統(tǒng)的解算速度。飛行實驗和分析表明:改進(jìn)算法能夠有效的提高姿態(tài)解算精度,收斂速度快,自適應(yīng)能力強(qiáng),穩(wěn)定可靠,具有較強(qiáng)的魯棒性,在干擾消失時能夠快速得到準(zhǔn)確姿態(tài)角。

傳感器;姿態(tài)解算;無跡卡爾曼濾波器;自適應(yīng)Sage-Husa算法;多旋翼飛行器

獲取準(zhǔn)確的姿態(tài)是無人飛行器實現(xiàn)自主飛行的前提條件[1],由于MEMS(Micro Electro Mechanical Systems)傳感器的高集成度、低成本,在微型飛行器上大量使用。多旋翼飛行器上的慣性測量單元主要由MEMS三軸陀螺儀、三軸加速度計、三軸磁強(qiáng)計組成。但是MEMS陀螺儀存在漂移問題,會導(dǎo)致累積誤差,因此需要對陀螺儀輸出的數(shù)據(jù)進(jìn)行有效的處理才能獲得穩(wěn)定可靠的姿態(tài)信息。

目前常用的姿態(tài)解算方法有Robert Mahony的互補(bǔ)濾波姿態(tài)解算算法[2]及其改進(jìn)算法[3]、Sebastian Madgwick提出的基于梯度下降法的姿態(tài)解算算法[7]、基于卡爾曼濾波器的姿態(tài)解算算法[8]以及粒子濾波姿態(tài)解算算法[9]?；パa(bǔ)濾波算法是比較實用的一種算法,適用于對姿態(tài)精確度要求不高的小型飛行器?；パa(bǔ)濾波姿態(tài)解算的改進(jìn)主要集中于濾波器系數(shù)的自適應(yīng)調(diào)節(jié)[5],以提高算法對于噪聲的補(bǔ)償,這種自適應(yīng)調(diào)節(jié)主要是結(jié)合了模糊算法,精度并不高。Madgwick將互補(bǔ)濾波的狀態(tài)更新方程由DCM(Direction Cosine Matrix)矩陣變?yōu)樗脑獢?shù)微分方程,大大提高了互補(bǔ)濾波算法的解算速度。Kalman濾波是目前較為適合多傳感器姿態(tài)融合的算法,但常規(guī)Kalman濾波算法需要系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性已知[14]。然而在多旋翼飛行器的多傳感器融合中,系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性往往是未知的且時變的,這會使常規(guī)Kalman濾波估計精度降低,失去算法最優(yōu)性,甚至導(dǎo)致濾波發(fā)散。擴(kuò)展卡爾曼濾波姿態(tài)解算算法是對非線性函數(shù)進(jìn)行線性化,但是在線性化過程中必須求解非線性函數(shù)的Jacobi矩陣,這給嵌入式微處理器帶來了大量的計算負(fù)載,降低了系統(tǒng)的實時性,尤其對于復(fù)雜模型的系統(tǒng),比較復(fù)雜且容易出錯。另外,擴(kuò)展卡爾曼濾波算法在系統(tǒng)達(dá)到穩(wěn)定狀態(tài)時,會喪失對于系統(tǒng)突變的跟蹤能力,將導(dǎo)致狀態(tài)估計不準(zhǔn)甚至發(fā)散[11]。針對擴(kuò)展卡爾曼濾波算法存在的問題,很多文獻(xiàn)引入了高斯QEKF算法、高斯IEKF算法、強(qiáng)跟蹤EKF算法[11]以及各種改進(jìn)算法[12],但是這些算法都是次優(yōu)濾波。

本文將自適應(yīng)算法和無跡卡爾曼濾波相結(jié)合,使用確定的Sigma點集來描述非線性函數(shù)的統(tǒng)計特性。Sigma點集經(jīng)過非線性函數(shù)傳播之后,后驗均值和方差可以達(dá)到2階精度而擴(kuò)展卡爾曼濾波算法只能達(dá)到1階精度。不像擴(kuò)展卡爾曼濾波算法對于非線性函數(shù)的近似,無跡卡爾曼濾波是對狀態(tài)隨機(jī)變量的近似,使用了真實非線性模型,由于不需要像二次濾波那樣計算Hession和Jacobian矩陣,提高了解算速度。由于加速度計和羅盤良好的低頻特性,使用加速度以及羅盤得到的姿態(tài)角作為觀測量來修正陀螺儀得到姿態(tài)角,可以有效抑制陀螺儀漂移和量化誤差。得益于自適應(yīng)無跡卡爾曼濾波器響應(yīng)速度快的特點,在導(dǎo)航坐標(biāo)系補(bǔ)償運動加速度對加速度計影響的前提,該算法可以可以進(jìn)一步減少運動加速度對于加速度計的影響。另外該算法可以在線實時更新系統(tǒng)量測噪聲,所以有更強(qiáng)的魯棒性和更高的解算精度。

1 非線性姿態(tài)解算系統(tǒng)建模

本文使用的是NED(北、東、地)右手坐標(biāo)系。姿態(tài)解算系統(tǒng)離散時間狀態(tài)空間模型如式(1)所示:

(1)

DCM矩陣微分方程由于需要求解9個變量,且涉及大量三角運算,會增加處理器運算負(fù)載。使用歐拉角微分方程更新姿態(tài)角不需要重正交化,可以簡化運算過程。因此我們使用歐拉角法來預(yù)測3個姿態(tài)角,非線性函數(shù)f(Xk-1)可以寫為式(2):

(2)

式中:w=[wxwywz]T為三軸陀螺儀得到的機(jī)體角速度。當(dāng)?shù)仁礁┭鼋菫?0°時方程具有奇異性,橫滾角與俯仰角變得不確定。在實際工程中,通常將俯仰角限制在一定的范圍以防止方程退化,如式(3)所示:

(3)

觀測方程的量測向量Z可以由MEMS三軸加速度計和磁強(qiáng)計直接計算。式(1)中的觀測方程可以寫為式(4):

(4)

由式(5)描述的坐標(biāo)系變換矩陣以及式(6)可以得到由加速度描述的橫滾角和俯仰角,如式(7)所示:

(5)

(6)

(7)

式中:Ab=[axayaz]T表示三軸加速度計的輸出值,An=[0 0g]T為重力加速度在參考坐標(biāo)系下的向量表示。

偏航角可以由三軸磁強(qiáng)計獲得,如式(8)所示:

Zψ=arctan(mx/my)+Δψ

(8)

式中:Mb=[mxmymz]T為磁強(qiáng)計在x軸、y軸和z軸的輸出值,Δψ為真北與磁北的夾角。機(jī)體在轉(zhuǎn)動過程中由于橫滾角和俯仰角的變化,每次姿態(tài)解算之后需要對mx和my進(jìn)行修正,如式(9)所示:

(9)

2 改進(jìn)自適應(yīng)無跡卡爾曼濾波算法

由上一部分得到的狀態(tài)空間方程,我們可以利用加速度計和磁強(qiáng)計得到的橫滾角、俯仰角以及偏航角來修正由陀螺儀更新后的姿態(tài)角。無跡卡爾曼濾波融合算法可以利用加速度計提供的準(zhǔn)確的靜態(tài)角度補(bǔ)償陀螺儀的橫滾角和俯仰角的漂移誤差,通過引入自適應(yīng)算法在線估計測量噪聲,可以有效降低對于加速度計震動噪聲的影響。下面介紹了完整的改進(jìn)型無跡卡爾曼濾波算法的解算步驟。

①計算Sigma點集,由于狀態(tài)變量為三維,所以取7個Sigma點。

(10)

②計算Sigma點集的權(quán)值

(11)

式中:下標(biāo)m表示均值,c表示協(xié)方差,上標(biāo)表示第幾個采樣點。參數(shù)λ=a2(n+κ)-n是縮放比例參數(shù),用于降低總的預(yù)測誤差。a的選取控制了采樣點的分布狀態(tài),κ為另外一個縮放比例參數(shù),用于保證矩陣(n+λ)P為半正定矩陣。參數(shù)β≥0是一個非負(fù)的權(quán)系數(shù),可以用來合并方程中高階項的動差。

③利用式(10)、式(11)獲得Sigma點集及其對應(yīng)的權(quán)值。

(12)

④計算Sigma點集的一步預(yù)測。

(13)

⑤計算系統(tǒng)狀態(tài)量的一步預(yù)測及協(xié)方差矩陣,由Sigma點集的預(yù)測值加權(quán)求和得到,其中權(quán)值由式(11)得到。

(14)

(15)

⑥根據(jù)一步預(yù)測值,由UT變換產(chǎn)生新的Sigma點集。

(16)

⑦將⑥式預(yù)測的Sigma點集代入觀測方程,得到預(yù)測的觀測量。

(17)

⑧將⑦得到的Sigma點集的觀測預(yù)測值,通過加權(quán)求和得到系統(tǒng)預(yù)測的均值及協(xié)方差。

(18)

(19)

(20)

(21)

(22)

⑨計算卡爾曼增益矩陣

(23)

⑩計算系統(tǒng)的狀態(tài)更新和協(xié)方差更新

(24)

(25)

βk=βk-1(βk-1+b)-1,β1=I

(26)

式中:b=diag([bφbθbψ])為漸消記憶因子0

由于加速度計容易受到電機(jī)震動等的干擾。本文采用了低通FIR濾波器對加速度信號進(jìn)行了處理,可以有效的抑制震動噪聲?；跐h明窗的線性相位FIR濾波器的傳遞函數(shù)在Z域中可以用式(27)表示:

(27)

式中:N是濾波器階數(shù),hk是濾波器脈沖響應(yīng)由截止頻率決定。為了有效的獲取加速度計的截止頻率,本文使用離散傅里葉變換來計算譜振幅,如式(28)所示:

(28)

圖1 姿態(tài)解算流程圖

式中:DFT表示離散傅里葉變換,Macc(f)是頻率(f)的譜振幅,macc(n)表示加速度計的采樣序列。整個姿態(tài)解算流程如圖1所示。

3 實驗與分析

為了驗證該姿態(tài)解算算法的有效性,選取了商用姿態(tài)板對算法進(jìn)行了驗證。該姿態(tài)板集成了MPU-6050芯片包括MEMS三軸加速度計和MEMS三軸角速度計,以及MEMS三軸磁強(qiáng)計HMC5883L芯片和一個氣壓高度計BPM180。姿態(tài)板使用了STM32F103T8作為處理核心,該處理器基于ARM CortexTM-M3內(nèi)核,通過板子上的8M晶體和PLL,STM32可以運行在最高72M時鐘。

實驗使用本文提出改進(jìn)型自適應(yīng)無跡卡爾曼濾波算法(簡記為:IAUKF)對姿態(tài)板采集的數(shù)據(jù)進(jìn)行了姿態(tài)解算并與擴(kuò)展卡爾曼濾波算法(EKF)以及Xsens 公司的MTi標(biāo)準(zhǔn)姿態(tài)曲線進(jìn)行了對比。姿態(tài)板采集的9軸數(shù)據(jù)如圖2所示。3種方法解算得到的橫滾角、俯仰角以及偏航角分別如圖3所示。

圖2 傳感器采集信號

從圖3中可以看出,改進(jìn)型自適應(yīng)無跡卡爾曼濾波算法相比擴(kuò)展卡爾曼濾波啊算具有更高的解算精度。為了進(jìn)一步驗證算法的數(shù)據(jù)融合效果,使用3D Optical Measurement系統(tǒng)對本文提出的算法(IAUKF)的靜態(tài)(w<5°/s)和動態(tài)(w≥5°/s)RMS(Root-Mean-Square)誤差進(jìn)行了檢測,實驗結(jié)果如表1所示。

圖3 IAUKF與EKF以及MTi解算效果對比

表1 靜態(tài)和動態(tài)RMS誤差

加速度計主要受到電機(jī)震動的影響,可以視為白噪聲,為了驗證觀測噪聲的在線更新效果,加入了白噪聲作為干擾項,圖4對比了自適應(yīng)濾波前后的橫滾角以及俯仰角姿態(tài)解算效果。

圖4中可以看到,無跡卡爾曼濾波算法和改進(jìn)型自適應(yīng)濾波相結(jié)合以后在噪聲擾動下更為穩(wěn)定,具有更好的魯棒性。為了驗證本文提出的算法(IAUFK)的收斂速度將初始姿態(tài)與目標(biāo)姿態(tài)設(shè)定偏差并與互補(bǔ)濾波(CF)以及擴(kuò)展卡爾曼濾波(EKF)做了對比試驗,如圖5所示。

圖4 噪聲擾動下橫滾角、俯仰角有無自適應(yīng)對比

圖5 3種算法的收斂性對比

從圖中可以看到,擴(kuò)展卡爾曼濾波算法收斂速度快于互補(bǔ)濾波算法,而本文提出的算法收斂速度比擴(kuò)展卡爾曼濾波算法更快。圖6展示了互補(bǔ)濾波算法、擴(kuò)展卡爾曼濾波算法以及本文提出的算法的解算時間。

圖6 3種算法運行1 000次每次需要的時間

從圖6中可以看出,傳統(tǒng)的互補(bǔ)濾波算法每次解算需要的時間最短,本文提出的算法和擴(kuò)展卡爾曼濾波算法需要的時間近似。為了測試IAUKF算法對于陀螺儀漂移的補(bǔ)償效果,將姿態(tài)板初始姿態(tài)置為零態(tài),然后任意旋轉(zhuǎn)姿態(tài)板,每隔3 min記錄一次零態(tài)姿態(tài)角,補(bǔ)償前后橫滾角、俯仰角對比如圖7所示。

從圖7可以看出,在算法補(bǔ)償前陀螺儀由于存在常值漂移、速率隨機(jī)游走、角度隨機(jī)游走以及各種噪聲的影響,漂移嚴(yán)重。本文提出的算法可以有效的補(bǔ)償陀螺漂移。

磁強(qiáng)計容易受到外部磁場干擾,偏航角誤差會隨著外部磁場變化,數(shù)據(jù)融合算法會對干擾起到一定的補(bǔ)償作用,但是偏差依然存在。實驗中加入一定強(qiáng)度外部磁場,偏航角產(chǎn)生一定誤差,當(dāng)外部磁場消失以后偏航角迅速得到修正,如圖8所示。

圖7 算法補(bǔ)償前后橫滾角、俯仰角

圖8 磁強(qiáng)計收到干擾時的偏航角

4 結(jié)束語

本文提出的改進(jìn)型自適應(yīng)無跡卡爾曼濾波算法可以有效的補(bǔ)償?shù)统杀綧EMS傳感器存在的姿態(tài)漂移以及噪聲影響,可以有效提高姿態(tài)解算精度。該算法將改進(jìn)型自適應(yīng)Sage-Husa算法與無跡卡爾曼濾波算法相結(jié)合,可以在線更新狀態(tài)方程的量測誤差,同時避免了使用擴(kuò)展卡爾曼濾波算法帶來的高階截斷誤差,然后使用加速度計以及磁強(qiáng)計作為量測信息校準(zhǔn)陀螺儀解算歐拉角的誤差,提高了基于慣性測量單元解算的準(zhǔn)確性與可靠性。試驗與仿真分析表明,該算法收斂速度快、解算速度與擴(kuò)展卡爾曼濾波算法相當(dāng)?shù)强梢蕴峁└叩淖藨B(tài)解算精度,靜態(tài)RMS誤差小于0.75°,動態(tài)RMS誤差小于1.65°,可以滿足對于高精度姿態(tài)控制的要求。但是,相對于傳統(tǒng)互補(bǔ)濾波算法,該算法在解算速度上存在較大差距,接下來實驗室將集中精力對于算法進(jìn)行優(yōu)化以提高算法的數(shù)據(jù)融合速度。

[1] 李迪,陳向堅,續(xù)志軍. 增益自適應(yīng)滑模控制器在微型飛行器飛行姿態(tài)控制中的應(yīng)用[J]. 光學(xué)精密工程,2013,21(5):1183-1191.

[2] Mahony R,Hamel T,Pflimlin J M. Complementary Filter Design on the Special Orthogonal Group SO(3)[C]//Decision and Control,2005 and 2005 European Control Conference. Cdc-Ecc’05. IEEE Conference on,2006:1477-1484.

[3] Yanlin Y U,Jiakui Z U,Zeng G. The Research on Attitude Determination Algorithm Based on Complementary Filter for Unmanned Helicopter[J]. Helicopter Technique,2014.

[4] Wu J,Zhou Z,Chen J,et al. Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors[J]. IEEE Sensors Journal,2016,16(18):1-1.

[5] 王立,章政,孫平. 一種自適應(yīng)互補(bǔ)濾波姿態(tài)估計算法[J]. 控制工程,2015(5):881-886.

[6] 孫金秋,游有鵬,傅忠云. 基于共軛梯度法和互補(bǔ)濾波相結(jié)合的姿態(tài)解算算法[J]. 傳感技術(shù)學(xué)報,2014(4):524-528.

[7] Madgwick S O,Harrison A J,Vaidyanathan A. Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm.[C]//IEEE International Conference on Rehabilitation Robotics. IEEE Int Conf Rehabil Robot,2011:1-7.

[8] Zhang S,Yu S,Liu C,et al. A Dual-Linear Kalman Filter for Real-Time Orientation Determination System Using Low-Cost MEMS Sensors.[J]. Sensors,2016,16(2):264.

[9] Wang Y,Wai D,Tomizuka M. Steady-State Marginalized Particle Filter for Attitude Estimation[C]//ASME 2014 Dynamic Systems and Control Conference. American Society of Mechanical Engineers,2014:V002T25A002-V002T25A002.

[10] Wei G,Li J. Adaptive Kalman Filtering for the Integrated SINS/DVL System[J]. Journal of Computational Information Systems,2013,16(9):6443-6450.

[11] 宋康寧,叢爽,鄧科,等. 自適應(yīng)強(qiáng)跟蹤卡爾曼濾波在陀螺穩(wěn)定平臺中的應(yīng)用[J]. 中國科學(xué)技術(shù)大學(xué)學(xué)報,2015(1):17-22.

[12] Zou Bo,Zhang Hua,Jiang Jun. 多傳感信息融合的改進(jìn)擴(kuò)展卡爾曼濾波定姿[J]. 計算機(jī)應(yīng)用研究,2014,31(4):1035-1038.

[13] 王丁偉,祖家奎,黃海. 基于MAEKF算法的航姿參考系統(tǒng)設(shè)計[J]. 傳感技術(shù)學(xué)報,2017,30(2):319-325.

[14] Hajiyev C,Soken H E. Robust Adaptive Kalman Filter for Estimation of UAV Dynamics in the Presence of Sensor/Actuator Faults[J]. Aerospace Science and Technology,2013,28(1):376-383.

[15] Makni A,Fourati H,Kibangou A Y. Adaptive Kalman Filter for MEMS-IMU Based Attitude Estimation Under External Acceleration and Parsimonious use of Gyroscopes[M]. 2014.

[16] 嚴(yán)恭敏,李四海,秦永元. 慣性儀器測試與數(shù)據(jù)分析[M]. 國防工業(yè)出版社,2012.

AnImprovedAdaptiveUnscentedKalmanFilterforAttitudeDetermination*

HOUJiangkuan1,MAJun1*,JIAHuayu2

(1.College of Physics and Optoelectronics,Taiyuan University of Science and Technology,Jinzhong Shanxi 030600,China; 2.College of Information Engineering,Taiyuan University of Science and Technology,Jinzhong Shanxi 030600,China)

An improved adaptive unscented Kalman filter for attitude determination is proposed,which can effectively solve the drift and noise problem of gyroscope and reduce the influence of motion acceleration on the accelerometer. Combining the improved adaptive Sage-Husa algorithm with the unscented Kalman filter,the statistic characteristic of the measurement noise is updated on-line,the anti-jamming capability of the system is improved,and the linearization error of the extended Kalman filter can be avoided. And we can get the precise Attitude angle. Only three Euler angles are updated each iteration which can improve the solution speed of the system. The flight experiment and analysis show that the improved algorithm can effectively improve the accuracy of attitude determination,improve convergence speed,strong adaptability,robustness and stability,and can get the accurate attitude angle quickly when the interference disappears.

sensor;attitude estimation;unscented Kalman filter;adaptive Sage-Husa algorithm;multi-rotor aircraft

TP27

A

1004-1699(2017)10-1518-07

10.3969/j.issn.1004-1699.2017.10.011

侯江寬(1991-),男,漢族,河北省石家莊市,碩士研究生,主要研究方向為導(dǎo)航與控制、智能控制,houjiangkuan0126@163.com;

馬珺(1980-),女,漢族,山西省太原市人,博士研究生,副教授,碩士研究生導(dǎo)師,主要研究方向為新型傳感器技術(shù)、智能控制理論,zymajun@126.com;

賈華宇(1977-),男,漢族,山西臨汾人,博士研究生,副教授,碩士研究生導(dǎo)師,主要研究方向為數(shù)?；旌霞呻娐吩O(shè)計。