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

金 梅， 張志福， 金 菊， 于國(guó)輝， 李文超

（1.燕山大學(xué)河北省測(cè)試計(jì)量技術(shù)與儀器重點(diǎn)實(shí)驗(yàn)室，河北秦皇島 066004；

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

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

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

（1.燕山大學(xué)河北省測(cè)試計(jì)量技術(shù)與儀器重點(diǎn)實(shí)驗(yàn)室，河北秦皇島 066004；

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

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

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

1 引 言

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

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

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

2 載體的運(yùn)動(dòng)姿態(tài)

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

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

圖2 歐拉角示意圖

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

當(dāng)加速度計(jì)處于靜止?fàn)顟B(tài)下，加速度計(jì)輸出載體坐標(biāo)內(nèi)的重力加速度［gx，gy，gz］，得到歐拉坐標(biāo)內(nèi)的俯仰角θ和橫滾角φ：

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

3 運(yùn)動(dòng)加速度處理

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

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

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

通過(guò)式（4）和式（5），可以估計(jì)X、Y、Z三軸的運(yùn)動(dòng)加速度值。估計(jì)的運(yùn)動(dòng)加速度可用來(lái)補(bǔ)償和修正X、Y、Z三軸的重力加速度可信度。因?yàn)楦鬏S重力加速度的可信度隨著運(yùn)動(dòng)加速度的增加而降低，本文根據(jù)各軸的運(yùn)動(dòng)運(yùn)動(dòng)加速度分別處理三個(gè)軸向的加速度分量［7］。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

為化簡(jiǎn)式（9）和式（11），針對(duì)載體坐標(biāo)內(nèi)各軸向的角速度、運(yùn)動(dòng)加速度大小和地磁場(chǎng)矢量大小進(jìn)行模糊邏輯狀態(tài)識(shí)別，分別判斷載體的運(yùn)動(dòng)狀態(tài)，見(jiàn)圖6。

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

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

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

表1 加速度計(jì)和陀螺儀的k

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

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

表2 磁力計(jì)和陀螺儀的k1

表3 磁力計(jì)和陀螺儀的k2

5 實(shí)驗(yàn)分析

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

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

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

圖7 實(shí)驗(yàn)1和實(shí)驗(yàn)2的測(cè)試結(jié)果

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

實(shí)驗(yàn)3中，將通話中的手機(jī)作為電磁干擾源，不斷地在IMU周圍平行移動(dòng)，實(shí)驗(yàn)結(jié)果如圖8所示。從實(shí)驗(yàn)結(jié)果可知，當(dāng)周圍存在明顯的電磁干擾時(shí)，地磁場(chǎng)矢量解算的偏航角存在明顯的角度波動(dòng)，修正后的偏航角角度波動(dòng)明顯變小。另外，實(shí)驗(yàn)中慣性系統(tǒng)處于靜止?fàn)顟B(tài)，輸出的滾動(dòng)角和俯仰角角度波動(dòng)小于0.5°，基本可以認(rèn)為是恒定值。因?yàn)槭艿酵饨珉姶鸥蓴_的影響，偏航角的誤差在5°左右，能滿足實(shí)際要求。

圖8 實(shí)驗(yàn)3的測(cè)試結(jié)果

實(shí)驗(yàn)4中，將IMU靜止平放，對(duì)平臺(tái)進(jìn)行間歇性的敲擊震動(dòng)測(cè)試，實(shí)驗(yàn)結(jié)果如圖9所示，圖9（a）為濾波后的滾動(dòng)角和俯仰角，圖9（b）為運(yùn)動(dòng)加速度修正前?？梢?jiàn)震動(dòng)IMU時(shí)，由重力加速度矢量直接解算的俯仰角和滾動(dòng)角存在較大幅度的角度波動(dòng)；濾波后有明顯改善。

圖9 實(shí)驗(yàn)4的測(cè)試結(jié)果

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

6 結(jié) 論

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

［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

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

金梅（1977－），女，黑龍江明水人，燕山大學(xué)講師，博士，主要研究方向?yàn)橐曈X(jué)伺服、智能控制及信號(hào)處理。meijin297＠126.com