張金藝,徐德政,李若涵,陳興秀,徐秦樂

(1．上海大學特種光纖與光接入網(wǎng)省部共建重點實驗室,上海 200444;

2．上海大學微電子中心,上海 200444;

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

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

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

(1．上海大學特種光纖與光接入網(wǎng)省部共建重點實驗室,上海 200444;

2．上海大學微電子中心,上海 200444;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1.4 四元數(shù)的合成

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

基于四元數(shù)的卡爾曼濾波算法流程如圖2所示.初始利用分解四元數(shù)算法計算狀態(tài)變量和誤差協(xié)方差P0,通過時間更新方程計算先驗估計值和先驗誤差協(xié)方差,由卡爾曼增益方程根據(jù)噪聲協(xié)方差矩陣Q和R計算卡爾曼濾波增益Kk.將分解四元數(shù)算法求得的靜態(tài)姿態(tài)四元數(shù)作為觀測變量zk,代入卡爾曼濾波的測量更新方程中,運用卡爾曼增益調(diào)整先驗估計值和新息zk?之間的權(quá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)及準靜態(tài)姿態(tài)估算算法,是利用載體靜止時所受作用力為重力加速度的原理進行姿態(tài)估算的.而當載體處于高速運動狀態(tài)時會引入除重力外的非線性加速度分量,從而導致分解四元數(shù)算法對靜態(tài)四元數(shù)的估算產(chǎn)生較大的偏差.這就造成觀測量不僅失去了本身的校準功能,反而成為誤差的來源.為避免這種情況,本研究設(shè)計了R自適應(yīng)調(diào)節(jié)卡爾曼增益方法,增加動態(tài)四元數(shù)估算結(jié)果的權(quán)重,以消除非線性加速度噪聲對整體姿態(tài)估算的影響.具體的方法為設(shè)置動、靜態(tài)閾值,通過判斷載體3軸合加速度與閾值間的關(guān)系,對離線標定的靜態(tài)?√測量誤差協(xié)方差矩陣?R作在線調(diào)節(jié).

(1)當δa≤ asta時,表示載體處于靜止狀態(tài),測量變量值準確可信,此時R可不作調(diào)整或略微調(diào)小,則

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

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

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

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

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

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

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

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

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

圖4 MEMS-IMU與旋轉(zhuǎn)實驗平臺Fig.4 MEMS-IMU and rotation platform

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

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

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

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

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

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

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

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

圖7 水平旋轉(zhuǎ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)軸補償方法的驗證

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

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

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

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

5 結(jié)束語

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

[1]蔡春龍,劉翼,劉一薇.MEMS儀表慣性組合導航系統(tǒng)發(fā)展現(xiàn)狀與趨勢[J].中國慣性技術(shù)學報, 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ù)卡爾曼濾波組合導航算法性能分析[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

上海市教委重點學科建設(shè)資助項目(J50104);上海市科委基金資助項目(08706201000,08700741000)

張金藝(1965—),男,研究員,博士生導師,博士,研究方向為通信類SoC設(shè)計與室內(nèi)無線定位技術(shù). E-mail:zhangjinyi@staff.shu.edu.cn