黃偉，田明，馮進良，王菲

（長春理工大學 光電工程學院，長春 130022）

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

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

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

1 姿態(tài)角原理

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

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

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

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

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

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

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

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

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

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

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

對于歐拉角轉(zhuǎn)化方向余弦法，從載體坐標系到導航坐標系的轉(zhuǎn)換表示為三個軸向獨立轉(zhuǎn)動的乘積：

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

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

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

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

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

2.1 互補濾波

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

圖1 互補濾波示意圖

2.2 卡爾曼濾波

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

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

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

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

（2）估計均方差方程：

式中，Pk為時刻的后驗估計協(xié)方差。

（3）增益方程：

式中，Kk為計算時刻的卡爾曼增益。

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

式中，Zk為量測值。

（5）估計均方誤差方程：

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

3.1 T-S 模糊控制器

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

圖3 模糊思維框圖

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

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

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

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

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

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

圖6 隸屬分布圖

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

表1 模糊規(guī)則庫

4 實驗與分析

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

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

圖7 卡爾曼濾波RTL 視圖

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

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

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

4.3 算法分析

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

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

圖10 仿真估計方差

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

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

圖11 橫滾角濾波對比示意圖

圖12 偏航角濾波對比示意圖

圖13 俯仰角濾波對比示意圖

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

表2 三種算法估計值與真實值的平均誤差和均方根誤差

5 結(jié)論

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