基于模糊卡爾曼濾波的姿態(tài)估計(jì)

黃偉，田明，馮進(jìn)良，王菲

（長(zhǎng)春理工大學(xué) 光電工程學(xué)院，長(zhǎng)春 130022）

手勢(shì)交互作為一種自然人機(jī)交互方式，是基于多傳感器設(shè)備捕捉肢體動(dòng)作，處理后生成控制器可識(shí)別的信息，從而實(shí)現(xiàn)操作的一種交互方式。該技術(shù)集成了傳感器技術(shù)、模式識(shí)別技術(shù)和電子信息技術(shù)，是現(xiàn)如今人機(jī)交互領(lǐng)域的重要基礎(chǔ)性研究之一。隨著新型體感交互設(shè)備的普及與最優(yōu)估計(jì)理論的深入，如何快速準(zhǔn)確地捕捉和處理人體手臂姿態(tài)信息完成機(jī)械臂的控制運(yùn)動(dòng)，從而達(dá)到實(shí)現(xiàn)主從手操作感知協(xié)同的目的，已成為自然人機(jī)交互領(lǐng)域的關(guān)鍵問題。伴隨傳感器設(shè)備的不斷發(fā)展，捕捉人體手臂信息進(jìn)行姿態(tài)融合解算從而控制機(jī)械臂的設(shè)備以及方法也越來越多。有基于圖像設(shè)備捕捉手臂的姿態(tài)信息實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制，有基于慣性傳感器獲取手臂的姿態(tài)實(shí)現(xiàn)機(jī)械臂的運(yùn)動(dòng)控制。由于每個(gè)傳感器都存在著一定的局限性，因此需要根據(jù)傳感器的不同特性進(jìn)行多傳感器互補(bǔ)融合實(shí)現(xiàn)手臂運(yùn)動(dòng)的姿態(tài)估計(jì)。

本文使用MEMS 傳感器對(duì)人體手臂姿態(tài)進(jìn)行采集估計(jì)，由于MEMS 傳感器的精度低，噪聲高，直接使用采集到的三軸角速度進(jìn)行姿態(tài)估計(jì)，隨著誤差的累積，會(huì)導(dǎo)致姿態(tài)角的發(fā)散，因此采用加速度計(jì)、陀螺儀和磁力計(jì)進(jìn)行傳感器信息融合，可以有效地控制姿態(tài)估計(jì)誤差。Wu等人[1]提出了基于互補(bǔ)濾波的姿態(tài)估計(jì)算法，該算法根據(jù)不同傳感器的特性進(jìn)行互補(bǔ)，計(jì)算量低，但是其濾波系數(shù)固定，根據(jù)經(jīng)驗(yàn)判斷，未能充分考慮到傳感器的噪聲因素，所以姿態(tài)估計(jì)的 精度較低。Hadri、Wu 等人[2-3]在 互 補(bǔ)濾波的基礎(chǔ)上，使用PI 控制器對(duì)四元數(shù)進(jìn)行更新，大大降低了陀螺儀的累積誤差，但是動(dòng)態(tài)響應(yīng)較差。Filho、吳濤等人[4-5]提出采用卡爾曼濾波進(jìn)行姿態(tài)估計(jì)，采用四元數(shù)作為狀態(tài)量，加速度計(jì)作為觀測(cè)量進(jìn)行狀態(tài)更新，該算法實(shí)時(shí)性較好，收斂較快，但是由于卡爾曼濾波模型中的系統(tǒng)過程協(xié)方差矩陣和量測(cè)過程噪聲協(xié)方差矩陣的統(tǒng)計(jì)特性難以準(zhǔn)確獲取。

為了獲取到更加準(zhǔn)確的量測(cè)過程噪聲協(xié)方差矩陣，本文提出一種基于T-S 模糊控制的卡爾曼濾波姿態(tài)估計(jì)方法，將加速度計(jì)數(shù)據(jù)的方差值作為T-S 模糊控制的輸入，得到方差系數(shù)ζ，用于表征卡爾曼濾波器中量測(cè)噪聲協(xié)方差的變化，實(shí)現(xiàn)量測(cè)協(xié)方差矩陣的實(shí)時(shí)調(diào)整，結(jié)合磁力計(jì)陀螺儀數(shù)據(jù)，得到最優(yōu)四元數(shù)估計(jì)，達(dá)到姿態(tài)解算的目的。

1 姿態(tài)角原理

坐標(biāo)系是姿態(tài)解算的關(guān)鍵部分。選取不同的坐標(biāo)系獲取的姿態(tài)信息也各不相同，常見的坐標(biāo)系為導(dǎo)航坐標(biāo)系和載體坐標(biāo)系。一般情況下稱導(dǎo)航坐標(biāo)系為n系，通常選擇地理坐標(biāo)系為導(dǎo)航坐標(biāo)系。地理坐標(biāo)系任取地球表面一點(diǎn)，以經(jīng)緯線的切線方向?yàn)閄、Y軸方向，以重力加速度方向?yàn)閆軸方向。在姿態(tài)解算中常以東北天、北東地為參考系。本文的導(dǎo)航坐標(biāo)系選擇東北天為參考系，記為OXnYnZn。載體坐標(biāo)系稱為b系，以運(yùn)載體的質(zhì)心為原點(diǎn)，將運(yùn)載體自身結(jié)構(gòu)作為坐標(biāo)系。以飛機(jī)航行為例，以飛機(jī)質(zhì)心為原點(diǎn)，飛機(jī)機(jī)頭航向?yàn)閄軸，水平向右為Y軸，豎直向上為Z軸，即為常見的右手直角坐標(biāo)系，記為OXbYbZb。

1.1 姿態(tài)角表示方法

常見的姿態(tài)解算表示方法有歐拉角法、四元數(shù)法、方向余弦矩陣法等[6]。其中，歐拉角法是一種比較直觀容易理解的表達(dá)方式，用三個(gè)參數(shù)來表示物體運(yùn)動(dòng)姿態(tài)，并且在計(jì)算量方面要比四元數(shù)法和方向余弦矩陣法小得多，不存在正交化處理，但是歐拉角存在萬向節(jié)鎖的現(xiàn)象，不能全方位表示姿態(tài)測(cè)量，涉及大量三角函數(shù)運(yùn)算，不利于硬件上的運(yùn)算實(shí)施

方向余弦法是采用九個(gè)參數(shù)表示，避免了歐拉角產(chǎn)生的奇異值問題，但是參數(shù)的增加也增加了計(jì)算成本，會(huì)減弱解算的實(shí)時(shí)性[7]。

四元數(shù)法進(jìn)行姿態(tài)解算，相比方向余弦法的計(jì)算量大大減少，而且還避免了歐拉角產(chǎn)生的問題，只需要求解四個(gè)微分方程，有效地提高了解算的實(shí)時(shí)性。四元數(shù)作為復(fù)數(shù)的一種擴(kuò)展形式，表示方式如下：

式中，i、j、k分別表示相對(duì)x，y，z軸的旋轉(zhuǎn)角度，因此可以得到用四元數(shù)表示的旋轉(zhuǎn)轉(zhuǎn)換矩陣如下：

式中，ωx、ωy、ωz為利用傳感器進(jìn)行數(shù)據(jù)融合獲取的載體坐標(biāo)系下的陀螺儀三軸角速度值。

由于人體手臂在擺動(dòng)運(yùn)動(dòng)的過程中對(duì)姿態(tài)解算的精度和實(shí)時(shí)性的要求比較高，通過陀螺儀三軸角速度信息，利用一階龍格庫(kù)塔法[8]對(duì)四元數(shù)進(jìn)行實(shí)時(shí)更新，得到更新矩陣如下：

對(duì)四元數(shù)進(jìn)行單位化，更準(zhǔn)確地表示姿態(tài)信息，公式如下：

1.2 各表示法的相互轉(zhuǎn)化

歐拉角法、方向余弦法、四元數(shù)法都是表征姿態(tài)角的一種方式，在實(shí)際應(yīng)用當(dāng)中，需要將載體坐標(biāo)系中的表示測(cè)量值轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下進(jìn)行姿態(tài)解算。

對(duì)于歐拉角轉(zhuǎn)化方向余弦法，從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換表示為三個(gè)軸向獨(dú)立轉(zhuǎn)動(dòng)的乘積：

其中，C(θ)、C(γ)、C(φ)表示為：

因此可以得到從載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系，旋轉(zhuǎn)變換矩陣如下：

若已知坐標(biāo)轉(zhuǎn)換矩陣內(nèi)的每個(gè)元素值，可以得到姿態(tài)角，表達(dá)式如下：

四元數(shù)與方向余弦法的轉(zhuǎn)換如式（2）的旋轉(zhuǎn)轉(zhuǎn)換矩陣。由式（11）、式（2）得到歐拉角法與四元數(shù)法的相互轉(zhuǎn)換關(guān)系，如下：

2 姿態(tài)融合算法

2.1 互補(bǔ)濾波

互補(bǔ)濾波算法通過不同傳感器的噪聲特性進(jìn)行互補(bǔ)，實(shí)現(xiàn)姿態(tài)的解算[9]。其核心是對(duì)不同的傳感器進(jìn)行不同的濾波方法，賦予一定的權(quán)重，最后求和得到最終信號(hào)。將加速度計(jì)數(shù)據(jù)進(jìn)行低通濾波，陀螺儀數(shù)據(jù)經(jīng)高通濾波后，相加求和得到融合姿態(tài)角，互補(bǔ)濾波示意圖如圖1 所示。

圖1 互補(bǔ)濾波示意圖

2.2 卡爾曼濾波

卡爾曼濾波作為一種線性最優(yōu)估計(jì)算法[10]，通過“預(yù)測(cè)-實(shí)測(cè)-修正”的模式，利用系統(tǒng)的量測(cè)更新值消除系統(tǒng)的隨機(jī)干擾，從而達(dá)到優(yōu)化還原系統(tǒng)的狀態(tài)。

卡爾曼濾波有兩大功能，即為預(yù)測(cè)和校正，如圖2 所示。預(yù)測(cè)為時(shí)間更新方程，校正為量測(cè)更新方程。時(shí)間更新方程通過判斷上一時(shí)刻的狀態(tài)值來估計(jì)當(dāng)前時(shí)刻的狀態(tài)變量先驗(yàn)估計(jì)值和協(xié)方差先驗(yàn)估計(jì)值，而量測(cè)方程則是將先驗(yàn)估計(jì)和當(dāng)前測(cè)量值計(jì)算得到后驗(yàn)估計(jì)，達(dá)到校正的目的。

圖2 卡爾曼濾波更新示意圖

（1）系統(tǒng)狀態(tài)一步預(yù)測(cè)方程：

（2）估計(jì)均方差方程：

式中，Pk為時(shí)刻的后驗(yàn)估計(jì)協(xié)方差。

（3）增益方程：

式中，Kk為計(jì)算時(shí)刻的卡爾曼增益。

（4）狀態(tài)估值計(jì)算方程：

式中，Zk為量測(cè)值。

（5）估計(jì)均方誤差方程：

3 T-S 模糊卡爾曼濾波算法

3.1 T-S 模糊控制器

模糊控制算法是一種非模型算法[11]。模糊控制算法外表用人類的語言進(jìn)行描述，通過將其轉(zhuǎn)換為數(shù)學(xué)函數(shù)語言，從而達(dá)到控制的目的。由此看來，模糊控制沒有一個(gè)明確的數(shù)學(xué)模型，以人的實(shí)際經(jīng)驗(yàn)為依據(jù)，實(shí)現(xiàn)一種智能算法控制。模糊算法一般分為Mamdani 模型和T-S 模型兩種模型。T-S 模糊系統(tǒng)可以看成一個(gè)用IF-THEN rules 模糊規(guī)則描述的輸入輸出關(guān)系，模糊思維框圖如圖3 所示。

圖3 模糊思維框圖

模糊推理思維無須建立準(zhǔn)確的數(shù)學(xué)模型，只需要根據(jù)系統(tǒng)輸入的模糊信息，建立模糊規(guī)則庫(kù)，通過輸入信息和模糊規(guī)則庫(kù)對(duì)比判斷，從而實(shí)現(xiàn)判斷辨識(shí)。T-S 模糊控制器的系統(tǒng)框圖如圖4 所示。

圖4 模糊控制系統(tǒng)框圖

3.2 T-S 模糊卡爾曼濾波實(shí)現(xiàn)

模糊控制對(duì)于處理不確定問題有著極好的能力，因此本文以T-S 型模糊算法和卡爾曼濾波結(jié)合實(shí)現(xiàn)姿態(tài)解算優(yōu)化[12]。通過模糊推理對(duì)加速度計(jì)的數(shù)據(jù)狀態(tài)進(jìn)行判斷，實(shí)現(xiàn)對(duì)卡爾曼濾波器中量測(cè)噪聲協(xié)方差的調(diào)整，因此本文主要設(shè)計(jì)思路是以加速度計(jì)的x軸和y軸數(shù)據(jù)的方差作為T-S 模糊控制的輸入，得到方差系數(shù)ζ，用于表征卡爾曼濾波器中量測(cè)噪聲協(xié)方差的變化，結(jié)合磁力計(jì)陀螺儀數(shù)據(jù)，得到陀螺儀隨機(jī)漂移最優(yōu)估計(jì)和四元數(shù)最優(yōu)估計(jì)，用陀螺儀隨機(jī)漂移最優(yōu)估計(jì)修正陀螺儀姿態(tài)角的漂移發(fā)散。圖5 為模糊推理卡爾曼濾波原理框圖。

圖5 模糊推理卡爾曼濾波原理框圖

T-S 模糊控制采用二維模糊推理器，將加速度計(jì)的x軸和y軸數(shù)據(jù)的方差δ1、δ2作為推理系統(tǒng)的輸入T1、T2，得到推理輸出的方差控制系數(shù)ζ。本文定義輸入方差的模糊集合分為正小PS、正中PM、正大PB 三個(gè)域，輸出模糊集合分為小S，中M，大L。假設(shè)隸屬分布如圖6 所示。

圖6 隸屬分布圖

可以認(rèn)為輸入方差小于等于0.01 時(shí)，認(rèn)為加速度計(jì)和磁力計(jì)的量測(cè)信息可靠；當(dāng)輸入方差在0.01 和0.015 之間，認(rèn)為量測(cè)信息相對(duì)可靠；當(dāng)輸入方差大于等于0.027 時(shí)，認(rèn)為量測(cè)信息不是相對(duì)可靠。建立模糊規(guī)則庫(kù)如表1 所示。

表1 模糊規(guī)則庫(kù)

4 實(shí)驗(yàn)與分析

4.1 卡爾曼濾波設(shè)計(jì)

本文在ZYNQ7020 上實(shí)現(xiàn)卡爾曼濾波，硬件實(shí)現(xiàn)上對(duì)卡爾曼濾波五個(gè)方程式進(jìn)行簡(jiǎn)化實(shí)現(xiàn)的RTL 視圖，如圖7 所示為卡爾曼濾波RTL 視圖。累計(jì)消耗3 075 個(gè)LUT、5 216 個(gè)寄存器、1 個(gè)RAM、54 個(gè)DSP 資源。

圖7 卡爾曼濾波RTL 視圖

4.2 T-S 模糊控制設(shè)計(jì)

本文在ZYNQ7020 上實(shí)現(xiàn)T-S 模糊控制器，如圖8 所示為T-S 模糊控制RTL 視圖，累積消耗6 956 個(gè)LUT、8 064 個(gè)寄存器資源。

圖8 T-S 模糊控制器RTL 視圖

4.3 算法分析

為了檢驗(yàn)算法的可行性，本文算法在Matlab上進(jìn)行驗(yàn)證仿真，選取隨機(jī)參數(shù)進(jìn)行仿真，仿真數(shù)據(jù)圖如圖9 所示，本文算法在針對(duì)隨機(jī)數(shù)據(jù)進(jìn)行仿真處理，可以看出本算法整體的快速收斂性較好，尤其在數(shù)據(jù)波動(dòng)較大的情況，本文算法依舊可以實(shí)現(xiàn)快速收斂穩(wěn)定。如圖10 所示，本文算法實(shí)現(xiàn)的估計(jì)誤差快速收斂的同時(shí)在顯著減小。

圖9 仿真數(shù)據(jù)

圖10 仿真估計(jì)方差

在此仿真基礎(chǔ)上，本文主要是利用傳感器MPU9250 對(duì)手臂運(yùn)動(dòng)姿態(tài)進(jìn)行測(cè)量，獲取手臂的加速度、角速度和磁力計(jì)數(shù)據(jù)的原始信息，數(shù)據(jù)共九組，采取動(dòng)態(tài)數(shù)據(jù)1 382 組，頻率為200 Hz，每采集一次進(jìn)行一次數(shù)據(jù)融合解算，使用本文算法對(duì)傳感器采集手臂運(yùn)動(dòng)姿態(tài)的原始信息進(jìn)行姿態(tài)解算。

如圖11 為互補(bǔ)濾波、卡爾曼濾波以及本文算法三種算法解算出來的橫滾角對(duì)比示意圖，圖12 為偏航角對(duì)比示意圖，圖13 為俯仰角對(duì)比示意圖。可以看出，互補(bǔ)濾波的融合解算效果最差，其次是卡爾曼濾波，響應(yīng)速度較好，本文算法實(shí)現(xiàn)的融合解算濾波效果最好，曲線平滑優(yōu)于互補(bǔ)濾波和卡爾曼濾波。

圖11 橫滾角濾波對(duì)比示意圖

圖12 偏航角濾波對(duì)比示意圖

圖13 俯仰角濾波對(duì)比示意圖

為了驗(yàn)證三個(gè)算法的濾波融合解算平穩(wěn)性，對(duì)俯仰角、橫滾角、偏航角進(jìn)行平均誤差和均方根誤差，如表2 為三種算法估計(jì)值與真實(shí)值的平均誤差和均方根誤差數(shù)據(jù)對(duì)比表?？梢缘贸鼋馑闳诤细┭鼋?，本文算法平均誤差比互補(bǔ)濾波降低了96.94%，比卡爾曼濾波降低了92.52%。本文算法均方根誤差比互補(bǔ)濾波降低了97.74%，比卡爾曼濾波降低了89.44%。

表2 三種算法估計(jì)值與真實(shí)值的平均誤差和均方根誤差

5 結(jié)論

本文基于模糊控制與卡爾曼濾波的原理，融合二者并在FPGA 內(nèi)實(shí)現(xiàn)采集手臂姿態(tài)測(cè)量解算算法。該算法使用四元數(shù)進(jìn)行解算姿態(tài)，避免了歐拉角存在的奇異點(diǎn)問題，且相比方向余弦矩陣法降低了計(jì)算量。實(shí)驗(yàn)表明：本文實(shí)現(xiàn)的算法有效抑制了姿態(tài)角漂移和噪聲，通過對(duì)加速度計(jì)的方差數(shù)據(jù)進(jìn)行模糊控制得到較精確的量測(cè)噪聲協(xié)方差的信息，融合解算出更精確的姿態(tài)角，且在傳感器進(jìn)行快速抖動(dòng)的前提下，本文算法可以實(shí)現(xiàn)快速跟蹤，與單獨(dú)的互補(bǔ)濾波和卡爾曼濾波相比，提高了解算精度，實(shí)時(shí)性較好，具有一定的意義。