邵艷巡, 鄧林紅, 戎海龍

(1.常州大學(xué) 計算機(jī)與人工智能學(xué)院 阿里云大數(shù)據(jù)學(xué)院,江蘇 常州 213164; 2.常州大學(xué) 生物醫(yī)學(xué)工程與健康科學(xué)研究院,江蘇 常州 213164; 3.常州大學(xué) 機(jī)械與軌道交通學(xué)院,江蘇 常州 213164)

0 引 言

慣性—地磁組合測量單元由于具有無需外界參考即可獲得運動體姿態(tài)的優(yōu)勢而被應(yīng)用于人體姿態(tài)跟蹤領(lǐng)域[1,2](包括虛擬現(xiàn)實和體育運動等)和生物醫(yī)學(xué)領(lǐng)域[3](包括醫(yī)療康復(fù)和手術(shù)器械跟蹤等)。慣性—地磁組合測量單元由微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)傳感器組成,包括三軸陀螺儀、三軸加速度計和三軸磁強(qiáng)計,測量得到的數(shù)據(jù)用于運動體姿態(tài)測量。

早期研究發(fā)現(xiàn),單獨使用陀螺儀結(jié)合測量算法可以獲得運動體姿態(tài),如四元數(shù)微分算法[4],但由于陀螺儀隨機(jī)誤差的影響使靜態(tài)出現(xiàn)發(fā)散現(xiàn)象,所以此類算法動態(tài)精度較高,靜態(tài)精度較差。對應(yīng)地,使用加速度計和磁強(qiáng)計結(jié)合測量算法也可以獲得運動體姿態(tài),如QUEST算法[5]和LMA算法[6]等,但由于運動體線性加速度和周圍磁場干擾使動態(tài)出現(xiàn)誤差,所以此類算法靜態(tài)精度較高,動態(tài)精度較差。為了得到動靜態(tài)性能兼顧的算法,近期研究主要是將3種傳感器數(shù)據(jù)通過融合姿態(tài)測量算法獲得姿態(tài)角,此類融合算法主要包括擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)算法[7]和互補濾波(CF)算法[8],為了消除線性加速度和周圍磁場的干擾,獲得準(zhǔn)確的運動體姿態(tài),研究者主要從算法公式上改進(jìn),例如文獻(xiàn)[9]提出的QEKF(quaternion-based EKF)算法、文獻(xiàn)[10]提出的LMA-CO(LMA-complementary)算法以及文獻(xiàn)[11]提出的GCF(generalized linear quarternion complementary filtering)算法等。

為了進(jìn)一步提升現(xiàn)有CF算法的性能,本文提出了一種基于慣性—地磁組合的多模型融合姿態(tài)測量算法。將一段連續(xù)的運動分為動態(tài)段和靜態(tài)段處理,最后將每段的姿態(tài)測量結(jié)果組合在一起得到此段連續(xù)運動的姿態(tài)。

1 算法總體框架

本文運動體姿態(tài)測量的流程如下：首先，通過慣性—地磁組合測量單元獲得數(shù)據(jù)；然后，通過可靠性判定準(zhǔn)則將每個時刻的運動狀態(tài)分類并識別；接著，確定每種運動狀態(tài)下傳感器數(shù)據(jù)的可靠性,并建立數(shù)據(jù)融合模型；最后，利用數(shù)據(jù)融合算法獲得姿態(tài)角。

2 數(shù)據(jù)提取

本文選用Xsen公司的慣性測量單元MTi傳感器模塊采集數(shù)據(jù),選用POLHEMUS公司的Long Ranger電磁跟蹤系統(tǒng)同步測量姿態(tài)角作為實驗基準(zhǔn)。慣性測量單元MTi和Long Ranger電磁跟蹤系統(tǒng)的采樣頻率分別為256 Hz和240 Hz。當(dāng)?shù)刂亓铀俣萭n為9.783 m/s2,當(dāng)?shù)氐卮艌鰪?qiáng)度h為0.862 5 μT,當(dāng)?shù)卮艃A角θ為32°。

3 運動狀態(tài)分類與識別

3.1 運動狀態(tài)分類

對于一段連續(xù)運動,本文將其分段處理：動態(tài)段和靜態(tài)段。其中,靜態(tài)段又分為有磁干擾和無磁干擾2種情況,于是將運動狀態(tài)分為3種,分別為無磁靜態(tài)、有磁靜態(tài)和動態(tài)。利用可靠性判定準(zhǔn)則實現(xiàn)運動狀態(tài)分類及識別。3種運動狀態(tài)的判定準(zhǔn)則如下：1)無磁靜態(tài)的可靠性判定準(zhǔn)則為：|‖a(k)‖-‖gn‖|≤ε1&‖G(k)‖≤ε2&|‖m(k)‖-‖h‖|≤ε3；2)有磁靜態(tài)的可靠性判定準(zhǔn)則為：|‖a(k)‖-‖gn‖|≤ε1&‖G(k)‖≤ε2&|‖m(k)‖-‖h‖|>ε3；3)動態(tài)的可靠性判定準(zhǔn)則為：|‖a(k)‖-‖gn‖|>ε1‖‖G(k)‖>ε2。其中，a(k)為k時刻的加速度,m(k)為k時刻的磁場強(qiáng)度,G(k)為k時刻的角速度,gn為當(dāng)?shù)刂亓铀俣?h為當(dāng)?shù)氐卮艌鰪?qiáng)度,ε1,ε2和ε3分別為加速度閾值、角速度閾值和磁場強(qiáng)度閾值。

3.2 運動狀態(tài)識別

從上一節(jié)中的可靠性判定準(zhǔn)則公式發(fā)現(xiàn),影響運動狀態(tài)識別的主要因素有2個：1)傳感器測量值選擇；2)最優(yōu)閾值選擇。

3.2.1 傳感器測量值選擇

本文采用加速度計、磁強(qiáng)計和陀螺儀本時刻測量值及前4個時刻的測量值的平均值作為加速度計、磁強(qiáng)計和陀螺儀本時刻的測量值。公式如下

a(k)=(a(k)+a(k-1)+a(k-2)+a(k-3)+

a(k-4))/5,k≥5

(1)

m(k)=(m(k)+m(k-1)+m(k-2)+m(k-3)+

m(k-4))/5,k≥5

(2)

G(k)=(G(k)+G(k-1)+G(k-2)+G(k-3)+

G(k-4))/5,k≥5

(3)

3.2.2 最優(yōu)閾值選擇

圖1 誤差值、發(fā)散值和誤差估計隨加速度(或角速度)閾值變化示意

4 數(shù)據(jù)融合模型建立與切換

本文定義了3種數(shù)據(jù)融合模型,分別為：AM模型、GA模型和G模型,并分別對應(yīng)無磁靜態(tài)、有磁靜態(tài)和動態(tài)。當(dāng)識別為無磁靜態(tài)時,加速度計和磁強(qiáng)計數(shù)據(jù)可靠,而陀螺儀數(shù)據(jù)不可靠,則舍棄陀螺儀數(shù)據(jù),僅用加速度計和磁強(qiáng)計數(shù)據(jù)進(jìn)行融合獲得姿態(tài)角,定義為AM模型；當(dāng)識別為有磁靜態(tài)時,只有加速度計數(shù)據(jù)可靠,磁強(qiáng)計和陀螺儀數(shù)據(jù)均不可靠,但由于加速度計數(shù)據(jù)只能求得俯仰角和滾轉(zhuǎn)角,無法得到偏航角,此時根據(jù)充分利用原則,由于存在磁場干擾無法使用磁強(qiáng)計數(shù)據(jù)獲得偏航角,只能用陀螺儀來估計航向,所以在此情況下,仍然使用陀螺儀數(shù)據(jù)計算得到偏航角,于是有磁靜態(tài)用于姿態(tài)測量的數(shù)據(jù)為加速度計和陀螺儀數(shù)據(jù),定義為GA模型；當(dāng)識別為動態(tài)時,加速度計和磁強(qiáng)計數(shù)據(jù)都不可靠,用于姿態(tài)測量的數(shù)據(jù)只有陀螺儀數(shù)據(jù),定義為G模型。如圖2為不同數(shù)據(jù)融合模型。

圖2 數(shù)據(jù)融合模型

5 融合姿態(tài)測量算法設(shè)計

QUEST算法[5]是基于Wahba問題提出的一種矢量定姿算法。本文首先使用QUEST算法融合加速度計和磁強(qiáng)計數(shù)據(jù),獲得方向余弦矩陣C2,然后利用四元數(shù)微分方程[4]處理陀螺儀數(shù)據(jù),獲得方向余弦矩陣C1,并結(jié)合融合姿態(tài)測量算法,得到最終的方向余弦矩陣C,并獲得姿態(tài)角。針對有無磁場干擾,在利用QUEST算法融合加速度計和磁強(qiáng)計數(shù)據(jù)獲得方向余弦矩陣時分為2種情況:1)無磁干擾時,加速度計和磁強(qiáng)計數(shù)據(jù)均可靠,可用于姿態(tài)測量,通過計算得到方向余弦矩陣為C21；2)有磁干擾時,磁強(qiáng)計數(shù)據(jù)不再可靠,即不能用于姿態(tài)測量,所以此時要將QUEST算法中,磁強(qiáng)計的占比系數(shù)設(shè)為0,使得只有加速度計數(shù)據(jù)參與姿態(tài)測量,通過計算得到的方向余弦矩陣為C22。

本文使用的融合姿態(tài)測量算法公式為

C=α×C1+(1-α)×C2

(4)

式中α為權(quán)重,C2根據(jù)上述情況取C21或C22。

不同的運動狀態(tài)使用不同的傳感器數(shù)據(jù)完成姿態(tài)測量,對應(yīng)第4章中不同的數(shù)據(jù)融合模型,當(dāng)識別為無磁靜態(tài)時,用于姿態(tài)測量的數(shù)據(jù)為加速度計和磁強(qiáng)計數(shù)據(jù),此時式(4)中α為0,則C=C21。同理當(dāng)識別為有磁靜態(tài)時,用于測量俯仰角和滾轉(zhuǎn)角的數(shù)據(jù)為加速度計數(shù)據(jù),此時α為0,則C=C22,由于加速度計數(shù)據(jù)不能求得偏航角,所以根據(jù)充分利用原則,在此狀態(tài)下利用陀螺儀數(shù)據(jù)進(jìn)行偏航角測量,此時α為1,則C=C1。同樣地,當(dāng)識別為動態(tài)時,用于姿態(tài)測量的數(shù)據(jù)為陀螺儀數(shù)據(jù),此時α為1,則C=C1。

6 實驗仿真

6.1 無磁場干擾動態(tài)實驗

首先將安裝有慣性—地磁組合測量單元的實驗裝置在水平桌面上靜止一段時間,然后進(jìn)行往復(fù)三維運動,一段時間后再次靜止于水平桌面上。

圖3 無磁場干擾動態(tài)實驗

取文獻(xiàn)[11]提出的GCF算法、文獻(xiàn)[10]提出的LMA-CO算法和文獻(xiàn)[9]提出的QEKF算法作為實驗對比算法。圖3(c)為本文算法與其他算法得到的姿態(tài)角與參考姿態(tài)角對比結(jié)果。表1為本文算法與GCF、LMA-CO和QEKF算法的姿態(tài)角均方根誤差(RMSE)。從圖3(c)和表1中分析發(fā)現(xiàn),本文算法與GCF算法相比,姿態(tài)角精度有明顯的提高,與LMA-CO算法和QEKF算法相比滾轉(zhuǎn)角和俯仰角精度明顯提高。

表1 不同算法的RMSE(°)

表2為本文算法與其他算法單位采樣點時間消耗,從表中發(fā)現(xiàn)本文算法時間消耗最低,即速度最快。

表2 不同算法單位采樣點時間消耗

6.2 有磁場干擾靜態(tài)實驗

將安裝有慣性—地磁組合測量單元的實驗裝置置于水平桌面上,手持手機(jī)在MTi旁邊不?；蝿?人為制造磁場干擾。圖4為磁干擾下靜態(tài)實驗的姿態(tài)角誤差,由于識別為有磁靜態(tài)時,用于姿態(tài)測量的只有加速度數(shù)據(jù),已知加速度數(shù)據(jù)只能求得滾轉(zhuǎn)角和俯仰角,不能求得偏航角,所以,此狀態(tài)下根據(jù)充分利用原則,偏航角使用陀螺儀數(shù)據(jù)求得,雖然誤差較大,但總好于無偏航角結(jié)果。從圖中發(fā)現(xiàn)滾轉(zhuǎn)角誤差和俯仰角誤差均小于0.4°。

圖4 有磁靜態(tài)過程的姿態(tài)角誤差

7 結(jié) 論

基于慣性—地磁組合測量單元的實驗結(jié)果對比表明：本文提出的算法與現(xiàn)有CF算法相比,既提高了精度,也提升了速度,與現(xiàn)有EKF算法相比滾轉(zhuǎn)角和俯仰角的精度都有明顯提高。