邵艷巡, 鄧林紅, 戎海龍

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

0 引 言

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

早期研究發(fā)現(xiàn),單獨(dú)使用陀螺儀結(jié)合測(cè)量算法可以獲得運(yùn)動(dòng)體姿態(tài),如四元數(shù)微分算法[4],但由于陀螺儀隨機(jī)誤差的影響使靜態(tài)出現(xiàn)發(fā)散現(xiàn)象,所以此類算法動(dòng)態(tài)精度較高,靜態(tài)精度較差。對(duì)應(yīng)地,使用加速度計(jì)和磁強(qiáng)計(jì)結(jié)合測(cè)量算法也可以獲得運(yùn)動(dòng)體姿態(tài),如QUEST算法[5]和LMA算法[6]等,但由于運(yùn)動(dòng)體線性加速度和周圍磁場(chǎng)干擾使動(dòng)態(tài)出現(xiàn)誤差,所以此類算法靜態(tài)精度較高,動(dòng)態(tài)精度較差。為了得到動(dòng)靜態(tài)性能兼顧的算法,近期研究主要是將3種傳感器數(shù)據(jù)通過(guò)融合姿態(tài)測(cè)量算法獲得姿態(tài)角,此類融合算法主要包括擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)算法[7]和互補(bǔ)濾波(CF)算法[8],為了消除線性加速度和周圍磁場(chǎng)的干擾,獲得準(zhǔn)確的運(yùn)動(dòng)體姿態(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)測(cè)量算法。將一段連續(xù)的運(yùn)動(dòng)分為動(dòng)態(tài)段和靜態(tài)段處理,最后將每段的姿態(tài)測(cè)量結(jié)果組合在一起得到此段連續(xù)運(yùn)動(dòng)的姿態(tài)。

1 算法總體框架

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

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

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

3 運(yùn)動(dòng)狀態(tài)分類與識(shí)別

3.1 運(yùn)動(dòng)狀態(tài)分類

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

3.2 運(yùn)動(dòng)狀態(tài)識(shí)別

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

3.2.1 傳感器測(cè)量值選擇

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

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ā)散值和誤差估計(jì)隨加速度(或角速度)閾值變化示意

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

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

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

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

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

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

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

(4)

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

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

6 實(shí)驗(yàn)仿真

6.1 無(wú)磁場(chǎng)干擾動(dòng)態(tài)實(shí)驗(yàn)

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

圖3 無(wú)磁場(chǎng)干擾動(dòng)態(tài)實(shí)驗(yàn)

取文獻(xiàn)[11]提出的GCF算法、文獻(xiàn)[10]提出的LMA-CO算法和文獻(xiàn)[9]提出的QEKF算法作為實(shí)驗(yàn)對(duì)比算法。圖3(c)為本文算法與其他算法得到的姿態(tài)角與參考姿態(tài)角對(duì)比結(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為本文算法與其他算法單位采樣點(diǎn)時(shí)間消耗,從表中發(fā)現(xiàn)本文算法時(shí)間消耗最低,即速度最快。

表2 不同算法單位采樣點(diǎn)時(shí)間消耗

6.2 有磁場(chǎng)干擾靜態(tài)實(shí)驗(yàn)

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

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

7 結(jié) 論

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