基于VR設(shè)備中IMU的頭部姿態(tài)感知算法研究

張雁鵬,高建勇,周志杰

(1.蘇州熱工研究院有限公司 設(shè)備管理部，廣東 深圳 518028; 2.華能山東石島灣核電有限公司 生產(chǎn)管理部，山東 榮成 264312)

0 引言

由于頭戴式顯示設(shè)備(HMD,head-mounted device)的發(fā)展，基于虛擬現(xiàn)實(VR,virtual reality)的教育培訓隨之流行開來[1-4]。由于VR技術(shù)具有集成性、交互性等特點，利用VR技術(shù)可以創(chuàng)造出一個真實的教學環(huán)境，可以方便的模擬課堂環(huán)境下不便于布置的一些場景，為教師教學提供了形象直觀的表達工具，將信息技術(shù)與學科課程的教與學融為一體。而在使用過程中，使用者可以與VR設(shè)備播放視頻中的對象進行交互，可以像真實場景中隨著頭部的轉(zhuǎn)動看到不同的景色，達到身臨其境的效果。因此，VR設(shè)備對使用者頭部姿態(tài)的感知尤為重要。

為了方便地使用VR頭戴式設(shè)備，必須滿足一些條件：首先，設(shè)備在形式上必須是獨立的，有自己獨立的電源和處理系統(tǒng)； 其次，設(shè)備的重量和功耗必須越低越好；最后，設(shè)備處理的延遲時間應該越小越好。采用外部視覺跟蹤設(shè)備對人體頭部姿態(tài)進行測量的方式[5-6]，需要安裝額外的裝置，破壞了VR設(shè)備使用的獨立性，同時極大地增加了使用的成本，且具有場地使用的局限性，同時由于通過外部設(shè)備進行姿態(tài)處理，存在時間延遲的問題。而使用低成本慣性測量單元(IMU，inertial measurement unit)與頭戴式設(shè)備固定連接的方式進行姿態(tài)測量，可以方便地通過VR設(shè)備中的嵌入式系統(tǒng)讀取IMU內(nèi)部的加速度、角速度以及磁場強度數(shù)據(jù)，進而運行姿態(tài)解算程序，在保證可靠精度的同時，降低了使用成本。

在IMU姿態(tài)解算中，歐拉角法、四元數(shù)法等為常用的依靠陀螺儀數(shù)據(jù)的姿態(tài)解算方法。由于歐拉角法導致的“萬向鎖”問題，大多數(shù)測姿系統(tǒng)多采用四元數(shù)法進行姿態(tài)更新，而求解四元數(shù)更新方程多采用四階龍格庫塔法[7]。龍格庫塔法通過在每個子區(qū)間的選定點上，用對一階導數(shù)的多次求值，來代替截斷泰勒級數(shù)中計算高階導數(shù)的方法。而這種常規(guī)的方法，將4個微分方程進行多次迭代，描述算法的邏輯簡單、精度較高，缺點是計算量大、實時性較差。而計算量所對應的就是整個VR設(shè)備的功耗問題。對于使用電池的頭戴式VR設(shè)備，功耗問題很大程度上影響了用戶體驗。同時，單純使用依靠陀螺儀數(shù)據(jù)的基于四元數(shù)更新的姿態(tài)解算方法，經(jīng)過長時間運行后，系統(tǒng)的航向角會發(fā)生發(fā)散，不利于提供穩(wěn)定的航向精度，因此需要使用地磁信息進行輔助。

針對準確感知頭部姿態(tài)和降低解算計算量的問題，本文設(shè)計了4種基于頭戴式IMU的姿態(tài)解算方法，包括單獨使用龍格庫塔法計算姿態(tài)角、龍格庫塔法更新四元數(shù)加融合地磁的擴展卡爾曼濾波算法、單獨使用三階泰勒法計算姿態(tài)角以及使用三階泰勒法更新四元數(shù)加融合地磁的濾波算法四種算法，并設(shè)計實驗對比了這四種算法的姿態(tài)精度和運算效率。

1 姿態(tài)感知算法

本文設(shè)計了4種基于頭戴式IMU的姿態(tài)解算方法，包括單獨使用龍格庫塔法計算姿態(tài)角、龍格庫塔法更新四元數(shù)加融合地磁的擴展卡爾曼濾波算法、單獨使用三階泰勒法計算姿態(tài)角以及使用三階泰勒法更新四元數(shù)加融合地磁的濾波算法四種算法。如圖1所示，整體框圖表示基于擴展卡爾曼濾波器(EKF,extended kalman filter)的姿態(tài)感知算法，即龍格庫塔法更新四元數(shù)加融合地磁的濾波算法和使用三階泰勒法更新四元數(shù)加融合地磁的濾波算法。圖中，虛線框部分表示只依靠陀螺儀數(shù)據(jù)的姿態(tài)解算方法。該算法通過不斷迭代四元數(shù)方程(如式(2)所示)求解姿態(tài)四元數(shù)，進而可以將四元數(shù)通過旋轉(zhuǎn)矩陣換算為3-D姿態(tài)角。按照四元數(shù)方程求解方法的不同，可以分為龍格庫塔法和三階泰勒展開法，也就是本文中使用的姿態(tài)解算方法中的兩種。四種算法的具體細節(jié)將會在下面的章節(jié)中一一解釋。

圖1 測姿算法整體結(jié)構(gòu)框圖

1.1 相關(guān)基礎(chǔ)知識

剛體在空間中的姿態(tài)可以通過四元數(shù)進行表示，四元數(shù)可以表示為：

q=q0+q1i+q2j+q3k

(1)

其中:q0表示四元數(shù)的實部或標量部分，而q1i+q2j+q3k表示虛部或矢量部分。

四元數(shù)通過四元數(shù)微分方程[8]求得：

(2)

(3)

由四元數(shù)和姿態(tài)角之間的關(guān)系可以將四元數(shù)轉(zhuǎn)化為相應的俯仰角θ、滾轉(zhuǎn)角γ、偏航角ψ：

(4)

1.2 四階龍格庫塔法

四階龍格庫塔法是求解四元數(shù)微分方程的經(jīng)典數(shù)值積分方法，通過在每個子區(qū)間的選定點上，用對一階導數(shù)的多次求值，來代替截斷泰勒級數(shù)中計算高階導數(shù)的方法，其求解過程可寫成以下遞推表達式：

(5)

其中：Δt為采樣時間，k0、k1、k2、k2為四階龍格庫塔法的系數(shù)，其表達式如下:

(6)

1.3 三階泰勒展開法

根據(jù)文獻[7]，另一種求解四元數(shù)微分方程的方法是對四元數(shù)微分方程進行泰勒級數(shù)展開，然后取三階項，可以表示為：

(7)

其中：Δφt+1和Δφt分別為t+1和t時刻的角增量。

1.4 融合地磁的間接擴展卡爾曼濾波方法

EKF是標準卡爾曼濾波在非線性情形下的一種擴展，它是一種高效率的遞歸濾波器(自回歸濾波器)，包含狀態(tài)方程和測量方程。EKF的基本思想是利用泰勒級數(shù)展開將非線性系統(tǒng)模型線性化，然后采用標準卡爾曼濾波框架對信號進行濾波[9]。在本文中，我們沒有直接利用EKF對系統(tǒng)的狀態(tài)量進行估計，而是采用了它的間接形式，即對系統(tǒng)狀態(tài)量的誤差量進行估計。這樣做的好處是，由于系統(tǒng)誤差量相對于直接狀態(tài)量屬于小值，所以對誤差方程的線性化相較于對直接狀態(tài)方程進行線性化，丟失的信息更少，因此采用間接方式使EKF估計的結(jié)果更接近于最優(yōu)值。

我們利用EKF的間接形式，融合地磁數(shù)據(jù)，對測姿系統(tǒng)的姿態(tài)誤差以及陀螺儀零位偏移誤差進行最優(yōu)估計。測姿算法整體結(jié)構(gòu)框圖如圖1所示。算法的主要流程為：首先，由當前時刻經(jīng)過零偏補償后的陀螺儀輸出y和前一時刻旋轉(zhuǎn)四元數(shù)的后驗估計q+通過四元數(shù)更新方程(該方程可以通過四階龍格庫塔法或者三階泰勒展開求解，也就是本文中所提到的龍格庫塔法和三階泰勒展開法)得到當前時刻旋轉(zhuǎn)四元數(shù)的先驗估計q-；其次，通過計算得到的q-計算當前時刻方向余弦矩陣的先驗估計Cnb-；進而，由EKF計算得出的姿態(tài)誤差向量δφ，然后通過方向余弦更新方程計算出補償后的方向余弦矩陣Cnb +；最后，計算得出3-D姿態(tài)向量φ和下一時刻旋轉(zhuǎn)四元數(shù)的后驗估計q+用于下一次算法迭代。

1.4.1 系統(tǒng)建模

姿態(tài)測量問題可以描述為離散隨機系統(tǒng)的估計問題：

xk=f(xk,wk)

zk=h(xk,εk)

(8)

其中:xk∈Rn為k時刻的狀態(tài)；zk∈Rm為k時刻測量值；f(·)和h(·)分別表示系統(tǒng)的狀態(tài)方程和測量方程；wk和εk分別表示過程噪聲和測量噪聲。

系統(tǒng)狀態(tài)向量xk包含旋轉(zhuǎn)四元數(shù)qk和陀螺儀零位偏移bω,k，可以寫為：

(9)

只考慮陀螺儀的零偏誤差，角速率真值ωk可由陀螺儀輸入信號yk獲得：

ωk=yk-bω,k+wω,k

(10)

其中:wω,k～N(0,Qω,k)表示陀螺儀的測量噪聲。

在本文中，將陀螺儀零位偏移模型表示為一階馬爾科夫模型[6]：

bω,k=bω,k-1+wbω,k

(11)

其中:wbω,k～N(0,Qbω,k)表示陀螺儀的零偏噪聲。

EKF通過對當前狀態(tài)估計的均值進行泰勒展開，從而將非線性系統(tǒng)模型線性化[9]。本文引入EKF的間接形式對姿態(tài)測量系統(tǒng)狀態(tài)向量xk的誤差δxk進行估計，進而對xk進行補償。因為誤差四元數(shù)δqk為小值，所以姿態(tài)誤差可以用對應的姿態(tài)誤差角δφk來表示[10]。因此，系統(tǒng)的誤差狀態(tài)向量將包含6維元素，即:

(12)

其中:δφk表示姿態(tài)誤差角，δbω,k表示陀螺儀零位偏移誤差。

1.4.2 誤差狀態(tài)模型

為了實現(xiàn)濾波器的推導，首先應寫出誤差的微分方程，然后將其離散化[8]。誤差的連續(xù)狀態(tài)方程可以描述為：

(13)

其中:wk～N(0,Qk)為過程噪聲向量；Fc,k和Gc,k分別表示連續(xù)系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和噪聲轉(zhuǎn)移矩陣，具體推導見[11]。

(14)

其中: [ωk×]表示更新后的角速率ωk的斜對稱矩陣；I表示單位矩陣；0表示零矩陣。

(15)

離散化后的誤差狀態(tài)方程為：

(16)

將離散轉(zhuǎn)移矩陣Φk進行一階泰勒級數(shù)展開(相當于將非線性模型線性化)，可得：

Φk=exp(Fc,kΔt)≈I6×6+Fc,kΔt

(17)

其中:Δt表示IMU的采樣間隔。

1.4.3 誤差測量模型

系統(tǒng)的誤差測量模型表示為：

zk=Hδxk+εk

(18)

其中:zk為誤差測量向量；H為測量轉(zhuǎn)移矩陣；εk～N(0,Qm,k)為測量噪聲的協(xié)方差矩陣。

在計算航向角誤差前，需要計算地磁航向角。假定三軸磁強計各軸與載體坐標系各軸重合，測量3個軸向的地磁場強度，令磁強計輸出為：

(19)

(20)

式中，n系下IMU的滾轉(zhuǎn)角γk、俯仰角θk通過公式(4)求得。

由于地磁北極與地理北極不完全一致，定義地球表面任意點的地磁場強度矢量所在的垂直平面(地磁子午面)與地理子午面的夾角為地磁偏角Bd，不同地區(qū)對應有不同的磁偏角。因此地磁航向角可以表示為：

(21)

濾波器的更新方程為：

(22)

2 實驗

2.1 實驗設(shè)置

為了驗證本文提出算法的有效性，本文設(shè)計了多姿態(tài)運動實驗。使用的IMU是SBG公司生產(chǎn)的IG-500N，包含三軸加速度計、三軸陀螺儀和三軸磁強計，具體性能指標如表1所示。

表1 IMU具體性能指標

多姿態(tài)運動實驗地點為室內(nèi)直線走廊，實驗者將IMU佩戴在頭部，依次進行蛇形、俯仰、晃動、轉(zhuǎn)圈和倒退等運動姿態(tài)，如圖2所示。

圖2 實驗環(huán)境及實驗姿態(tài)演示

2.2 實驗結(jié)果及分析

本文使用Intel i5-540M處理器的筆記本電腦作為硬件平臺，使用Matlab R2013a作為軟件處理平臺。將IMU中自帶商業(yè)姿態(tài)解算軟件計算的結(jié)果作為基準，計算單獨使用龍格庫塔法計算姿態(tài)角、龍格庫塔法更新四元數(shù)加融合地磁的擴展卡爾曼濾波算法、單獨使用三階泰勒法計算姿態(tài)角和使用三階泰勒法更新四元數(shù)加融合地磁的濾波算法四種算法的解算精度，并列在同一表格中，如表2所示。解算精度由最大偏差值、最小偏差值、平均偏差值和偏差值標準差四項指標來衡量。該四項指標由當前算法的計算結(jié)果與基準值差值的絕對值計算得出。同時，為了評估算法的運行效率，給出了計算14 000組數(shù)據(jù)所需的時間。

表2 不同算法解算出姿態(tài)角的精度

按照算法依賴的傳感器數(shù)據(jù)，將這4種算法分為兩組。由于只依賴陀螺儀數(shù)據(jù)，將四階龍格庫塔法和三階泰勒展開法劃分為第一組，其余依賴陀螺儀和磁強計數(shù)據(jù)的兩種算法劃分為第二組。首先，進行組內(nèi)比較。三階泰勒展開法相對于四階龍格庫塔法來說，解算精度基本一致，但是三階泰勒展開法在解算相同數(shù)據(jù)量的前提下，解算時間明顯優(yōu)于四階龍格庫塔法；三階泰勒展開法加地磁修正算法和四階龍格庫塔法加地磁修正算法也具有相同的特點。其次，進行組間比較。融合地磁的卡爾曼濾波算法解算精度都要明顯優(yōu)于單獨使用兩種四元數(shù)更新算法，但是解算時間相對較長。由此可得，在保證較高解算精度和較少時間的前提下，三階泰勒法更新四元數(shù)加融合地磁的濾波算法具有更優(yōu)的性能。

將IMU內(nèi)部商業(yè)姿態(tài)解算算法實時輸出的姿態(tài)角和使用三階泰勒法更新四元數(shù)加融合地磁的濾波算法解算出的姿態(tài)角畫在同一坐標系中，如圖3所示。從圖中可以看出，該算法可以有效地解算出人體頭部的實際姿態(tài)。

圖3 多姿態(tài)運動解算結(jié)果對比

3 結(jié)束語

針對頭戴式IMU的姿態(tài)解算問題，本文設(shè)計了4種姿態(tài)解算方法，并對比了這4種算法在不同運動狀態(tài)下的姿態(tài)精度。使用三階泰勒展開法更新四元數(shù)，相比于四階龍格庫塔法具有更短的計算時間，同時提出的基于擴展卡爾曼濾波器的姿態(tài)測量算法，融合了地磁數(shù)據(jù)，保證了姿態(tài)解算的精度。本文實驗結(jié)果表明，使用四元數(shù)微分方程的三階泰勒展開遞推式更新四元數(shù)，同時利用擴展卡爾曼濾波器融合地磁信息進行修正的姿態(tài)解算方法，在保證較高解算精度和較少計算時間的前提下，能夠有效、穩(wěn)定的輸出高精度姿態(tài)數(shù)據(jù)。但是，本文提出的算法在磁干擾較強的環(huán)境中會出現(xiàn)較大的解算誤差。在未來的工作中，期望能夠找到一種補償磁干擾環(huán)境中姿態(tài)解算誤差的方法來解決此類問題。