一種穩定平臺中陀螺漂移濾波算法的設計與實現

王小英，林　志，連軍政

（常熟理工學院 電氣與自動化工程學院，江蘇 常熟 215500）

一種穩定平臺中陀螺漂移濾波算法的設計與實現

王小英，林志，連軍政

（常熟理工學院 電氣與自動化工程學院，江蘇 常熟 215500）

針對穩定平臺工作中陀螺儀漂移問題，采用以陀螺儀角速度傳感器的輸出量作為觀測量，建立陀螺儀漂移的數學模型和狀態方程，然后利用卡爾曼濾波對含有噪聲的測量數據進行濾波處理的方式改善漂移問題.仿真結果表明，該方式可以有效抑制陀螺儀信號中的擾動，提高陀螺儀穩定平臺的工作精度.

穩定平臺；陀螺漂移；卡爾曼濾波

在各種需要穩定環境工作的系統中，為保持系統整體的穩定運行，通常采用陀螺儀穩定平臺去除姿態擾動以實現穩定控制.陀螺儀作為穩定平臺的核心測量元件，其本身的誤差對平臺性能有著很大影響，比如陀螺儀常值漂移會造成平臺漂移隨時間的增長而變大，使得所采集的位置數據產生較大的誤差.當陀螺穩定平臺應用于圖像采集系統中時，這樣的誤差最終所帶來的是圖像的擾動，使得采集清晰圖像的工作難以實現.自身誤差所帶來的不穩定因素也限制了陀螺穩定平臺的應用領域，因此對陀螺儀誤差進行補償的必要性也凸現出來.研究表明，對陀螺儀誤差的補償，可以通過對其進行模型辨識和濾波處理的方式，一般使用小波分析、神經網絡等方法對陀螺儀的隨機誤差進行建模分析.但在實際操作中，這些方法得到的模型一般具有較高的階數，進而不適合低成本系統的實時在線估計［1］.因此在實際應用中更多的是采用時序分析法，對陀螺儀隨機誤差建立AR模型對誤差進行補償，并利用卡爾曼濾波進行處理，這在實際的低成本應用中更加適合.本文以穩定平臺中的陀螺儀作為研究對象，分析了陀螺儀漂移誤差及其算法模型，然后在算法模型的基礎上對卡爾曼濾波器進行設計驗證，最后結合系統在MATLAB中對陀螺儀輸出數據進行仿真分析，取得了較好的效果.

1　卡爾曼濾波器設計

通過卡爾曼濾波方法對陀螺儀的隨機漂移進行處理時，首先需對陀螺儀的輸出信號進行預處理并使其滿足平穩隨機序列，然后建立相應的時序模型，并對其檢驗適用性.這類建模方法在文獻［2-3］中已經給出了說明.另外本文綜合考慮了系統實時性要求及模型適用性要求等方面因素，故采用AR（1）模型對陀螺儀隨機噪聲進行建模分析.

在陀螺儀隨機漂移誤差中主要包括系統中產生的有色噪聲和系統運行時陀螺自身產生的測量白色噪聲［4］.通過AR（1）模型描述陀螺儀的隨機漂移誤差后，將AR（1）模型改為狀態空間模型，然后采用狀態擴增法，得到狀態空間方程.

設系統方程為

設測量方程為

上式中，Wk-1是系統噪聲，為有色噪聲，Vk是零均值白噪聲序列的觀測噪聲，其中Wk-1滿足方程

通過狀態擴增法對卡爾曼濾波方程進行狀態推導，所得狀態方程為

并得到新的系統方程及觀測方程：

式中：Vk是當均值為零值時的白噪聲序列，滿足卡爾曼濾波的要求并能進行相應濾波方程的推導.

在實際的陀螺隨機漂移數據處理中，為陀螺的隨機漂移量，ωk為陀螺的實際角速率值，Γk,k-1、φk,k-1、Hk均為單位矩陣，Πk,k-1是建立陀螺隨機漂移模型的參數值［5］.

2　濾波器性能的檢驗

2.1仿真驗證

本文以MPU6050陀螺儀模塊為仿真試驗對象來進行檢測.MPU6050是一個數位運動處理硬件加速引擎，包含3軸陀螺儀、3軸加速器，并可通過I2C端口連接其他磁力傳感器、加速器等.可處理運動感測的復雜數據，降低了運動處理運算對操作系統的負荷，并為應用開發提供了架構化的API.

本文采用該模塊的零偏不穩定性參數對漂移誤差模型的準確性和濾波器的有效性進行驗證.在仿真驗證時，首先根據MPU6050陀螺的參數設置MATLAB的卡爾曼濾波程序中的參數，分別為陀螺儀0.0015°/s，加速計0.7 mg，測量噪聲0.0030°/s和35 mg.在數據采集時，先將陀螺放在一個平穩的桌面上，盡量使陀螺儀輸出值是零值或者處在某個特定的常值.采集此時陀螺儀X軸的數據，比較加入卡爾曼濾波算法前后的數據穩定性，從而判定濾波的性能.

未加濾波算法時，采集陀螺儀的X軸數據并將其導入MATLAB，從而得到X軸原始角速度的仿真圖，如圖1所示，角度的跳動在±2°位置左右.理想狀態下的X軸數據應該是過0水平線，在實際測量中，通過X軸原始角速度仿真圖可以看出，陀螺儀在靜態情況下，X軸輸出信號是在0均值的水平線上隨時間作無規律跳動的隨機變量.

而加入卡爾曼濾波算法處理數據后，重新運行程序，以同樣的方式采集陀螺儀的X軸數據將其導入MATLAB，得到濾波后的X軸角速度仿真圖，如圖2所示，可見其角度的跳動在±1°位置左右.采用卡爾曼濾波方式可以很好地抑制陀螺儀的隨機漂移誤差，降低陀螺的隨機噪聲，是一種行之有效的降低誤差的方法.

2.2適應性試驗

卡爾曼濾波器設計中，默認的系統狀態仿真矩陣為單位矩陣，這樣就會使角速率值在一定的角度周期范圍內不會隨卡爾曼遞推公式變化.針對這種情況，可以通過提高采樣的頻率方法來解決.將之前的100HZ采集數據改為1000 HZ的采樣數據進行仿真.分別對5°、15°、50°、150°振幅的陀螺信號進行數據仿真，采集原始工作中直接建模得到的濾波前的數據和加入卡爾曼算法改善后的濾波后的數據，將二者采集的數據進行比較分析，以說明其適用性.

圖1　X軸原始角速度仿真圖

圖2　濾波后的X軸角速度仿真圖

表1和表2分別是陀螺儀輸出角速率的誤差均值和誤差標準差的數值.從表1濾波前后的數據比較來看，濾波前后角速度誤差均值的變化不是很明顯.甚至出現在高角度時濾波后的誤差均值大于濾波前的情況.

表1　適應性搖擺試驗角速率誤差均值（1000 Hz）

表2　適應性搖擺試驗角速率誤差標準差（1000 Hz）

但對表2的角度誤差標準差進行比較分析時發現加入卡爾曼濾波算法后誤差標準差有了很大程度的降低，濾波前的標準差在0.635左右，濾波后的標準差則穩定在0.093左右，濾波后的誤差標準差穩定在一個較小范圍內.從標準差的作用及其特性可知，誤差標準差的降低表明了所采集的數據的離散程度降低，穩定性有了很大的提升.因此采用這樣的處理方法可以實現數據的穩定處理.

3　系統測試

為了便于觀察卡爾曼算法在整個系統中的濾波效果，結合系統對陀螺輸出數據進行仿真.下面主要對靜態工作和動態工作時的數據進行仿真.

3.1靜態工作數據仿真

系統運行時，將MPU6050陀螺模塊放置在平穩的桌面上，使其處在靜止工作狀態，真實反饋信號為零.然后以1000 HZ的采樣頻率對MPU6050模塊中輸出的角度值進行采集，將采集數據以.txt形式導入MAT?LAB中生成模型，可以得到如圖3所示的靜態工作仿真圖.

3.2動態工作數據仿真

系統運行時，輕輕旋轉MPU6050陀螺模塊，使陀螺輸出的反饋信號不為零，然后利用同樣的方法對原始數據和濾波后的數據進行采集.為了更明顯的觀察，分別對原始數據和濾波后的數據進行MATLAB仿真.得到如圖4所示的動態工作仿真圖.

3.3分析總結

通過濾波前后數據仿真圖的對比可以看出，MPU6050陀螺模塊無論是處在靜態工作狀態還是動態工作狀態，都能有效抑制信號中的擾動現象，使陀螺輸出的反饋信號趨向平穩，有效地提高了系統在運行時的工作精度.所以卡爾曼濾波算法是提高穩定控制系統精度的關鍵部分，且在實際應用中簡單方便.

圖3　靜態工作仿真

圖4　動態工作仿真

4　結論

本文針對采用傳統陀螺儀建模方式［2-4］后隨機誤差較大的問題，通過對MPU6050陀螺進行試驗和仿真，采集其在靜態和動態狀態下的數據，利用卡爾曼濾波算法對數據進行處理，并對誤差的均值和標準差進行了比較分析，實驗結果表明陀螺儀精度可以在此方法下得到有效提高，是一種可行的簡便方法.

［1］袁廣民，李曉瑩，常洪龍，等.MEMS陀螺隨機誤差補償在提高姿態參照系統精度中的應用［J］.西北工業大學學報，2008，26 （6）：777-781.

［2］吉訓生，王壽榮.MEMS陀螺儀隨機漂移誤差研究［J］.宇航學報，2006，27（4）：640-642.

［3］楊叔子，吳雅.時間序列分析的工程應用［M］.武漢：華中理工大學出版社，2007：87-93.

［4］吳富梅，楊元喜.基于高階AR模型的陀螺隨機漂移模型［J］.測繪學報，2007，36（4）：389-394.

［5］戴洪德，周紹磊.卡爾曼濾波及其實時應用［J］.應用數學，2010（4）：42-91.

Design and Implementation of a Gyro
Drift Filtering Algorithm of Stabilized Platform

WANG Xiao-ying，LIN Zhi，LIAN Jun-zheng
（School of Electrical and Automation Engineering,Changshu Institute of Technology,Changshu 215500,China）

For the gyro drift problem of stabilized platform,this paper established a gyro drift mathematic model and state equation based on angular velocity output of gyro sensor and removed the noises data through kalman filter.Simulation results show that kalman filtering can effectively restrain the disturbance of gyro signal and im?prove the working precision of the gyro stabilized platform.

stable platform；gyro drift；kalman filter

TP391.9

A

1008-2794（2015）02-0082-04

2013-11-03

蘇州市科技計劃項目“集圖像采集功能的新型3G網絡環境監測小型浮標研制”（SGZ2012064）；國家自然科學基金項目“基于螞蟻智能搜索行為的多細胞自動跟蹤及其應用研究”（61273312）；江蘇省大學生創新訓練計劃項目“海洋浮標陀螺穩定控制攝像系統的研制”（201410333005Z）

通訊聯系人：王小英，副教授，工學博士，研究方向：嵌入式系統、圖形圖像處理、計算機網絡等，E-mail：xiaoying_wang@cslg.cn.