盛娟紅，張志安，邢炳楠

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

0　引　言

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

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

1　姿態(tài)的運(yùn)動(dòng)學(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)動(dòng)體的空間位置可看做依次繞y軸做航向、繞z軸做俯仰、繞x軸做橫滾的復(fù)合運(yùn)動(dòng)，因此，有

(1)

從而推出

(2)

其中

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

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

2.1　擴(kuò)展Kalman濾波器

(3)

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

(4)

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

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

(5)

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

(6)

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

預(yù)測(cè)(時(shí)間更新)方程為

(7)

量測(cè)更新方程為

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

(8)

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

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

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

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

1) Allan方差原理

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

(9)

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

對(duì)于陀螺儀而言，系統(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 方差分析方法來辨識(shí)，并且誤差分離結(jié)果較好，對(duì)應(yīng)關(guān)系如表 1 所示[11].

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

2) Allan方差分析

實(shí)驗(yàn)采用的是目前市場(chǎng)上比較常見的MPU6050姿態(tài)傳感器，以100 Hz的頻率在實(shí)驗(yàn)室單軸轉(zhuǎn)臺(tái)采集陀螺儀輸出的靜態(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方差的雙對(duì)數(shù)圖，如圖 3 所示.

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

根據(jù)Allan方差的雙對(duì)數(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陀螺儀動(dòng)態(tài)噪聲估計(jì)

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

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

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

(10)

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

(11)

式中：b是一個(gè)常值，取值范圍為0

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

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

(12)

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

圖 4　MPU-6050傳感器檢測(cè)軸及其方向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)對(duì)實(shí)際使用中物體運(yùn)動(dòng)快慢的不同，傳感器的測(cè)量范圍以及傳感器輸出頻率等等都可通過設(shè)置相關(guān)寄存器進(jìn)行設(shè)定. 本文所設(shè)定的陀螺儀量程為±1 000 (°)/s，加速度計(jì)量程為±8g，姿態(tài)傳感器的數(shù)據(jù)輸出頻率為100 Hz.

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

在具體的試驗(yàn)過程中使用遙控器控制搖擺平臺(tái)在俯仰方向進(jìn)行隨機(jī)角度擺動(dòng)，在來回的擺動(dòng)過程中采集三軸陀螺儀和三軸加速度計(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)行解算，同時(shí)通過常規(guī)自適應(yīng)濾波算法計(jì)算，計(jì)算結(jié)果與真實(shí)角度值比照，求取誤差的絕對(duì)值來作為精度比較的依據(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 為本文擺動(dòng)實(shí)驗(yàn)中采集得到的數(shù)據(jù)仿真圖. 從仿真圖 5 與圖 6 可以發(fā)現(xiàn)，使用本文基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)算法解算出來的角度曲線和實(shí)際的角度曲線基本吻合； 通過圖 7 可以發(fā)現(xiàn)，相對(duì)于常規(guī)的濾波算法，該算法解算出來的角度曲線更為接近編碼器解算的實(shí)際角度曲線. 說明本文中提出的基于MEMS陀螺儀和加速度計(jì)的自適應(yīng)算法有良好的抑制動(dòng)態(tài)噪聲能力, 能提高系統(tǒng)對(duì)環(huán)境變化的適應(yīng)性，得到更好的濾波效果.

4　結(jié)　論

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