李 鑫，孟翔飛，戴 梅，顧啟民

（常熟理工學院，江蘇常熟215500）

基于自適應濾波的MEMS姿態(tài)確定方法

李 鑫*，孟翔飛，戴 梅，顧啟民

（常熟理工學院，江蘇常熟215500）

針對消費類電子設備對姿態(tài)測量系統(tǒng)的需求，本文提出了一種基于MEMS加速度計、陀螺儀和磁強計的九軸姿態(tài)確定算法。針對實際系統(tǒng)中傳感器量測噪聲未知的情況，首先介紹了一種基于矢量觀測器的矩陣Kalman濾波姿態(tài)確定算法，然后利用殘差匹配技術，設計了一種基于殘差匹配的自適應濾波方法。論文采用自適應濾波對傳感器量測噪聲進行估計，并將估計的量測噪聲代入線性矩陣Kalman濾波算法，有效解決了線性矩陣Kalman濾波需要準確量測噪聲統(tǒng)計信息的缺陷。最后設計了仿真實驗驗證本文提出的算法，并將其與線性矩陣Kalman濾波算法比較。仿真結果表明，自適應矩陣Kalman濾波的姿態(tài)旋轉誤差角為0.609 1°，標準差為0.300 9°，能夠有效的估計傳感器量測噪聲，并具有更高的姿態(tài)確定精度和穩(wěn)定性。

姿態(tài)確定；自適應濾波；矩陣Kalman濾波；向量觀測器

姿態(tài)測量是慣性導航的關鍵技術，其應用場合從航天器到水下航行器，從軍事設備到民用消費類電子［1-3］。傳統(tǒng)的捷聯慣性導航系統(tǒng)均采用高精度的慣性測量傳感器進行姿態(tài)確定，由于傳感器體積大，價格昂貴，不適合民用消費類電子。隨著MEMS技術的發(fā)展，新型MEMS傳感器具有體積小、功耗低，價格便宜等優(yōu)點，在消費類電子設備中得到了廣泛的應用［4-6］。

姿態(tài)測量可以直接采用理想陀螺儀測量值計算得到。但是由于傳感器存在零偏誤差，直接采用陀螺儀測量值，會導致姿態(tài)發(fā)散，因此需要外部輔助信息對其進行校正。在MEMS系統(tǒng)中，常采用加速度計和磁強計作為觀測矢量，設計基于矢量觀測器的姿態(tài)確定算法，對陀螺儀更新姿態(tài)進行校正［7-9］。

常用的MEMS姿態(tài)確定方法有基于最優(yōu)估計理論的非線性Kalman濾波方法以及基于最小二乘理論的K-矩陣方法［10-11］。采用非線性理論的Kalman濾波方法能夠在姿態(tài)確定的過程中，實時估計陀螺儀零偏，利用估計的陀螺零偏補償傳感器誤差，進而得到更高的姿態(tài)確定精度［12］。但是，非線性濾波計算量大，收斂速度慢，對實際系統(tǒng)性能要求較高，不利于其在消費類電子中的實現?；谧钚《死碚摰腒-矩陣Kalman濾波方法是一種線性Kalman濾波姿態(tài)確定方法，具有計算簡便、濾波效果好的優(yōu)點［13-14］。由線性Kalman濾波理論可知，在狀態(tài)估計過程中，往往需要對量測噪聲進行準確的噪聲特性統(tǒng)計，這限制了線性Kalman濾波的應用范圍。在實際系統(tǒng)中，MEMS加速度計與磁強計量測噪聲常隨溫度及地域影響，不易準確得到，若直接使用線性矩陣Kalman濾波，會導致系統(tǒng)濾波精度下降，不利于姿態(tài)確定系統(tǒng)穩(wěn)定可靠運行。針對未知噪聲統(tǒng)計濾波，常采用自適應濾波技術，殘差匹配自適應濾波具有計算簡單的優(yōu)點，非常適合簡易系統(tǒng)應用［15-16］?；谏鲜龇治觯疚脑诂F有矩陣Kalman濾波技術的基礎上，巧妙地利用了K-矩陣的歸一化特性，提出采用殘差匹配自適應濾波方法，實現了基于MEMS傳感器的姿態(tài)確定。

1 系統(tǒng)模型

本節(jié)首先介紹一般的矢量觀測器模型，并在此基礎上介紹基于矢量觀測器的K-矩陣模型，最后依據四元數更新方法，給出一般矩陣Kalman濾波模型。

1.1 矢量觀測器量測模型

利用導航系下地磁及重力為參考矢量，并考慮磁強計與加速度計均進行了初步校準，在一般MEMS姿態(tài)確定中，常用的矢量觀測器模型為：

其中，ri∈R3為參考矢量，可通過地磁及重力分布特性準確獲取；為觀測矢量，可通過磁強計與加速度計測得；υi∈R3為量測噪聲；下標i=1,2分別表示磁強計與重力矢量。

1.2K-矩陣模型

針對磁強計及加速度計的矢量觀測模型，可以利用最小二乘方法構造K-矩陣，并通過解析方法得到姿態(tài)四元數。常用的基于Wahba問題的K-矩陣模型如下所示：

其中，

其中，N=2。上式為理想量測矢量bi與參考矢量ri構成的標準K-矩陣，在實際系統(tǒng)中，往往存在量測誤差，因此需要利用最優(yōu)估計理論進行姿態(tài)確定。下面考慮含有噪聲的觀測矢量，并構造基于K-矩陣的矩陣Kalman濾波模型。

1.3 矩陣Kalman濾波模型

根據姿態(tài)四元數與K-矩陣之間的傳遞關系［11］，當觀測矢量含有加性隨機噪聲時，一般的矩陣Kalman濾波模型可以表示為：

其中，過程噪聲為狀態(tài)相關噪聲，即：

式中，ηk表示陀螺量測隨機噪聲；Δt表示采樣時間。矩陣模型量測噪聲主要由觀測矢量噪聲組成，可表示為：

其中，

通過上面分析，式（5）中的各未知參量均可由式（6）～式（8）表示，利用式（5）構造的矩陣Kalman濾波模型，設計線性Kalman濾波算法設計，實現K-矩陣的最優(yōu)估計。

2 自適應矩陣Kalman濾波

基于式（5）一般矩陣Kalman濾波模型，本節(jié)首先介紹了線性矩陣Kalman濾波模型，并針對線性矩陣Kalman濾波的缺陷，設計了一種基于殘差匹配的自適應矩陣Kalman濾波。

2.1 線性矩陣Kalman濾波

由文獻［12］可知，基于K-矩陣模型的線性矩陣Kalman濾波流程如下：

①時間更新：

②量測更新：

其中，

由式（9）～式（18）可知，矩陣Kalman濾波是一種線性Kalman濾波，滿足線性Kalman濾波的最小方差特性，且易于實際系統(tǒng)中應用。

2.2 自適應濾波

由于線性Kalman濾波需要準確已知的量測噪聲矩陣Rk+1，才能實現最優(yōu)濾波的效果，但在實際系統(tǒng)中，往往不能準確得到。當量測噪聲統(tǒng)計不準確時，系統(tǒng)只能達到次優(yōu)濾波的效果。因此，本節(jié)在線性矩陣Kalman濾波的基礎上，引入殘差匹配自適應濾波技術，通過估計的量測噪聲，對濾波算法進行補償，有效提高了在量測噪聲矩陣未知的情況下，估計姿態(tài)的精度和穩(wěn)定性。

殘差匹配自適應是一種常用的自適應濾波方法，利用殘差匹配自適應濾波能夠有效的減小量測噪聲未知情況下，矩陣Kalman濾波對姿態(tài)估計結果的影響。所以，本文利用K-矩陣歸一化特性，結合殘差匹配技術，對上述矩陣Kalman濾波算法進行改進。由K-矩陣的歸一化特性可知：

根據新息匹配原理，得到：

對式（20）進行求解可得：

假設每次量測噪聲都具有相同的誤差類型，則式（21）可以簡化為：

其中，

根據式（19）～（23）分析，利用四元數一步預測作為最優(yōu)四元數代入式（22），結合式（19），則估計的量測噪聲可表示為：

利用式（24）估計的量測噪聲，代入矩陣Kalman濾波算法，計算量測噪聲矩陣Rk+1，即可實現基于K-矩陣的自適應矩陣Kalman濾波。

3 最優(yōu)四元數提取及誤差計算

由于矩陣Kalman濾波估計的狀態(tài)量為姿態(tài)K-矩陣，為直觀表示姿態(tài)誤差，需要從估計的最優(yōu)K-矩陣中提取姿態(tài)角信息。利用K-矩陣定義及Wahba問題可知，姿態(tài)最優(yōu)四元數為K-矩陣對應特征值為1的特征矢量。在求解最優(yōu)姿態(tài)矩陣時，利用羅德里格斯參數法，得到最優(yōu)姿態(tài)四元數為

為分析方便，定義估計最優(yōu)四元數與真實MEMS姿態(tài)四元數之間的誤差四元數為：

其中，qk表示真實姿態(tài)四元數，δ?k|k表示誤差四元數標量部分，δ?k|k表示旋轉誤差角。

4 實驗驗證

本節(jié)設計仿真實驗驗證本文提出的自適應矩陣Kalman濾波算法。假設傳感器零偏及安裝誤差均已通過校正獲得，并補償到系統(tǒng)中。仿真過程中，設定陀螺隨機噪聲為，真實量測噪聲為μ=1°，考慮MEMS傳感器以固定角速度繞z軸旋轉，旋轉角速度為ω=8π/7 200×[0 0 1]Trad/s。為仿真自適應濾波，假設不準確的量測噪聲選取為μ=0.01°，仿真時間設定為5 400 s，系統(tǒng)開始采用線性矩陣Kalman濾波，在運行到2 000 s后采用自適應濾波。濾波初始參數設定為仿真結果如下圖所示。

圖1表示估計的誤差四元數，從圖1可以看出，誤差四元數滿足歸一化特性，且在2 000 s之后，噪聲特性明顯減小。

圖1 誤差四元數

圖2表示量測噪聲，其中藍色實線表示濾波算法中實際使用的量測噪聲，紅色虛線表示真實的量測噪聲。

圖2 估計量測噪聲

從圖2可以看出，自適應濾波能夠估計量測噪聲，并利用估計的量測噪聲補償未知的初始值。這有利于矩陣Kalman濾波提高姿態(tài)K-矩陣的估計精度。

圖3表示旋轉誤差角，從圖3可以看出，旋轉誤差角沒有明顯的發(fā)散現象，這是由于矩陣Kalman濾波的調節(jié)作用，利用磁強計及加速度計為參考基準，校準陀螺儀更新的姿態(tài)。由旋轉誤差角可知，在前2 000 s內系統(tǒng)旋轉誤差角均值為1.2 003°，標準差為0.641 5°，體現為姿態(tài)估計精度差，噪聲特性明顯，這是因為線性矩陣Kalman濾波直接采用預設的量測噪聲進行姿態(tài)估計，由于預設值較小，濾波結果體現為對量測信息的可信度較高，因此濾波結果噪聲較大。在2 000 s之后，采用自適應濾波估計量測噪聲，旋轉誤差角均值穩(wěn)定在0.601 9°左右，標準差為0.300 9°。統(tǒng)計結果表明自適應濾波具有更高的濾波精度及更好的濾波穩(wěn)定性。

圖3 旋轉誤差角

5 結論

本文研究了一種基于殘差匹配的自適應矩陣Kalman濾波，并且針對MEMS應用，設計了仿真實驗。算法實現過程中，利用K-矩陣的歸一化特性，設計了一種特殊的殘差計算方法。利用自適應矩陣Kalman濾波的迭代原理，對傳感器量測噪聲進行實時估計，并且利用估計的量測噪聲對矩陣Kalman濾波進行校正。

針對MEMS實際系統(tǒng)應用，設計了仿真實驗，仿真實驗結果表明，本文提出的自適應濾波能夠有效的估計未知的量測噪聲，克服了線性矩陣Kalman濾波算法需要已知的量測噪聲模型的缺點，并且提高系統(tǒng)姿態(tài)的估計精度和穩(wěn)定性。此外，該濾波算法可以進行MEMS傳感器初始對準，為MEMS慣性導航應用提供了新的有效方法。

［1］馬建萍.GPS輔助捷聯慣導系統(tǒng)動基座初始對準新方法［J］.傳感技術學報，2010，23（11）：1656-1661.

［2］孫偉，李婉秋，初婧，等.基于誤差修正技術的井下人員MEMS定位方法［J］.傳感技術學報，2014，27（7）：898-904.

［3］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測量系統(tǒng)［J］.傳感技術學報，2011，24（5）：642-646.

［4］米剛，田增山，金悅，等.基于MIMU和磁力計的姿態(tài)更新算法研究［J］.傳感技術學報，2015，28（1）：43-48.

［5］蘇菲，金志剛，王柄鑒.基于慣性傳感器的便攜式消防員搜救系統(tǒng)［J］.計算機應用研究，2015，32（12）：3677-3681.

［6］Wachter Z.A Cost Effective Motion Platform for Performance Testing of MEMS-Based Attitude and Heading Reference Systems［J］.Journal of Intelligent&Robotic Systems，2013，70（1-4）：411-419.

［7］Madgwick S O H，Harrison A J L，Vaidyanathan R.Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm［C］//Rehabilitation Robotics（ICORR），2011 IEEE International Conference on.IEEE，2011：1-7.

［8］孫偉，丁偉，李瑞豹，等.基于小波降噪的MEMS慣導系統(tǒng)對準方法研究［J］.傳感技術學報，2015，28（6）：814-818.

［9］高勇，曹娟娟，唐海紅，等.磁強計輔助基于MEMS慣性器件SINS的初始對準方法研究［J］.航天控制，2013，31（3）：50-54.

［10］Markley F L，Crassidis J L.Fundamentals of Spacecraft Attitude Determination and Control［M］.Springer，2014.

［11］Shuster M D，Oh S D.Three-Axis Attitude Determination from Vector Observations［J］.Journal of Guidance，Control，and Dynamics，1981，4（1）：70-77.

［12］Markley F L，Crassidis J L，Cheng Y.Nonlinear Attitude Filtering Methods［C］//AIAA Guidance，Navigation，and Control Conference.2005：15-18.

［13］Choukroun D，Weiss H，Bar-Itzhack I Y，et al.Kalman Filtering for Matrix Estimation［J］.Aerospace and Electronic Systems，IEEE Transactions on，2006，42（1）：147-159.

［14］Choukroun D，Weiss H，Bar-Itzhack I Y，et al.Quaternion Estimation from Vector Observations Using a Matrix Kalman Filter［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48（4）：3133-3158.

［15］Choukroun D.Adaptive Optimal-REQUEST Algorithm for Attitude Determination［C］//AIAA Guidance，Navigation and Control Conference and Exhibit.2007：20-23.

［16］Choukroun D，Bar-Itzhack I Y，Oshman Y.Novel Quaternion Kalman Filter［J］.IEEE Transactions on Aerospace and Electronic Systems，2006，42（1）：174-190.

李 鑫（1983-），男（漢族），安徽省蒙城縣人，碩士生，實驗師，主要研究領域為MEMS技術及智能控制，lixin_cx@ 163.com。

Research on the Attitude Determination of MEMS Based on Adaptive Filter

LI Xin*，MENG Xiangfei，DAI Mei，GU Qimin
（School of Electric and Automatic Engineering，Changshu Institute of Technology，Changshu Jiangsu215500，China）

In view of the demand for attitude measurement system in the consumer electronics devices，a novel attitude determination method based on MEMS accelerometer，MEMS gyroscope and MEMS magnetometer is put forward in this paper.Because the statistics information of the measurement noise of the inertial sensors in the actual system is unknown，a matrix Kalman filtering for attitude determination algorithm based on the vector observer is introduced.In addition，the adaptive filtering method is designed on the basis of the residual matching technology.With this method，the measurement noise can be estimated，and the difficulty of obtaining the accurate statistics of measurement noise for the traditional matrix Kalman filtering is overcome.Finally，the simulation test is designed，and the results show that the rotation error angle is 0.609 1°，and the standard deviation is 0.300 9°，which indicates that the measurement noise of the sensors can be determinated effectively，and the adaptive filter is more accurate and more stable by comparing with the traditional matrix Kalman filtering.

MEMS；attitude determination；adaptive filter；matrix Kalman filter；observation vector

TP 212.9；V249

A

1004-1699（2016）12-1853-05

??7230

10.3969/j.issn.1004-1699.2016.12.012

2016-05-30修改日期：2016-07-15