基于最小二乘法MEMS慣性傳感器姿態(tài)解算算法

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

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

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

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

1 姿態(tài)解算

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

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

1.1 三軸加速度計誤差建模及標定

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

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

(1)

(2)

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

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

位置1，轉臺設備Z軸向下，設定此時[Ax1Ay1Az1]=[0 0 1]，在該位置采集n1個原始加速度值[AxAyAz]，此時

(3)

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

位置2，轉臺設備Z軸向下，設定此時[Ax1Ay1Az1]=[0 0 -1]，在該位置采集n2個原始加速度值[AxAyAz]，此時

(4)

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

位置3，轉臺設備Z軸向下，設定此時[Ax1Ay1Az1]=[0 1 0]，在該位置采集n3個原始加速度值[AxAyAz]，此時

(5)

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

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

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

Yn×3=wn×4X4×3

(6)

(7)

利用最小二乘法可得:

X4×3=[WTW]-1WTY

(8)

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

設導航坐標系為onxnynzn，人體坐標系為obxbybzb，空間的關系如下：

(9)

(10)

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

(11)

歐拉角的更新為：

(12)

(13)

結合X1、X2、X3可以得出：

(14)

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

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

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

(15)

其中

(16)

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

(17)

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

(18)

其中

(19)

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

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

航向角與三軸磁力計信號是非線性關系，采用擴展卡爾曼濾波[13]，分為奇異值時濾波和非奇異值時濾波兩種情況.

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

(20)

觀察方程為：

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

(21)

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

(22)

(23)

其中:HN、HE、HD為三軸磁力計各軸的磁場強度.

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

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

(24)

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

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

(25)

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

(26)

(27)

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

(28)

當俯仰角接近90°時，觀察方程為：

(29)

當俯仰角接近-90°時，觀察方程為：

(30)

2 實驗及結果分析

如圖2所示，實驗設備為三軸慣性轉臺和MEMS慣性傳感器，測試時將傳感器固定在轉臺上.

圖2 慣性轉臺和慣性傳感器Figure 2 Inertial turntable and inertial sensor

2.1 三軸加速度計標定實驗

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

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

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

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

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

圖5 隨意轉動輸出歐拉角Figure 5 Rotate the output Euler Angle freely

圖6 奇異點轉動輸出歐拉角Figure 6 Singular point rotation output Euler Angle

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

3 結 語

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