何百岳 張文安

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

0 引言

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

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

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

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

1 問題描述

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

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

2 慣性傳感器信號分析和處理

加速計和磁力計的量測信號可以分成以下3 部分:

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

陀螺儀的量測方程如下:

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

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

2.1 基于加速度計的嚴重干擾拒絕

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

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

2.2 基于磁力計的嚴重干擾拒絕

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

綜上所述,磁場的觀測向量可以寫成如下形式:

2.3 無反三角運算的磁傾角判定規(guī)則

在判定是否存在較大磁場干擾時,判定條件需要計算每個時刻的磁傾角。反三角函數(shù)的計算會給嵌入式系統(tǒng)帶來較大的計算負擔。而本文通過化簡,剔除了每一時刻的反三角函數(shù)估計運算,新的判定條件可以寫為

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

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

3 動態(tài)狀態(tài)下的姿態(tài)估計策略

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

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

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

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

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

其中,ωt是三軸陀螺儀的角速度量測值,[ω×]是反對稱矩陣的表示方法。

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

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

3.3 無逆Kalman 濾波器設計

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

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

對于本文研究的動態(tài)系統(tǒng)式(9)來說,標準Kalman 濾波器所示如下:

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

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

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

最后,狀態(tài)四元數(shù)需要被標準化表示旋轉。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 實驗和討論

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

圖1 實驗場景

5.1 實驗1 慢速且無磁場干擾

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

表1 估計結果對比表

表2 時間耗損比較(實驗1)

圖2 姿態(tài)估計結果圖

圖3 估計差值結果圖

5.2 實驗2 快速運動且存在磁場干擾

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

表3 時間耗損對比表(實驗2)

表4 估計結果對比表

圖4 噪聲干擾圖

圖5 姿態(tài)估計結果圖

圖6 估計差值結果圖

圖7 姿態(tài)估計結果圖

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

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

圖8 傳感器量測模值

圖9 姿態(tài)估計結果圖

圖10 累積誤差圖

表5 估計結果對比表

表6 時間耗損比較(實驗3)

6 結論

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

綜上所述,實驗結果證明了本文提出的方法的有效性。