基于無(wú)逆Kalman 濾波器的姿態(tài)估計(jì)算法①

何百岳 張文安

(浙江工業(yè)大學(xué)信息工程學(xué)院 杭州310023)

0 引言

姿態(tài)估計(jì)在生產(chǎn)生活中得到了廣泛的應(yīng)用,例如醫(yī)療健康[1-3]、人機(jī)交互[4]等等。近年來(lái),隨著磁-慣性傳感器(magnetic/inertial measurement unit,MIMU)的迅速發(fā)展,這種易于穿戴、價(jià)格較低的傳感器得到了研究人員的廣泛關(guān)注[5]。

但是磁-慣性傳感器的應(yīng)用仍然存在一定的困難。例如,陀螺儀積分會(huì)帶來(lái)姿態(tài)的漂移[6],加速度計(jì)容易受到外部線性加速的干擾[7-8],磁力計(jì)易受到外部磁場(chǎng)的干擾[9-10]。基于Kalman 濾波器的多傳感器融合算法能實(shí)現(xiàn)多傳感器信息的融合、信息互補(bǔ),提高估計(jì)精度。文獻(xiàn)[11]引入了乘積擴(kuò)展Kalman 濾波器(multiplicative extended Kalman filter,MEKF),在濾波過(guò)程中估計(jì)偏差來(lái)提高精度。文獻(xiàn)[12]在擴(kuò)展Kalman 濾波器的基礎(chǔ)上,增加了隱馬爾可夫模型(hidden Markov model,HMM)分辨狀態(tài),并采取了自適應(yīng)方法調(diào)整濾波參數(shù),提高了估計(jì)精度。文獻(xiàn)[13]在Kalman 濾波的框架下,將加速度量測(cè)和磁力計(jì)量測(cè)分解,降低了磁場(chǎng)干擾對(duì)重力方向姿態(tài)角的影響。基于閾值的方法將陀螺儀量測(cè)作為過(guò)程方程,姿態(tài)解算得到的四元數(shù)作為觀測(cè)方程,實(shí)現(xiàn)了信息的融合[14-15]。這些方法已能達(dá)到不錯(cuò)的估計(jì)精度,但是對(duì)于現(xiàn)實(shí)的應(yīng)用,實(shí)時(shí)性能也很重要。基于標(biāo)準(zhǔn)Kalman 濾波器(standard Kalman filter,SKF)的方法需要對(duì)新息矩陣求逆。求逆的運(yùn)算復(fù)雜度是O(n3)[16],這給系統(tǒng)帶來(lái)了較大的計(jì)算負(fù)擔(dān)。特別地,嵌入式設(shè)備更是難以在進(jìn)行這種運(yùn)算的同時(shí)確保實(shí)時(shí)性能。文獻(xiàn)[17]針對(duì)主對(duì)角占優(yōu)的矩陣,提出了應(yīng)用近似逆來(lái)代替精確逆值計(jì)算的無(wú)逆Kalman 濾波(inverse free Kalman filter,IFKF)算法。這種算法將求逆運(yùn)算替代為一個(gè)復(fù)雜度為O(n2)的運(yùn)算。姿態(tài)估計(jì)應(yīng)用中,由于磁-慣性傳感器的特性,測(cè)量噪聲協(xié)方差矩陣通常被假設(shè)為項(xiàng)值較大的對(duì)角陣。這導(dǎo)致了新息協(xié)方差矩陣自然地成為一個(gè)對(duì)角占優(yōu)矩陣,從而保證了泰勒級(jí)數(shù)展開(kāi)的收斂性。因此,無(wú)逆Kalman 濾波器適用于基于磁-慣性傳感器的人體姿態(tài)估計(jì)應(yīng)用。

在各種運(yùn)動(dòng)情況下,單一的姿態(tài)估計(jì)策略不能反映復(fù)雜的人體運(yùn)動(dòng)情況,最終導(dǎo)致計(jì)算資源的浪費(fèi)和精度的下降。在行人導(dǎo)航應(yīng)用中,零速度更新(zero velocity update,ZUPT)是一種基于步態(tài)的分類(lèi)估計(jì)策略。這種策略能有效提高室內(nèi)導(dǎo)航的精度[18-19]。對(duì)其他運(yùn)動(dòng)模式識(shí)別,例如對(duì)慢走、坐姿、乘電梯的識(shí)別,也能有效提高導(dǎo)航精度[20]。但是在姿態(tài)估計(jì)中此類(lèi)的討論仍然較少,這值得進(jìn)一步研究。針對(duì)以上問(wèn)題,本文提出將運(yùn)動(dòng)情況分成穩(wěn)態(tài)和動(dòng)態(tài)兩種情況分別采用不同的估計(jì)策略。

根據(jù)上述討論,精度和計(jì)算效率都是姿態(tài)估計(jì)的重要指標(biāo)。本文從引入無(wú)逆Kalman 濾波器、設(shè)計(jì)無(wú)反三角函數(shù)判定條件和設(shè)計(jì)雙策略方法3 個(gè)方面,提出了一種準(zhǔn)確、計(jì)算效率較高的姿態(tài)估計(jì)方法。

1 問(wèn)題描述

姿態(tài)估計(jì)主要研究肢體坐標(biāo)系相對(duì)參考坐標(biāo)系的旋轉(zhuǎn)運(yùn)動(dòng)情況,即估計(jì)人體坐標(biāo)系相對(duì)參考坐標(biāo)系的空間轉(zhuǎn)動(dòng)。正如前文所述,雖然歐拉角能較為直觀地描述運(yùn)動(dòng)姿態(tài),但是會(huì)存在萬(wàn)向節(jié)死鎖問(wèn)題。另外,四元數(shù)法計(jì)算更為高效,所以本文采用旋轉(zhuǎn)四元數(shù)來(lái)描述任意的人體運(yùn)動(dòng)姿態(tài)。姿態(tài)估計(jì)的目標(biāo)是要找到一個(gè)可以將在肢體坐標(biāo)系下的任意向量x旋轉(zhuǎn)到參考坐標(biāo)系下的旋轉(zhuǎn)四元數(shù)。這個(gè)旋轉(zhuǎn)關(guān)系可以由以下方程表示:

在姿態(tài)估計(jì)領(lǐng)域,一個(gè)典型的體感網(wǎng)絡(luò)系統(tǒng)需要同時(shí)處理多個(gè)磁力-慣性傳感器的數(shù)據(jù)。進(jìn)一步地,姿態(tài)估計(jì)往往是人體姿態(tài)估計(jì)和運(yùn)動(dòng)跟蹤的一個(gè)組成部分,姿態(tài)估計(jì)值很可能是其他應(yīng)用的輸入,如手臂運(yùn)動(dòng)軌跡重建就需要姿態(tài)估計(jì)數(shù)據(jù)。如果姿態(tài)估計(jì)的計(jì)算效率過(guò)低,則會(huì)降低整個(gè)系統(tǒng)的實(shí)時(shí)性能。鑒于以上原因,這一類(lèi)系統(tǒng)亟需一種姿態(tài)估計(jì)準(zhǔn)確而且計(jì)算復(fù)雜度較低的方法。為此,本文提出了一種計(jì)算效率較高同時(shí)又不影響估計(jì)精度的姿態(tài)估計(jì)算法。

2 慣性傳感器信號(hào)分析和處理

加速計(jì)和磁力計(jì)的量測(cè)信號(hào)可以分成以下3 部分:

其中,g是重力加速度,m是地球磁場(chǎng)強(qiáng)度,bA是由于人體運(yùn)動(dòng)而被引入的線性加速度干擾,bM是環(huán)境中的鐵磁物質(zhì)產(chǎn)生的磁場(chǎng)干擾,nA和nM分別是傳感器量測(cè)產(chǎn)生的傳感器噪聲,都可被假設(shè)為零均值的高斯白噪聲。

陀螺儀的量測(cè)方程如下:

其中,ω是角速度,nG是測(cè)量的噪聲,可被認(rèn)為是均值為0 的高斯白噪聲。

姿態(tài)估計(jì)的精度依賴(lài)于傳感器測(cè)量的準(zhǔn)確程度,然而,傳感器的量測(cè)會(huì)受到線性加速度和外部磁場(chǎng)干擾的影響。根據(jù)慣性傳感器的特性,很多研究工作都采用了基于閾值的方法[14-15]來(lái)降低干擾的影響。鑒于這種方法在工程應(yīng)用上的便利性和有效性,本文算法以其為基本框架,并對(duì)其進(jìn)行修改以提高計(jì)算效率。特別地,文獻(xiàn)[14]在考慮磁場(chǎng)干擾時(shí),需要引入反三角函數(shù)運(yùn)算,這對(duì)設(shè)備的計(jì)算能力提出了較高的要求。為避免復(fù)雜的反三角函數(shù)運(yùn)算,本文設(shè)計(jì)了無(wú)反三角函數(shù)運(yùn)算的磁場(chǎng)噪聲評(píng)價(jià)指標(biāo),并將其稱(chēng)為嚴(yán)重干擾拒絕方法(severe disturbance reject method,SDR)以示區(qū)別。

2.1 基于加速度計(jì)的嚴(yán)重干擾拒絕

加速度計(jì)測(cè)量的是傳感器的加速度大小和方向。其中,在人體運(yùn)動(dòng)的范圍內(nèi),地球表面的重力加速度可被近似認(rèn)為是大小和方向都不隨時(shí)間改變的矢量。那么,只要知道參考坐標(biāo)系下重力加速度和最終姿態(tài)的重力加速度,就能獲得當(dāng)前姿態(tài)和豎直方向下的夾角。然而,在姿態(tài)運(yùn)動(dòng)的情況下,加速度計(jì)的量測(cè)值是地球重力加速度和運(yùn)動(dòng)加速度的矢量和。這個(gè)矢量和通常和重力加速度的方向是不相同的。那么,就認(rèn)為運(yùn)動(dòng)帶來(lái)的線性加速度是一個(gè)干擾。一旦這個(gè)干擾過(guò)大,則加速度計(jì)的測(cè)量會(huì)被認(rèn)為是不可靠的。這時(shí),預(yù)測(cè)的加速度,即,會(huì)比直接量測(cè)出來(lái)的加速度更可靠。因此,觀測(cè)向量為

其中,g是參考坐標(biāo)系下的重力加速度(在傳感器完全靜止的情況下測(cè)得),是時(shí)間更新得到的預(yù)測(cè)四元數(shù),計(jì)算公式如下文的式(7)所示,εA1和εA2是由多次實(shí)驗(yàn)得出的運(yùn)動(dòng)加速度干擾檢測(cè)閾值。

2.2 基于磁力計(jì)的嚴(yán)重干擾拒絕

基于磁場(chǎng)強(qiáng)度來(lái)判定航向角是一種很常用的方法,只要知道參考坐標(biāo)系下地球磁場(chǎng)強(qiáng)度和最終姿態(tài)的傳感器坐標(biāo)系下的磁場(chǎng)強(qiáng)度,就能獲得當(dāng)前姿態(tài)在水平方向的朝向。此方法只考慮了勻強(qiáng)磁場(chǎng)下的導(dǎo)航問(wèn)題,但是,室內(nèi)場(chǎng)景下會(huì)存在鐵磁物體以及電線,這些物品會(huì)產(chǎn)生額外的磁場(chǎng),導(dǎo)致勻強(qiáng)磁場(chǎng)假設(shè)不能成立。鑒于以上問(wèn)題,本文設(shè)定了2 個(gè)判定條件來(lái)檢測(cè)磁場(chǎng)強(qiáng)度是否存在嚴(yán)重干擾的情況。通常,在無(wú)干擾的環(huán)境下,傳感器只能檢測(cè)到地磁場(chǎng)。那么,可以通過(guò)檢測(cè)磁場(chǎng)強(qiáng)度的大小來(lái)判定是否存在較大的干擾,一旦磁場(chǎng)強(qiáng)度變化過(guò)大,則認(rèn)為這一時(shí)刻的磁場(chǎng)測(cè)量是不準(zhǔn)確的。此外,很多文獻(xiàn)都引入了磁傾角作為判斷指標(biāo)。磁傾角指磁場(chǎng)強(qiáng)度和重力加速度之間的夾角,在磁場(chǎng)強(qiáng)度不變的情況下,磁傾角不會(huì)有較大的變化。更重要的是,相對(duì)于磁場(chǎng)強(qiáng)度模值,磁傾角對(duì)磁場(chǎng)變化更敏感,因此磁傾角更能用于指導(dǎo)判定是否出現(xiàn)嚴(yán)重干擾的情況。文獻(xiàn)[10]列舉了一個(gè)磁場(chǎng)強(qiáng)度變化而磁場(chǎng)強(qiáng)度幅值不變的例子,這種情況下,引入磁傾角作為判定條件很有必要。

綜上所述,磁場(chǎng)的觀測(cè)向量可以寫(xiě)成如下形式:

2.3 無(wú)反三角運(yùn)算的磁傾角判定規(guī)則

在判定是否存在較大磁場(chǎng)干擾時(shí),判定條件需要計(jì)算每個(gè)時(shí)刻的磁傾角。反三角函數(shù)的計(jì)算會(huì)給嵌入式系統(tǒng)帶來(lái)較大的計(jì)算負(fù)擔(dān)。而本文通過(guò)化簡(jiǎn),剔除了每一時(shí)刻的反三角函數(shù)估計(jì)運(yùn)算,新的判定條件可以寫(xiě)為

其中,mt是傳感器坐標(biāo)系下的地磁場(chǎng)量測(cè),gt是參考坐標(biāo)系下的重力加速度,是前文中獲得的可靠的重力觀測(cè)。特別地,不隨時(shí)間變化,可以在初始時(shí)刻被確定。

由于判定條件式(6)不需要任何的反三角函數(shù)運(yùn)算,計(jì)算復(fù)雜度相比于原來(lái)的判定條件式(5)會(huì)減少,從而減少了算法的執(zhí)行時(shí)間。

3 動(dòng)態(tài)狀態(tài)下的姿態(tài)估計(jì)策略

在動(dòng)態(tài)的情況下,MIMU 中單一的傳感器不足以提供準(zhǔn)確的姿態(tài)信息,比如說(shuō)陀螺儀的估計(jì)會(huì)受到積分漂移的影響,加速度計(jì)對(duì)重力的測(cè)量會(huì)受到運(yùn)動(dòng)產(chǎn)生的線性加速度的干擾,以及磁力計(jì)受到外部磁場(chǎng)的干擾導(dǎo)致無(wú)法測(cè)量出地磁場(chǎng)的磁場(chǎng)強(qiáng)度。鑒于以上問(wèn)題,利用基于最優(yōu)估計(jì)理論的Kalman 濾波器進(jìn)行多傳感器信息融合,實(shí)現(xiàn)信息的互補(bǔ)是很有必要的。

動(dòng)態(tài)狀態(tài)下的姿態(tài)估計(jì)策略主要有以下3 個(gè)步驟組成。首先,通過(guò)四元數(shù)運(yùn)動(dòng)學(xué)獲得離散的狀態(tài)更新方程。其次,用嚴(yán)重干擾拒絕器獲得更為可靠的陀螺儀和加速度計(jì)量測(cè)值。將可靠的量測(cè)值用最優(yōu)兩向量四元數(shù)估計(jì)方法(optimal two-observation quaternion estimation method,O2OQ 估計(jì)算法)計(jì)算得到四元數(shù)量測(cè),作為量測(cè)值。最后,設(shè)計(jì)相應(yīng)的IFKF 濾波器,將角速度量測(cè)和旋轉(zhuǎn)四元數(shù)量測(cè)融合,得到最終的姿態(tài)估計(jì)結(jié)果。

3.1 基于陀螺儀的姿態(tài)更新

基于四元數(shù)的離散時(shí)間狀態(tài)更新過(guò)程方程如下[21]:

其中,qt代表第t時(shí)刻從參考坐標(biāo)系到肢體坐標(biāo)系的旋轉(zhuǎn)四元數(shù),Δt是一個(gè)相對(duì)短的時(shí)間間隔,對(duì)于本系統(tǒng)是10 ms。Y(ωt) 可以由下式表示:

其中,ωt是三軸陀螺儀的角速度量測(cè)值,[ω×]是反對(duì)稱(chēng)矩陣的表示方法。

3.2 基于加速度計(jì)和磁力計(jì)確定姿態(tài)

研究姿態(tài)解算最著名的問(wèn)題就叫做Wahba 問(wèn)題[22]。本文在考慮計(jì)算效率和估計(jì)精度2 個(gè)指標(biāo)后,選擇兩向量最優(yōu)四元數(shù)估計(jì)方法來(lái)解算姿態(tài),具體步驟參見(jiàn)文獻(xiàn)[22]。為保證解算出的姿態(tài)的準(zhǔn)確性,本文使用了前述的嚴(yán)重干擾拒絕(SDR)方法來(lái)獲得t時(shí)刻的重力加速度觀測(cè)值和磁場(chǎng)強(qiáng)度觀測(cè)值。

3.3 無(wú)逆Kalman 濾波器設(shè)計(jì)

系統(tǒng)的狀態(tài)量設(shè)定為姿態(tài)四元數(shù)qt,觀測(cè)量是qO2OQ,t,由O2OQ 算法計(jì)算獲得。整個(gè)動(dòng)態(tài)系統(tǒng)可表示為式(9)。

過(guò)程噪聲協(xié)方差Qt-1表示為

對(duì)于本文研究的動(dòng)態(tài)系統(tǒng)式(9)來(lái)說(shuō),標(biāo)準(zhǔn)Kalman 濾波器所示如下:

由式(11)可得,標(biāo)準(zhǔn)Kalman 濾波器的估計(jì)過(guò)程需要對(duì)新息協(xié)方差矩陣求矩陣逆,這一運(yùn)算過(guò)程會(huì)給整個(gè)系統(tǒng)帶來(lái)較大的計(jì)算負(fù)擔(dān),從而降低系統(tǒng)的實(shí)時(shí)性。為避免其產(chǎn)生的不良影響,本系統(tǒng)引入了無(wú)逆的Kalman 濾波算法。在人體姿態(tài)估計(jì)的應(yīng)用中,量測(cè)噪聲一般都會(huì)被設(shè)定成對(duì)角線較大的矩陣,這導(dǎo)致了新息協(xié)方差矩陣是一個(gè)對(duì)角占優(yōu)的矩陣。對(duì)角占優(yōu)這個(gè)特性可以保證矩陣在一階泰勒展開(kāi)后,只取泰勒展開(kāi)式的低階項(xiàng)也能保證收斂性。換句話說(shuō),使用新息矩陣一階泰勒展開(kāi)的低階項(xiàng)的逆矩陣和來(lái)近似代替矩陣的精確逆值不會(huì)引起較大的誤差。

其中,Cii=Pii +Rii,P和R分別代表自協(xié)方差矩陣和量測(cè)協(xié)方差矩陣。

除此之外,無(wú)逆Kalman 濾波的形式式(12)中,每一行互不相關(guān),這個(gè)特性適合于多個(gè)處理器的并行運(yùn)算,能夠進(jìn)一步加快運(yùn)算速度。

最后,狀態(tài)四元數(shù)需要被標(biāo)準(zhǔn)化表示旋轉(zhuǎn)。

其中,分子部分是無(wú)逆Kalman 濾波器對(duì)姿態(tài)的估計(jì)值,分母中‖·‖運(yùn)算表示求向量的模值。

根據(jù)文獻(xiàn)[17],本文研究的動(dòng)態(tài)系統(tǒng)式(9)是完全一致可觀的,(θt,) 是完全一致能控的,而且初始時(shí)刻的狀態(tài)協(xié)方差P0是半正定的或者正定的,所以是有界的。由此可知,本文設(shè)計(jì)的基于無(wú)逆Kalman 濾波器的姿態(tài)估計(jì)算法是收斂的。

4 穩(wěn)態(tài)狀態(tài)下的姿態(tài)估計(jì)策略

人體運(yùn)動(dòng)情況變化復(fù)雜,針對(duì)特定的情況應(yīng)該因地制宜地采用一些更加方便靈活的估計(jì)策略。針對(duì)人體在較長(zhǎng)時(shí)間幾乎不動(dòng)的情況下,例如手臂放在桌上休息,或者躺在床上,本文設(shè)計(jì)了一種特殊的估計(jì)策略。實(shí)際情況下,穩(wěn)定狀態(tài)可能會(huì)經(jīng)常出現(xiàn),并且持續(xù)較長(zhǎng)的時(shí)間,單獨(dú)研究它的特性很有意義。

4.1 基于加速度計(jì)的穩(wěn)定狀態(tài)檢測(cè)

在穩(wěn)定的狀態(tài)下,加速度計(jì)只能檢測(cè)到重力加速度,因此,每個(gè)軸檢測(cè)到的加速度分量和上一時(shí)刻會(huì)是相同的。這樣,根據(jù)相鄰時(shí)刻每一軸上檢測(cè)到加速度的差值可以判定系統(tǒng)是否處于穩(wěn)態(tài)。

其中,ax(t),ay(t)和az(t)表示了傳感器坐標(biāo)下,X軸,Y軸和Z軸在時(shí)刻t的量測(cè)。

4.2 基于陀螺儀的穩(wěn)定狀態(tài)檢測(cè)

只要檢測(cè)傳感器的量測(cè)是否小于一個(gè)閾值,就能判定傳感器是否處于穩(wěn)定狀態(tài)。判定公式如下:

其中,ωx(t)、ωy(t)和ωz(t)表示傳感器坐標(biāo)下,X軸、Y軸和Z軸在時(shí)刻t的原始量測(cè),thgro,X、thgro,Y和thgro,Z分別是判定穩(wěn)定的X軸、Y軸、Z軸的閾值,是多次實(shí)驗(yàn)得出的經(jīng)驗(yàn)參數(shù)。

4.3 基于時(shí)間閾值的穩(wěn)定狀態(tài)檢測(cè)

陀螺儀和加速度傳感器對(duì)于較小的運(yùn)動(dòng)都擁有較高的靈敏性,但是一些特殊情況的出現(xiàn)可能會(huì)導(dǎo)致估計(jì)策略的頻繁切換,這均會(huì)降低系統(tǒng)的穩(wěn)定性。因此,在判定穩(wěn)定的時(shí)候,不但要考慮當(dāng)前時(shí)刻是否穩(wěn)定,也要結(jié)合過(guò)去一段時(shí)間的狀態(tài)進(jìn)行分析評(píng)判,為此判定穩(wěn)態(tài)時(shí)間周期被引入。判定穩(wěn)態(tài)時(shí)間周期要求:在整個(gè)時(shí)間周期內(nèi),所有時(shí)刻都滿足加速度計(jì)和陀螺儀的判定條件,系統(tǒng)才被認(rèn)為進(jìn)入了穩(wěn)定狀態(tài)。判定條件可以寫(xiě)成以下形式:

其中,count是計(jì)數(shù)時(shí)刻數(shù),thinterval是設(shè)定的時(shí)間閾值。

4.4 基于時(shí)間閾值的穩(wěn)定狀態(tài)檢測(cè)

在穩(wěn)定狀態(tài)下,每一時(shí)刻的估計(jì)值都與上一時(shí)刻相等,這一策略能夠避免較為復(fù)雜的姿態(tài)估計(jì)過(guò)程,既能減少系統(tǒng)估計(jì)的運(yùn)算量,又能減少系統(tǒng)中由于計(jì)算產(chǎn)生的能量損耗。

姿態(tài)的維持和協(xié)方差的預(yù)測(cè)想法來(lái)源于文獻(xiàn)[23],具體的遞推方程如下所示。

綜上所述,本文提出的算法的偽代碼如算法1所示。

5 實(shí)驗(yàn)和討論

實(shí)驗(yàn)使用了Xsens 公司生產(chǎn)的MTi-300 慣性傳感器,包括1 個(gè)三軸加速度計(jì),1 個(gè)三軸陀螺儀和1個(gè)三軸磁力計(jì),磁力-慣性傳感器的采樣頻率是100 Hz。實(shí)驗(yàn)場(chǎng)景如圖1 所示,其中8 個(gè)OptiTrack攝像頭環(huán)繞在實(shí)驗(yàn)人員周?chē)?同時(shí)以100 Hz 的頻率獲得前臂剛體的姿態(tài)信息。

圖1 實(shí)驗(yàn)場(chǎng)景

5.1 實(shí)驗(yàn)1 慢速且無(wú)磁場(chǎng)干擾

實(shí)驗(yàn)者被要求在實(shí)驗(yàn)室的中間較寬闊的地方做運(yùn)動(dòng),這樣可以避免各種鐵磁設(shè)備對(duì)實(shí)驗(yàn)的干擾。同時(shí),為了避免較大的線性加速度噪聲,測(cè)試人員被要求手臂旋轉(zhuǎn)運(yùn)動(dòng)盡可能緩慢。如前文所述,Opti-Track 系統(tǒng)能夠計(jì)算獲得精度極高的姿態(tài)值,因此OptiTrack 系統(tǒng)的量測(cè)值可以作為人體前臂的標(biāo)準(zhǔn)值,用于評(píng)價(jià)估計(jì)值的精確度。圖2 展示了標(biāo)準(zhǔn)Kalman 濾波器和無(wú)逆Kalman 濾波器的估計(jì)結(jié)果,這兩個(gè)濾波器的估計(jì)值都非常接近OptiTrack 的量測(cè)。為了便于讀者區(qū)分,圖3 展示了標(biāo)準(zhǔn)Kalman 濾波器和無(wú)逆Kalman 濾波器姿態(tài)估計(jì)值的差值,其中由于標(biāo)準(zhǔn)Kalman 濾波算法是更為精準(zhǔn)的計(jì)算方法,因此標(biāo)準(zhǔn)Kalman 濾波器的估計(jì)值是基準(zhǔn)值。從圖3可以清晰地看出,基于無(wú)逆Kalman 濾波器的姿態(tài)估值與基于標(biāo)準(zhǔn)Kalman 濾波器的姿態(tài)估值之間的差異非常小,比方向四元數(shù)要小很多個(gè)數(shù)量級(jí)。因此,新引入的無(wú)逆Kalman 濾波器并不會(huì)明顯地影響姿態(tài)估計(jì)的精度。本文為了方便展示精度,提出了均方根誤差(root mean square errors,RMSE)這一指標(biāo)。它計(jì)算每一步估計(jì)值和真實(shí)值(OptiTrack 系統(tǒng)的量測(cè)值)直接誤差的均值,因此,RMSE 能夠指示姿態(tài)估計(jì)的精確度。表1 展示了基于標(biāo)準(zhǔn)Kalman和基于無(wú)逆Kalman 的姿態(tài)估計(jì)結(jié)果,同時(shí)也比較了包含SDR 和不包含SDR 的估計(jì)結(jié)果的RMSE。從表1 結(jié)果發(fā)現(xiàn),在保留四位小數(shù)的情況下,無(wú)逆Kalman 濾波器并不會(huì)降低精度。而且由于干擾較小,SDR 對(duì)性能的提升并不明顯。表2 展示了計(jì)算時(shí)間耗損。算法時(shí)間耗損描述了相同的實(shí)驗(yàn)數(shù)據(jù)經(jīng)過(guò)多次運(yùn)算使用時(shí)間的平均值,本文的時(shí)間耗損是10次運(yùn)算的平均時(shí)間。因此它能夠在統(tǒng)計(jì)意義上描述運(yùn)算的復(fù)雜程度。顯然,無(wú)逆Kalman 濾波器的計(jì)算時(shí)間要少很多,幾乎是標(biāo)準(zhǔn)Kalman 濾波器計(jì)算時(shí)間的70%。實(shí)驗(yàn)結(jié)果顯示,動(dòng)態(tài)的情況下,無(wú)逆Kalman 濾波器具備更高的計(jì)算效率。

表1 估計(jì)結(jié)果對(duì)比表

表2 時(shí)間耗損比較(實(shí)驗(yàn)1)

圖2 姿態(tài)估計(jì)結(jié)果圖

圖3 估計(jì)差值結(jié)果圖

5.2 實(shí)驗(yàn)2 快速運(yùn)動(dòng)且存在磁場(chǎng)干擾

和實(shí)驗(yàn)1 相比,實(shí)驗(yàn)2 是一個(gè)更為復(fù)雜的場(chǎng)景。實(shí)驗(yàn)人員被要求將手臂以較快速度旋轉(zhuǎn),同時(shí)也靠近磁鐵,以造成一定的磁場(chǎng)干擾。從圖4 可以看出整個(gè)實(shí)驗(yàn)大部分都是在動(dòng)態(tài)策略下運(yùn)行,虛線分別表示實(shí)驗(yàn)中設(shè)定的上下閾值。另外,在執(zhí)行過(guò)程中還存在幾段磁場(chǎng)干擾。從圖5 可以看出,包含嚴(yán)重干擾拒絕(SDR)的標(biāo)準(zhǔn)Kalman 濾波估計(jì)結(jié)果和無(wú)逆Kalman 濾波估計(jì)結(jié)果和OptiTrack 估計(jì)都很接近。圖6 展示了標(biāo)準(zhǔn)Kalman 濾波器和無(wú)逆Kalman濾波器的差值。同樣地,文中以標(biāo)準(zhǔn)Kalman 濾波器的姿態(tài)估計(jì)值作為基準(zhǔn)值,無(wú)逆Kalman 濾波器的姿態(tài)估值與其差異非常小,因此,可以認(rèn)為這兩種方法有相接近的估計(jì)精度。與之相對(duì)的是,表3 展示了算法的時(shí)間耗損。其中,使用無(wú)逆Kalman 濾波器的估計(jì)方法后,計(jì)算時(shí)間只相當(dāng)于原來(lái)計(jì)算時(shí)間的67%。進(jìn)一步來(lái)說(shuō),本文使用的是單線程的處理方法。一旦轉(zhuǎn)換成多線程的方法,運(yùn)算時(shí)間能夠進(jìn)一步地減少。另外,本文還關(guān)心嚴(yán)重干擾拒絕的效果。圖7 比較了是否包含嚴(yán)重干擾拒絕的無(wú)逆Kalman濾波器估計(jì)的姿態(tài)四元數(shù)。顯然,含有嚴(yán)重干擾拒絕的算法能夠獲得更高的估計(jì)精度。除了圖片之外,表4 能夠更為直觀地展示精度的高低。標(biāo)準(zhǔn)Kalman 濾波器和無(wú)逆Kalman 濾波器在RMSE 的指標(biāo)上表現(xiàn)非常一致,但是一旦沒(méi)有包含嚴(yán)重干擾拒絕方法,姿態(tài)估計(jì)的精度會(huì)出現(xiàn)大幅下滑。結(jié)合實(shí)驗(yàn)1 來(lái)看,無(wú)逆Kalman 濾波算法能夠在提高運(yùn)算效率的同時(shí)不影響估計(jì)的精度,而嚴(yán)重干擾拒絕能夠較大程度地緩解時(shí)間較短的外部干擾。

表3 時(shí)間耗損對(duì)比表(實(shí)驗(yàn)2)

表4 估計(jì)結(jié)果對(duì)比表

圖4 噪聲干擾圖

圖5 姿態(tài)估計(jì)結(jié)果圖

圖6 估計(jì)差值結(jié)果圖

圖7 姿態(tài)估計(jì)結(jié)果圖

5.3 實(shí)驗(yàn)3 穩(wěn)態(tài)情況和切換時(shí)刻的姿態(tài)估計(jì)性能

實(shí)驗(yàn)3 的運(yùn)動(dòng)過(guò)程描述如下:實(shí)驗(yàn)人員被要求先快速旋轉(zhuǎn)手臂到某一角度。然后放在桌子上保持手臂不動(dòng),這時(shí)另外一名實(shí)驗(yàn)人員拿磁鐵不斷在周?chē)蝿?dòng),引入磁場(chǎng)干擾。最后再快速運(yùn)動(dòng)回到原始位置。而圖8 能夠較好地反映出這樣的實(shí)驗(yàn)場(chǎng)景。這樣的運(yùn)動(dòng)情景會(huì)存在多次的動(dòng)態(tài)和穩(wěn)態(tài)的狀態(tài)切換,而且長(zhǎng)時(shí)間的磁場(chǎng)擾動(dòng),使得文獻(xiàn)[14,15]提出的基于閾值的方法失效,大量的磁力計(jì)量測(cè)數(shù)據(jù)被丟棄,導(dǎo)致姿態(tài)漂移。但是本文提出的穩(wěn)態(tài)策略則主要用于解決這一問(wèn)題。圖9 展示了雙策略較標(biāo)準(zhǔn)Kalman 濾波方法更接近真實(shí)值,這是因?yàn)榇艌?chǎng)干擾幅值大而且時(shí)間長(zhǎng),大量的量測(cè)數(shù)據(jù)被拒絕丟棄。使用陀螺儀積分的姿態(tài)估計(jì)值不能夠得到矯正,因此會(huì)產(chǎn)生一定程度上的漂移。圖10 展示了累積誤差的圖像。累積誤差指整個(gè)運(yùn)動(dòng)時(shí)間段內(nèi),估計(jì)值和真實(shí)值之間的絕對(duì)誤差總和,指示了估計(jì)器的精度。另外,從表5 可以看出雙策略的方法能夠達(dá)到比較高的精度,這種方法精度略高于標(biāo)準(zhǔn)Kalman 估計(jì)方法。表6 顯示雙策略方法比原來(lái)的基于標(biāo)準(zhǔn)Kalman 的方法耗時(shí)明顯縮短。因此,雙策略方法確實(shí)是一種有效維持精度且運(yùn)算快速的方法。

圖8 傳感器量測(cè)模值

圖9 姿態(tài)估計(jì)結(jié)果圖

圖10 累積誤差圖

表5 估計(jì)結(jié)果對(duì)比表

表6 時(shí)間耗損比較(實(shí)驗(yàn)3)

6 結(jié)論

本文針對(duì)現(xiàn)有基于標(biāo)準(zhǔn)Kalman 濾波器的姿態(tài)估計(jì)算法復(fù)雜度較高的問(wèn)題,應(yīng)用無(wú)逆Kalman 濾波器,設(shè)計(jì)了基于無(wú)逆Kalman 濾波器的姿態(tài)估計(jì)方法。無(wú)論是干擾較小的實(shí)驗(yàn)1 還是干擾較大的實(shí)驗(yàn)2,這種方法都能夠獲得和基于標(biāo)準(zhǔn)Kalman 濾波器估計(jì)方法相近的計(jì)算精度,同時(shí)計(jì)算時(shí)間大幅下降。這說(shuō)明了本文提出算法的高效性和高精確度。進(jìn)一步地,實(shí)驗(yàn)3 證明,在穩(wěn)態(tài)的情況下,雙策略確實(shí)能夠降低計(jì)算量,同時(shí)避免了姿態(tài)的漂移,能夠獲得更高的精度。

綜上所述,實(shí)驗(yàn)結(jié)果證明了本文提出的方法的有效性。