張國亮,曾 成,2

(1.河北工業(yè)大學(xué)電子信息工程學(xué)院，天津 300401；2.天津市電子材料與器件重點實驗室，天津 300401)

0 引言

由于微機電系統(tǒng)(micro electro mechanical system，MEMS)陀螺儀具有低精度、漂移高的缺點，不能滿足高精度的定位需求[1]。2003年，文獻[2]提出虛擬陀螺技術(shù)，即選用多個成本低、精度一般的陀螺構(gòu)成陣列，對相同目標(biāo)進行角速度檢測，通過濾波融合方法輸出一個估計的高精度角速率值。目前，MEMS虛擬陀螺研究[3-5]大都側(cè)重于利用同一類型、同一量程的MEMS虛擬陀螺數(shù)據(jù)融合來達(dá)到提高角速度測量精度的目的。但是對載體運動動態(tài)范圍很大的情形，單一量程的陀螺在工作過程中難以兼顧大動態(tài)量程和高傳感器精度的要求。

為此，本文設(shè)計了新型多量程MEMS虛擬陀螺。首先通過改進SPI方式完成傳感器陣列數(shù)據(jù)讀取，然后將虛擬陀螺數(shù)據(jù)進行Allan方差分析和基于一階AR模型的卡爾曼濾波數(shù)據(jù)融合，最后對比在靜態(tài)條件下，相同個數(shù)與型號的虛擬陀螺在選擇不同量程數(shù)據(jù)進行融合時的濾波融合效果、1σ標(biāo)準(zhǔn)差以及陀螺儀各項噪聲降低效果；在同一次動態(tài)實驗中，對比多量程虛擬陀螺與單一量程虛擬陀螺通過姿態(tài)解算獲取的姿態(tài)變化軌跡與最終姿態(tài)角解算精度。

1 多量程傳感器陣列系統(tǒng)搭建

1.1 虛擬陀螺模塊

本文采用運動型六軸傳感器ICM42605搭建多量程虛擬陀螺，該傳感器包括三軸陀螺儀與三軸加速度計，可供選擇量程范圍廣，符合本課題研究，可通過軟件選擇量程包括±15.625(°)/s，±31.25(°)/s，±62.5(°)/s，±125(°)/s，±250(°)/s，±500(°)/s，±1 000(°)/s，±2 000(°)/s，考慮到成本以及虛擬陀螺隨著規(guī)模擴大，提升效果逐漸減弱，選擇16個同類型傳感器搭建陣列，采用量程與個數(shù)配比如表1所示。

表1 虛擬陀螺量程個數(shù)配比

1.2 改進SPI數(shù)據(jù)讀取方式

ICM42605的SPI通信速率最高可達(dá)到24 MHz，STM32F429IGT6通過晶振倍頻輸出頻率可達(dá)到180 MHz，SPI的時鐘信號線頻率可通過波特率發(fā)生器進行分頻，輸出速率最高可達(dá)22.5 MHz。當(dāng)單獨讀取一個ICM42605數(shù)據(jù)時，需要讀取三軸陀螺儀數(shù)據(jù)、三軸加速度計數(shù)據(jù)、溫度傳感器數(shù)據(jù)，共7軸數(shù)據(jù)，每軸輸出數(shù)據(jù)都是16位，單個傳感器讀取時間為4.98 μs。當(dāng)用1組SPI與16個普通GPIO作為傳感器軟件片選讀取16個ICM42605數(shù)據(jù)時，讀取時間為79.64 μs?？紤]到慣性傳感器數(shù)據(jù)具有同步性，不可延時，盡量縮短一次采集過程所需時間，提出了一種新型數(shù)據(jù)采集方式。

本系統(tǒng)采用改進SPI方式完成數(shù)據(jù)讀取，如圖1所示，使用主控芯片STM32F429IGT6 4組SPI外設(shè)SPI2、SPI3、SPI4、SPI5，每組SPI控制4個傳感器工作在一種量程，共用該組SPI的SCK、MISO、MOSI。片選選用SPI硬件片選，將硬件片選信號與4組SPI共用的4組GPIO-CS1、CS2、CS3、CS4作為或門芯片SN74LV32ADR輸入電平，將或門輸出電平作為傳感器片選輸入信號。例如當(dāng)CS1輸出低電平時，同時使能4組SPI硬件片選，可同時選通CS1控制的4個傳感器，實現(xiàn)SPI硬件片選下的一主多從，讀取時間可縮短1/4。

圖1 陣列數(shù)據(jù)讀取方式

數(shù)據(jù)讀取過程中利用SPI的DMA(direct memory access,直接存儲區(qū)訪問)模式，DMA在不占用CPU時可實現(xiàn)內(nèi)部存儲與各種外設(shè)之間高速搬移數(shù)據(jù)，將4組SPI分別配置為不同DMA數(shù)據(jù)流，不同SPI可實現(xiàn)數(shù)據(jù)同時讀取，保證數(shù)據(jù)實時性。初始化各種外設(shè)后，進入定時中斷，完成傳感器數(shù)據(jù)讀取與數(shù)據(jù)合并，在4組SPI各自DMA中斷中取消使能硬件片選，清除中斷標(biāo)志，給予共用GPIO高電平，斷開傳感器與SPI之間通信，在定時中斷外將傳感器數(shù)據(jù)進行整合。

1.3 硬件系統(tǒng)平臺

本實驗搭建的硬件系統(tǒng)平臺，如圖2所示。

圖2 硬件系統(tǒng)平臺

2 多量程虛擬陀螺濾波融合方法

對于內(nèi)含多種噪聲的信號，需要將干擾噪聲濾除，卡爾曼濾波因其使用條件低，被廣泛應(yīng)用。在對陀螺儀數(shù)據(jù)進行濾波前，需要針對陀螺儀隨機誤差進行分析獲取建模因子，本系統(tǒng)采用式(1)一階AR模型，針對實際角速率狀態(tài)進行建模[6]：

ωk=-bωk-1+μk

(1)

式中：b為模型參數(shù)；μk為零均值白噪聲。

卡爾曼濾波的狀態(tài)方程表示為

XK=AK·XK-1+ζK-1·ψK-1

(2)

式中：ψ為系統(tǒng)噪聲，cov[ψn,ψm]=qδnm，E[ψK]=0；XK=ωK；AK=-b；ζK-1=1。

多量程MEMS虛擬陀螺雖然由16個傳感器組成，但是每次只選擇一種最佳量程，即選擇4個傳感器數(shù)據(jù)進行融合，K=1,2,3,4。

觀測方程為

Zk=CK·XK+φK

(3)

本系統(tǒng)采用同一型號傳感器組成陣列，利用其相關(guān)性可進一步優(yōu)化濾波融合技術(shù)，則觀測噪聲協(xié)方差矩陣R為

(4)

式中：ρnm(n,m=1,2,3,4)為傳感器相關(guān)性系數(shù)；σ2為陀螺儀噪聲方差，通過Allan方差法[7-8]獲得。

預(yù)測下一步方程：

(5)

均方誤差預(yù)測：

PK|K-1=APK-1|K-1AT+ψK

(6)

濾波增益矩陣：

KK=PK|K-1CT|(CPK|K-1CT|+φK)

(7)

狀態(tài)估計方程：

(8)

均方誤差更新：

PK|K=(I-KKC)PK|K-1

(9)

3 姿態(tài)信息獲取

動態(tài)條件下虛擬陀螺的提升效果可通過解算虛擬陀螺的輸出姿態(tài)軌跡與最終姿態(tài)角解算精度得出。常見的姿態(tài)角解算算法有歐拉角法、方向余弦法、四元數(shù)法等，其中四元數(shù)法[9]因其計算量小、精度高，可避免奇異性被廣泛使用在姿態(tài)解算上。四元數(shù)姿態(tài)解算變量為

Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(10)

式中：q0、q1、q2、q3為實數(shù)；i、j、k是相互正交的單位向量。

四元數(shù)微分方程為

(11)

(12)

式中：ωx、ωy、ωz分別為陀螺儀在載體坐標(biāo)系下繞X、Y、Z軸的角速度。

因該陣列計算數(shù)據(jù)量少，得到陀螺儀的三軸角速度后，根據(jù)以下公式更新四元數(shù)。

(13)

(14)

(15)

(16)

式中T為采樣時間。

由于在計算過程會產(chǎn)生誤差，需要對更新的四元數(shù)進行歸一化處理，處理完畢即可根據(jù)以下公式進行姿態(tài)角解算：

(17)

Y=-arcsin[2(q1q3-q0q2)]

(18)

(19)

4 實驗分析

4.1 靜態(tài)測試

將多量程虛擬陀螺系統(tǒng)置于靜止轉(zhuǎn)臺上，初始化虛擬陀螺，靜置1 h，等待陀螺儀輸出數(shù)據(jù)穩(wěn)定后，以200 Hz的采樣頻率連續(xù)采集5 h的數(shù)據(jù)，取其中3 h數(shù)據(jù)進行實驗分析。

在同一次靜態(tài)測量中，每種量程陀螺個數(shù)都是4，將4種量程離線數(shù)據(jù)分別完成進行Allan誤差分析與濾波融合，以X軸為例，得到如圖3～圖6所示的結(jié)果，從圖中可以看出虛擬陀螺經(jīng)過濾波后輸出信號的噪聲都有很大程度降低。

圖3 ±15.625(°)/s量程虛擬陀螺融合效果

圖4 ±62.5(°)/s量程虛擬陀螺融合效果

圖5 ±125(°)/s量程虛擬陀螺融合效果

圖6 ±2 000(°)/s量程虛擬陀螺融合效果

根據(jù)所選量程的不同得到的融合效果也不同，±15.625(°)/s量程虛擬陀螺效果最佳，±62.5(°)/s量程虛擬陀螺，±125(°)/s量程虛擬陀螺，±2 000(°)/s量程虛擬陀螺融合效果依次減弱，且其原始信號噪聲逐步增大。

對不同量程下陀螺組合陣列的原始數(shù)據(jù)與融合數(shù)據(jù)進行1σ標(biāo)準(zhǔn)差分析，如表2所示。以X軸為例，4種量程組合陣列1σ標(biāo)準(zhǔn)差的均值分別為0.249 5(°)/s，0.321 2(°)/s、0.449 1(°)/s、0.494 0(°)/s，融合后分別為0.057 9(°)/s、0.097 8(°)/s、0.160 1(°)/s、0.183 8(°)/s，分別降低了76.8%、69.5%、64.4%、62.3%。

表2 1σ標(biāo)準(zhǔn)差分析

分別對4種組合量程陣列進行Allan方差數(shù)據(jù)擬合得到隨機噪聲數(shù)值，表3列舉了角度隨機游走，零偏不穩(wěn)定性，量化噪聲的原始數(shù)據(jù)與經(jīng)過濾波融合后輸出數(shù)據(jù)。

表3 濾波后的誤差提升效果

從表3可得，經(jīng)過濾波后噪聲數(shù)值都有所降低。零偏不穩(wěn)定性是衡量虛擬陀螺性能的主要參數(shù)。該陀螺儀的原始零偏不穩(wěn)定性數(shù)據(jù)為51.1 387(°)/h，經(jīng)過濾波融合后，4種量程陣列零偏不穩(wěn)定性數(shù)據(jù)分別為5.551 0(°)/h、6.133 9(°)/h、8.404 5(°)/h、12.512 8(°)/h，分別降低了89.1%、88%、83.6%、75.5%。

靜態(tài)實驗結(jié)果表明，當(dāng)檢測對象處于靜態(tài)時，多量程MEMS虛擬陀螺可選擇最小量程數(shù)據(jù)進行融合，得到的檢測效果要優(yōu)于大量程陣列。

4.2 動態(tài)測試

將多量程MEMS虛擬陀螺置于轉(zhuǎn)臺上，靜置30 min，等待輸出數(shù)據(jù)穩(wěn)定后，進行模擬載體大動態(tài)實驗，控制讀取時間相同，在動態(tài)行為結(jié)束后，將虛擬陀螺置于起始位置，采集15～20 s數(shù)據(jù)。

在同一次動態(tài)實驗中，數(shù)據(jù)采集完成之后，對整個過程離線數(shù)據(jù)進行預(yù)處理，首先對所有傳感器陀螺儀數(shù)據(jù)進行判斷，將超出量程的數(shù)據(jù)用nan代替，將數(shù)據(jù)進行濾波融合時，輸入16個傳感器三軸陀螺儀數(shù)據(jù)，利用Matlab中isnan函數(shù)進行判斷，將不是nan的數(shù)據(jù)重新整合到數(shù)組中，根據(jù)數(shù)組長度判斷，每次動態(tài)選擇4個相同量程的數(shù)據(jù)進行融合，長度與量程關(guān)系如表4所示。

表4 數(shù)組長度與量程數(shù)據(jù)選擇關(guān)系

數(shù)據(jù)預(yù)處理與濾波融合后，對整個過程利用四元數(shù)方式進行姿態(tài)角解算，通過解算陣列回到起始位置時數(shù)據(jù)的精度可以衡量整個虛擬陀螺系統(tǒng)在大動態(tài)下的性能，以X軸為例，實驗結(jié)果如圖7～圖9所示。

圖7 多量程虛擬陀螺姿態(tài)變化

圖8 ±2 000(°)/s量程虛擬陀螺姿態(tài)變化

圖9 ±15.625(°)/s量程虛擬陀螺姿態(tài)變化

從圖7～9中可以看出，在模擬大動態(tài)條件下，多量程虛擬陀螺，±2 000(°)/s量程虛擬陀螺能解算出相似的姿態(tài)變化軌跡，最終姿態(tài)角偏離程度小；±15.625(°)/s量程虛擬陀螺解算結(jié)果與二者不符，且最終姿態(tài)角偏離程度大。

分別對3類陣列再次置于起始位置時姿態(tài)數(shù)據(jù)作殘差分析，實驗結(jié)果如表5所示。

表5 最終姿態(tài)角殘差分析

從表5可以看出，3種量程組合下，虛擬陀螺解算出X軸最終姿態(tài)角分別為0.181 9°、0.335 8°、10.040 8°。實驗結(jié)果表明：多量程MEMS虛擬陀螺最終姿態(tài)角解算精度要優(yōu)于單一量程虛擬陀螺，更接近真實情況。

5 結(jié)束語

本文設(shè)計了多量程MEMS虛擬陀螺，利用改進SPI數(shù)據(jù)讀取方式，提高了數(shù)據(jù)讀取速率。在靜態(tài)實驗中，分析對比相同數(shù)量虛擬陀螺選擇不同量程時陣列融合效果、1σ標(biāo)準(zhǔn)差以及降噪能力，由實驗數(shù)據(jù)可知，多量程虛擬陀螺1σ標(biāo)準(zhǔn)差降低了76.8%，零偏不穩(wěn)定性下降了89.1%；在動態(tài)實驗中，多量程虛擬陀螺不僅能獲取與大量程虛擬陀螺相似的姿態(tài)變化軌跡，還能獲得優(yōu)于單一小量程陣列的姿態(tài)解算精度。實驗結(jié)果說明：靜態(tài)和動態(tài)實驗下，多量程MEMS虛擬陀螺的姿態(tài)監(jiān)測與降噪能力效果優(yōu)于單一量程虛擬陀螺。