0 引言

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

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

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

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ī)動(dòng)加速度，h為磁場(chǎng)參考向量，e_g、e_a、e_m分別是陀螺儀、加速度計(jì)及磁強(qiáng)計(jì)互不相關(guān)的高斯隨機(jī)白噪聲。

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

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

2.1 傾角卡爾曼濾波

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

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

其中，a⁺(k-1)為上時(shí)刻后驗(yàn)估計(jì)，a^-(k)為本時(shí)刻先驗(yàn)估計(jì)，c_a為量綱。

    由式(3)的第二項(xiàng)方程得到觀測(cè)方程：

    對(duì)角陣R_acc往往用于克服不存在突變轉(zhuǎn)動(dòng)曲線運(yùn)動(dòng)導(dǎo)致的加速度干擾。當(dāng)存在突變轉(zhuǎn)動(dòng)加速度的情況下，對(duì)協(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ù)小、零、正小、正大。本文考慮到三角形運(yùn)算簡(jiǎn)單，對(duì)模糊變量的隸屬度函數(shù)采用對(duì)稱三角形，err1、err2模糊化的隸屬度函數(shù)μ(x)如圖2。模糊規(guī)則如表1所示。

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

2.2 偏航角卡爾曼濾波

式中h_my，h_mx為無噪聲下的理論磁場(chǎng)水平分量，e_mx、e_my為磁場(chǎng)水平噪聲。

    由上可得觀測(cè)方程：

    算法流程如圖3所示。

3.1 靜態(tài)測(cè)試

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

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

3.2 動(dòng)態(tài)測(cè)試

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

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

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

3.3 磁場(chǎng)干擾測(cè)試

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

4 結(jié)論

    本文針對(duì)在加速度干擾條件下姿態(tài)精度下降及干擾相互耦合等問題，提出基于模糊調(diào)整的串級(jí)LKF濾波方法，。該方法采用分層設(shè)計(jì)解決了磁場(chǎng)干擾對(duì)傾角的影響，并重點(diǎn)研究根據(jù)運(yùn)動(dòng)狀態(tài)信息模糊調(diào)整協(xié)方差陣減小機(jī)動(dòng)加速度干擾和對(duì)加速度進(jìn)行補(bǔ)償?shù)姆椒?。?shí)驗(yàn)驗(yàn)證，該方法消除了累計(jì)誤差，提高了機(jī)動(dòng)加速度干擾下的姿態(tài)估計(jì)精度，消除了磁場(chǎng)干擾對(duì)傾角的影響，比較適合推廣至低成本姿態(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] 吳杰，閆建國(guó).基于修正的卡爾曼濾波的姿態(tài)估計(jì)算法研究[J].計(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ī)動(dòng)的航姿測(cè)量系統(tǒng)設(shè)計(jì)[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）