馬正華，盧成俊，戎海龍，賀小捧

(常州大學(xué) 信息科學(xué)與工程學(xué)院， 江蘇 常州 213000)

?

基于Kalman濾波算法的陀螺儀動(dòng)態(tài)漂移補(bǔ)償研究

馬正華，盧成俊，戎海龍，賀小捧

(常州大學(xué) 信息科學(xué)與工程學(xué)院， 江蘇 常州 213000)

應(yīng)用MEMS陀螺儀測(cè)量人體手臂運(yùn)動(dòng)姿態(tài)時(shí)，針對(duì)陀螺儀受線加速度干擾導(dǎo)致測(cè)量姿態(tài)發(fā)散的問(wèn)題，提出基于Kalman濾波算法的姿態(tài)誤差補(bǔ)償方法；該方法首先將陀螺儀采集到的角速度通過(guò)方向余弦算法解算得到姿態(tài)角，并將陀螺儀動(dòng)態(tài)漂移造成的姿態(tài)角誤差視為時(shí)變信號(hào)，通過(guò)建立姿態(tài)角漂移誤差的狀態(tài)方程及觀測(cè)方程，應(yīng)用卡爾曼濾波算法，實(shí)現(xiàn)對(duì)姿態(tài)角漂移誤差的估計(jì)，最終達(dá)到對(duì)陀螺儀動(dòng)態(tài)漂移誤差的補(bǔ)償；實(shí)驗(yàn)與仿真結(jié)果表明，應(yīng)用該算法能夠有效的抑制線加速度干擾導(dǎo)致的陀螺儀測(cè)量的姿態(tài)發(fā)散，適用于陀螺儀對(duì)人體手臂運(yùn)動(dòng)姿態(tài)的測(cè)量。

姿態(tài)測(cè)量；方向余弦算法；卡爾曼濾波算法；姿態(tài)補(bǔ)償

0 引言

獲取真實(shí)準(zhǔn)確的人體運(yùn)動(dòng)姿態(tài)信息是對(duì)人體開(kāi)展生物力學(xué)方面的研究和對(duì)人體的各種運(yùn)動(dòng)信息進(jìn)行檢測(cè)、跟蹤以及統(tǒng)計(jì)分析的基礎(chǔ)。MEMS慣導(dǎo)器件由于體積小、成本低、易于數(shù)字化和動(dòng)態(tài)測(cè)量特性良好等優(yōu)點(diǎn)成為人體姿態(tài)測(cè)量的理想選擇[1-3]。對(duì)陀螺儀輸出進(jìn)行積分便能得到姿態(tài)信息[4]，而由于本身特性的限制， MEMS陀螺儀的輸出噪聲以及隨機(jī)漂移導(dǎo)致解算得到的姿態(tài)產(chǎn)生漂移誤差[5-6]，并隨時(shí)間推移而累積增大，因此陀螺儀通常與其他姿態(tài)傳感器組合使用獲得穩(wěn)定可靠的姿態(tài)信息。目前傳統(tǒng)的方法是采用融合算法，以加速度傳感器、地磁傳感器輸出姿態(tài)角來(lái)修正陀螺儀的輸出。

Sabatini AM提出了一種VSD-EKF(variable-state imension extend kalman filter)算法,該算法假設(shè)目標(biāo)物體在緩慢移動(dòng),即假設(shè)載體沒(méi)有線加速度干擾,用 MARG 傳感器估計(jì)人體三維姿態(tài)信息,但該算法解算精度只排除了環(huán)境磁場(chǎng)影響和陀螺儀隨機(jī)誤差影響,沒(méi)有考慮到人體姿態(tài)變化帶來(lái)的加速度變化導(dǎo)致加速度計(jì)干擾的情況[7]。文獻(xiàn)[8]采用的測(cè)量融合方法將加速度傳感器和陀螺儀傳感器數(shù)據(jù)進(jìn)行加權(quán)平均，可應(yīng)用于動(dòng)態(tài)環(huán)境，但測(cè)量精度不高。Rehbinder H等人[9]基于陀螺儀和加速計(jì)兩種類型的傳感器，提出了一個(gè)姿態(tài)解算算法，但由于缺少磁強(qiáng)計(jì)信息，而使得航向角的累積誤差不能得到消除。文獻(xiàn)[10]通過(guò)建立加速度和陀螺儀的誤差數(shù)據(jù)模型，采用卡爾曼濾波方法預(yù)測(cè)飛行器的飛行姿態(tài)，能夠較好抑制陀螺儀漂移造成的姿態(tài)發(fā)散，但不適用于運(yùn)動(dòng)狀態(tài)變化較快的場(chǎng)景。文獻(xiàn)[11]和文獻(xiàn)[12]采用了融合了加速度計(jì)和地磁計(jì)的擴(kuò)展卡爾曼(EKF)算法，但在載體存在線加速度時(shí)，加速度傳感器給出的是運(yùn)動(dòng)加速度和重力加速度的疊加值，此時(shí)計(jì)算出的載體姿態(tài)角就有較大的誤差；同時(shí)在室內(nèi)使用地磁計(jì)時(shí)特別容易受周圍環(huán)境的干擾而產(chǎn)生隨機(jī)誤差，影響磁強(qiáng)計(jì)的輸出[13]。

由于人體姿態(tài)運(yùn)動(dòng)的特殊性譬如人體手臂運(yùn)動(dòng)，運(yùn)動(dòng)時(shí)常常伴隨著短時(shí)間內(nèi)線加速度有較大幅度變化等情況。在這種情況下，由于加速度計(jì)輸出的信息既包含重力加速度信息又包含運(yùn)動(dòng)加速度信息，導(dǎo)致采用融合算法受加速計(jì)輸出信息不準(zhǔn)確的影響其解算得到的姿態(tài)信息其與實(shí)際姿態(tài)信息存在很大誤差[14]。因此本文針對(duì)以上融合算法在線加速度干擾情況下手臂姿態(tài)解算誤差較大的問(wèn)題，采取單獨(dú)使用陀螺儀的輸出信號(hào)解算姿態(tài)，避免引入加速度計(jì)和地磁計(jì)受干擾的數(shù)據(jù)。而在利用陀螺儀對(duì)運(yùn)動(dòng)載體姿態(tài)進(jìn)行測(cè)量時(shí)，載體線加速度能夠使陀螺儀產(chǎn)生漂移[15]，進(jìn)而產(chǎn)生姿態(tài)誤差，即載體姿態(tài)漂移程度與陀螺儀所測(cè)載體的運(yùn)動(dòng)復(fù)雜度有關(guān)[16]。因此，本文將姿態(tài)漂移誤差視為時(shí)變信號(hào)，認(rèn)為該姿態(tài)漂移誤差大小與上一時(shí)刻的的姿態(tài)漂移誤差大小及當(dāng)前時(shí)刻姿態(tài)角的變化量相關(guān)，據(jù)此建立陀螺儀姿態(tài)漂移誤差狀態(tài)方程；選取陀螺儀輸出姿態(tài)角的低通濾波作為陀螺儀姿態(tài)漂移誤差觀測(cè)方程，在此基礎(chǔ)上，應(yīng)用卡爾曼濾波算法，對(duì)姿態(tài)漂移誤差進(jìn)行估計(jì)，從而達(dá)到補(bǔ)償姿態(tài)誤差，提高慣性器件測(cè)量手臂運(yùn)動(dòng)姿態(tài)精度的目的。

1 關(guān)于四元數(shù)與方向余弦矩陣的姿態(tài)角分析

通常將慣性測(cè)量單元固定安裝在運(yùn)動(dòng)載體上，由運(yùn)載體的機(jī)體軸確定的坐標(biāo)系一般稱之為載體坐標(biāo)系b，與之相對(duì)應(yīng)的慣導(dǎo)系統(tǒng)所采用的坐標(biāo)系稱為導(dǎo)航坐標(biāo)系n。確定載體的姿態(tài)和航向?qū)嶋H上就是確定載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的方位關(guān)系。而載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的轉(zhuǎn)換可以通過(guò)四元數(shù)法或者歐拉角法實(shí)現(xiàn)，由于四元數(shù)法可以避免歐拉角法的奇異問(wèn)題，因而被廣泛采用。經(jīng)3次旋轉(zhuǎn)后的導(dǎo)航坐標(biāo)系和機(jī)體坐標(biāo)系之間的坐標(biāo)關(guān)系可由一個(gè)矩陣來(lái)表示，即方向余弦矩陣：

(1)

利用四元數(shù)可確定出導(dǎo)航坐標(biāo)系至載體坐標(biāo)系的方向余弦矩陣:

(2)

(3)

(4)

由 式(1)(2)便可求得姿態(tài)角：

(5)

2 卡爾曼濾波器的設(shè)計(jì)

本文將陀螺儀動(dòng)態(tài)漂移造成的姿態(tài)誤差視為時(shí)變信號(hào)，并建立相應(yīng)的狀態(tài)方程和觀測(cè)方程，通過(guò)卡爾曼濾波得到較為準(zhǔn)確的姿態(tài)誤差估計(jì)值，從而補(bǔ)償姿態(tài)角的誤差。圖1所示為卡爾曼濾波過(guò)程設(shè)計(jì)框圖。

圖1 卡爾曼濾波過(guò)程設(shè)計(jì)框圖

2.1 陀螺儀姿態(tài)漂移誤差的狀態(tài)方程

利用陀螺儀測(cè)量手臂運(yùn)動(dòng)姿態(tài)，陀螺儀傳感器信號(hào)經(jīng)過(guò)AD轉(zhuǎn)換器變成計(jì)算機(jī)可處理的數(shù)字信號(hào)。在使用這些數(shù)字信號(hào)進(jìn)行手臂姿態(tài)解算前還應(yīng)補(bǔ)償陀螺儀的靜態(tài)隨機(jī)漂移，以減少陀螺儀靜態(tài)隨機(jī)漂移對(duì)姿態(tài)解算造成的影響。經(jīng)過(guò)處理后陀螺儀角速度通過(guò)方向余弦算法便可得到三軸姿態(tài)角。由于線加速度的干擾造成陀螺儀輸出的漂移，解算得到的姿態(tài)角與實(shí)際姿態(tài)存在一個(gè)姿態(tài)漂移誤差，即在k時(shí)刻，實(shí)際姿態(tài)角與解算得到的姿態(tài)角存在如下關(guān)系：

(6)

式中,θ(k)′為k時(shí)刻實(shí)際姿態(tài)角，θ(k)為k時(shí)刻陀螺儀輸出的角速度通過(guò)方向余弦算法解算得到的姿態(tài)角，error(k)為k時(shí)刻的姿態(tài)漂移誤差。

在利用陀螺儀對(duì)運(yùn)動(dòng)載體進(jìn)行測(cè)量時(shí)，載體線加速度使陀螺儀產(chǎn)生漂移造成的姿態(tài)誤差，與陀螺儀所測(cè)載體的運(yùn)動(dòng)復(fù)雜度有關(guān)[16]。本文將該運(yùn)動(dòng)復(fù)雜度描述為k時(shí)刻姿態(tài)角相對(duì)于k-1時(shí)刻姿態(tài)角的變化量，并認(rèn)為由陀螺儀漂移帶來(lái)的姿態(tài)誤差是累積的，據(jù)此建立姿態(tài)漂移誤差模型方程：

(7)

其中:error(k)為k時(shí)刻的姿態(tài)漂移誤差，error(k-1)為k-1時(shí)刻姿態(tài)誤差，δ為某一常數(shù)。按照以下方式選取系統(tǒng)狀態(tài)矩陣X=errork，A=I，B=δ，控制量u(k)=θ(k)-θ(k-1)，便可建立姿態(tài)漂移誤差的狀態(tài)方程：

(8)

2.2 陀螺儀姿態(tài)漂移誤差的觀測(cè)方程

陀螺儀傳感器輸出的是角速度，當(dāng)靜置時(shí)，陀螺儀輸出的角速度便是陀螺儀的靜態(tài)漂移。而當(dāng)陀螺儀測(cè)量運(yùn)動(dòng)載體時(shí)，陀螺儀輸出的是載體的角速度，由線加速度導(dǎo)致的陀螺儀動(dòng)態(tài)漂移無(wú)法直接測(cè)量得到，因此可將角速度經(jīng)積分變?yōu)樽藨B(tài)角，通過(guò)姿態(tài)角誤差來(lái)表征載體運(yùn)動(dòng)時(shí)陀螺儀的漂移情況。本文采用陀螺儀在k時(shí)刻及k時(shí)刻前幾個(gè)時(shí)刻解算得到的姿態(tài)角之間的低通濾波，作為陀螺儀漂移造成的姿態(tài)漂移誤差的觀測(cè)量，即姿態(tài)漂移誤差觀測(cè)量為：

(9)

其中:k為采樣時(shí)間，a、b、c為某一常數(shù)，滿足a+b+c=1，θ(k)、θ(k-1)、θ(k-2)、θ(k-3)分別為陀螺儀在k、k-1、k-2、k-3時(shí)刻解算得到的姿態(tài)角。

2.3 Kalman濾波算法的實(shí)現(xiàn)

基于以上姿態(tài)漂移誤差的狀態(tài)方程和觀測(cè)方程應(yīng)用Kalman濾波算法，以對(duì)姿態(tài)誤差進(jìn)行估計(jì)，從而達(dá)到補(bǔ)償姿態(tài)，提高慣性器件測(cè)量手臂運(yùn)動(dòng)姿態(tài)精度的目的。

卡爾曼濾波過(guò)程分為以下幾步實(shí)現(xiàn)：

1)首先由陀螺儀輸出的k時(shí)刻、k-1時(shí)刻的角速度通過(guò)方向余弦算法得到對(duì)應(yīng)時(shí)刻的姿態(tài)角θ(k)、θ(k-1)，則由(7)式可得姿態(tài)漂移誤差的狀態(tài)方程：

error(k/k-1)=error(k-1/k-1)+δ·(θ(k)-

(10)

可估計(jì)得到k時(shí)刻利用上一狀態(tài)預(yù)測(cè)的結(jié)果，其中error(k-1/k-1)是k-1時(shí)刻的姿態(tài)漂移誤差最優(yōu)的結(jié)果，設(shè)定0時(shí)刻的error(0)為0，δ的值為某一常數(shù)。

2)更新對(duì)應(yīng)于error(k/k-1)的協(xié)方差：

(11)

其中:A=I，P(k-1/k-1)是error(k-1/k-1)對(duì)應(yīng)的協(xié)方差，sigQ是系統(tǒng)過(guò)程的協(xié)方差。

3)計(jì)算卡爾曼增益：

(12)

其中:H=I，sigR為觀測(cè)方程系統(tǒng)過(guò)程協(xié)方差。

4)更新k時(shí)刻姿態(tài)誤差的最優(yōu)估計(jì)：

error(k/k)=error(k/k-1)+Kg(k)(Z(k)-

(13)

其中:Z(k)是姿態(tài)誤差的觀測(cè)值。

5)更新k時(shí)刻error(k/k)的協(xié)方差：

(14)

這樣，就基本完成k時(shí)刻Kalman濾波算法的所有計(jì)算，k時(shí)刻姿態(tài)漂移誤差的最優(yōu)估計(jì)error(k/k)的大小即為姿態(tài)角需補(bǔ)償?shù)拇笮?。?dāng)系統(tǒng)進(jìn)行到k+1時(shí)刻，回到第1步進(jìn)行計(jì)算，式(12)和式(14)保證濾波算法的持續(xù)性與遞歸性。

3 實(shí)驗(yàn)與仿真研究

為驗(yàn)證以上基于Kalman濾波的陀螺儀動(dòng)態(tài)漂移補(bǔ)償算法的有效性，實(shí)驗(yàn)選用了Xsens公司提供的慣性測(cè)量單元MTi10系列傳感器模塊來(lái)采集實(shí)驗(yàn)數(shù)據(jù)，MTI慣性測(cè)量單元包括三軸陀螺儀、三軸加速度計(jì)和三軸磁強(qiáng)計(jì)，本實(shí)驗(yàn)僅選取其中三軸陀螺儀的輸出數(shù)據(jù)。同時(shí)，實(shí)驗(yàn)還采用POLHEMUS公司提供的LongRanger電磁跟蹤系統(tǒng)，該裝置能對(duì)載體空間姿態(tài)進(jìn)行精確測(cè)量，可作為慣性系統(tǒng)姿態(tài)輸出的對(duì)照基準(zhǔn)。根據(jù)選用的測(cè)量單元，由官方手冊(cè)資料查詢得到陀螺儀的標(biāo)準(zhǔn)差為0.006rad/s，MTI慣性測(cè)量單元和LongRanger電磁跟蹤系統(tǒng)采樣頻率分別是256Hz和240Hz。實(shí)驗(yàn)所做的手臂運(yùn)動(dòng)可以描述為：手握測(cè)量裝置在近似水平面內(nèi)做圓周運(yùn)動(dòng)，同時(shí)手部也做一定的角度擺動(dòng)，運(yùn)動(dòng)約10s左右后，停止動(dòng)作，并將手臂恢復(fù)至初始姿態(tài)。

基于采集的數(shù)據(jù)，并根據(jù)MTI慣性測(cè)量單元與電磁跟蹤系統(tǒng)裝置的采樣頻率的不同對(duì)采集的數(shù)據(jù)作出必要的處理后，得到圖2所示陀螺儀采集的實(shí)測(cè)數(shù)據(jù)解算出的姿態(tài)變化曲線同電磁跟蹤系統(tǒng)實(shí)測(cè)數(shù)據(jù)解算出的實(shí)際姿態(tài)變化曲線的對(duì)比?？梢钥吹剑瑱M滾角的陀螺儀漂移最為明顯，10s后姿態(tài)解算誤差最大已經(jīng)達(dá)到10°，這是因?yàn)?，手部擺動(dòng)運(yùn)動(dòng)帶來(lái)了線加速度，而線加速度的干擾導(dǎo)致了陀螺儀漂移，進(jìn)而導(dǎo)致解算出的姿態(tài)開(kāi)始發(fā)散；俯仰角和偏航角方向受線加速度干擾影響較小，由陀螺儀解算得到的姿態(tài)信息大體體現(xiàn)了手臂實(shí)際運(yùn)動(dòng)姿態(tài)，但由于陀螺儀的隨機(jī)漂移影響，其姿態(tài)累計(jì)誤差在逐漸增大，所得的姿態(tài)信息在緩慢發(fā)散。

圖2 陀螺儀解算得到的姿態(tài)與電磁跟蹤系統(tǒng)得到的
實(shí)際運(yùn)動(dòng)姿態(tài)變化曲線對(duì)比

圖3所示為經(jīng)本文算法補(bǔ)償過(guò)的姿態(tài)曲線與陀螺儀采集的實(shí)測(cè)數(shù)據(jù)解算出的姿態(tài)曲線和電磁跟蹤系統(tǒng)解算得到的實(shí)際運(yùn)動(dòng)姿態(tài)曲線的對(duì)比?？梢钥吹?，經(jīng)本文算法補(bǔ)償過(guò)后，橫滾角姿態(tài)發(fā)散情況得到抑制，其解算誤差縮小在3°左右，姿態(tài)測(cè)量精度相比使用本文算法前得到提高，體現(xiàn)了本文算法在補(bǔ)償由于陀螺儀動(dòng)態(tài)漂移導(dǎo)致姿態(tài)誤差的有效性；同樣地，俯仰角和偏航角的姿態(tài)解算誤差相比原先縮小在1°左右，測(cè)量精度也得到一定提高。

圖3 加入本文算法補(bǔ)償后姿態(tài)變化曲線對(duì)比

4 結(jié)論

應(yīng)用MEMS慣導(dǎo)器件測(cè)量人體手臂運(yùn)動(dòng)姿態(tài)時(shí)，針對(duì)陀螺儀受線加速度干擾導(dǎo)致姿態(tài)發(fā)散的問(wèn)題，本文介紹了一種基于Kalman濾波算法的姿態(tài)誤差補(bǔ)償方法。該方法將姿態(tài)漂移誤差視為時(shí)變信號(hào)，據(jù)此建立陀螺儀姿態(tài)漂移誤差的狀態(tài)方程和觀測(cè)方程，在此基礎(chǔ)上，應(yīng)用卡爾曼濾波算法，實(shí)現(xiàn)對(duì)姿態(tài)角漂移誤差的估計(jì)，最終達(dá)到對(duì)陀螺儀動(dòng)態(tài)漂移誤差的補(bǔ)償。實(shí)驗(yàn)與仿真結(jié)果表明，本文算法能夠有效的抑制由線加速度干擾導(dǎo)致的陀螺儀測(cè)量的姿態(tài)發(fā)散，適用于陀螺儀對(duì)人體手臂運(yùn)動(dòng)姿態(tài)的測(cè)量。

[1]RoetenbergD,LuingeHJ,BatenC,etal.Compensationofmagneticdisturbancesimprovesinertialandmagneticsensingofhumanbodysegmentorientation[J].NeuralSystemsandRehabilitationEngineering,IEEETransactionson, 2005, 13(3): 395-405.

[2]VeltinkPH,SlyckeP,HemssemsJ,etal.Threedimensionalinertialsensingoffootmovementsforautomatictuningofatwo-channelimplantabledrop-footstimulator[J].Medicalengineering&physics, 2003, 25(1): 21-28.

[3] 李金鳳, 王慶輝, 劉曉梅,等. 基于MEMS慣性器件的行人室內(nèi)定位系統(tǒng)[J]. 計(jì)算機(jī)測(cè)量與控制, 2014, 22(11):3761-3763.

[4]MillerRB.Anewstrapdownattitudealgorithm[J].JournalofGuidance,Control,andDynamics, 1983, 6(4): 287-291.

[5] 王 帥, 魏 國(guó). 卡爾曼濾波在四旋翼飛行器姿態(tài)測(cè)量中的應(yīng)用 [J]. 兵工自動(dòng)化, 2011, 30(1): 73-80.

[6] 秦永元，張洪鉞，汪淑華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998.

[7]SabatiniAM.Variable-State-DimensionKalman-basedFilterfororientationdeterminationusinginertialandmagneticsensors[J].Sensors, 2012, 12(7): 8491-506.

[8] 李媛媛, 張立峰. 多傳感器自適應(yīng)加權(quán)融合算法及其應(yīng)用研究[J]. 自動(dòng)化與儀器儀表, 2008(2):10-13.

[9]RehbinderH,HuX.Drift-freeattitudeestimationforacceleratedrigidbodies☆[A].RoboticsandAutomation, 2001.Proceedings2001ICRA.IEEEInternationalConferenceon[C].IEEE, 2001(4):4244-4249.

[10] 馮智勇, 曾 瀚, 張 力,等. 基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J]. 西南師范大學(xué)學(xué)報(bào):自然科學(xué)版, 2011, 36(4):137-141.

[11]YunX,BachmannER.Design,implementation,andexperimentalresultsofaquaternion-basedKalmanfilterforhumanbodymotiontracking[J].Robotics,IEEETransactionson, 2006, 22(6): 1216-1227.

[12] 吳 杰, 閆建國(guó). 基于修正的卡爾曼濾波的姿態(tài)估計(jì)算法研究[J]. 計(jì)算機(jī)仿真, 2012, 29(2):54-57.

[13]BirdJ,ArdenD.Indoornavigationwithfoot-mountedstrapdowninertialnavigationandmagneticsensors[EmergingOpportunitiesforLocalizationandTracking][J].IEEEWirelessCommunications, 2011, 18:28-35.

[14] 戎海龍, 戴先中, 劉信羽. 廚師手握鍋具運(yùn)動(dòng)信息獲取方法[J]. 儀器儀表學(xué)報(bào), 2009 (11): 2291-2297.

[15]LiuC,ZhouZ,FuX.AttitudedeterminationforMAVsusingaKalmanfilter[J].TsinghuaScience&Technology, 2008, 13(5): 593-597.

[16]LambrechtS,JonkersI,PonsJL.Identificationanddecompositionoferrorin3Dmotioncaptureusinginertialandmagneticsensor.ConvergingClinicalandEngineeringResearchonNeurorehabilitationBiosystems&Biorobotics[Z]. 2013, 1: 721-725.

Research on Compensation of Dynamic Drift of Gyroscope Based on Kalman Filtering Algorithm

Ma Zhenghua，Lu Chengjun，Rong Hailong，He Xiaopeng

(College of Information Science and Engineering，Changzhou University，Changzhou 213000,China)

In order to solve the problem of attitude divergence caused by gyroscope which is interfered by the linear acceleration when use MEMS inertial navigation device to measure the attitude of the human arm movement, propose an attitude error compensation method based on Kalman filter algorithm. In this method, the attitude angle will be calculated first by angular velocity gyroscope output through direction cosine algorithm solution, through the establishment of the attitude angle drift error state equation and observation equation, attitude angle drift error will be estimated accurately and eliminated then. The experimental and simulation results show that , algorithm used in this paper can suppress the divergent attitude effectively and is suitable for gyroscope to measure the attitude of the human arm movement.

attitude measurement；direction cosine algorithm；Kalman filtering algorithm；attitude compensation

2016-03-19；

2016-04-25。

馬正華(1962-)，男，江蘇昆山人，教授，碩士研究生導(dǎo)師，主要從事嵌入式系統(tǒng)應(yīng)用、計(jì)算機(jī)控制技術(shù)、電力電子技術(shù)等方向的研究。

1671-4598(2016)09-0191-04

10.16526/j.cnki.11-4762/tp.2016.09.054

TP3

A