余 樂， 李洋洋， 李陽光， 吳 超， 王 瑤， 陳 巖

(北京工商大學(xué) 計算機(jī)與信息工程學(xué)院 食品安全大數(shù)據(jù)技術(shù)北京市重點實驗室，北京 100048)

?

用于虛擬現(xiàn)實的九軸慣性傳感單元設(shè)計*

余 樂， 李洋洋， 李陽光， 吳 超， 王 瑤， 陳 巖

(北京工商大學(xué) 計算機(jī)與信息工程學(xué)院 食品安全大數(shù)據(jù)技術(shù)北京市重點實驗室，北京 100048)

設(shè)計并實現(xiàn)了一種基于卡爾曼濾波的用于虛擬現(xiàn)實中運(yùn)動追蹤的關(guān)鍵裝置—九軸慣性傳感單元。以4元數(shù)為狀態(tài)向量，在卡爾曼濾波器中，通過由加速度計和磁強(qiáng)計得到的姿態(tài)角對陀螺儀的漂移進(jìn)行補(bǔ)償。慣性傳感單元的采樣頻率達(dá)到1 kHz。通過連接手機(jī)上位機(jī)測試，很好地實現(xiàn)了姿態(tài)角的融合和矯正。

虛擬現(xiàn)實; 姿態(tài)估計; 卡爾曼濾波; 慣性傳感單元

0 引 言

近年來，虛擬現(xiàn)實技術(shù)在游戲、電影、直播等領(lǐng)域取得了巨大成功。慣性傳感單元是其運(yùn)動姿態(tài)捕捉的關(guān)鍵裝置之一[1，2]。

目前，陀螺儀、加速度計和磁力計是主流的姿態(tài)測量組合[3]，但是三者測量的數(shù)據(jù)通過算法融合才能確定最終的姿態(tài)。常見的姿態(tài)解算算法中，歐拉角法[4～6]在解算過程中會產(chǎn)生奇異點；方向余弦法[5，6]參數(shù)太多，計算過程復(fù)雜；4元素法[6，7]由于實時性好，計算簡單，不會產(chǎn)生奇異點等優(yōu)點，是目前最主流的姿態(tài)解算算法。但是，陀螺儀隨著時間的積分誤差越來越大，同時，磁力計本身也很容易受到周圍磁場的干擾。

另外，在計算簡單的同時保證姿態(tài)解算的精度，是姿態(tài)解算算法研究的關(guān)注點[8]。

提出了一種基于卡爾曼濾波的九軸慣性傳感單元的校正算法，選取姿態(tài)4元數(shù)作為狀態(tài)變量，簡化系統(tǒng)維數(shù)，減少計算量。用加速度計和磁強(qiáng)計估計載體的姿態(tài)角，利用歐拉角和4元數(shù)之間的關(guān)系將其轉(zhuǎn)換為4元數(shù)，將此值作為姿態(tài)4元數(shù)的觀測值，并列出其與狀態(tài)量之間的約束關(guān)系式。通過卡爾曼濾波估計出載體的姿態(tài)角，降低對陀螺精度的依賴，提高姿態(tài)確定的精度。最終，將九軸傳感單元電路掛載到手機(jī)上位機(jī)程序中，進(jìn)行驗證。

1 系統(tǒng)方案

如圖1所示，慣性傳感單元電路系統(tǒng)結(jié)構(gòu)包含微控制器STM32 F103C8T6，6軸陀螺儀MPU6000，3軸磁力計HMC5983，電源模塊LP5900SD—3.3以及用于與上位機(jī)連接的USB_OTG接口。

圖1 系統(tǒng)結(jié)構(gòu)圖

2 擴(kuò)展卡爾曼濾波

圖2 擴(kuò)展卡爾曼濾波算法結(jié)構(gòu)

本文由磁強(qiáng)計和加速度計得到測量值，由陀螺儀解算得到的狀態(tài)量進(jìn)行數(shù)據(jù)融合，最終計算出系統(tǒng)的姿態(tài)。

2.1 狀態(tài)方程

本文選取姿態(tài)4元數(shù)作為狀態(tài)向量X=[q0q1q2q3]T，并且利用4元數(shù)的更新方程Qk+1=Qk·qk，建立離散濾波方程

Xk=AkXk+Wk

(1)

(2)

2.2 測量方程

4元數(shù)的測量值利用前述加速度計和磁強(qiáng)計所轉(zhuǎn)換的4元數(shù)，建立相應(yīng)的測量方程

(3)

Zk=CkXk+Vk

(4)

2.3 濾波算法

由于測量方程和狀態(tài)方程都是離散方程，因此，可以直接利用離散卡爾曼濾波算法的公式，對時間和測量的信息進(jìn)行更新，計算步驟為：

1)給定算法的初值，狀態(tài)向量的初值為4元數(shù)的初值，估計均方差陣可以去單位矩陣。

2)計算步驟(1)的預(yù)測值

(5)

(6)

(7)

3)計算濾波增益

(8)

4)計算狀態(tài)的估計值

(9)

5)計算均方誤差

Pk+1=(I-Kk+1Ck+1)Pk+1/k

(10)

令k=k+1,重復(fù)上述步驟，如圖3所示為算法流程。

流程總體上分為加載數(shù)據(jù)、設(shè)置初值、設(shè)置濾波參數(shù)，卡爾曼濾波、輸出結(jié)果5步。首先將傳感器中采集的數(shù)據(jù)保存到文件中，在程序的開始將其加載，然后分別利用陀螺儀的數(shù)據(jù)和加速度計、磁強(qiáng)計的數(shù)據(jù)計算出狀態(tài)量和測量量的初值，設(shè)置卡爾曼濾波器的參數(shù)后，將初值代入卡爾曼濾波器的函數(shù)中得到濾波后的結(jié)果，通過圖形顯示出濾波前后的對比結(jié)果。

圖3 程序設(shè)計流程

3 硬件設(shè)計與測試結(jié)果

3.1 九軸慣性傳感單元設(shè)計與測試

圖4為九軸陀螺儀單元，其中包含1個主控單元STM32F103C8T6，1個6軸陀螺儀MPU6000，1個磁強(qiáng)計HMC5983，以及1個電源降壓芯片LP5900SD—3.3。

圖4 九軸慣性傳感單元

在靜止?fàn)顟B(tài)下，采集傳感器的數(shù)據(jù)，通過濾波算法后顯示結(jié)果。圖5為陀螺儀3個姿態(tài)角的測量角度與濾波角度之間的誤差。

從圖中可以看出，陀螺儀具有測量誤差。而經(jīng)過卡爾曼濾波，初步實現(xiàn)了校正和融合功能，在一定程度上保證了測量結(jié)果的精準(zhǔn)度。y軸和z軸的測量結(jié)果比較準(zhǔn)確，可以估計出傳感器的橫滾角和俯仰角。濾波后，x軸的漂移得到了校準(zhǔn)，從而保證了航向角的準(zhǔn)確。

3.2 手機(jī)上位機(jī)測試

設(shè)置慣性傳感單元的采樣頻率為1kHz。通過連接手機(jī)端的上位機(jī)程序測試，將慣性傳感單元分別沿著x軸，y軸和z軸3個方向擺放，如圖6所示?？梢钥闯?慣性傳感單元可以正常測試出姿態(tài)角。

圖5 測量角度和濾波角度對比

圖6 慣性傳感單元沿不同方向擺放測試結(jié)果

4 結(jié)束語

設(shè)計并實現(xiàn)了一種基于卡爾曼濾波的九軸慣性傳感單元(即陀螺儀，加速度計和磁強(qiáng)計)。以4元數(shù)為狀態(tài)向量，在卡爾曼濾波器中通過加速度計和磁強(qiáng)計得到的姿態(tài)角對陀螺儀的漂移進(jìn)行補(bǔ)償。通過連接手機(jī)上位機(jī)測試，結(jié)果顯示:九軸慣性傳感單元可以很好地實現(xiàn)姿態(tài)角的融合與矯正。

[1]FriederWittmann,OlivierLambercy,RomanRGonzenbach,etal.Assessment-drivenarmtherapyathomeusinganIMU-basedvirtualrealitysystem[C]∥IEEEInternationalConferenceonRehabilitationRobotics(ICORR),2015:707-712.

[2]CostaRaphael,GuoRongkai,QuarlesJohn.Towardsusableunderwatervirtualrealitysystems[C]∥IEEEVirtualReality(VR),2017:271-272.

[3]NaeemMumtaz,SanaArif,NimraQadeer，etal.DevelopmentofalowcostwirelessIMUusingMEMSsensorsforpedestriannavigation[C]∥InternationalConferenceonCommunication,ComputingandDigitalSystems,Islamabad,2017:310-315.

[4]ChulWooKang,YoungMinYoo,ChanGookPark.PerformanceimproveofattitudeestimationusingmodifiedEulerangle-basedKalmanfilter[J].JournalofInstituteofControl,RoboticsandSystems,2008，14(9):881-885.

[5] 孫克誠.三軸飛行仿真平臺控制系統(tǒng)的設(shè)計與研究[D].南昌：南昌航空大學(xué)，2016.

[6] 王偉棟.基于微慣性技術(shù)的數(shù)據(jù)手套研究[D].上海：上海交通大學(xué)，2014.

[7]HsuYuLiang,WangJeenShing,ChangCheWei.Awearableinertialpedestriannavigationsystemwithquaternion-basedextendedKalmanfilterforpedestrianlocalization[J].IEEEJournals&Magazines,2017,17(10):3193-3206.

[8]SimonD.Optimalstateestimation:Kalman,H-infinity,andnonlinearapproaches[M].HoboKen:Wiley&Sons,2006:309-332.

陳 巖 (1963-),女，博士，教授，博士生導(dǎo)師，主要從事無線傳感器網(wǎng)絡(luò)技術(shù)應(yīng)用、數(shù)字通信抗干擾技術(shù)工作。

Design of 9-axis inertial measurement unit for virtual reality*

YU Le, LI Yang-yang, LI Yang-guang, WU Chao, WANG Yao, CHEN Yan

(Beijing Key Laboratory of Big Data Technology for Food Safety,School of Computer and Information Engineering,Beijing Technology and Business University,Beijing 100048,China)

A 9-axis inertial measurement unit(IMU)based on Kalman filtering which is key device for motion tracking in virtual reality(VR)technology is designed and realized.Quaternion is used as state vector,and the attitude angle which is calculated from accelerometers and magnetometers is used to compensate for gyro drift.The IMU is designed with a sampling frequency of 1 kHz.Test by connecting cell phone and upper PC,the 9-axis IMU can achieve fusion and correction of attitude angle very well.

virtual reality; attitude estimation; Kalman filtering; inertial measurement unit(IMU)

10.13873/J.1000—9787(2017)07—0100—03

2017—05—31

北京市自然科學(xué)基金資助項目(4174086);國家自然科學(xué)基金資助項目(61473009)

TP 391

A

1000—9787(2017)07—0100—03

余 樂(1983-),男,通訊作者,博士，講師，碩士生導(dǎo)師,從事類腦計算與人工智能工作,E—mail:ladd_u@163.com。