劉 震，王雪梅，倪文波

（西南交通大學(xué)機械工程學(xué)院，四川 成都 610031）

基于MEMS傳感器的高精度姿態(tài)角測量研究

劉 震，王雪梅，倪文波

（西南交通大學(xué)機械工程學(xué)院，四川 成都 610031）

針對傳統(tǒng)姿態(tài)參考系統(tǒng)姿態(tài)解算容易受到載體運動加速度的干擾，導(dǎo)致系統(tǒng)精度變低、穩(wěn)定性變差等問題，提出一種改進的卡爾曼濾波算法。該算法建立基于四元數(shù)的慣性系統(tǒng)姿態(tài)解算數(shù)學(xué)模型，并根據(jù)載體運動加速度的大小，適時調(diào)整卡爾曼濾波器的量測噪聲方差的大小，以此減弱卡爾曼濾波過程中運動加速度對姿態(tài)角解算精度的影響。采用MEMS三軸陀螺儀、加速度計和磁阻傳感器完成載體在電梯升降過程中的測量，對實驗測量數(shù)據(jù)進行姿態(tài)解算，結(jié)果表明改進后的卡爾曼濾波算法能夠有效減小運動加速度對姿態(tài)解算的影響，姿態(tài)角的均方根誤差相對于傳統(tǒng)的姿態(tài)參考系統(tǒng)降低約40%。

姿態(tài)解算；運動加速度；互補濾波；卡爾曼濾波；MEMS傳感器

0 引 言

載體的姿態(tài)角是描述載體坐標(biāo)系與地理坐標(biāo)系相對位置的物理量，可以確定載體在空間中所處狀態(tài)。姿態(tài)角測量系統(tǒng)廣泛應(yīng)用于軍事、航空航天、航海、無人機、機器人、人體姿態(tài)識別[1-3]等方面。高精度的姿態(tài)角測量系統(tǒng)一般采用高精度的機械陀螺儀或激光陀螺儀，但是價格昂貴。隨著微機電系統(tǒng)（microelectro-mechanical systems，MEMS）慣性傳感器[4]以及計算機技術(shù)的迅速發(fā)展，姿態(tài)角測量系統(tǒng)逐漸向低成本、小型化和高精度發(fā)展。MEMS陀螺儀、加速度傳感器和磁阻傳感器被廣泛用于姿態(tài)角的測量中。慣性系統(tǒng)通過陀螺儀測量載體旋轉(zhuǎn)過程中的角速度，并對角速度積分得到載體的姿態(tài)角。然而MEMS陀螺儀較大的漂移誤差[5]，致使積分得到的姿態(tài)角存在較大的累積誤差，長時間使用的精度較差。由加速度傳感器和磁阻傳感器組成的姿態(tài)參考系統(tǒng)[6]，以當(dāng)?shù)氐闹亓铀俣群偷卮畔蛄孔鳛閰⒖迹纱擞嬎愠鲚d體傾角和航向角。姿態(tài)參考系統(tǒng)具有良好的靜態(tài)特性，不存在累積誤差，但在動態(tài)情況下容易受到載體運動加速度和環(huán)境磁場等因素的干擾[7-8]。

解決上述問題通常采用互補濾波[9]和卡爾曼濾波[10]兩種方法。陀螺儀的動態(tài)特性好，而姿態(tài)參考系統(tǒng)的靜態(tài)特性好，互補濾波法就是根據(jù)各傳感器在頻域上的互補特性，對兩個系統(tǒng)的傳感器信息進行融合，來得到更準(zhǔn)確的姿態(tài)角估計?；パa濾波方法的效果很大程度上取決于截止頻率的選擇，但由于載體運動的隨機性，實際應(yīng)用中很難確定一個最佳的截止頻率?？柭鼮V波則利用GPS、加速度傳感器或磁阻傳感器等外部觀測信息對慣性系統(tǒng)的姿態(tài)角誤差進行補償[11]，來得到更加可靠的姿態(tài)角估計值，卻難以消除運動加速度對姿態(tài)角估計的影響。

基于此，本文在分析比較傳統(tǒng)濾波估計方法的基礎(chǔ)上，利用直接安裝在載體上的MEMS陀螺儀、加速度傳感器和磁阻傳感器構(gòu)成測量系統(tǒng)，采用一種改進的卡爾曼濾波方法對載體的姿態(tài)角進行估計，通過識別運動加速度以調(diào)整卡爾曼濾波器中測量噪聲方差的大小，來減小運動加速度對姿態(tài)估計的影響，從而提高姿態(tài)角的估計精度。

1 姿態(tài)角求解原理

導(dǎo)航系統(tǒng)中載體的姿態(tài)角通常使用俯仰角θ、橫滾角γ、航向角φ來描述。載體姿態(tài)的變化是指載體坐標(biāo)系相對于參考坐標(biāo)系的空間轉(zhuǎn)動，由歐拉定理[12]可知，載體坐標(biāo)系相對于參考坐標(biāo)系的轉(zhuǎn)動可通過3次獨立的旋轉(zhuǎn)來實現(xiàn)。以地理坐標(biāo)系XnYnZn為參考坐標(biāo)系，坐標(biāo)軸Xn、Yn、Zn分別指向東、北、天方向，假設(shè)初始載體坐標(biāo)系XbYbZb與參考坐標(biāo)系重合。載體的姿態(tài)角依次按如下的基本旋轉(zhuǎn)確定：旋轉(zhuǎn)過程如圖 1[12]所示。

圖1 載體姿態(tài)角的確定

地理坐標(biāo)系XnYnZn與載體坐標(biāo)系XbYbZb的空間關(guān)系可由下式表示：

計算坐標(biāo)之間的相對轉(zhuǎn)動關(guān)系主要有兩種方法：歐拉角法和四元數(shù)法[13]。相對于歐拉角法，四元數(shù)法不僅可全角度描述姿態(tài)，避免了歐拉角的奇異問題，而且計算快捷，所以本研究采用四元數(shù)法計算載體姿態(tài)角。四元數(shù)與姿態(tài)矩陣的關(guān)系為

由捷聯(lián)慣性四元數(shù)相關(guān)理論[12]，得四元數(shù)的微分方程為

采用比卡算法或龍格庫塔算法[14]求解微分方程（4），得到四元數(shù)的4個值，進而計算出姿態(tài)矩陣（3）。 其中 ωx、ωy、ωz由下式計算得到：

ωie——地球自轉(zhuǎn)角速率；

R——地球平均半徑；

L——載體所在的緯度；

VE、VN——載體的東向速度和北向速度。

1.1 慣性系統(tǒng)解算方法

慣性系統(tǒng)解算姿態(tài)角是通過陀螺儀測量載體的角速度，對角速度積分得到載體的姿態(tài)角。由四元數(shù)理論，其姿態(tài)角計算公式為

1.2 姿態(tài)參考系統(tǒng)解算方法

姿態(tài)參考系統(tǒng)由加速度傳感器和磁阻傳感器組成。由加速度傳感器測量值可確定載體的俯仰角θ和橫滾角γ，結(jié)合磁阻傳感器測量值可進一步解算出航向角φ。當(dāng)載體坐標(biāo)系相對于地理坐標(biāo)系靜止時，加速度傳感器測量輸出滿足下式：

式中：ax、ay、az——載體靜止時加速度傳感器在3個坐標(biāo)軸方向的測量值；

g——載體所在地的重力加速度。

由式（7）可得俯仰角θ和橫滾角γ的計算式：

地球磁場在地理坐標(biāo)系中的分量與磁阻傳感器的測量值滿足下式：

式中：mx、my、mz——磁阻傳感器在3個坐標(biāo)軸方向的測量值；

Mx、My、Mz——載體在地理坐標(biāo)系下的三軸磁場值。

由于航向角的大小與加速度傳感器的輸出無關(guān)[15]，故可假設(shè)航向角φ=0°。把由加速度傳感器解算出的俯仰角θ和橫滾角γ代入Cbn中，由式（9）可得：

航向角可由下式計算得到：

2 傳感器信號預(yù)處理

載體運動時，直接安裝在載體上的MEMS傳感器易受各種高頻振動干擾，對姿態(tài)解算不利，必要的低通濾波處理可以提高信號的信噪比，使姿態(tài)角的解算結(jié)果更加準(zhǔn)確、可靠。

磁阻傳感器使用時會受周圍磁性物質(zhì)干擾，使得磁阻傳感器的輸出不能準(zhǔn)確反映地球磁場矢量，其間的誤差稱為“羅差”。常使用最小二乘誤差補償法和橢圓假設(shè)補償法對“羅差”進行補償，由于最小二乘誤差補償法需借助高精度的轉(zhuǎn)臺才能完成，而橢圓假設(shè)補償法不需要借助任何工具，并且效果理想，故本文采用橢圓假設(shè)補償法對“羅差”進行補償。

橢圓假設(shè)補償法是基于“羅差”對輸出結(jié)果的宏觀影響來設(shè)計的[15]，在理想情況下，將磁阻傳感器在水平面旋轉(zhuǎn)一周，其X軸輸出和Y軸輸出畫在同一個坐標(biāo)系里，理論上會得到一個圓心在原點的圓，由于受周圍磁性物質(zhì)的干擾，其結(jié)果往往是圓心不在原點的橢圓，如圖2中藍色橢圓所示；通過橢圓假設(shè)補償法校正后，結(jié)果如圖2中紅色圓所示。如校正前mx=-0.0162Gs，my=-0.0176Gs；對應(yīng)校正后mx=-0.0110Gs，my=-0.0118Gs。

圖2 磁阻傳感器“羅差”校正前后比較

3 基于四元數(shù)的改進卡爾曼濾波算法

慣性系統(tǒng)姿態(tài)角解算方法具有良好的動態(tài)特性，但存在較大的隨時間累積的誤差；姿態(tài)參考系統(tǒng)靜態(tài)特性好，且不存在累積誤差。利用卡爾曼濾波方法，可以把兩個系統(tǒng)有效地結(jié)合起來，優(yōu)勢互補，提高姿態(tài)角估計計算精度。為此需建立相應(yīng)的狀態(tài)空間方程和觀測方程。以慣性系統(tǒng)解算的姿態(tài)角誤差Δθ、Δγ、Δφ作為狀態(tài)變量， 慣性系統(tǒng)解算的姿態(tài)角θINS、γINS、φINS與姿態(tài)參考系統(tǒng)解算的姿態(tài)角 θAZ，γAZ，φAZ之差作為觀測變量Z。考慮傳統(tǒng)的卡爾曼濾波方法難以消除運動加速度對姿態(tài)角估計的影響，本文根據(jù)載體運動加速度的大小，適時調(diào)整卡爾曼濾波器的量測噪聲方差R的大小，以此來減弱運動加速度對姿態(tài)角估計精度的影響。如果運動加速度的絕對值|as|大于閾值Δ，則量測噪聲方差選取R2，否則量測噪聲方差選取R1，且R2＞R1。慣性系統(tǒng)的初始四元數(shù)Q（0）通過姿態(tài)參考系統(tǒng)確定，以保證慣性系統(tǒng)初始姿態(tài)的準(zhǔn)確。最后，用慣性系統(tǒng)解算的姿態(tài)角θINS、 γINS、φINS減去卡爾曼濾波器估計的姿態(tài)角誤差φ，得到補償后的姿態(tài)角解算姿態(tài)角之前，對傳感器數(shù)據(jù)進行預(yù)處理。本文的姿態(tài)角解算原理框圖如圖3所示。

選取載體的3個姿態(tài)角誤差作為狀態(tài)變量，卡爾曼濾波器的數(shù)學(xué)模型如下：

狀態(tài)空間方程：

觀測方程：

其中狀態(tài)向量X=[ΔθΔγΔφ]；F為狀態(tài)轉(zhuǎn)移矩陣，由式（14）確定，式中h為載體所在地的高度；G為噪聲驅(qū)動矩陣；W為過程噪聲向量，假設(shè)為零均值高斯白噪聲；Z為觀測量，由式（15）確定；H為觀測矩陣，H=I3×3；V為觀測噪聲向量，也假設(shè)為與W和觀測信號不相關(guān)的、零均值高斯白噪聲。

卡爾曼濾波過程如下：

根據(jù)卡爾曼濾波的基本方程，得到狀態(tài)一步預(yù)測：

Fk，k-1——k-1時刻到k時刻的一步轉(zhuǎn)移矩陣；

Gk，k-1——k-1時刻到k時刻的系統(tǒng)噪聲驅(qū)動矩陣；

Wk——k時刻的過程噪聲。

一步預(yù)測誤差方差陣：

圖3 姿態(tài)解算算法原理框圖

式中：Pk，k-1——利用k-1時刻的誤差方差陣預(yù)測k時刻的誤差方差陣；

Pk-1——k-1時刻的誤差方差陣；

Qk——k時刻的過程噪聲方差陣。

狀態(tài)估計：

Kk——k時刻的增益矩陣；

Zk——k時刻的觀測值；

Hk——k時刻的觀測矩陣。

濾波增益矩陣：

式中Rk是k時刻的量測噪聲方差陣。

估計誤差方陣：

式中I為單位陣。

Rk的取值由下式確定：

式中R2＞R1，具體的大小可根據(jù)經(jīng)驗選取；|as|為運動加速度的模，Δ為判斷是否存在運動加速度的閾值，實際應(yīng)用中，其大小按照經(jīng)驗值設(shè)置。當(dāng)存在較大的運動加速度時，Rk增大，Kk減小，減弱了姿態(tài)參考系統(tǒng)對應(yīng)的觀測量在狀態(tài)估計中的權(quán)重，相對的增大了慣性系統(tǒng)預(yù)測值的權(quán)重，以此來削弱運動加速度對姿態(tài)角估算精度的影響。

4 實驗驗證及結(jié)果分析

本文的姿態(tài)角測量系統(tǒng)以STM32芯片為核心，由MPU6050傳感器（包括三軸陀螺儀和三軸加速度傳感器）和HMC5883L三軸磁阻傳感器組成。STM32芯片通過I2C接口讀取3個傳感器測量的數(shù)據(jù)，再由串口把數(shù)據(jù)發(fā)送到PC計算機進行保存和進一步數(shù)據(jù)處理。

為了分析載體運動加速度對姿態(tài)解算精度的影響，本文特別設(shè)計了一個運動模型，利用電梯升降過程中存在的運動加速度檢測其對姿態(tài)解算精度的影響。將測量單元安裝在45°角的三角木塊的傾斜表面，如圖4所示，測量電梯在升降過程中角速度、加速度和磁場的變化。

讓安裝有測量單元的三角木塊固定于電梯地面，電梯從1樓靜止開始上升，2樓經(jīng)停，最終停在3樓，實驗時長57 s，電梯在每層樓啟停的過程中都存在運動加速度。3個軸的加速度測量值如圖5所示，從圖中可以看出Y軸和Z軸存在明顯運動加速度。加速度數(shù)據(jù)中存在高頻干擾，可對其進行低通濾波處理。

圖4 測量單元安裝示意圖

圖5 三軸加速度測量值

分別采用慣性系統(tǒng)解算法和姿態(tài)參考系統(tǒng)解算法對載體姿態(tài)角進行解算，結(jié)果如圖6所示。從圖中可以看到，慣性系統(tǒng)通過積分解算得到的3個姿態(tài)角存在明顯的隨時間的累積誤差；姿態(tài)參考系統(tǒng)解算得到的俯仰角在含有運動加速度的時間段里，存在明顯的角度誤差。航向角由于周圍磁性物質(zhì)的干擾，也有較大的誤差。

圖6 慣性系統(tǒng)和姿態(tài)參考系統(tǒng)的姿態(tài)角比較

使用互補濾波算法進行姿態(tài)解算，其姿態(tài)角誤差如圖7所示。從圖中可以看到，互補濾波解算結(jié)果不存在像慣性系統(tǒng)一樣的累積誤差，受運動加速度的影響也較小，但是可以看到，其初始階段存在較大的誤差；而且截止頻率fc的大小對濾波效果有明顯影響：fc越大，誤差的收斂速度越快，但并不是fc越大精度越高。從航向角誤差圖中可以看出，互補濾波算法的fc很難有一個最優(yōu)的選擇。雖然使用PI調(diào)節(jié)和互補濾波相結(jié)合的算法[16]，可以提高誤差的收斂速度，但是會導(dǎo)致超調(diào)和振蕩等問題。

圖7 互補濾波算法計算的姿態(tài)角誤差

使用卡爾曼濾波算法進行姿態(tài)估算，從圖5中可以看到，Y軸和Z軸存在運動加速度，識別Y軸方向的運動加速度取as=ay-ay0，ay0是Y軸加速度的第1個測量值。識別Z軸方向的運動加速度取as=az-az0，az0是Z軸加速度的第1個測量值。在卡爾曼濾波過程中，據(jù)此通過式（21）調(diào)整測量噪聲方差大小，根據(jù)本實驗的測試數(shù)據(jù)，取R1=0.012，R2=0.0252，Δ=0.01g?？柭鼮V波解算的姿態(tài)角誤差如圖8所示。從圖中可以看出，改進后的卡爾曼濾波在含有運動加速度的時間內(nèi)，輸出的姿態(tài)角精度比一般的卡爾曼濾波高；對比互補濾波算法，雖然互補濾波消除了運動加速度的干擾，但其初始誤差大，且過程中有較大的振蕩。

圖8 卡爾曼濾波與互補濾波比較

以下式計算各種姿態(tài)角算法的均方根誤差，結(jié)果如表1所示。

式中：n——數(shù)據(jù)長度；

xk——k時刻解算出的姿態(tài)角值；

x——姿態(tài)角真實值。

從表中可以看出，姿態(tài)參考系統(tǒng)對磁場進行校正后，航向角的精度提高了?；パa濾波算法隨著截止頻率fc增大，俯仰角和橫滾角的精度得到提高，但是航向角的精度并沒有得到明顯改善?；パa濾波算法結(jié)合PI調(diào)節(jié)后，精度有所提高，但其仍然比不上卡爾曼濾波算法的精度。改進的卡爾曼濾波算法，由于能根據(jù)載體運動加速度對量測噪聲方差R進行調(diào)整，削弱了運動加速度對姿態(tài)角的影響，能獲得更高的姿態(tài)角解算精度。從俯仰角的解算結(jié)果可以得到，改進卡爾曼濾波算法相對于姿態(tài)參考系統(tǒng)均方根誤差降低了約40%。

在上述相同的實驗條件下，當(dāng)電梯升降時使俯仰角的大小改變，相應(yīng)的測量、解算結(jié)果如圖9所示。從圖中可以看出，慣性系統(tǒng)俯仰角解算結(jié)果存在明顯的隨時間的累積誤差。姿態(tài)參考系統(tǒng)解算出的俯仰角在含有運動加速度的時間段里，也存在明顯的角度誤差，其分別出現(xiàn)在8～12s，19～23s，45～49s的時間段內(nèi)，最大角度誤差達到14°。這是由于俯仰角越大，Y軸的重力加速度分量就越接近重力加速度g，俯仰角對載體的運動加速度就越敏感，由此引起的俯仰角誤差越大。對比一般的卡爾曼濾波算法與本文提出的改進卡爾曼濾波算法，在含有運動加速度的時間段里，改進卡爾曼濾波算法可以減小運動加速度對俯仰角解算的影響，與俯仰角不變情況下實驗結(jié)論相同。

表1 各種姿態(tài)角算法的均方根誤差比較

圖9 慣性系統(tǒng)、姿態(tài)參考系統(tǒng)和卡爾曼濾波的俯仰角比較

5 結(jié)束語

本文針對運動加速度對姿態(tài)參考系統(tǒng)姿態(tài)角解算的影響，提出了一種改進卡爾曼濾波算法。該算法能根據(jù)載體運動加速度的大小，適時調(diào)整卡爾曼濾波器的量測噪聲方差，來減小姿態(tài)參考系統(tǒng)對應(yīng)的觀測變量在卡爾曼濾波過程中的權(quán)重，相對的增大慣性系統(tǒng)在卡爾曼濾波過程中的權(quán)重，以此削弱運動加速度對姿態(tài)角解算的影響。通過對含有運動加速度的實驗數(shù)據(jù)進行姿態(tài)解算，對比經(jīng)典的互補濾波算法和傳統(tǒng)的卡爾曼濾波算法，表明本文提出的改進后的卡爾曼濾波算法能夠有效減小運動加速度對姿態(tài)解算的影響，獲得更高精度的姿態(tài)角解算結(jié)果。

[1]呂印新，肖前貴，胡壽松.基于四元數(shù)互補濾波的無人機姿態(tài)解算[J].燕山大學(xué)學(xué)報，2014（2）：175-180.

[2]萬良金.基于多傳感器信息融合的機器人姿態(tài)測量技術(shù)研究[D].北京：北京交通大學(xué)，2015.

[3]杜波，劉奭昕.人體姿態(tài)檢測裝置的設(shè)計[J].長春工程學(xué)院學(xué)報（自然科學(xué)版），2012，13（4）：105-107.

[4]陳曦.基于MEMS慣性傳感器的高精度姿態(tài)測量關(guān)鍵技術(shù)研究[D].杭州：浙江大學(xué)，2014.

[5]陳旭光，楊平，陳意.MEMS陀螺儀零位誤差分析與處理[J].傳感技術(shù)學(xué)報，2012，25（5）：628-632.

[6]米剛，田增山，金悅，等.基于MIMU和磁力計的姿態(tài)更新算法研究[J].傳感技術(shù)學(xué)報，2015（1）：43-48.

[7]張秋陽.無人機姿態(tài)測算及其誤差補償研究[D].長沙：中南大學(xué)，2011.

[8]盧志才，高敏，賈春寧.基于地磁測量的火箭彈滾轉(zhuǎn)角解算誤差補償方法研究[J].中國測試，2014，40（2）：9-12.

[9]VASCONCELOS J F，CARDEIRA B，SILVESTRE C，et al.Discrete-time complementary filters for attitude and position estimation： design， analysis and experimental validation[J].IEEE Transactions on Control Systems Technology，2011，19（1）：181-198.

[10]BRIGANTE C M N，ABBATE N，BASILE A，et al. Towardsminiaturization ofa MEMS-based wearable motion capture system[J].IEEE Transactions on Industrial Electronics，2011，58（8）：3234-3241.

[11]GAO C，ZHAO G，LU J.In-flight misalignment attitude estimation for UAV based on GPS and UKF/MPF filter[C]∥Guidance,Navigation and Control Conference(CGNCC), 2014 IEEE Chinese.IEEE，2015：452-457.

[12]秦永元.慣性導(dǎo)航[M].2版.北京：科學(xué)出版社，2014：130-136.

[13]陳亮.基于對偶四元數(shù)的捷聯(lián)慣性導(dǎo)航算法[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2011.

[14]王勵揚，翟昆朋，何文濤，等.四階龍格庫塔算法在捷聯(lián)慣性導(dǎo)航中的應(yīng)用[J].計算機仿真，2014，31（11）：56-59.

[15]徐昺.基于MEMS傳感器與ZigBee網(wǎng)絡(luò)的人體動作捕捉系統(tǒng)的設(shè)計與實現(xiàn)[D].成都：電子科技大學(xué)，2013.

[16]郭曉鴻，楊忠，陳喆，等.EKF和互補濾波器在飛行姿態(tài)確定中的應(yīng)用[J].傳感器與微系統(tǒng)，2011，30（11）：149-152.

（編輯：莫婕）

Research on attitude angle measurement with high precision based on MEMS sensors

LIU Zhen，WANG Xuemei，NI Wenbo
（School of Mechanical Engineering，Southwest Jiaotong University，Chengdu 610031，China）

Since the conventional attitude and heading reference system are vulnerable to the interference of motion acceleration，an improved Kalman filter algorithm was proposed in the paper. Quaternion attitude mathematical model based on inertial system was built up.The improved Kalman filter algorithm can reduce the impact of motion acceleration on attitude algorithm in the process of Kalman filter by timely adjusting the observation noise variance according to the motion acceleration of vehicle.The experimental data were measured by MEMS three-axis gyroscope，accelerometer and magnetoresistive sensors during riding up and down in the elevator.The results demonstrate that the improved Kalman filter algorithm can significantly reduce the impact of motion acceleration on attitude estimation.And compared to traditional attitude and heading reference system，attitude RMSE with improved Kalman filter algorithm has decreased by 40%.

attitude estimation；motion acceleration；complementary filtering；Kalman filter；MEMS sensor

A

：1674-5124（2017）02-0006-07

10.11857/j.issn.1674-5124.2017.02.002

2016-06-12；

：2016-07-22

劉 震（1990-），男，陜西西安市人，碩士研究生，專業(yè)方向為測控技術(shù)及應(yīng)用。

王雪梅（1968-），女，副教授，博士，研究方向為現(xiàn)代測控技術(shù)及其在軌道交通中的應(yīng)用。