基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)姿態(tài)測量算法

盛娟紅，張志安，邢炳楠

(南京理工大學(xué) 機(jī)械工程學(xué)院，江蘇 南京 210094)

0　引　言

隨著低成本慣性器件傳感器技術(shù)的逐漸成熟，利用MEMS陀螺儀和加速度計(jì)進(jìn)行姿態(tài)測量成為近年來捷聯(lián)式低成本、全固態(tài)測量系統(tǒng)的研究熱點(diǎn)之一[1-3]. 它具有體積小、成本低、重量輕、能耗低、可靠性高等優(yōu)點(diǎn)，在各個領(lǐng)域都有廣泛的應(yīng)用[4，5].

由于器件的硬件精度問題，陀螺儀本身存在漂移誤差，且易受振動、溫度等外部環(huán)境因素干擾，導(dǎo)致漂移誤差逐漸累積變大，因此，單一的MEMS陀螺儀傳感器無法保證長時間測姿的準(zhǔn)確性[6]. 文獻(xiàn)[7] 采用的是多傳感器進(jìn)行加權(quán)平均的融合算法來保證算法可適應(yīng)動態(tài)環(huán)境,但是測量精度有待提高； 文獻(xiàn)[8]通過建立多傳感器誤差數(shù)據(jù)模型比較準(zhǔn)確地預(yù)測飛行體的姿態(tài)，但是卻不能適應(yīng)動態(tài)飛行環(huán)境. 本文設(shè)計(jì)了一種基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)姿態(tài)測量算法在線實(shí)時估計(jì)運(yùn)動體的姿態(tài)角度. 算法基于擴(kuò)展卡爾曼濾波算法來實(shí)現(xiàn)數(shù)據(jù)融合，利用帶遺傳因子的限定記憶長度的Allan方差在線估計(jì)運(yùn)動時傳感器數(shù)據(jù)誤差的量測噪聲，實(shí)時修正角度估計(jì)誤差，從而有效解決了動態(tài)噪聲和姿態(tài)最優(yōu)估計(jì)的問題.

1　姿態(tài)的運(yùn)動學(xué)模型

圖 1　導(dǎo)航坐標(biāo)系Fig.1　Navigation coordinate system

本文采用彈道方程中的基準(zhǔn)坐標(biāo)系為導(dǎo)航坐標(biāo)系，如圖 1 所示. 其中OX軸沿水平線指向射擊方向，OY軸鉛直向上，OZ軸按右手法則確定，垂直于射擊面指向右方.

運(yùn)動體的空間位置可看做依次繞y軸做航向、繞z軸做俯仰、繞x軸做橫滾的復(fù)合運(yùn)動，因此，有

(1)

從而推出

(2)

其中

角標(biāo)n,b分別代表導(dǎo)航坐標(biāo)系、由運(yùn)動體確定的動坐標(biāo)系；Wx,Wy,Wz指陀螺儀輸出的3個角速度.

2　自適應(yīng)擴(kuò)展Kalman濾波算法

2.1　擴(kuò)展Kalman濾波器

(3)

其中，Wk=[WγWψWθ]T是狀態(tài)噪聲序列； [WxWyWz]T是陀螺輸出的角速率. 同時，得到狀態(tài)方程的雅克比矩陣為

(4)

其中，a11,a13,a21,a23,a31為

利用重力場測量姿態(tài)傾角時，把三軸加速度計(jì)測得的線加速度值轉(zhuǎn)化成姿態(tài)角度信息，以此作為量測值，量測噪聲為Vk. 其中，根據(jù)量測值和狀態(tài)值可知Hk為3×3的單位矩陣，此處忽略不寫，寫出量測方程為

(5)

由于航向角ψ的方向與重力場方向正交，此處加速度計(jì)不能用來預(yù)測估計(jì)航向角度值. 其他兩個角為

(6)

根據(jù)狀態(tài)方程與量測方程可以得到預(yù)測(時間更新)與量測更新方程，即

預(yù)測(時間更新)方程為

(7)

量測更新方程為

Px,k=I-KkHkPx,k/k-1,

(8)

為了盡可能消除量測噪聲時變的這部分影響，本文在利用Allan方差估計(jì)量測噪聲的同時，加入了遺忘因子以及限定記憶長度的算法思想，實(shí)時估計(jì)量測白噪聲方差參數(shù)，以期獲得更為準(zhǔn)確的姿態(tài)解算結(jié)果. 下面開始給出系統(tǒng)噪聲方差和實(shí)時動態(tài)量測噪聲的獲取方法.

2.2　改進(jìn)自適應(yīng)Kalman濾波

2.2.1陀螺儀系統(tǒng)噪聲估計(jì)

陀螺儀的系統(tǒng)噪聲主要由陀螺儀系統(tǒng)內(nèi)部機(jī)理決定，相對來說比較穩(wěn)定. 濾波時，未知的噪聲參數(shù)越多，濾波越容易發(fā)散，所以應(yīng)盡可能減少未知噪聲參數(shù)，并且相比于大數(shù)據(jù)量的靜態(tài)測量，對系統(tǒng)噪聲在線自適應(yīng)估計(jì)的精度更低. 綜合以上兩點(diǎn)，本文對于系統(tǒng)噪聲不采用實(shí)時估計(jì)的方法，主要利用Allan方差對MEMS傳感器測得的靜態(tài)數(shù)據(jù)事先進(jìn)行建模分析，獲得MEMS傳感器誤差的隨機(jī)噪聲種類，確定各項(xiàng)系數(shù),從而求出系統(tǒng)噪聲.

1) Allan方差原理

Allan方差是一種基于時域分析的分析方法，可以比較容易地對各種誤差源以及整個噪聲統(tǒng)計(jì)特性進(jìn)行細(xì)致的辨識，獲取隨機(jī)噪聲種類，確定噪聲系數(shù)，因此被廣泛地用來辨識慣性傳感器的噪聲誤差源[9]. Allan可按式(9)來估算.

(9)

假設(shè)采樣頻率是fs，數(shù)據(jù)樣本個數(shù)是M，則τL=L/fs，NL=M/L. 把數(shù)據(jù)樣本分成L長度的串，計(jì)算每個數(shù)據(jù)串的平均值Ωk，然后按照式(9)計(jì)算.

對于陀螺儀而言，系統(tǒng)噪聲主要有量化噪聲(quantization noise, QN)、角度隨機(jī)游走(angular random walk, ARW )、角速率隨機(jī)游走(rate random walk, RRW)、零偏不穩(wěn)定噪聲(bias instability, BI)以及速率斜坡(rate ramp, RR)[10]. 這五種噪聲可以通過 Allan 方差分析方法來辨識，并且誤差分離結(jié)果較好，對應(yīng)關(guān)系如表 1 所示[11].

表 1　常見噪聲系數(shù)與Allan方差的對應(yīng)關(guān)系

2) Allan方差分析

實(shí)驗(yàn)采用的是目前市場上比較常見的MPU6050姿態(tài)傳感器，以100 Hz的頻率在實(shí)驗(yàn)室單軸轉(zhuǎn)臺采集陀螺儀輸出的靜態(tài)數(shù)據(jù)，得到隨機(jī)漂移數(shù)據(jù)樣本，如圖 2 所示.

圖 2　陀螺儀隨機(jī)漂移數(shù)據(jù)Fig.2　Gyroscope random drift data

根據(jù)陀螺靜態(tài)數(shù)據(jù)計(jì)算Allan方差，得到Allan方差的雙對數(shù)圖，如圖 3 所示.

圖 3　陀螺儀Allan方差雙對數(shù)圖Fig.3　Allan variance double logarithmic curve of gyroscope

根據(jù)Allan方差的雙對數(shù)圖確認(rèn)該陀螺儀主要存在角速率隨機(jī)游走、角度隨機(jī)游走和零偏不穩(wěn)定性3種噪聲. 根據(jù)表1即可得到三軸陀螺儀的噪聲系數(shù)，如表 2 所示.

表 2　各軸陀螺儀噪聲系數(shù)

根據(jù)表 2 的數(shù)據(jù)分析可知，該陀螺儀的角度隨機(jī)游走噪聲系數(shù)較小，主要誤差項(xiàng)為零偏不穩(wěn)定性和角速率隨機(jī)游走，以此兩項(xiàng)求得白噪聲方差作為系統(tǒng)噪聲方差值.

2.2.2陀螺儀動態(tài)噪聲估計(jì)

量測噪聲主要由外部環(huán)境造成，容易發(fā)生變化，且存在一定的不可預(yù)知性. 因此，對量測噪聲進(jìn)行自適應(yīng)處理，有利于保證系統(tǒng)濾波的穩(wěn)定性.

本文在采用Allan方差來估計(jì)量測白噪聲方差參數(shù)的同時，加入了遺傳因子和限定于記憶的算法思想. 一方面對舊數(shù)據(jù)加入了遺傳因子，降低了舊數(shù)據(jù)對誤差辨識的影響，提高了新數(shù)據(jù)對誤差辨識的作用； 另一方面，限定算法的記憶長度，即辨識數(shù)據(jù)達(dá)到一定個數(shù)的時候，每增加一個新數(shù)據(jù)，同時剔除一個舊數(shù)據(jù)，克服了僅帶遺傳因子的漸消記憶算法中老數(shù)據(jù)時刻影響辨識的缺點(diǎn)，更能有效地實(shí)時估計(jì)量測噪聲.

式(10)是Allan方差估計(jì)公式的遞推.

(10)

從式(10)可以看出，Allan方差遞推公式最后得到的結(jié)果和帶遺傳因子的漸消記憶算法在形式上是一致的.

(11)

式中：b是一個常值，取值范圍為0

通過調(diào)整常值b的大小，可適當(dāng)調(diào)整過往數(shù)據(jù)在Allan方差計(jì)算中所占比重，相比較于式(10)可進(jìn)行一定的比重調(diào)整，有利于提高辨識精度. 并且本文只利用最新固定長度的N個數(shù)據(jù)來遞推方差值，這個固定長度可以利用Allan方差仿真已知數(shù)學(xué)模型方差變化的數(shù)據(jù)來估計(jì)得到. 通過多次實(shí)驗(yàn)仿真驗(yàn)證，本文中N取600時，既能保證數(shù)據(jù)方差的及時更新，也能使計(jì)算結(jié)果的波動盡量小.

當(dāng)獲取的數(shù)據(jù)長度大于固定長度N時，可以將式(10)轉(zhuǎn)化為式(12)，即

(12)

3　實(shí)驗(yàn)仿真驗(yàn)證

圖 4　MPU-6050傳感器檢測軸及其方向Fig.4　The detection axis and its orientation of MPU-6050 sensor

3.1　實(shí)驗(yàn)條件

本文所使用的姿態(tài)傳感器為較常見的MPU-6050，內(nèi)部集成有三軸MEMS陀螺儀和三軸加速度計(jì). 為了應(yīng)對實(shí)際使用中物體運(yùn)動快慢的不同，傳感器的測量范圍以及傳感器輸出頻率等等都可通過設(shè)置相關(guān)寄存器進(jìn)行設(shè)定. 本文所設(shè)定的陀螺儀量程為±1 000 (°)/s，加速度計(jì)量程為±8g，姿態(tài)傳感器的數(shù)據(jù)輸出頻率為100 Hz.

試驗(yàn)是利用搖擺平臺在俯仰方向擺動來測取數(shù)據(jù)的，將MPU-6050安裝在搖擺平臺上，將Y軸平行于旋轉(zhuǎn)主軸方向，MPU-6050和搖擺平臺的俯仰運(yùn)動方向保持一致.

在具體的試驗(yàn)過程中使用遙控器控制搖擺平臺在俯仰方向進(jìn)行隨機(jī)角度擺動，在來回的擺動過程中采集三軸陀螺儀和三軸加速度計(jì)數(shù)據(jù)以及8191線編碼器數(shù)據(jù)，其中，編碼器數(shù)據(jù)得到的旋轉(zhuǎn)角度精度小于0.04°，可以作為旋轉(zhuǎn)過程中的真實(shí)角度值，以方便比較.

3.2　實(shí)驗(yàn)結(jié)果

多次進(jìn)行俯仰方向的搖擺實(shí)驗(yàn)，利用采集到的數(shù)據(jù)通過本文改進(jìn)之后的自適應(yīng)濾波算法進(jìn)行解算，同時通過常規(guī)自適應(yīng)濾波算法計(jì)算，計(jì)算結(jié)果與真實(shí)角度值比照，求取誤差的絕對值來作為精度比較的依據(jù). 本文選取了其中一組實(shí)驗(yàn)數(shù)據(jù)仿真圖進(jìn)行分析，數(shù)據(jù)仿真圖如圖 5～7 所示.

圖 5　實(shí)驗(yàn)數(shù)據(jù)仿真的整體圖Fig.5　The overall map of experimental data simulation

圖 6　實(shí)驗(yàn)數(shù)據(jù)仿真的誤差圖Fig.6　Error graph of experimental data simulation

圖 7　實(shí)驗(yàn)數(shù)據(jù)仿真局部圖Fig.7　The partial map of experimental data simulation

圖 5～7 為本文擺動實(shí)驗(yàn)中采集得到的數(shù)據(jù)仿真圖. 從仿真圖 5 與圖 6 可以發(fā)現(xiàn)，使用本文基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)算法解算出來的角度曲線和實(shí)際的角度曲線基本吻合； 通過圖 7 可以發(fā)現(xiàn)，相對于常規(guī)的濾波算法，該算法解算出來的角度曲線更為接近編碼器解算的實(shí)際角度曲線. 說明本文中提出的基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)算法有良好的抑制動態(tài)噪聲能力, 能提高系統(tǒng)對環(huán)境變化的適應(yīng)性，得到更好的濾波效果.

4　結(jié)　論

本文對姿態(tài)求解算法進(jìn)行了一定的研究，提出了一種基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)姿態(tài)測量算法. 首先，通過對MEMS陀螺靜態(tài)數(shù)據(jù)進(jìn)行Allan方差分析，得到MEMS陀螺儀系統(tǒng)噪聲估計(jì)； 然后，采用擴(kuò)展卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，同時，在對動態(tài)噪聲估計(jì)的過程中加入了遺傳因子、限定記憶長度的算法思想； 最后，通過實(shí)驗(yàn)和仿真，驗(yàn)證了本文提出的算法可有效地跟蹤噪聲參數(shù)的變化,提高了姿態(tài)角度測量精度.