9軸MEMS-IMU實(shí)時(shí)姿態(tài)估算算法

張金藝,徐德政,李若涵,陳興秀,徐秦樂(lè)

(1．上海大學(xué)特種光纖與光接入網(wǎng)省部共建重點(diǎn)實(shí)驗(yàn)室,上海 200444;

2．上海大學(xué)微電子中心,上海 200444;

3．上海大學(xué)教育部新型顯示與系統(tǒng)應(yīng)用重點(diǎn)實(shí)驗(yàn)室,上海 200444)

9軸MEMS-IMU實(shí)時(shí)姿態(tài)估算算法

張金藝1,2,3,徐德政2,李若涵1,陳興秀1,徐秦樂(lè)2

(1．上海大學(xué)特種光纖與光接入網(wǎng)省部共建重點(diǎn)實(shí)驗(yàn)室,上海 200444;

2．上海大學(xué)微電子中心,上海 200444;

3．上海大學(xué)教育部新型顯示與系統(tǒng)應(yīng)用重點(diǎn)實(shí)驗(yàn)室,上海 200444)

隨著對(duì)微機(jī)電系統(tǒng)-慣性測(cè)量單元(micro-electro-mechanical system-inertial measurement unit,MEMS-IMU)在室內(nèi)定位、動(dòng)態(tài)追蹤等應(yīng)用領(lǐng)域中的需求日益迫切,使得具有高精度、低成本和實(shí)時(shí)性的MEMS-IMU模塊設(shè)計(jì)成為研究熱點(diǎn).針對(duì)MEMS-IMU的核心技術(shù)——姿態(tài)估算進(jìn)行研究,設(shè)計(jì)了一種基于四元數(shù)的9軸MEMS-IMU實(shí)時(shí)姿態(tài)估算算法.該算法運(yùn)用分解四元數(shù)算法處理加速度和磁感應(yīng)強(qiáng)度數(shù)據(jù),計(jì)算出靜態(tài)四元數(shù);通過(guò)角速度與四元數(shù)的微分關(guān)系估算動(dòng)態(tài)四元數(shù);運(yùn)用卡爾曼濾波融合動(dòng)、靜態(tài)四元數(shù),進(jìn)而實(shí)現(xiàn)實(shí)時(shí)姿態(tài)估算.針對(duì)分解四元數(shù)算法中存在的奇異值問(wèn)題,提出了轉(zhuǎn)軸補(bǔ)償方法對(duì)其修正,以實(shí)現(xiàn)全姿態(tài)估算;考慮動(dòng)態(tài)情況下的非線性加速度分量對(duì)姿態(tài)估算精度的影響,設(shè)計(jì)了R自適應(yīng)卡爾曼濾波器,以進(jìn)一步提高姿態(tài)估算算法的精度.驗(yàn)證結(jié)果表明,R自適應(yīng)卡爾曼濾波器能夠有效抑制加速度噪聲,提高姿態(tài)估算精度;同時(shí),轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法能夠準(zhǔn)確估算奇異值點(diǎn)的姿態(tài)信息,并且計(jì)算時(shí)間僅為原“借角”補(bǔ)償方法的50%左右,有效提高了整體算法的實(shí)時(shí)性.

微機(jī)電系統(tǒng)-慣性測(cè)量單元;姿態(tài)估算;分解四元數(shù)算法;奇異值補(bǔ)償;卡爾曼濾波

微機(jī)電系統(tǒng)-慣性測(cè)量單元(micro-electro-mechanical system-inertial measurement unit, MEMS-IMU)實(shí)現(xiàn)了傳感器信息獲取、傳輸、處理的一體化集成,具備體積小、成本低、便于大批量生產(chǎn)等優(yōu)點(diǎn)[1],因而已廣泛應(yīng)用于便攜式智能設(shè)備中.近年來(lái),隨著手持式智能設(shè)備的蓬勃發(fā)展,催生了通過(guò)MEMS-IMU實(shí)現(xiàn)室內(nèi)定位、動(dòng)態(tài)追蹤等復(fù)雜功能的應(yīng)用需求,使得具有高精度、低成本和實(shí)時(shí)性的MEMS-IMU模塊設(shè)計(jì)逐漸成為慣性技術(shù)的發(fā)展方向.而姿態(tài)估算作為MEMS-IMU的核心技術(shù)也越來(lái)越受到國(guó)內(nèi)外專家學(xué)者的關(guān)注.

陀螺儀是目前最常用的姿態(tài)傳感器,但是由于存在漂移、累積誤差等缺陷而無(wú)法長(zhǎng)期穩(wěn)定地提供姿態(tài)信息.針對(duì)陀螺儀存在的問(wèn)題,文獻(xiàn)[2]利用傾角儀和磁力計(jì)對(duì)陀螺儀提供的姿態(tài)信息進(jìn)行補(bǔ)償,最先提出了互補(bǔ)卡爾曼濾波姿態(tài)估算的思想.文獻(xiàn)[3]設(shè)計(jì)了基于歐拉角的互補(bǔ)卡爾曼濾波器融合9軸MEMS傳感器數(shù)據(jù)實(shí)現(xiàn)姿態(tài)估算.然而由于該算法需要計(jì)算大量三角函數(shù),并且在運(yùn)算中易產(chǎn)生奇異值,因此基于四元數(shù)的卡爾曼濾波器更普遍地應(yīng)用于姿態(tài)估算.文獻(xiàn)[4]使用靜態(tài)姿態(tài)估算算法將加速度和磁感應(yīng)強(qiáng)度數(shù)據(jù)轉(zhuǎn)化為四元數(shù),然后運(yùn)用基于四元數(shù)的卡爾曼濾波器融合傳感器數(shù)據(jù),得到最終的姿態(tài)估算結(jié)果.目前,常用的靜態(tài)四元數(shù)估算算法有高斯牛頓算法和QUEST算法[5],這兩種算法在本質(zhì)上屬于代數(shù)方法,需要建立精確的磁感應(yīng)模型,以避免豎直方向的姿態(tài)估算受到影響;同時(shí)兩種算法在計(jì)算中存在若干奇異值點(diǎn),給實(shí)際應(yīng)用帶來(lái)很大的困擾[6].文獻(xiàn)[7]給出一種分解四元數(shù)的方法,將四元數(shù)按歐拉角方向分解并合成,達(dá)到了較好的姿態(tài)估算效果.文獻(xiàn)[8]在運(yùn)算中運(yùn)用半角公式,避免了三角函數(shù)的運(yùn)算,提高了計(jì)算效率.美中不足的是該算法使用了三參數(shù)法推導(dǎo)四元數(shù),因此也不可避免地引入了奇異值.文獻(xiàn)[9]在此基礎(chǔ)上設(shè)計(jì)了兩段式擴(kuò)展卡爾曼濾波器,分別使用加速度和磁感應(yīng)數(shù)據(jù)估算豎直和水平方向的姿態(tài),同樣消除了磁偏量的非精確匹配對(duì)姿態(tài)估算的干擾.不過(guò)由于擴(kuò)展卡爾曼濾波作為一種次優(yōu)濾波,在線性化過(guò)程中存在誤差,同時(shí)需要求解觀測(cè)方程的雅可比矩陣,對(duì)于高維問(wèn)題而言過(guò)于繁瑣,故在性能上并不如四元數(shù)卡爾曼濾波方法[10].

綜合分析上述研究成果,本研究使用分解四元數(shù)算法(factored quaternion algorithm, FQA)計(jì)算靜態(tài)四元數(shù);利用角速度與四元數(shù)的微分關(guān)系估算動(dòng)態(tài)四元數(shù);運(yùn)用卡爾曼濾波融合動(dòng)、靜態(tài)四元數(shù),進(jìn)而實(shí)現(xiàn)實(shí)時(shí)姿態(tài)估算.針對(duì)分解四元數(shù)算法中存在的奇異值問(wèn)題,提出轉(zhuǎn)軸補(bǔ)償方法來(lái)修正奇異值,從而實(shí)現(xiàn)全姿態(tài)估算;針對(duì)動(dòng)態(tài)情況下的非重力加速度干擾姿態(tài)估算問(wèn)題,設(shè)計(jì)了R自適應(yīng)卡爾曼濾波器,以進(jìn)一步提高姿態(tài)估算的精度.驗(yàn)證結(jié)果表明,R自適應(yīng)卡爾曼濾波器能夠有效抑制加速度噪聲,提高姿態(tài)估算精度;同時(shí),轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法能夠準(zhǔn)確估算奇異值點(diǎn)的姿態(tài),且計(jì)算時(shí)間僅為原“借角”補(bǔ)償方法的50%左右,有效提高了整體算法的實(shí)時(shí)性.

1 四元數(shù)物理意義剖析

四元數(shù)Q=[qwqxqyqz],可用于表示載體在空間中的姿態(tài),其一般表現(xiàn)形式為

式中,qw,qx,qy,qz為4個(gè)實(shí)數(shù),i,j,k為四元數(shù)的3個(gè)虛系數(shù)單位的基.由此可知,四元數(shù)實(shí)際上由一個(gè)標(biāo)量qw和一個(gè)矢量q構(gòu)成.當(dāng)四元數(shù)滿足單位化條件,即+++=1時(shí),四元數(shù)的物理意義就可以理解為剛體繞過(guò)定點(diǎn)的轉(zhuǎn)軸n轉(zhuǎn)過(guò)角度α,則式(1)中的四元數(shù)又可以表示為

如果此時(shí)再作一次轉(zhuǎn)動(dòng)變換,記作Q′,則兩次轉(zhuǎn)動(dòng)可以等效為一個(gè)轉(zhuǎn)動(dòng)四元數(shù)

向量r經(jīng)上述轉(zhuǎn)動(dòng)后得到向量r′,則r′和r的轉(zhuǎn)換關(guān)系為

式(3)和(4)中,?表示四元數(shù)乘法.假設(shè)代表合成的四元數(shù)Qsyn=[q0q1q2q3],則對(duì)應(yīng)的Q?=[q0?q1?q2?q3].

2 轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法

分解四元數(shù)算法[7-8]是一種只需單步計(jì)算的靜態(tài)及準(zhǔn)靜態(tài)姿態(tài)估算算法.該算法利用四元數(shù)的物理意義,將載體的轉(zhuǎn)動(dòng)分解并合成以求得姿態(tài)四元數(shù).但是由于使用三參數(shù)法推導(dǎo)四元數(shù),從而不可避免地存在奇異值.下面將有針對(duì)性地提出轉(zhuǎn)軸補(bǔ)償方法來(lái)修正奇異值,以實(shí)現(xiàn)全姿態(tài)估算.

2.1 分解四元數(shù)算法

初始載體與北東地(north east down,NED)參考系重合,此時(shí)首先繞z軸轉(zhuǎn)過(guò)航向角(yaw),記作?;然后再繞y軸轉(zhuǎn)過(guò)俯仰角(pitch),記作θ;最后繞x軸轉(zhuǎn)過(guò)橫滾角(roll),記作γ.利用上述轉(zhuǎn)動(dòng)對(duì)應(yīng)的歐拉角可表示載體相對(duì)參考系的任意姿態(tài),稱為三參數(shù)法定姿.分解四元數(shù)算法就是應(yīng)用三參數(shù)法,分別求解沿歐拉角方向的四元數(shù),然后合成求得姿態(tài)四元數(shù).

2.1.1 俯仰角方向的四元數(shù)

由于載體沿水平方向的旋轉(zhuǎn)并不會(huì)影響垂直方向的姿態(tài),因此首先求解俯仰角和橫滾角的四元數(shù).當(dāng)載體初始靜止時(shí),測(cè)得的加速度a=[0 0 g]T.若載體繞y軸旋轉(zhuǎn)θ角度,則加速度在y軸的分量ay仍為0,同時(shí)有將測(cè)得的加速度數(shù)據(jù)單位化,得到單位化后的則有

由于俯仰角θ∈[?π/2,π/2],則運(yùn)用三角函數(shù)的半角關(guān)系可得

式中,sign(a)為取符函數(shù),定義規(guī)則如下:當(dāng)a>0時(shí),sign(a)=1;反之,當(dāng)a<0時(shí), sign(a)=?1.參考式(2)可得表征俯仰角的四元數(shù)

2.1.2 橫滾角方向的四元數(shù)

載體再繞x軸旋轉(zhuǎn)γ角度,測(cè)得加速度x軸分量ax不變,同時(shí)ay和az分別為

則橫滾角的余弦值和正弦值分別為

值得一提的是,式(10)中的cosθ出現(xiàn)在分母的位置,隱含有cos=0的限制條件.因此,當(dāng)俯仰角θ=±π/2時(shí),式(10)不再適用于求解橫滾角,否則會(huì)引入奇異值.對(duì)于奇異值點(diǎn)的補(bǔ)償方法將在第3節(jié)中討論.由于橫滾角γ∈(?π,π],參見式(7),則表征橫滾角的四元數(shù)

2.1.3 航向角方向的四元數(shù)

假設(shè)航向角為φ,測(cè)得磁感應(yīng)強(qiáng)度mb=[mbxmbymbz]T.為求得航向角φ,需先將其轉(zhuǎn)換到水平平面上,利用式(4)得到轉(zhuǎn)換后的等效磁感應(yīng)強(qiáng)度

若忽略磁北方向和正北方向之間的磁偏角,并以BN代表地磁場(chǎng)的北向分量,則此時(shí)水平方向的磁感應(yīng)強(qiáng)度與地磁場(chǎng)強(qiáng)度水平分量之間有如下關(guān)系:

為計(jì)算簡(jiǎn)便,可將水平方向的磁感應(yīng)強(qiáng)度與地磁場(chǎng)強(qiáng)度作單位化處理,得到單位化水平方向磁場(chǎng)強(qiáng)度和則計(jì)算可得航向角的余弦值和正弦值分別為

由于φ∈(?π,π],參考式(7)中的半角公式,則航向角的旋轉(zhuǎn)四元數(shù)

2.1.4 四元數(shù)的合成

由式(8),(11)和(15)分別求得橫滾角、俯仰角和航向角的旋轉(zhuǎn)四元數(shù)后,運(yùn)用式(3)對(duì)3個(gè)四元數(shù)分量進(jìn)行合成,得到姿態(tài)四元數(shù)

至此,分解四元數(shù)算法利用載體靜止時(shí)的固有重力加速度以及地磁場(chǎng)強(qiáng)度作為參考,根據(jù)四元數(shù)的物理意義,通過(guò)分解-合成四元數(shù)的方法,消除了傳統(tǒng)靜態(tài)姿態(tài)估算算法中由磁感應(yīng)模型非精確匹配造成的豎直方向姿態(tài)估算誤差.運(yùn)算過(guò)程中使用半角公式避免了三角函數(shù)運(yùn)算,利用四元數(shù)運(yùn)算提高了算法的計(jì)算效率.只是在運(yùn)算過(guò)程中由于存在若干奇異值點(diǎn),需要額外的補(bǔ)償方法來(lái)修正奇異值.

2.2 轉(zhuǎn)軸補(bǔ)償方法對(duì)奇異值的修正

當(dāng)俯仰角θ=±π/2時(shí),使用式(10)計(jì)算橫滾角會(huì)出現(xiàn)奇異值,這將導(dǎo)致分解四元數(shù)算法無(wú)法正確估算姿態(tài).下面就奇異值的補(bǔ)償方法展開討論,提出采用轉(zhuǎn)軸補(bǔ)償方法來(lái)修正奇異值,通過(guò)轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法實(shí)現(xiàn)對(duì)全姿態(tài)的估算.

文獻(xiàn)[8]給出了一種補(bǔ)償方法——借角法.借角法的基本思想是在cosθ<ε時(shí)(ε為接近0的正數(shù)),將俯仰角額外轉(zhuǎn)過(guò)α角度,此時(shí)等效俯仰角為θ?α,同時(shí)將測(cè)得的原始加速度和磁感應(yīng)數(shù)據(jù)也作相應(yīng)四元數(shù)轉(zhuǎn)化后,代入分解四元數(shù)算法進(jìn)行計(jì)算,將這個(gè)過(guò)程稱為“借角”.計(jì)算得到四元數(shù)后,再乘以額外轉(zhuǎn)角的四元數(shù)qα,將求得的四元數(shù)轉(zhuǎn)化回實(shí)際的姿態(tài)四元數(shù),稱這個(gè)過(guò)程為“還角”.借角法雖然可以估算奇異值點(diǎn)的姿態(tài)四元數(shù),但是在估算奇異值點(diǎn)的姿態(tài)時(shí)增加了多次四元數(shù)乘法運(yùn)算,使得計(jì)算量明顯增加.

而本研究則從分析奇異值的產(chǎn)生原因入手,結(jié)合幾何理解和物理意義,提出一種轉(zhuǎn)換坐標(biāo)軸的補(bǔ)償方法,以下簡(jiǎn)稱轉(zhuǎn)軸法.轉(zhuǎn)軸法可以在幾乎不增加基礎(chǔ)算法運(yùn)算量的前提下實(shí)現(xiàn)奇異值點(diǎn)的姿態(tài)估算,從而提高整體算法的實(shí)時(shí)性.

在z-y-x的旋轉(zhuǎn)順序及層級(jí)定義下,當(dāng)載體繞中間層級(jí)的y軸旋轉(zhuǎn)至俯仰角為±π/2時(shí),載體坐標(biāo)系x軸與參考坐標(biāo)系z(mì)軸重合,即第一層級(jí)與第三層級(jí)的轉(zhuǎn)軸重合,這會(huì)導(dǎo)致旋轉(zhuǎn)中第三個(gè)自由度旋轉(zhuǎn)失效,即歐拉角姿態(tài)估算中常見的萬(wàn)向節(jié)鎖(gimbal lock)問(wèn)題.通過(guò)上述分析不難發(fā)現(xiàn),可采用解萬(wàn)向節(jié)鎖的方法結(jié)合四元數(shù)計(jì)算來(lái)消除奇異值.實(shí)際操作中若遇到萬(wàn)向節(jié)鎖,一般會(huì)先將萬(wàn)向節(jié)旋轉(zhuǎn)系統(tǒng)還原至初始位置,然后沿航向角轉(zhuǎn)過(guò)π角度,再沿橫滾角轉(zhuǎn)過(guò)π角度,最后沿俯仰角轉(zhuǎn)至另一側(cè)的π/2角度.同理,當(dāng)θ接近±π/2時(shí),啟用轉(zhuǎn)軸補(bǔ)償法修正分解四元數(shù)算法實(shí)現(xiàn)對(duì)奇異值點(diǎn)姿態(tài)的估算.第一步:將測(cè)得的加速度及磁感應(yīng)強(qiáng)度數(shù)據(jù)轉(zhuǎn)軸匹配為載體在另一側(cè)水平面的數(shù)據(jù);第二步:將俯仰角看作0?,代入式(11)和(15)中計(jì)算橫滾角和航向角對(duì)應(yīng)的四元數(shù)qγ和qφ;第三步:將原俯仰角θ對(duì)應(yīng)的四元數(shù)qθ代入式(16)中,得到合成的四元數(shù).由此可見,轉(zhuǎn)軸補(bǔ)償方法中除數(shù)據(jù)匹配外,沒有增加分解四元數(shù)算法的運(yùn)算量,就完成了對(duì)奇異值的修正.圖1給出了轉(zhuǎn)軸補(bǔ)償法中數(shù)據(jù)匹配的示例,當(dāng)載體由位置1(載體水平正面向上)沿俯仰角方向轉(zhuǎn)至位置2(載體豎立左側(cè)正面、右側(cè)反面)時(shí),發(fā)生萬(wàn)向節(jié)鎖.此時(shí),應(yīng)用轉(zhuǎn)軸補(bǔ)償方法,將位置2的慣性數(shù)據(jù)匹配至位置3(載體水平反面向上).易知,位置3中x軸數(shù)據(jù)應(yīng)為位置2中z軸測(cè)得數(shù)據(jù)的相反數(shù),且z軸數(shù)據(jù)應(yīng)為位置2中x軸測(cè)得的數(shù)據(jù).將位置2的慣性數(shù)據(jù)轉(zhuǎn)軸匹配,然后代入分解四元數(shù)算法即可估算出奇異值點(diǎn)的姿態(tài).

一般而言,改變載體的旋轉(zhuǎn)順序即改變轉(zhuǎn)軸的層級(jí)關(guān)系,但這樣并不能有效消除奇異值點(diǎn),而僅僅是使奇異值點(diǎn)轉(zhuǎn)移.而轉(zhuǎn)軸補(bǔ)償方法的實(shí)質(zhì)是在奇異值出現(xiàn)時(shí)將旋轉(zhuǎn)順序由原來(lái)的z-y-x轉(zhuǎn)變?yōu)閦-x-y,但是由于四元數(shù)運(yùn)算順序沒有改變,也就是轉(zhuǎn)軸之間的層級(jí)關(guān)系并沒有發(fā)生改變,因此不會(huì)引入其他奇異值點(diǎn).

圖1 轉(zhuǎn)軸補(bǔ)償方法的數(shù)據(jù)匹配示例Fig.1 An example of the data matching in axies-exchanged compensation method

3 R自適應(yīng)卡爾曼濾波

傳感器數(shù)據(jù)融合是指在系統(tǒng)中使用多種傳感器,利用不同傳感器的特性來(lái)克服其他傳感器的不足,從而獲得更準(zhǔn)確的數(shù)據(jù)[11].本研究設(shè)計(jì)基于四元數(shù)的卡爾曼濾波器融合9軸MEMS傳感器數(shù)據(jù),利用四元數(shù)微分關(guān)系構(gòu)建狀態(tài)方程,將分解四元數(shù)算法提供的靜態(tài)四元數(shù)作為觀測(cè)修正值,極大減少了卡爾曼濾波器的計(jì)算量.針對(duì)動(dòng)態(tài)條件下非重力加速度分量對(duì)姿態(tài)估算的干擾,本研究設(shè)計(jì)了R自適應(yīng)卡爾曼濾波器來(lái)準(zhǔn)確獲取姿態(tài)四元數(shù).

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

下面介紹基于四元數(shù)的卡爾曼濾波器的設(shè)計(jì),令狀態(tài)變量為四元數(shù),即x=q,而測(cè)量變量由分解四元數(shù)算法提供,即z=qm.以A和H表示狀態(tài)矩陣和輸出矩陣,w為過(guò)程激勵(lì)噪聲, v為觀測(cè)噪聲,Q和R分別為A和H的協(xié)方差矩陣.則離散時(shí)間的卡爾曼濾波方程如下:

在慣性運(yùn)動(dòng)系統(tǒng)中,角速度ω與四元數(shù)q具有如下四元數(shù)微分關(guān)系:

對(duì)式(18)的四元數(shù)微分方程作線性化及離散化的近似處理,令qk?1與qk的采樣更新時(shí)間間隔為h,則可得離散時(shí)間狀態(tài)方程如下:

由于對(duì)四元數(shù)微分方程作線性離散的近似處理,省去了求解微分方程的繁瑣運(yùn)算.同時(shí),由于分解四元數(shù)算法中隱含有四元數(shù)單位化,因而測(cè)量方程中并不需要額外的約束方程.整個(gè)卡爾曼濾波方程僅為4階,且輸出矩陣H為4×4階單位陣,這樣極大地減少了卡爾曼濾波算法的運(yùn)算量,提高了姿態(tài)估算的實(shí)時(shí)性.

基于四元數(shù)的卡爾曼濾波算法流程如圖2所示.初始利用分解四元數(shù)算法計(jì)算狀態(tài)變量和誤差協(xié)方差P0,通過(guò)時(shí)間更新方程計(jì)算先驗(yàn)估計(jì)值和先驗(yàn)誤差協(xié)方差,由卡爾曼增益方程根據(jù)噪聲協(xié)方差矩陣Q和R計(jì)算卡爾曼濾波增益Kk.將分解四元數(shù)算法求得的靜態(tài)姿態(tài)四元數(shù)作為觀測(cè)變量zk,代入卡爾曼濾波的測(cè)量更新方程中,運(yùn)用卡爾曼增益調(diào)整先驗(yàn)估計(jì)值和新息zk?之間的權(quán)重,最終得到準(zhǔn)確的姿態(tài)估算結(jié)果.

圖2 基于四元數(shù)的卡爾曼濾波算法流程Fig.2 Flow of the quaternion-based Kalman filter

3.2 R的自適應(yīng)調(diào)節(jié)

分解四元數(shù)算法作為一種靜態(tài)及準(zhǔn)靜態(tài)姿態(tài)估算算法,是利用載體靜止時(shí)所受作用力為重力加速度的原理進(jìn)行姿態(tài)估算的.而當(dāng)載體處于高速運(yùn)動(dòng)狀態(tài)時(shí)會(huì)引入除重力外的非線性加速度分量,從而導(dǎo)致分解四元數(shù)算法對(duì)靜態(tài)四元數(shù)的估算產(chǎn)生較大的偏差.這就造成觀測(cè)量不僅失去了本身的校準(zhǔn)功能,反而成為誤差的來(lái)源.為避免這種情況,本研究設(shè)計(jì)了R自適應(yīng)調(diào)節(jié)卡爾曼增益方法,增加動(dòng)態(tài)四元數(shù)估算結(jié)果的權(quán)重,以消除非線性加速度噪聲對(duì)整體姿態(tài)估算的影響.具體的方法為設(shè)置動(dòng)、靜態(tài)閾值,通過(guò)判斷載體3軸合加速度與閾值間的關(guān)系,對(duì)離線標(biāo)定的靜態(tài)?√測(cè)量誤差協(xié)方差矩陣?R作在線調(diào)節(jié).

(1)當(dāng)δa≤ asta時(shí),表示載體處于靜止?fàn)顟B(tài),測(cè)量變量值準(zhǔn)確可信,此時(shí)R可不作調(diào)整或略微調(diào)小,則

(2)當(dāng)asta<δa ≤adyn且|δak?δak?1|≤ asta時(shí),表示載體運(yùn)動(dòng)趨勢(shì)不明顯,則Rk=R.若|δak?δak?1|>asta,則物體有變加速度運(yùn)動(dòng)的趨勢(shì),但是此時(shí)的測(cè)量值仍有一定的參考價(jià)值,因此設(shè)置

(3)當(dāng)δa>adyn時(shí),表示載體處于高速運(yùn)動(dòng)狀態(tài),此時(shí)靜態(tài)四元數(shù)計(jì)算得到的測(cè)量值完全失去參考修正價(jià)值,可令

運(yùn)用上述規(guī)則在線調(diào)整測(cè)量誤差協(xié)方差矩陣,并將調(diào)整后的Rk代入卡爾曼增益公式中,計(jì)算卡爾曼增益Kk.通過(guò)卡爾曼增益來(lái)調(diào)整動(dòng)、靜態(tài)四元數(shù)在卡爾曼濾波中所占的權(quán)重,從而獲得準(zhǔn)確的姿態(tài)估算結(jié)果.

4 驗(yàn)證系統(tǒng)構(gòu)建與結(jié)果分析

下面通過(guò)云臺(tái)旋轉(zhuǎn)實(shí)驗(yàn)平臺(tái)模擬載體的各種姿態(tài),利用9軸MEMS-IMU采集慣性測(cè)量數(shù)據(jù),采用Matlab軟件實(shí)現(xiàn)轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法和R自適應(yīng)卡爾曼濾波算法,仿真得到姿態(tài)估算結(jié)果.

4.1 驗(yàn)證系統(tǒng)的構(gòu)建

本驗(yàn)證系統(tǒng)由硬件和軟件兩部分組成.硬件部分由9軸MEMS-IMU模塊和上位機(jī)組成,系統(tǒng)架構(gòu)如圖3所示,其中MEMS-IMU(見圖4(a))主要由MEMS傳感器和微處理器構(gòu)成.陀螺儀為STMicroelectronics公司的L3G4200D,根據(jù)應(yīng)用需要選用±500 dps量程,靈敏度為17.5 mdps/digit;加速度計(jì)為Freescale公司的MMA8452Q,選用±2 g量程,靈敏度約為0.001 g/count;磁力計(jì)為Freescale公司的MAG3110,量程為±1 000μT,靈敏度為0.1μT/bit.微處理器通過(guò)I2C總線采集MEMS傳感器的原始慣性信息,運(yùn)用閾值濾波、水平校準(zhǔn)及零偏補(bǔ)償?shù)确椒A(yù)處理原始數(shù)據(jù),然后通過(guò)Uart通信方式以115 200 bit/s的速率將預(yù)處理后的慣性傳感器數(shù)據(jù)傳輸至上位機(jī).軟件部分采用Matlab 2011b作為算法驗(yàn)證平臺(tái)以實(shí)現(xiàn)轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法和R自適應(yīng)卡爾曼濾波算法,來(lái)分析姿態(tài)估算算法的性能.

圖4(b)為旋轉(zhuǎn)實(shí)驗(yàn)平臺(tái),將9軸MEMS-IMU固定于照相機(jī)云臺(tái)上,借助云臺(tái)可以實(shí)現(xiàn)航向角由?180?～180?、橫滾角由?180?～180?以及俯仰角由0?～90?的旋轉(zhuǎn)測(cè)試.因此,云臺(tái)旋轉(zhuǎn)實(shí)驗(yàn)平臺(tái)基本上可以達(dá)到測(cè)試要求.

圖3 驗(yàn)證系統(tǒng)架構(gòu)Fig.3 Architecture of verification system

圖4 MEMS-IMU與旋轉(zhuǎn)實(shí)驗(yàn)平臺(tái)Fig.4 MEMS-IMU and rotation platform

4.2 驗(yàn)證結(jié)果與分析

雖然四元數(shù)算法在姿態(tài)估算過(guò)程中具有運(yùn)算速度快、非奇異性等優(yōu)勢(shì),但是四元數(shù)不能直觀反映載體的姿態(tài)信息,因此需將實(shí)驗(yàn)結(jié)果由四元數(shù)轉(zhuǎn)化為歐拉角后顯示出來(lái).根據(jù)人們的使用習(xí)慣,以角度制表示歐拉角,并將航向角由?180?～180?的表示方式改為0?～360?來(lái)顯示. 4.2.1分解四元數(shù)算法的性能測(cè)試

(1)水平旋轉(zhuǎn)實(shí)驗(yàn).通過(guò)與高斯牛頓算法進(jìn)行對(duì)比,驗(yàn)證采用分解四元數(shù)算法估算姿態(tài)的優(yōu)勢(shì).將MEMS-IMU置于水平面上沿航向角正向旋轉(zhuǎn),在經(jīng)預(yù)設(shè)點(diǎn)時(shí)作短暫停留,如此往復(fù)7圈后靜止于預(yù)設(shè)點(diǎn).圖5分別給出了高斯牛頓算法和分解四元數(shù)算法對(duì)上述姿態(tài)變化的估算結(jié)果,橫軸為采樣點(diǎn)數(shù),縱軸為姿態(tài)角的角度.兩種算法對(duì)航向角的估算結(jié)果幾乎一致,而分解四元數(shù)算法對(duì)俯仰角和橫滾角的姿態(tài)估算更為準(zhǔn)確.這是由于分解四元數(shù)算法中的磁感應(yīng)數(shù)據(jù)僅用于估算水平方向上航向角姿態(tài),并不參與垂直方向上俯仰角和橫滾角的姿態(tài)估算,因此屏蔽了磁感應(yīng)模型的干擾,使得誤差較小.

圖5 高斯牛頓算法與分解四元數(shù)算法對(duì)水平旋轉(zhuǎn)實(shí)驗(yàn)的姿態(tài)估算結(jié)果對(duì)比Fig.5 Results comparison between Gauss-Newton algorithm and FQA in horizontal rotationexperiment

(2)奇異值點(diǎn)測(cè)試.由式(10)可知,當(dāng)俯仰角達(dá)到90?時(shí),分解四元數(shù)算法會(huì)出現(xiàn)奇異值.因此設(shè)計(jì)如下奇異值點(diǎn)測(cè)試:將完成水平旋轉(zhuǎn)測(cè)試的MEMS-IMU模塊沿俯仰角方向作0?～90?的旋轉(zhuǎn),并在俯仰角90?處作短暫停留,然后恢復(fù)原狀.采用分解四元數(shù)算法對(duì)上述運(yùn)動(dòng)姿態(tài)進(jìn)行估算的結(jié)果如圖6所示,圖(a)中未修正的分解四元數(shù)算法在俯仰角90?處出現(xiàn)了奇異值,表明算法對(duì)于航向角和橫滾角的估算存在較大波動(dòng),完全偏離了實(shí)際值.這說(shuō)明分解四元數(shù)算法無(wú)法準(zhǔn)確估算奇異值點(diǎn)的姿態(tài),需要配合補(bǔ)償方法才能實(shí)現(xiàn)全姿態(tài)估算.圖(b)則給出使用轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法對(duì)奇異值點(diǎn)的估算結(jié)果,基本與實(shí)際情況一致.關(guān)于采用轉(zhuǎn)軸補(bǔ)償方法修正奇異值點(diǎn)的分析將于4.2.3節(jié)中詳細(xì)討論.

圖6 分解四元數(shù)算法對(duì)奇異值點(diǎn)的姿態(tài)估算Fig.6 Attitude estimation of singularity from FQA with and without compensation

4.2.2 R自適應(yīng)卡爾曼濾波器的仿真驗(yàn)證

由圖5和6可知,使用分解四元數(shù)算法得到的姿態(tài)波形中存在明顯的毛刺,這主要是由加速度計(jì)的測(cè)量噪聲及其他非重力加速度分量造成的.下面將測(cè)試R自適應(yīng)卡爾曼濾波融合動(dòng)態(tài)數(shù)據(jù)對(duì)姿態(tài)估算結(jié)果的影響.依照式(20)～(22)的R自適應(yīng)調(diào)節(jié)規(guī)則改進(jìn)卡爾曼濾波器,將通過(guò)分解四元數(shù)算法得到的姿態(tài)估算結(jié)果作為卡爾曼濾波的觀測(cè)值輸入,經(jīng)濾波得到最終的姿態(tài)估算結(jié)果.相應(yīng)的R自適應(yīng)卡爾曼濾波對(duì)水平旋轉(zhuǎn)實(shí)驗(yàn)的姿態(tài)估算結(jié)果如圖7所示,與圖5對(duì)比發(fā)現(xiàn),經(jīng)濾波后俯仰角和橫滾角的波形明顯更為平滑.這主要是由于R自適應(yīng)卡爾曼濾波器有效地調(diào)節(jié)了卡爾曼增益,加大了動(dòng)態(tài)姿態(tài)數(shù)據(jù)的權(quán)重,從而顯著抑制了除重力加速度外的其他非線性加速度分量對(duì)姿態(tài)估算的影響,得到了較為準(zhǔn)確的姿態(tài)估算結(jié)果.

圖7 水平旋轉(zhuǎn)實(shí)驗(yàn)經(jīng)R自適應(yīng)卡爾曼濾波后的姿態(tài)估算結(jié)果Fig.7 Attitude estimation of R-adapted Kalman filter in horizontal rotation experiment

4.2.3 奇異值轉(zhuǎn)軸補(bǔ)償方法的驗(yàn)證

圖8 轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法對(duì)奇異點(diǎn)的姿態(tài)估算結(jié)果(經(jīng)卡爾曼濾波)Fig.8 Attitude estimation(after Kalman filter)of the FQA with axis-exchanged compensation method

(1)轉(zhuǎn)軸補(bǔ)償方法對(duì)奇異值修正效果的測(cè)試.圖8給出了經(jīng)卡爾曼濾波后的轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法對(duì)奇異值點(diǎn)的估算結(jié)果.觀察當(dāng)俯仰角由0?開始緩慢增大至接近90?時(shí),補(bǔ)償標(biāo)志位翻轉(zhuǎn)為1,表明開啟了轉(zhuǎn)軸補(bǔ)償方法來(lái)估算奇異值點(diǎn)的姿態(tài),此時(shí)航向角和橫滾角翻轉(zhuǎn)180?,表示整個(gè)載體處于翻面的狀態(tài).而當(dāng)俯仰角迅速轉(zhuǎn)離90?時(shí),補(bǔ)償標(biāo)志位隨即歸0,航向角和橫滾角也立刻恢復(fù)原值.測(cè)試結(jié)果表明,轉(zhuǎn)軸補(bǔ)償方法能夠準(zhǔn)確估算奇異值點(diǎn)的姿態(tài)信息,從而彌補(bǔ)了分解四元數(shù)算法存在的不足.

(2)計(jì)算時(shí)間測(cè)試.運(yùn)用Matlab軟件中的運(yùn)行時(shí)間統(tǒng)計(jì)功能粗略估計(jì)轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法的計(jì)算時(shí)間.Matlab仿真平臺(tái)所在的計(jì)算機(jī)處理器(CPU)為Intel i5-3317U,主頻為1.7 GHz,安裝內(nèi)存(RAM)為8 GB.分別將文獻(xiàn)[8]中的“借角”補(bǔ)償方法以及本研究提出的轉(zhuǎn)軸補(bǔ)償方法代入分解四元數(shù)算法來(lái)估算奇異值點(diǎn)的姿態(tài).圖9為采用兩種補(bǔ)償方法計(jì)算奇異值點(diǎn)姿態(tài)所需時(shí)間的對(duì)比,對(duì)照?qǐng)D8可以發(fā)現(xiàn),在采樣點(diǎn)為150～300的區(qū)間內(nèi),需要補(bǔ)償修正分解四元數(shù)算法.而本研究提出的轉(zhuǎn)軸補(bǔ)償方法在此區(qū)間內(nèi)的運(yùn)算時(shí)間僅為“借角”補(bǔ)償法的50%左右,這主要是因?yàn)榻杞欠ㄔ诮琛⑦€角的過(guò)程中,額外增加了多次四元數(shù)乘法運(yùn)算,而本方法只是將慣性傳感數(shù)據(jù)轉(zhuǎn)換坐標(biāo)軸匹配,幾乎沒有增加算法的運(yùn)算量,因此在估算奇異值點(diǎn)的姿態(tài)時(shí)具有更高的運(yùn)算效率.

圖9 轉(zhuǎn)軸補(bǔ)償法與借角補(bǔ)償法[8]在估算奇異點(diǎn)姿態(tài)時(shí)的運(yùn)算時(shí)間對(duì)比Fig.9 Comparison of the efficiency between the axis-exchanged compensation method and the method in Ref.[8]

5 結(jié)束語(yǔ)

本研究設(shè)計(jì)了一種基于四元數(shù)的9軸MEMS-IMU實(shí)時(shí)姿態(tài)估算算法,運(yùn)用轉(zhuǎn)軸補(bǔ)償方法修正分解四元數(shù)算法中存在的奇異值,實(shí)現(xiàn)了靜態(tài)全姿態(tài)估算;設(shè)計(jì)了R自適應(yīng)卡爾曼濾波器融合動(dòng)、靜態(tài)姿態(tài),以抑制動(dòng)態(tài)條件下非重力加速度對(duì)姿態(tài)估算的影響,實(shí)時(shí)且準(zhǔn)確地估算出載體姿態(tài).經(jīng)驗(yàn)證表明,采用R自適應(yīng)卡爾曼濾波器能夠有效提高姿態(tài)估算精度.同時(shí),轉(zhuǎn)軸補(bǔ)償-分解四元數(shù)算法能夠準(zhǔn)確估算奇異值點(diǎn)的姿態(tài),并且計(jì)算時(shí)間僅為原“借角”補(bǔ)償方法的50%左右,有效提高了整體算法的實(shí)時(shí)性.本算法能夠?qū)崿F(xiàn)實(shí)時(shí)、準(zhǔn)確的全姿態(tài)估算,非常適用于行人航跡推算以及動(dòng)態(tài)追蹤等應(yīng)用領(lǐng)域,具有較大的現(xiàn)實(shí)意義和應(yīng)用價(jià)值.

[1]蔡春龍,劉翼,劉一薇.MEMS儀表慣性組合導(dǎo)航系統(tǒng)發(fā)展現(xiàn)狀與趨勢(shì)[J].中國(guó)慣性技術(shù)學(xué)報(bào), 2009,17(5):562-567.

[2]Foxlin E.Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter[C]//Virtual Reality Annual International Symposium.1996:185-194.

[3]Zhang R,Hoeflinger F,Reindl L.Inertial sensor based indoor localization and monitoring system for emergency responders[J].IEEE Sensors Journal,2013,13(2):838-848.

[4]Lee J K,Park E J.Minimum-order Kalman filter with vector selector for accurate estimation of human body orientation[J].IEEE Transactions on Robotics,2009,25(5):1196-1201.

[5]Shuster M D.Filter QUEST or REQUEST[J].Journal of Guidance,Control,and Dynamics, 2009,32(2):643-645.

[6]Liu C,Liu F,Jiang Z,et al.New method for initial orientation estimation in dynamic state using MEMS magnetometers and accelerometers[C]//Modelling,Identification&Control(ICMIC). 2012:29-34.

[7]Conrado A.Implementation of a quaternion-based Kalman filter for human body motion tracking using MARG sensors[D].Monterey:Naval Postgraduate School,2004.

[8]Yun X P,Bachmann E R,McGhee R B.A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements[J].IEEE Transactions on Instrumentation and Measurement,2008,57(3):638-650.

[9]Sabatelli S,Galgani M,Fanucci L,et al.A double-stage Kalman filter for orientation tracking with an integrated processor in 9-D IMU[J].IEEE Transactions on Instrumentation and Measurement,2013,62(3):590-598.

[10]高顯忠,侯中喜,王波,等.四元數(shù)卡爾曼濾波組合導(dǎo)航算法性能分析[J].控制理論與應(yīng)用,2013, 30(2):171-177.

[11]Brigante C M N,Abbate N,Basile A,et al.Towards miniaturization of a MEMS-based wearable motion capture system[J].IEEE Transactions on Industrial Electronics,2011,58(8): 3234-3241.

9-axis MEMS-IMU real-time data fusion algorithm for attitude estimation

ZHANG Jin-yi1,2,3,XU De-zheng2,LI Ruo-han1, CHEN Xing-xiu1,XU Qin-le2
(1.Key Laboratory of Specialty Fiber Optics and Optical Access Networks,
Shanghai University,Shanghai 200444,China;
2.Microelectronic Research and Development Center,Shanghai University,Shanghai 200444,China;
3.Key Laboratory of Advanced Displays and System Application,Ministry of Education, Shanghai University,Shanghai 200444,China)

To meet urgent application demands in indoor location and motion tracking, studies on low-cost high-resolution and real-time micro-electro-mechanical system-inertial measurement unit(MEMS-IMU)have attracted much attention.This paper presents a quaternion-based data fusion algorithm for real-time attitude estimation,including factored quaternion algorithm(FQA)for static attitude estimation,and Kalman filtering fordata fusion.A singularity avoidance method,axis-exchanged compensation,is proposed to modify the FQA,allowing the algorithm to track at all attitudes.An R-adapted module is designed to adjust the Kalman gain,which effectively restrains noise due to dynamic nonlinear acceleration,and improves attitude estimation accuracy.Experimental results show that the R-adapted Kalman filter can accurately estimate attitudes in real-time.Additionally,FQA with an axis-exchanged method has good performance in estimating attitudes of singularity points,and the computational efficiency is higher than a previous method by 50%.

micro-electro-mechanical system-inertial measurement unit(MEMS-IMU); attitude estimation;factored quaternion algorithm(FQA);singularity compensation;Kalman filter(KF)

TP 212.9;TP 391;TN 966;TN 967

A

1007-2861(2015)05-0547-13

10.3969/j.issn.1007-2861.2014.01.037

2014-01-02

上海市教委重點(diǎn)學(xué)科建設(shè)資助項(xiàng)目(J50104);上海市科委基金資助項(xiàng)目(08706201000,08700741000)

張金藝(1965—),男,研究員,博士生導(dǎo)師,博士,研究方向?yàn)橥ㄐ蓬怱oC設(shè)計(jì)與室內(nèi)無(wú)線定位技術(shù). E-mail:zhangjinyi@staff.shu.edu.cn