0 引言

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

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

    為解決在機動加速度干擾下精度下降及耦合干擾問題，本文通過狀態反饋估計機動加速度，實現對加速度補償，并利用模糊規則動態調整協方差陣，以降低機動加速度干擾，同時串級結構消除了磁場信息對傾角的影響，最后設計實驗驗證本文方法的有效性。

1 姿態描述與傳感器模型

1.1 坐標系和旋轉矩陣

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

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

1.2 傳感器模型

    MEMS傳感器輸出模型：

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

2 串級LKF濾波設計

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

2.1 傾角卡爾曼濾波

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

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

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

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

    對角陣R_acc往往用于克服不存在突變轉動曲線運動導致的加速度干擾。當存在突變轉動加速度的情況下，對協方差陣采用模糊規則進行調整，協方差陣模型如下：

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

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

2.2 偏航角卡爾曼濾波

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

    由上可得觀測方程：

    算法流程如圖3所示。

3.1 靜態測試

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

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

3.2 動態測試

    圖6為機動加速度干擾下，本文算法和文獻[5]EKF算法得到的姿態角與參考平臺姿態角對比的誤差值。

    表2為加速度估計、EKF與本文算法3種算法估計的姿態角與參考平臺姿態角對比的誤差值均方差數據對比。

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

3.3 磁場干擾測試

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

4 結論

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

參考文獻

[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] 吳杰，閆建國.基于修正的卡爾曼濾波的姿態估計算法研究[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].電光與控制，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] 秦永元.慣性導航(第二版)[M].北京：科學出版社，2015.

作者信息:

杜紅彬，侯秋華，康小明

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