金 梅， 張志福， 金 菊， 于國輝， 李文超

（1.燕山大學(xué)河北省測試計量技術(shù)與儀器重點實驗室，河北秦皇島 066004；

2.河北工業(yè)大學(xué)土木工程學(xué)院，天津 300401； 3.秦皇島視聽機械研究所，河北秦皇島 066000）

基于模糊邏輯的兩步卡爾曼濾波器設(shè)計

金 梅1， 張志福1， 金 菊2， 于國輝3， 李文超1

（1.燕山大學(xué)河北省測試計量技術(shù)與儀器重點實驗室，河北秦皇島 066004；

2.河北工業(yè)大學(xué)土木工程學(xué)院，天津 300401； 3.秦皇島視聽機械研究所，河北秦皇島 066000）

在對慣性運動跟蹤系統(tǒng)的建模分析中，常采用基于計算機的集中式卡爾曼濾波算法進行數(shù)據(jù)處理。由于該方法存在算法復(fù)雜，處理數(shù)據(jù)速度慢等問題，難以在嵌入式系統(tǒng)中實現(xiàn)高速運動跟蹤，提出一種基于模糊邏輯的自適應(yīng)兩步卡爾曼濾波算法。該方法根據(jù)人體不同的運動狀態(tài)調(diào)整卡爾曼濾波器，實驗結(jié)果證明所提的方法能夠更好地估計各個傳感器的測量精度，減少了運算量，并在一定程度上提高了濾波器的容錯性能。

計量學(xué)；慣性運動跟蹤系統(tǒng)；模糊邏輯；兩步卡爾曼濾波

1 引 言

運動跟蹤技術(shù)可以實時地測量人體各個關(guān)鍵部位的運動狀態(tài)信息，為步態(tài)識別、影視制作和機器手臂交互控制等應(yīng)用提供關(guān)鍵數(shù)據(jù)［1，2］。目前慣性運動跟蹤系統(tǒng)主要使用慣性微機電傳感器和磁力計實現(xiàn)角度估計，典型的組成結(jié)構(gòu)是三軸陀螺儀、三軸加速度計和三軸磁力計的九自由度測量系統(tǒng)。因為微機電陀螺儀存在明顯的固有偏差和噪聲誤差，因此姿態(tài)角的誤差會時間而不斷增長。另一方面，微機電加速度計測量值是重力加速度和運動加速度的總和，因此需要對運動加速度進行估計和處理；磁力計也會受到外界磁場的干擾，包括軟磁和硬磁干擾，磁力計自適應(yīng)誤差補償?shù)耐瑫r，也需要準確估計外界磁場的干擾。

通過對慣性運動跟蹤系統(tǒng)建模分析，人們設(shè)計了多種卡爾曼（Kalman）濾波算法，其中大部分是采用基于計算機的集中式卡爾曼濾波器［3］，為了進一步提高卡爾曼濾波的估計精度，人們先后提出了多種自適應(yīng)調(diào)整方法［4，5］，盡管各種自適應(yīng)濾波的實現(xiàn)方法有所不同，但都是根據(jù)信息序列，更多依靠統(tǒng)計特性，并不能準確地判別其實際的運動狀態(tài)，這些不能準確地判別其實際的運動狀態(tài)，同時這也存在一定的時間滯后性。

為了提高估計精度和減少運算量，提出一種適用于嵌入式系統(tǒng)的基于模糊邏輯的兩步卡爾曼濾波算法，該法根據(jù)人體的運動狀態(tài)實現(xiàn)卡爾曼的自適應(yīng)調(diào)整，能夠更好地估計各個傳感器的測量精度，而且數(shù)據(jù)采集和姿態(tài)解算在各個模塊內(nèi)完成，避免了大量的數(shù)據(jù)傳輸，降低了運動跟蹤系統(tǒng)的復(fù)雜性，使慣性跟蹤技術(shù)得到進一步的普及應(yīng)用。

2 載體的運動姿態(tài)

設(shè)計了慣性跟蹤系統(tǒng)的測試模塊，主要由HMC5883（Honeywell提供的弱磁干擾的AMR傳感器），ADXL345（ADI公司生產(chǎn)的三軸加速度計）和L3G4200（意法半導(dǎo)體提供的三軸數(shù)字陀螺儀）組成，見圖1；采用歐拉角表示載體的運動姿態(tài)，見圖2。

圖1 慣性跟蹤系統(tǒng)測試系統(tǒng)

圖2 歐拉角示意圖

根據(jù)陀螺儀輸出的各軸角速度（ωx，ωy，ωz），在已知K－1時刻歐拉姿態(tài)角的情況下，根據(jù)積分計算得到K時刻的導(dǎo)航坐標下各軸的角速度：

當加速度計處于靜止狀態(tài)下，加速度計輸出載體坐標內(nèi)的重力加速度［gx，gy，gz］，得到歐拉坐標內(nèi)的俯仰角θ和橫滾角φ：

根據(jù)地磁場矢量可以估計出偏航角、俯仰角和橫滾角。因為環(huán)境中的電磁干擾，采用地磁場計算俯仰和偏航角的精度很難得到保證，在此磁力計只用于計算和修正偏航角，載體坐標內(nèi)的地磁場為［mxz，myz，mzz］，地理坐標X-Y平面內(nèi)的地磁場為［mxi，myi，mzi］，偏航角（Yaw）ψ可由式（3）得出：

3 運動加速度處理

A.D.Young的人體運動模型將人體運動簡化為剛體的圓周旋轉(zhuǎn)運動［6］，Ji、Si分別是關(guān)節(jié)和傳感器安裝位置。假設(shè)某一關(guān)節(jié)的旋轉(zhuǎn)角速度為ω，角加速度為α，旋轉(zhuǎn)中心節(jié)點為p，可得p點的載體坐標內(nèi)的運動加速度為：

式中l(wèi)t和lr分別為切向加速度和徑向加速度。將載體坐標的運動加速度轉(zhuǎn)換到地理坐標內(nèi)，可以建立全身的運動加速度估計方程：

式中：lWp為地理坐標下旋轉(zhuǎn)中心節(jié)點p的運動加速度；qJK為關(guān)節(jié)JK的姿態(tài)；q*JK為qJK的共軛四元數(shù)。

通過式（4）和式（5），可以估計X、Y、Z三軸的運動加速度值。估計的運動加速度可用來補償和修正X、Y、Z三軸的重力加速度可信度。因為各軸重力加速度的可信度隨著運動加速度的增加而降低，本文根據(jù)各軸的運動運動加速度分別處理三個軸向的加速度分量［7］。

4 基于模糊邏輯的自適應(yīng)濾波器

4.1 慣性運動跟蹤系統(tǒng)中的兩步卡爾曼濾波

引入了2步卡爾曼濾波算法設(shè)計濾波器，見圖3。2步卡爾曼濾波由兩個子卡爾曼濾波器組成：第1步，進行俯仰角和橫滾角的卡爾曼濾波。陀螺儀更新狀態(tài)向量，根據(jù)修正后的重力矢量更新觀測向量，構(gòu)成2維的卡爾曼濾波器，具體如式（6），ΔT為采樣間隔。

圖3 基于兩步卡爾曼的AHRS設(shè)計

第2步，進行偏航角的卡爾曼濾波。根據(jù)第1步的估計結(jié)果，將其代入地磁場矢量方程，即式（3），將其從載體坐標轉(zhuǎn)換到地理坐標的平面內(nèi)，得到偏航角的觀測向量，陀螺儀更新偏航角的狀態(tài)向量，構(gòu)成了1維的卡爾曼濾波：

解耦后的兩步卡爾曼濾波依次進行俯仰角、橫滾角和偏航角的狀態(tài)更新和角度估計。

4．2 基于模糊邏輯的自適應(yīng)兩步卡爾曼濾波器

卡爾曼濾波器是根據(jù)狀態(tài)向量和觀測向量的誤差大小，進行權(quán)重的分配，而向量的誤差大小則是由狀態(tài)向量的方差矩陣Q和觀測向量的方差矩陣R反映。狀態(tài)向量和觀測向量的誤差共同組成了卡爾曼濾波的信息序列，為了準確地分析當前各個傳感器的誤差大小和載體的實際運行狀態(tài)，本文將各軸向的角速度、運動加速度和外部磁場強度作為模糊邏輯的判斷依據(jù)，并調(diào)整狀態(tài)向量的方差矩陣Q和觀測向量的方差矩陣R。根據(jù)模糊邏輯的判斷結(jié)果，將信息序列包含的測量誤差分配給Q、R。

基于模糊邏輯的橫滾角和俯仰角的卡爾曼濾波器見圖4。

陀螺儀更新的載體姿態(tài)誤差，如式（8）。當載體處于不同運動狀態(tài)時，加速度計測量俯仰角和橫滾角的可靠性有所變化，如式（9）。θG和θA分別為陀螺儀和加速度計的俯仰角，ψG和ψA分別為陀螺儀和加速度計的偏航角，偏航角的卡爾曼濾波器如圖5所示。

圖4 橫滾角和俯仰角自適應(yīng)分散卡爾曼示意圖

圖5 基于模糊邏輯的偏航角自適應(yīng)分散卡爾曼示意圖

隨著角速度的增加，陀螺儀的估計誤差也會增加，包括零偏和累積誤差，Q的自適應(yīng)調(diào)整方程為：

矩陣R為磁場強度、角速度和信息間的方程，R陣的自適應(yīng)調(diào)整方程為：

為化簡式（9）和式（11），針對載體坐標內(nèi)各軸向的角速度、運動加速度大小和地磁場矢量大小進行模糊邏輯狀態(tài)識別，分別判斷載體的運動狀態(tài)，見圖6。

圖6中，SS、SL、MS、ML、LS、LL為模糊邏輯的6種狀態(tài)，MF為隸屬函數(shù)。根據(jù)式（5）可以估計各軸的運動加速度。式（9）的矩陣R調(diào)整方程，可以簡化為：

圖6 模糊邏輯成員函數(shù)

式中k為模糊邏輯輸出，具體如表1所示。

表1 加速度計和陀螺儀的k

根據(jù)式（3），代入俯仰角和橫滾角的估計結(jié)果可以得到地磁場矢量估計的偏航角，根據(jù)模糊邏輯，將式（11）簡化可得：

式中，k1，k2為模糊邏輯輸出，具體值如表2、表3所示。

表2 磁力計和陀螺儀的k1

表3 磁力計和陀螺儀的k2

5 實驗分析

將上述算法在MSP430F169（單芯片微控制器）編程，實現(xiàn)肢體的角度估計，結(jié)果通過UART總線按45 Hz發(fā)送到PC機。為測試兩步卡爾曼濾波器的角度估計性能，設(shè)計4組實驗。實驗1和2中，IMU分別繞X、Y軸以100°／s旋轉(zhuǎn)，測試各軸的估計精度。實驗3和4中，對IMU施加電磁干擾和震動干擾，測試濾波器對外部干擾的故障檢測和容錯性能。

實驗1中，IMU以100°／s圍繞X軸旋轉(zhuǎn)，實驗結(jié)果如圖7（a）。

實驗2中，IMU以100°／s圍繞Y軸旋轉(zhuǎn)，實驗結(jié)果如圖7（b）所示。

圖7 實驗1和實驗2的測試結(jié)果

從實驗結(jié)果可知，實驗1中的橫滾角和實驗2中的俯仰角動態(tài)特性良好，濾波后曲線連續(xù)，能夠?qū)崟r的測量IMU的姿態(tài)角。實驗1中，俯仰角和偏航角應(yīng)該為恒定值；從實驗結(jié)果看，俯仰角和偏航角的誤差，在2.0°以內(nèi)。實驗2中，橫滾角和偏航角和應(yīng)該為恒定值；從實驗結(jié)果看，橫滾角和偏航角的誤差，在2.5°以內(nèi)。和橫滾角和俯仰角相比，偏航角的角度估計方差偏大，因為偏航角的角度估計是基于橫滾角和俯仰角的估計結(jié)果，另一方面，地磁場的干擾復(fù)雜，地磁場矢量的修正效果有限，所以偏航角的估計精度有所降低。

實驗3中，將通話中的手機作為電磁干擾源，不斷地在IMU周圍平行移動，實驗結(jié)果如圖8所示。從實驗結(jié)果可知，當周圍存在明顯的電磁干擾時，地磁場矢量解算的偏航角存在明顯的角度波動，修正后的偏航角角度波動明顯變小。另外，實驗中慣性系統(tǒng)處于靜止狀態(tài)，輸出的滾動角和俯仰角角度波動小于0.5°，基本可以認為是恒定值。因為受到外界電磁干擾的影響，偏航角的誤差在5°左右，能滿足實際要求。

圖8 實驗3的測試結(jié)果

實驗4中，將IMU靜止平放，對平臺進行間歇性的敲擊震動測試，實驗結(jié)果如圖9所示，圖9（a）為濾波后的滾動角和俯仰角，圖9（b）為運動加速度修正前?？梢娬饎覫MU時，由重力加速度矢量直接解算的俯仰角和滾動角存在較大幅度的角度波動；濾波后有明顯改善。

圖9 實驗4的測試結(jié)果

以上4組實驗證明，本文提出的自適應(yīng)兩步卡爾曼濾波器，動態(tài)測量誤差基本在2°以內(nèi)，靜態(tài)測量誤差在0.5°內(nèi)，對外界的電磁干擾和震動干擾有較好的容錯能力，能夠滿足普通工程要求。

6 結(jié) 論

在人體慣性跟蹤系統(tǒng)中，經(jīng)常采用卡爾曼濾波器進行數(shù)據(jù)處理，為了進一步提高濾波器的估計精度并減少運算量，本文提出了基于模糊邏輯的兩步卡爾曼濾波算法。該算法令每個濾波器獨立并行工作，針對各個傳感器的不同特性，進行分布式數(shù)據(jù)處理，從而簡化了傳統(tǒng)卡爾曼濾波的建模，同時引入了模糊控制算法，提高了系統(tǒng)的故障檢測和隔離能力，減小容錯計算負擔。實驗結(jié)果證明，該算法計算簡單，具有較好的動態(tài)和靜態(tài)估計效果。

［1］ Welch G，Bishop G，Vicci L，et al.The HiBall tracker：High-performance wide-area tracking for virtual and augmented environment［C］／／VRST'39，London，UK，1999，1－10.

［2］ Roetenberg D，Slycke P，Ventevogel A，et al.A portable magnetic position and orientation tracker［J］.Sensors and Actuators A，2007，135：426－432

［3］ Sutton R.Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation system［J］.Control engineer practice，2004，12：1531－1539

［4］ Chaer W S，Bishop R H.A Mixture-of-Experts Framework for Adaptive Kalman Filtering［J］.IEEE TRANSACTIONS ON SYSTEMS，MAN AND CYBERNETICS—PART B：CYBERNETICS，1997，27（3）：452－464.

［5］ Escamilla-Ambrosio P J，Mort N.Multisensor Data Fusion Architecture Based on Adaptive Kalman Filters and Fuzzy logic Performance Assessment［C］／／ISIF，Annapolis，USA，2002，1542－1549

［6］ Young A D，Ling M J.Distributed Estimation of linear Acceleration for Improved Accuracy in Wireless Inertial Motion Capture［C］／／IPSN'10，Stockholm，Sweden，2010，256－267.

［7］ Yang Y，Miao L J.Adaptive federal Kalman filtering for SINS／GPS Integrated System［J］.Journal of Beijing Institute of Technology，2003，12（4）：371－375

Design of an Adaptive Two-step Kalman Filter Based on Fuzzy Logic

JIN Mei1， ZHANG Zhi-fu1， JIN JU2， YU Guo-hui3， LIWen-chao1
（1.Measurement Technology and Instrumentation Key Lab of Hebei Province，Yanshan University，Qinhuangdao，Hebei066004，China；2.School of Civil Engineering，Hebei University of Technology，Tianjin 300401，China；3.Audio-Visual Machinery Research Institute，Qinhuangdao，Hebei066004，China）

To analyze the modeling of the inertial／magnetic tracking system，the centralized Kalman filter algorithm depending on PC is often used to process the data.Because this algorithm is complex and the data processing speed is slow，it is difficult to realize high-speed motion tracking in the embedded system.An adaptive two-step Kalman filtermethod based on fuzzy logic for real-time motion tracking of human body is proposed.The parameters of Kalman filter are selfadjusted according to the differentmotion states of human body.Themethod can not only improve the estimation accuracy and fault-tolerant performance of the system to a certain degree，but also reduce thematrix dimension.

Metrology；Motion tracking；Fuzzy logic；Two-step Kalman filter

TB93

A

1000-1158（2014）04-0355-05

10．3969／j．issn．1000-1158．2014．04．11

2013-08-09；

2013-11-05

國家自然科學(xué)基金（61077071）

金梅（1977－），女，黑龍江明水人，燕山大學(xué)講師，博士，主要研究方向為視覺伺服、智能控制及信號處理。meijin297＠126.com