一種應(yīng)用于三軸穩(wěn)定器中的陀螺儀漂移抑制方法

程小輝，張 宇

（桂林理工大學(xué)a.廣西嵌入式技術(shù)與智能系統(tǒng)重點(diǎn)實(shí)驗(yàn)室；b.信息科學(xué)與工程學(xué)院，廣西桂林541006）

0 引 言

科學(xué)技術(shù)的進(jìn)步推動(dòng)了微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）的快速發(fā)展，而人工智能的浪潮也為MEMS 器件的發(fā)展帶來(lái)新的方法。由于MEMS器件的特征長(zhǎng)度在微米級(jí)甚至納米級(jí)，且質(zhì)量輕、功耗低、可靠性高、靈敏性好、易于集成，因此逐漸取代了傳統(tǒng)的機(jī)械傳感器。其中，微機(jī)械陀螺儀即MEMS陀螺儀作為一種靈敏可靠的慣性器件被廣泛用于無(wú)人機(jī)、慣性導(dǎo)航等運(yùn)動(dòng)傳感器中。

傳統(tǒng)機(jī)械陀螺儀主要利用的是角動(dòng)量守恒原理來(lái)維持機(jī)械的平衡；而MEMS 陀螺儀主要利用科里奧利力的原理來(lái)維持平衡。MEMS陀螺儀精度的高低決定了它所應(yīng)用控制裝置的穩(wěn)定性，因此，用現(xiàn)有的人工智能技術(shù)提高陀螺儀的精度是不可忽視的方法。陀螺儀的精度主要由誤差來(lái)決定，即陀螺儀的漂移值。陀螺儀的漂移誤差分為確定性漂移誤差和隨機(jī)漂移誤差［1］，確定性漂移誤差是由系統(tǒng)干擾力矩和陀螺儀自轉(zhuǎn)軸偏離給定方向的進(jìn)動(dòng)累計(jì)等等各種確定性的因素造成的，通過(guò)建立確定誤差的數(shù)學(xué)模型進(jìn)行相應(yīng)的誤差補(bǔ)償可以基本消除確定性漂移誤差。而陀螺儀的隨機(jī)漂移是由干擾力矩中隨機(jī)性部分引起的，隨機(jī)漂移誤差變化規(guī)律沒(méi)有確定的形式，也無(wú)法用確定的數(shù)學(xué)函數(shù)來(lái)加以描述，所以不能用常規(guī)方法加以補(bǔ)償。因此只能從算法方向入手，通過(guò)算法來(lái)對(duì)隨機(jī)誤差進(jìn)行分析補(bǔ)償，才能削弱隨機(jī)漂移誤差，抑制陀螺儀的漂移誤差，提高陀螺儀的精度。

針對(duì)陀螺儀的漂移問(wèn)題，車曉蕊等［2］通過(guò)單軸正反連續(xù)旋轉(zhuǎn)的解調(diào)技術(shù)和卡爾曼濾波減小了姿態(tài)角誤差，提高了系統(tǒng)的精度。張佳舒等［3］通過(guò)RBF算法將機(jī)械臂的控制誤差收斂到零，實(shí)驗(yàn)表明，該方法在控制誤差收斂的問(wèn)題上有較好的效果。Wang 等［4］將基于相空間的線性分解和卡爾曼濾波算法應(yīng)用于單軸的捷聯(lián)慣導(dǎo)系統(tǒng)能夠有效地降低陀螺儀的噪聲干擾。徐韓等［5］通過(guò)阿倫方差法驗(yàn)證了改進(jìn)的卡爾曼濾波可以有效地對(duì)MEMS 陀螺進(jìn)行誤差補(bǔ)償。Peng 等［6］用Allan方差法對(duì)陀螺儀的隨機(jī)誤差進(jìn)行了分析。吳鵬等［7］將卡爾曼濾波應(yīng)用與慣性系統(tǒng)中，得到了預(yù)期的成果。還有很多學(xué)者用了不同的方法對(duì)MEMS 陀螺儀的漂移進(jìn)行抑制［8-13］。

RBF神經(jīng)網(wǎng)絡(luò)具有非線性變換的特性和高度并行的運(yùn)算能力，在理論上可以以任意精度逼近非線性函數(shù)的非線性函數(shù)［14-15］。本文根據(jù)陀螺的隨機(jī)漂移誤差需要預(yù)測(cè)補(bǔ)償?shù)奶攸c(diǎn)，通過(guò)RBF神經(jīng)網(wǎng)絡(luò)進(jìn)行補(bǔ)償，并在之后進(jìn)行卡爾曼濾波算法可以很好地補(bǔ)償陀螺儀具有慢時(shí)變、非線性的特點(diǎn)的隨機(jī)漂移誤差累計(jì)，對(duì)隨機(jī)誤差進(jìn)行消除。因此通過(guò)神經(jīng)網(wǎng)絡(luò)RBF 算法與卡爾曼濾波相結(jié)合對(duì)MEMS 陀螺的隨機(jī)漂移誤差進(jìn)行建模補(bǔ)償，可以抑制陀螺儀的隨機(jī)誤差漂移，達(dá)到預(yù)期效果。

1 模型的建立

1.1 三軸穩(wěn)定器模型

三軸平衡穩(wěn)定器的模型如圖1 所示，手持握把中嵌入有控制芯片，控制穩(wěn)定器3 個(gè)不同位置中的航向電機(jī)、橫滾電機(jī)和俯仰電機(jī)。每個(gè)電機(jī)外部都有可控的旋轉(zhuǎn)軸。使用時(shí)，將相機(jī)正向推入卡座中與穩(wěn)定器通過(guò)端口相連，打開開關(guān)，穩(wěn)定器的俯仰軸、橫滾軸、航向軸就會(huì)在內(nèi)部算法的指令下對(duì)相機(jī)位置進(jìn)行穩(wěn)定調(diào)整。當(dāng)相機(jī)位置發(fā)生變化時(shí)，芯片監(jiān)測(cè)到相機(jī)位置變化，會(huì)通過(guò)電路驅(qū)動(dòng)電機(jī)控制外部旋轉(zhuǎn)軸來(lái)對(duì)這些不平衡量進(jìn)行調(diào)整，通過(guò)陀螺儀、加速度計(jì)等內(nèi)部模塊調(diào)整系統(tǒng)參數(shù)從而控制3 個(gè)電機(jī)的位置使得相機(jī)始終處于平衡位置，達(dá)到相機(jī)拍攝畫面平穩(wěn)的目的。

穩(wěn)定器旋轉(zhuǎn)的方向按右手法則定義，即右手大拇指指向軸向，四指彎曲的方向就是該軸旋轉(zhuǎn)的方向。X軸角度即為繞X軸旋轉(zhuǎn)方向的角度，Y軸角度即為繞Y軸旋轉(zhuǎn)方向的角度，Z軸角度即為繞Z軸旋轉(zhuǎn)方向的角度。

圖1 三軸穩(wěn)定器坐標(biāo)示意圖

1.2 陀螺儀姿態(tài)線性模型

通過(guò)對(duì)采集的數(shù)據(jù)進(jìn)行分析，根據(jù)載體坐標(biāo)下陀螺儀存在的漂移和高斯白噪聲誤差，建立陀螺儀數(shù)學(xué)模型［16］

式中：φg（t）為陀螺儀的輸出；φ（t）為輸出角速度；ε（t）為隨機(jī)漂移誤差

vg為測(cè)量白噪聲；M1、M2和T是根據(jù)陀螺儀的實(shí)際輸出得到。將式（2）進(jìn)行微分可得：

將式（3）離散后得到差分模型：

式中，Ts是采樣時(shí)間。

2 穩(wěn)定器中陀螺儀的誤差補(bǔ)償

2.1 徑向基函數(shù)（RBF）神經(jīng)網(wǎng)絡(luò)原理

在RBF網(wǎng)絡(luò)中，輸入層到隱含層的映射是非線性的，輸入層僅僅起到傳輸信號(hào)作用，因此輸入層和隱含層之間是連接權(quán)值為1 的連接。而隱含層到輸出層的映射是線性的。隱含層到輸出層采用線性優(yōu)化策略因而學(xué)習(xí)速度較快［17］。

RBF的神經(jīng)網(wǎng)絡(luò)模型如圖2 所示。圖中：x1（k），x2（k），…，xn（k）表示輸入變量，在本文中代表圖1 中三軸穩(wěn)定器橫滾軸、俯仰軸和航向軸3 個(gè)軸的角速度輸入經(jīng)建模處理后的輸入值；R1，R2，…，Rn表示的是輸入的徑向基函數(shù)；ωh1，ωh2，…，ωhn表示的是隱含層每個(gè)神經(jīng)元的權(quán)值；n表示輸入的節(jié)點(diǎn)數(shù)。應(yīng)用于RBF算法的流程圖如圖3 所示。

圖3 RBF算法流程圖

因?yàn)楦咚够瘮?shù)有表示形式簡(jiǎn)單、徑向?qū)ΨQ、光滑性好、任意階導(dǎo)數(shù)都存在的特點(diǎn)，因此RBF 神經(jīng)網(wǎng)絡(luò)的激活函數(shù)選擇了高斯函數(shù)：

RBF神經(jīng)網(wǎng)絡(luò)的輸出為

設(shè)d是樣本的期望輸出值，則基函數(shù)的方差可表示為

算法步驟如下：

基于K-均值聚類的方法求解基函數(shù)中心c。

（1）網(wǎng)絡(luò)初始化。隨機(jī)選取j個(gè)訓(xùn)練樣本作為聚類中心ci（i＝1，2，…，j）。

（2）將輸入的訓(xùn)練樣本集合按最近鄰規(guī)則分組。按照xp與中心ci之間的歐式距離，將xp分配到輸入樣本的各個(gè)聚類集合中。

（3）重新調(diào)整聚類中心。計(jì)算新的聚類中心ci，ci是聚類集合中訓(xùn)練樣本的平均值，如果ci不變，則為RBF最終的基中心，否則再?gòu)牡冢?）步開始重新求解。

（4）求解方差σi。因?yàn)檫x擇了高斯函數(shù)作為基函數(shù)，因此，方差求解為

式中，cmax為選取中心之間的最大距離。

（5）計(jì)算隱含層和輸出層之間的權(quán)值。通過(guò)最小二乘法計(jì)算得到隱含層至輸出層之間的神經(jīng)元連接權(quán)值。

2.2 卡爾曼濾波的基本原理

卡爾曼濾波是一種時(shí)域?yàn)V波方法，在線性高斯的假設(shè)下，采用空間狀態(tài)方法描述系統(tǒng)，用遞推形式實(shí)現(xiàn)算法可以直接獲得后驗(yàn)概率的解析方程的解，是一種在慣性系統(tǒng)和導(dǎo)航系統(tǒng)等穩(wěn)定系統(tǒng)中被廣泛應(yīng)用的最優(yōu)估計(jì)理論方法［12］。

應(yīng)用于陀螺儀卡爾曼濾波算法流程如圖4 所示。

圖4 卡爾曼濾波算法流程圖

（1）狀態(tài)預(yù)測(cè)。通過(guò)狀態(tài)轉(zhuǎn)移概率及上一時(shí)刻的后驗(yàn)概率p（xk｜yk-1）得出一步預(yù)測(cè)概率分布，從而得到狀態(tài)預(yù)測(cè)的均值和方差。

目標(biāo)狀態(tài)預(yù)測(cè)的均值：

目標(biāo)狀態(tài)預(yù)測(cè)的方差：

（2）由歸一化系數(shù)p（yk｜yk-1）似然函數(shù)與一步狀態(tài)預(yù)測(cè)概率的乘積中的狀態(tài)進(jìn)行積分，可以得到觀測(cè)轉(zhuǎn)移的概率分布，從而得到目標(biāo)觀測(cè)的均值和方差，并可算出卡爾曼增益，卡爾曼增益可以用來(lái)權(quán)衡預(yù)測(cè)與觀測(cè)對(duì)狀態(tài)濾波的貢獻(xiàn)。

目標(biāo)觀測(cè)預(yù)測(cè)的均值：

目標(biāo)觀測(cè)預(yù)測(cè)的方差：

卡爾曼增益

（3）利用遞推貝葉斯公式p（xk｜yk）算得狀態(tài)的后驗(yàn)概率，從而得到目標(biāo)狀態(tài)的均值和方差。

目標(biāo)狀態(tài)（后驗(yàn)）的均值：

目標(biāo)狀態(tài)（后驗(yàn)）的方差：

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

綜合考慮三軸穩(wěn)定器的性能和成本，本實(shí)驗(yàn)的MCU 選用ARM Cortex-M 內(nèi)核的32 位微控制器STM32，傳感器選用WT931 模塊，模塊內(nèi)封裝內(nèi)集成了高精度陀螺儀、加速度計(jì)和地磁場(chǎng)傳感器。它能通過(guò)I2C接口連接MCU，以數(shù)字量的形式輸出加速度信號(hào)和角速度信號(hào)，該模塊輸出頻率為500 Hz，波特率為921.600 Mb/s，工作電壓3.3 V。數(shù)據(jù)在穩(wěn)定器鎖定模式下進(jìn)行采集，采集時(shí)間為5 min。將數(shù)據(jù)通過(guò)Matlab進(jìn)行實(shí)驗(yàn)仿真，并輸出數(shù)據(jù)圖。圖5 是陀螺儀在未經(jīng)過(guò)RBF神經(jīng)網(wǎng)絡(luò)估測(cè)和卡爾曼濾波的情況下的角速度輸出數(shù)據(jù)。

圖5 原始狀態(tài)下陀螺儀的輸出信號(hào)

對(duì)神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練的過(guò)程如下，首先通過(guò)MATLAB工具箱構(gòu)建一個(gè)輸入層為3 并含有5 個(gè)神經(jīng)元的隱含層神經(jīng)網(wǎng)絡(luò)，其中，輸入層與隱含層的連接權(quán)值為1，陀螺儀取前950 000 個(gè)漂移數(shù)據(jù)為訓(xùn)練樣本，訓(xùn)練的誤差精度設(shè)為3.8 ×10-6，對(duì)經(jīng)過(guò)神經(jīng)網(wǎng)絡(luò)訓(xùn)練和卡爾曼濾波的陀螺儀角速度進(jìn)行輸出，輸出數(shù)據(jù)如圖6 ～11 所示。

圖6 鎖定狀態(tài)下MEMS陀螺儀x軸的輸出圖

圖7 改進(jìn)后的鎖定狀態(tài)下x軸的輸出

圖8 鎖定狀態(tài)下MEMS陀螺儀y軸的輸出

圖9 改進(jìn)后的鎖定狀態(tài)下y軸的輸出

圖10 鎖定狀態(tài)下MEMS陀螺儀z軸的輸出

圖11 改進(jìn)后的鎖定狀態(tài)下z軸的輸出

圖6、7 分別是未經(jīng)神經(jīng)網(wǎng)絡(luò)估測(cè)和卡爾曼濾波的陀螺儀軸輸出曲線圖與通過(guò)訓(xùn)練和濾波的陀螺儀軸的輸出曲線圖。圖8 ～11 分別是y軸和z軸改進(jìn)前與改進(jìn)后的輸出曲線圖。通過(guò)實(shí)驗(yàn)結(jié)果對(duì)比圖可以看出，該方法在抑制陀螺儀的漂移問(wèn)題上真實(shí)有效，可以達(dá)到陀螺儀的漂移角速度在0.1°/s的范圍內(nèi)。

圖12、13 是在鎖定狀態(tài)下航向角的輸出圖。由圖可見，經(jīng)過(guò)神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)補(bǔ)償和卡爾曼濾波后的航向角輸出。未改進(jìn)前，航向角的漂移是一直在增加的，改進(jìn)后航向角的漂移被抑制在±5°之間，說(shuō)明此改進(jìn)方法真實(shí)有效。

圖12 鎖定狀態(tài)下原始航向角輸出

圖13 改進(jìn)后的航向角輸出

4 結(jié) 語(yǔ)

本文通過(guò)RBF 神經(jīng)網(wǎng)絡(luò)方法對(duì)陀螺儀的輸出進(jìn)行控制，結(jié)合卡爾曼濾波算法對(duì)陀螺儀的隨機(jī)漂移進(jìn)行抑制，減小慣性誤差，提高系統(tǒng)的精度，來(lái)實(shí)現(xiàn)三軸平衡穩(wěn)定器精確維持相機(jī)畫面穩(wěn)定的目的。選擇徑向基函數(shù)RBF神經(jīng)網(wǎng)絡(luò)的優(yōu)點(diǎn)是，將原本陀螺儀漂移的非線性問(wèn)題變?yōu)榫€性可分，可以快速地估測(cè)陀螺儀的誤差并準(zhǔn)確的預(yù)測(cè)補(bǔ)償，而使用卡爾曼濾波算法進(jìn)行誤差補(bǔ)償?shù)膬?yōu)點(diǎn)是，不需要設(shè)計(jì)任何數(shù)學(xué)模型，只需要根據(jù)過(guò)去的經(jīng)驗(yàn)來(lái)學(xué)習(xí)、處理數(shù)據(jù)，用于數(shù)據(jù)預(yù)測(cè)、分類、模式識(shí)別等各種數(shù)據(jù)處理場(chǎng)合。大多數(shù)情況下，使用卡爾曼濾波算法得到的結(jié)果要優(yōu)于傳統(tǒng)的數(shù)學(xué)統(tǒng)計(jì)方法，能夠適用于影響因素多、變化大的場(chǎng)合。

通過(guò)仿真實(shí)驗(yàn)可以說(shuō)明使用RBF 神經(jīng)網(wǎng)絡(luò)算法與卡爾曼濾波算法相結(jié)合可以對(duì)陀螺儀的隨機(jī)漂移誤差進(jìn)行抑制，能夠?qū)崿F(xiàn)預(yù)期功能，達(dá)到三軸平衡穩(wěn)定器在攝影者拍攝時(shí)保持拍攝畫面穩(wěn)定的目的。