一種應用于三軸穩定器中的陀螺儀漂移抑制方法

程小輝，張 宇

（桂林理工大學a.廣西嵌入式技術與智能系統重點實驗室；b.信息科學與工程學院，廣西桂林541006）

0 引 言

科學技術的進步推動了微機電系統（Micro Electro Mechanical System，MEMS）的快速發展，而人工智能的浪潮也為MEMS 器件的發展帶來新的方法。由于MEMS器件的特征長度在微米級甚至納米級，且質量輕、功耗低、可靠性高、靈敏性好、易于集成，因此逐漸取代了傳統的機械傳感器。其中，微機械陀螺儀即MEMS陀螺儀作為一種靈敏可靠的慣性器件被廣泛用于無人機、慣性導航等運動傳感器中。

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

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

RBF神經網絡具有非線性變換的特性和高度并行的運算能力，在理論上可以以任意精度逼近非線性函數的非線性函數［14-15］。本文根據陀螺的隨機漂移誤差需要預測補償的特點，通過RBF神經網絡進行補償，并在之后進行卡爾曼濾波算法可以很好地補償陀螺儀具有慢時變、非線性的特點的隨機漂移誤差累計，對隨機誤差進行消除。因此通過神經網絡RBF 算法與卡爾曼濾波相結合對MEMS 陀螺的隨機漂移誤差進行建模補償，可以抑制陀螺儀的隨機誤差漂移，達到預期效果。

1 模型的建立

1.1 三軸穩定器模型

三軸平衡穩定器的模型如圖1 所示，手持握把中嵌入有控制芯片，控制穩定器3 個不同位置中的航向電機、橫滾電機和俯仰電機。每個電機外部都有可控的旋轉軸。使用時，將相機正向推入卡座中與穩定器通過端口相連，打開開關，穩定器的俯仰軸、橫滾軸、航向軸就會在內部算法的指令下對相機位置進行穩定調整。當相機位置發生變化時，芯片監測到相機位置變化，會通過電路驅動電機控制外部旋轉軸來對這些不平衡量進行調整，通過陀螺儀、加速度計等內部模塊調整系統參數從而控制3 個電機的位置使得相機始終處于平衡位置，達到相機拍攝畫面平穩的目的。

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

圖1 三軸穩定器坐標示意圖

1.2 陀螺儀姿態線性模型

通過對采集的數據進行分析，根據載體坐標下陀螺儀存在的漂移和高斯白噪聲誤差，建立陀螺儀數學模型［16］

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

vg為測量白噪聲；M1、M2和T是根據陀螺儀的實際輸出得到。將式（2）進行微分可得：

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

式中，Ts是采樣時間。

2 穩定器中陀螺儀的誤差補償

2.1 徑向基函數（RBF）神經網絡原理

在RBF網絡中，輸入層到隱含層的映射是非線性的，輸入層僅僅起到傳輸信號作用，因此輸入層和隱含層之間是連接權值為1 的連接。而隱含層到輸出層的映射是線性的。隱含層到輸出層采用線性優化策略因而學習速度較快［17］。

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

圖3 RBF算法流程圖

因為高斯基函數有表示形式簡單、徑向對稱、光滑性好、任意階導數都存在的特點，因此RBF 神經網絡的激活函數選擇了高斯函數：

RBF神經網絡的輸出為

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

算法步驟如下：

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

（1）網絡初始化。隨機選取j個訓練樣本作為聚類中心ci（i＝1，2，…，j）。

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

（3）重新調整聚類中心。計算新的聚類中心ci，ci是聚類集合中訓練樣本的平均值，如果ci不變，則為RBF最終的基中心，否則再從第（1）步開始重新求解。

（4）求解方差σi。因為選擇了高斯函數作為基函數，因此，方差求解為

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

（5）計算隱含層和輸出層之間的權值。通過最小二乘法計算得到隱含層至輸出層之間的神經元連接權值。

2.2 卡爾曼濾波的基本原理

卡爾曼濾波是一種時域濾波方法，在線性高斯的假設下，采用空間狀態方法描述系統，用遞推形式實現算法可以直接獲得后驗概率的解析方程的解，是一種在慣性系統和導航系統等穩定系統中被廣泛應用的最優估計理論方法［12］。

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

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

（1）狀態預測。通過狀態轉移概率及上一時刻的后驗概率p（xk｜yk-1）得出一步預測概率分布，從而得到狀態預測的均值和方差。

目標狀態預測的均值：

目標狀態預測的方差：

（2）由歸一化系數p（yk｜yk-1）似然函數與一步狀態預測概率的乘積中的狀態進行積分，可以得到觀測轉移的概率分布，從而得到目標觀測的均值和方差，并可算出卡爾曼增益，卡爾曼增益可以用來權衡預測與觀測對狀態濾波的貢獻。

目標觀測預測的均值：

目標觀測預測的方差：

卡爾曼增益

（3）利用遞推貝葉斯公式p（xk｜yk）算得狀態的后驗概率，從而得到目標狀態的均值和方差。

目標狀態（后驗）的均值：

目標狀態（后驗）的方差：

3 實驗結果

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

圖5 原始狀態下陀螺儀的輸出信號

對神經網絡進行訓練的過程如下，首先通過MATLAB工具箱構建一個輸入層為3 并含有5 個神經元的隱含層神經網絡，其中，輸入層與隱含層的連接權值為1，陀螺儀取前950 000 個漂移數據為訓練樣本，訓練的誤差精度設為3.8 ×10-6，對經過神經網絡訓練和卡爾曼濾波的陀螺儀角速度進行輸出，輸出數據如圖6 ～11 所示。

圖6 鎖定狀態下MEMS陀螺儀x軸的輸出圖

圖7 改進后的鎖定狀態下x軸的輸出

圖8 鎖定狀態下MEMS陀螺儀y軸的輸出

圖9 改進后的鎖定狀態下y軸的輸出

圖10 鎖定狀態下MEMS陀螺儀z軸的輸出

圖11 改進后的鎖定狀態下z軸的輸出

圖6、7 分別是未經神經網絡估測和卡爾曼濾波的陀螺儀軸輸出曲線圖與通過訓練和濾波的陀螺儀軸的輸出曲線圖。圖8 ～11 分別是y軸和z軸改進前與改進后的輸出曲線圖。通過實驗結果對比圖可以看出，該方法在抑制陀螺儀的漂移問題上真實有效，可以達到陀螺儀的漂移角速度在0.1°/s的范圍內。

圖12、13 是在鎖定狀態下航向角的輸出圖。由圖可見，經過神經網絡預測補償和卡爾曼濾波后的航向角輸出。未改進前，航向角的漂移是一直在增加的，改進后航向角的漂移被抑制在±5°之間，說明此改進方法真實有效。

圖12 鎖定狀態下原始航向角輸出

圖13 改進后的航向角輸出

4 結 語

本文通過RBF 神經網絡方法對陀螺儀的輸出進行控制，結合卡爾曼濾波算法對陀螺儀的隨機漂移進行抑制，減小慣性誤差，提高系統的精度，來實現三軸平衡穩定器精確維持相機畫面穩定的目的。選擇徑向基函數RBF神經網絡的優點是，將原本陀螺儀漂移的非線性問題變為線性可分，可以快速地估測陀螺儀的誤差并準確的預測補償，而使用卡爾曼濾波算法進行誤差補償的優點是，不需要設計任何數學模型，只需要根據過去的經驗來學習、處理數據，用于數據預測、分類、模式識別等各種數據處理場合。大多數情況下，使用卡爾曼濾波算法得到的結果要優于傳統的數學統計方法，能夠適用于影響因素多、變化大的場合。

通過仿真實驗可以說明使用RBF 神經網絡算法與卡爾曼濾波算法相結合可以對陀螺儀的隨機漂移誤差進行抑制，能夠實現預期功能，達到三軸平衡穩定器在攝影者拍攝時保持拍攝畫面穩定的目的。