0 引言

    近年來，以MEMS(Micro-Electro-Mechanical System)陀螺儀,、加速度計,、磁強(qiáng)計為組合的姿態(tài)測量系統(tǒng)在車輛自主駕駛、微型飛行器,、人體運動分析等領(lǐng)域得到廣泛應(yīng)用^[1-3],。低成本MEMS器件由于漂移及高噪聲的特點，需要組合傳感器通過姿態(tài)解算得到穩(wěn)定的姿態(tài)信息^[4],。而當(dāng)存在長時間的機(jī)動加速度和磁場干擾時,，無法獲得準(zhǔn)確的姿態(tài)信息。因此,，采用合理的姿態(tài)描述模型及特定的解算方法實現(xiàn)信息融合一直是個棘手的問題,。 

    針對以上問題，國內(nèi)外學(xué)者提出了一些改進(jìn)算法^[4-8]。文獻(xiàn)[4]提出修正卡爾曼濾波算法,，由于無法補(bǔ)償外部干擾，估計精度下降明顯,；文獻(xiàn)[5]采用擴(kuò)展卡爾曼濾波融合方法,，由于磁力計耦合干擾而影響傾角精度，另外EKF存在高階項忽略帶來的誤差問題,，且維數(shù)高,、計算復(fù)雜；文獻(xiàn)[6]采用殘差的思想識別機(jī)動加速度,，但是沒有考慮偏航角,；文獻(xiàn)[7]采用旋轉(zhuǎn)矩陣EKF，該算法實現(xiàn)了機(jī)動加速度補(bǔ)償和降低了算法復(fù)雜度,，但是無法實現(xiàn)全姿態(tài)估計,。

    為解決在機(jī)動加速度干擾下精度下降及耦合干擾問題，本文通過狀態(tài)反饋估計機(jī)動加速度,，實現(xiàn)對加速度補(bǔ)償,，并利用模糊規(guī)則動態(tài)調(diào)整協(xié)方差陣，以降低機(jī)動加速度干擾,，同時串級結(jié)構(gòu)消除了磁場信息對傾角的影響,，最后設(shè)計實驗驗證本文方法的有效性。

1 姿態(tài)描述與傳感器模型

1.1 坐標(biāo)系和旋轉(zhuǎn)矩陣

    本文按照Z-Y-X旋轉(zhuǎn)方向從載體坐標(biāo)系(B系)旋轉(zhuǎn)到大地坐標(biāo)系(E系,，定義X/Y/Z正軸分別指向北/東/天)得到旋轉(zhuǎn)矩陣為：

式中,，c和s分別代表cos和sin，Ψ,、θ,、φ表示偏航角、俯仰角及翻滾角,。φ,、θ和Ψ的獲取表達(dá)如下：

1.2 傳感器模型

    MEMS傳感器輸出模型：

式中，ω為陀螺儀理想角速率輸出,，b_a,、b_g為加速度和陀螺儀零偏，G=(0,，0,，g)^T為重力向量，g取9.8 m/s²,，a為機(jī)動加速度,，h為磁場參考向量，e_g、e_a,、e_m分別是陀螺儀,、加速度計及磁強(qiáng)計互不相關(guān)的高斯隨機(jī)白噪聲。

2 串級LKF濾波設(shè)計

    本文主要利用旋轉(zhuǎn)矩陣中第三行向量與傾斜角相關(guān)而與偏航角無關(guān)的特性,，采取分層設(shè)計兩個線性卡爾曼濾波器,，以逐級獲得傾斜角和偏航角的最優(yōu)估計。

2.1 傾角卡爾曼濾波

式中Δt為采樣時間,。狀態(tài)在遞推過程中會出現(xiàn)發(fā)散情況,，必須通過加速度數(shù)據(jù)進(jìn)行校正。然而在機(jī)動條件下加速度受到機(jī)動加速度的很大干擾,，必須對機(jī)動加速度進(jìn)行估計,，以提升動態(tài)條件下的估計精度。

    當(dāng)載體朝著一定方向運動時,，不存在突變轉(zhuǎn)動條件下機(jī)動加速度a在相鄰短暫時刻之間不能突變,，所以本文采用文獻(xiàn)[7]的一階次低通連續(xù)過程模型。加速度模型如圖1所示,。上一時刻機(jī)動加速度的后驗估計a⁺(k-1)可根據(jù)上時刻量測加速度和最優(yōu)理論加速度之差獲得,，即：a⁺(k-1)=y_a(k-1)-b_a-gx₁(k-1)。根據(jù)加速度模型可得下一時刻的先驗估計：

其中,，a⁺(k-1)為上時刻后驗估計,，a^-(k)為本時刻先驗估計，c_a為量綱,。

    由式(3)的第二項方程得到觀測方程：

    對角陣R_acc往往用于克服不存在突變轉(zhuǎn)動曲線運動導(dǎo)致的加速度干擾,。當(dāng)存在突變轉(zhuǎn)動加速度的情況下，對協(xié)方差陣采用模糊規(guī)則進(jìn)行調(diào)整,，協(xié)方差陣模型如下：

    利用模糊準(zhǔn)則設(shè)定err1,、err2為模糊輸入變量，K_a,、K_g為模糊輸出變量,。err1、err2={NB,，NS,，ZE，PS,，PB},，元素分別代表負(fù)大、負(fù)小,、零,、正小、正大。本文考慮到三角形運算簡單,，對模糊變量的隸屬度函數(shù)采用對稱三角形,，err1、err2模糊化的隸屬度函數(shù)μ(x)如圖2,。模糊規(guī)則如表1所示,。

    由表1K_a、K_g取值,，可得噪聲協(xié)方差陣：

2.2 偏航角卡爾曼濾波

式中h_my，h_mx為無噪聲下的理論磁場水平分量,，e_mx,、e_my為磁場水平噪聲。

    由上可得觀測方程：

    算法流程如圖3所示,。

3.1 靜態(tài)測試

    采用加速度計解算,、本文融合算法解算、陀螺儀解算3種方法獲取靜態(tài)姿態(tài)信息,。

    圖4中加速度計靜止下獲得的姿態(tài)角噪聲干擾大,，本文算法解算的姿態(tài)角消除了噪聲及漂移導(dǎo)致的姿態(tài)累計誤差，實際上平臺放置存在小角度傾斜,，解算的姿態(tài)角偏離零點,，但達(dá)到穩(wěn)定角度后無發(fā)散和超調(diào)現(xiàn)象，精度也有較大提升,；圖5可得通過陀螺儀得到的姿態(tài)角會由于噪聲和陀螺儀漂移而逐漸發(fā)散,，靜止下也無法穩(wěn)定到固定角度。

3.2 動態(tài)測試

    圖6為機(jī)動加速度干擾下,，本文算法和文獻(xiàn)[5]EKF算法得到的姿態(tài)角與參考平臺姿態(tài)角對比的誤差值,。

    表2為加速度估計、EKF與本文算法3種算法估計的姿態(tài)角與參考平臺姿態(tài)角對比的誤差值均方差數(shù)據(jù)對比,。

    結(jié)果表明,，本文算法能夠提高在機(jī)動加速度干擾下的姿態(tài)估計精度。動態(tài)條件下本文算法得到的俯仰角和翻滾角均方差小于擴(kuò)展卡爾曼濾波算法,，而靜態(tài)條件下主要為了克服累計誤差,，同樣保持著高精度的特性。

3.3 磁場干擾測試

    在靜止的采集平臺附近添加硬鐵等磁場干擾,，姿態(tài)角信息輸出如圖7,。圖7表明，雖然EKF算法偏航角波動較小,，但已影響到翻滾角精度,，俯仰角產(chǎn)生角度波動；本文算法由于兩級濾波器解耦合，第二級濾波無法影響水平姿態(tài),，保持正確角度不變,，提高了水平姿態(tài)角精度。

4 結(jié)論

    本文針對在加速度干擾條件下姿態(tài)精度下降及干擾相互耦合等問題,，提出基于模糊調(diào)整的串級LKF濾波方法,，。該方法采用分層設(shè)計解決了磁場干擾對傾角的影響,，并重點研究根據(jù)運動狀態(tài)信息模糊調(diào)整協(xié)方差陣減小機(jī)動加速度干擾和對加速度進(jìn)行補(bǔ)償?shù)姆椒?。實驗驗證，該方法消除了累計誤差,，提高了機(jī)動加速度干擾下的姿態(tài)估計精度,，消除了磁場干擾對傾角的影響，比較適合推廣至低成本姿態(tài)系統(tǒng)中使用,。

參考文獻(xiàn)

[1] ZIHAJEHZADEH S,，LOH D，LEE T J,，et al.A cascaded Kalman filter-based GPS/MEMS IMU integration for sports applications[J].Measurement,，2015，73：200-210.

[2] KANG C W,，PARK C G.Attitude estimation with accelerometers and gyros using fuzzy tuned kalman filter[C].Proc.of ECC,，2009：3713-3718.

[3] LIGORIO G，SABATINI A M.A novel Kalman filter for human motion tracking with an inertial based dynamic inclinometer[J].IEEE transactions on Biomedical Engineering,，2015,，62(8)：2033-2043.

[4] 吳杰，閆建國.基于修正的卡爾曼濾波的姿態(tài)估計算法研究[J].計算機(jī)仿真,，2012,，29(2)：54-57.

[5] SABATIN A M.Quaternion based extended Kalman filter fordetermining orientation by inertial and magnetic sensing[J].IEEE Transactions on Biomedical Engineering，2006,，53(7)：1346-1356.

[6] SUH Y S.Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of externalacceleration[J].IEEE Transactions on Instrumentation and Measurement,，2010，59(12)：3296-3305．

[7] LEE J K,，PARK E J,，NOBINOVITCH S.Estimation of attitude and external acceleration using inertial sensor measurement during various dynamic conditions[J].IEEE Transactions on Instrumentation and Measurement，2012,，61(8)：2262-2273．

[8] 肖文建,，路平，張曦郁.一種面向機(jī)動的航姿測量系統(tǒng)設(shè)計[J].電光與控制,，2014,，21(5)：83-87.

[9] MAHONY R,，HAMEL T，PFLIMLIN J M.Nonlinear complementary filters on the special orthogonal group[J].IEEE Transactions on Automatic Control,，2008,，53：1203-1218.

[10] 秦永元.慣性導(dǎo)航(第二版)[M].北京：科學(xué)出版社，2015.

作者信息:

杜紅彬,，侯秋華,，康小明

（華東理工大學(xué) 信息科學(xué)與工程學(xué)院，上海200237）