熊新炎， 黃 濤， 溫子豪， 張 童

(哈爾濱商業(yè)大學(xué) 輕工學(xué)院，哈爾濱 150028)

人體姿態(tài)捕捉廣泛應(yīng)用于計(jì)算機(jī)游戲制作、軍事訓(xùn)練、人體醫(yī)療康復(fù)、行為預(yù)判、動作識別等領(lǐng)域.傳統(tǒng)的機(jī)械式姿態(tài)捕捉存在運(yùn)動幅度受限、無法捕獲較為復(fù)雜的姿態(tài).光學(xué)式姿態(tài)捕捉系統(tǒng)造價(jià)昂貴，標(biāo)定光點(diǎn)會出現(xiàn)遮擋，后期處理時(shí)間長[1].隨著MEMS微機(jī)電技術(shù)和計(jì)算機(jī)技術(shù)的快速發(fā)展，MEMS慣性傳感器在可穿戴便攜設(shè)備中應(yīng)用越來越成熟，低成本慣性測量單元具有體積小、質(zhì)量輕、功耗低、性價(jià)比高等特點(diǎn)，在人體姿態(tài)捕捉系統(tǒng)得到普遍應(yīng)用[2].由于外界噪聲和磁場變化等多因素影響，MEMS慣性傳感器無法長時(shí)間對姿態(tài)進(jìn)行精確測量，其精度和穩(wěn)定性較低[3].

本文采用三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)組成的9維慣性測量單元對姿態(tài)進(jìn)行測量，對三軸加速度計(jì)進(jìn)行標(biāo)定及建立誤差模型，利用最小二乘法確認(rèn)誤差參數(shù)[4].根據(jù)歐拉角法對傾向角和航向角分開求解特點(diǎn)，減小磁場變化干擾[5].再對歐拉角中奇異值問題進(jìn)行改進(jìn)，分為兩種狀態(tài)的濾波模式，克服了傳統(tǒng)歐拉角中的奇異值問題.

1 姿態(tài)解算

常用的姿態(tài)解算方法有歐拉角、方向余弦法、四元數(shù)法.方向余弦法方程參數(shù)多，計(jì)算量較大，效率低，不方便對姿態(tài)的變化的理解.四元數(shù)法計(jì)算量小，易于操作，但易受外界磁場影響.歐拉角表達(dá)姿態(tài)形象直觀，簡單易懂，但會出現(xiàn)萬向鎖現(xiàn)象[6].由于歐拉角法求取時(shí)傾斜角和航向角分離的特點(diǎn)，求取的傾斜角不受外界磁場變化干擾.為此，對傳統(tǒng)的歐拉角中奇異值問題進(jìn)行改進(jìn)，分為一般姿態(tài)值和奇異值狀態(tài)姿態(tài)值兩種濾波模式.

傳感器在測量時(shí)會出現(xiàn)誤差，例如敏感軸比例系數(shù)誤差、零點(diǎn)偏移、外界溫度等等因素會導(dǎo)致出現(xiàn)芯片靈敏度變差，測量精度不準(zhǔn)等問題[7].對MEMS慣性傳感器進(jìn)行誤差標(biāo)定補(bǔ)償.姿態(tài)解算流程圖如圖1所示.

1.1 三軸加速度計(jì)誤差建模及標(biāo)定

根據(jù)上述的影響因素建立三軸加速度計(jì)誤差模型[8]：

圖1 姿態(tài)解算流程Figure 1 Attitude calculation process

(1)

(2)

其中:Ax、Ay、Az為三軸加速度計(jì)輸出原始值，Ax1、Ay1、Az1為三軸加速度計(jì)進(jìn)行誤差補(bǔ)償后輸出值，A_SCx、A_SCy、A_SCz為三軸加速度計(jì)的三軸比例系數(shù)，A_Kyx、A_Kzx、A_Kxy、A_Kzy、A_Kxz、A_Kyz為耦合因數(shù)，A_OSx、A_OSy、A_OSz為零位偏移.

對于三軸加速度計(jì)的誤差不穩(wěn)定性，采用靜態(tài)六位置法對比例系數(shù)、安裝誤差和零位偏移進(jìn)行標(biāo)定，借助高精度轉(zhuǎn)臺進(jìn)行測試，利用最小二乘法對六位置法測試數(shù)據(jù)進(jìn)行最小誤差估計(jì)得出12個(gè)誤差系數(shù)[9].將式(2)用Y=WX代替，矩陣X由12個(gè)待測系數(shù)組成，矩陣W為三軸加速度計(jì)原始值，矩陣Y為已知的重力加速度值.

位置1，轉(zhuǎn)臺設(shè)備Z軸向下，設(shè)定此時(shí)[Ax1Ay1Az1]=[0 0 1]，在該位置采集n1個(gè)原始加速度值[AxAyAz]，此時(shí)

(3)

其中:矩陣Y1每一行元素為[0 0 1]，w1為原始加速度值.

位置2，轉(zhuǎn)臺設(shè)備Z軸向下，設(shè)定此時(shí)[Ax1Ay1Az1]=[0 0 -1]，在該位置采集n2個(gè)原始加速度值[AxAyAz]，此時(shí)

(4)

其中:矩陣Y2每一行元素為[0 0 -1]，w2為原始加速度值.

位置3，轉(zhuǎn)臺設(shè)備Z軸向下，設(shè)定此時(shí)[Ax1Ay1Az1]=[0 1 0]，在該位置采集n3個(gè)原始加速度值[AxAyAz]，此時(shí)

(5)

其中:矩陣Y3每一行元素為[0 1 0]，w3為原始加速度值.

以此類推，采集位置4、位置5、位置6時(shí)的數(shù)據(jù).

將上述數(shù)據(jù)進(jìn)行整理，令n=n1+n2+n3+n4+n5+n6，得

Yn×3=wn×4X4×3

(6)

(7)

利用最小二乘法可得:

X4×3=[WTW]-1WTY

(8)

1.2 改進(jìn)歐拉角法姿態(tài)解算

設(shè)導(dǎo)航坐標(biāo)系為onxnynzn，人體坐標(biāo)系為obxbybzb，空間的關(guān)系如下：

(9)

(10)

當(dāng)靜止或者勻速運(yùn)動時(shí)，用加速度信號對姿態(tài)角進(jìn)行估計(jì)[11]，由已知加速度重力分量得出X1、X2、X3.

(11)

歐拉角的更新為：

(12)

(13)

結(jié)合X1、X2、X3可以得出：

(14)

其中:ωx、ωy、ωz為各軸的角速度，Δt為采樣時(shí)間.

1.2.1 濾波階段KF1，傾斜角濾波

傾斜角濾波階段使用的是加速度信號和角速度信號，傾斜角和加速度信號是線性關(guān)系，采用標(biāo)準(zhǔn)卡爾曼濾波器，濾波過程如下[12]：

(15)

其中

(16)

誤差系數(shù)矩陣為：

(17)

更新數(shù)據(jù)為：

(18)

其中

(19)

Hk=-gI3×3，Qk-1為三軸陀螺儀輸出的噪聲協(xié)方差矩陣，Rk-1為三軸加速計(jì)測量的噪聲協(xié)方差矩陣，測量值yk為加速度測量值.

1.2.2 濾波階段KF2，航向角濾波

航向角與三軸磁力計(jì)信號是非線性關(guān)系，采用擴(kuò)展卡爾曼濾波[13]，分為奇異值時(shí)濾波和非奇異值時(shí)濾波兩種情況.

對非奇異值時(shí)， 航向角認(rèn)定為標(biāo)量， 由航向角的微分方程的誤差協(xié)方差矩陣pk,2的更新矩陣為：

(20)

觀察方程為：

yk=h(X1,X2,X3,φ)=Hb

(21)

誤差協(xié)方差矩陣為：

(22)

(23)

其中:HN、HE、HD為三軸磁力計(jì)各軸的磁場強(qiáng)度.

對于出現(xiàn)奇異值時(shí)，濾波器模式為奇異值模式.俯仰角接近90°時(shí)，狀態(tài)方程為φ-λ，俯仰角接近-90°時(shí)，狀態(tài)方程為φ+λ.狀態(tài)方程使用方向余弦更新矩陣[14]，φ-λ或φ+λ的誤差協(xié)方差矩陣為Pk,3，當(dāng)俯仰角不是奇異值時(shí)，Pk,3為：

Pk,3=Pf+Pφ≈Pk(2,2)+Pk,2

(24)

由于X2的誤差很小，所以X2的誤差被認(rèn)為是橫滾角誤差，在正常情況下Pk,3每次迭代過程中必須用上式初始化.

用方向余弦矩陣用來更新誤差協(xié)方差為：

(25)

通過引入元素φ+λ和φ-λ解決[15]，此時(shí)，微分方程為：

(26)

(27)

使用矩陣G,更新誤差協(xié)方差為：

(28)

當(dāng)俯仰角接近90°時(shí)，觀察方程為：

(29)

當(dāng)俯仰角接近-90°時(shí)，觀察方程為：

(30)

2 實(shí)驗(yàn)及結(jié)果分析

如圖2所示，實(shí)驗(yàn)設(shè)備為三軸慣性轉(zhuǎn)臺和MEMS慣性傳感器，測試時(shí)將傳感器固定在轉(zhuǎn)臺上.

圖2 慣性轉(zhuǎn)臺和慣性傳感器Figure 2 Inertial turntable and inertial sensor

2.1 三軸加速度計(jì)標(biāo)定實(shí)驗(yàn)

將三軸加速度計(jì)在上述的位置1、位置2、位置3、位置4、位置5、位置6的進(jìn)行測試，為了消除實(shí)驗(yàn)過程中可能存在的操作失誤等因素，對三軸加速計(jì)靜態(tài)原始數(shù)據(jù)多次采集，得到由加速度原始數(shù)據(jù)組成的W和已知重力加速度向量Y，再結(jié)合式(5)利用最小二乘法求得12個(gè)標(biāo)定系數(shù)組成的矩陣X4×3.采集靜態(tài)原始數(shù)據(jù)如圖3、4所示，并求出12個(gè)標(biāo)定參數(shù)的均值和標(biāo)準(zhǔn)差.實(shí)驗(yàn)結(jié)果表明，求得的誤差參數(shù)的標(biāo)準(zhǔn)差非常小，可以很好的對三軸加速度計(jì)的安裝誤差、比例系數(shù)和安裝誤差進(jìn)行補(bǔ)償.

圖3 三軸加速度計(jì)各軸的零偏和比例系數(shù)Figure 3 Zero bias and proportionality coefficients of three axis accelerometer

圖4 三軸加速度計(jì)各軸的耦合系數(shù)Figure 4 Coupling coefficient of each axis of three axis accelerometer

2.2 MEMS慣性傳感器動態(tài)實(shí)驗(yàn)

將慣性轉(zhuǎn)臺進(jìn)行隨意轉(zhuǎn)動、奇異點(diǎn)位置轉(zhuǎn)動和外界磁場變化時(shí)轉(zhuǎn)動，輸出改進(jìn)的歐拉角法和四元數(shù)法的兩種姿態(tài)角，其結(jié)果如圖5～7所示，在一般轉(zhuǎn)動和奇異點(diǎn)轉(zhuǎn)動的情況下改進(jìn)的歐拉角與四元數(shù)輸出結(jié)果相近，避免的傳統(tǒng)的歐拉角法奇異點(diǎn)問題，在出現(xiàn)外界磁場變化時(shí)，改進(jìn)歐拉角法比四元數(shù)法更加穩(wěn)定，誤差更小.

圖5 隨意轉(zhuǎn)動輸出歐拉角Figure 5 Rotate the output Euler Angle freely

圖6 奇異點(diǎn)轉(zhuǎn)動輸出歐拉角Figure 6 Singular point rotation output Euler Angle

圖7 外界磁場突變輸出歐拉角Figure 7 External magnetic field mutation output euler Angle

3 結(jié) 語

經(jīng)過三軸加速度計(jì)的標(biāo)定實(shí)驗(yàn)和MEMS慣性傳感器的動態(tài)實(shí)驗(yàn)得知，基于最小二乘法確定誤差參數(shù)的標(biāo)準(zhǔn)差非常小，對三軸加速計(jì)標(biāo)定補(bǔ)償效果好；基于對歐拉角奇異值問題改進(jìn)后，使得俯仰角在正負(fù)90°時(shí)姿態(tài)角不受影響；基于歐拉角法對傾斜角和航向角分開求解特點(diǎn)，大大減小磁場變化對姿態(tài)角的干擾，比四元數(shù)法在性能上更加優(yōu)越，得到的姿態(tài)角精度高且穩(wěn)定性好.因此，在人體姿態(tài)捕捉時(shí)，運(yùn)用該方法可以獲得高精度的姿態(tài)角.