王小英，林　志，連軍政

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

一種穩(wěn)定平臺中陀螺漂移濾波算法的設(shè)計與實現(xiàn)

王小英，林志，連軍政

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

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

穩(wěn)定平臺；陀螺漂移；卡爾曼濾波

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

1　卡爾曼濾波器設(shè)計

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

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

設(shè)系統(tǒng)方程為

設(shè)測量方程為

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

通過狀態(tài)擴(kuò)增法對卡爾曼濾波方程進(jìn)行狀態(tài)推導(dǎo)，所得狀態(tài)方程為

并得到新的系統(tǒng)方程及觀測方程：

式中：Vk是當(dāng)均值為零值時的白噪聲序列，滿足卡爾曼濾波的要求并能進(jìn)行相應(yīng)濾波方程的推導(dǎo).

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

2　濾波器性能的檢驗

2.1仿真驗證

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

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

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

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

2.2適應(yīng)性試驗

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

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

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

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

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

表2　適應(yīng)性搖擺試驗角速率誤差標(biāo)準(zhǔn)差（1000 Hz）

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

3　系統(tǒng)測試

為了便于觀察卡爾曼算法在整個系統(tǒng)中的濾波效果，結(jié)合系統(tǒng)對陀螺輸出數(shù)據(jù)進(jìn)行仿真.下面主要對靜態(tài)工作和動態(tài)工作時的數(shù)據(jù)進(jìn)行仿真.

3.1靜態(tài)工作數(shù)據(jù)仿真

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

3.2動態(tài)工作數(shù)據(jù)仿真

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

3.3分析總結(jié)

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

圖3　靜態(tài)工作仿真

圖4　動態(tài)工作仿真

4　結(jié)論

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

［1］袁廣民，李曉瑩，常洪龍，等.MEMS陀螺隨機(jī)誤差補(bǔ)償在提高姿態(tài)參照系統(tǒng)精度中的應(yīng)用［J］.西北工業(yè)大學(xué)學(xué)報，2008，26 （6）：777-781.

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

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

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

［5］戴洪德，周紹磊.卡爾曼濾波及其實時應(yīng)用［J］.應(yīng)用數(shù)學(xué)，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網(wǎng)絡(luò)環(huán)境監(jiān)測小型浮標(biāo)研制”（SGZ2012064）；國家自然科學(xué)基金項目“基于螞蟻智能搜索行為的多細(xì)胞自動跟蹤及其應(yīng)用研究”（61273312）；江蘇省大學(xué)生創(chuàng)新訓(xùn)練計劃項目“海洋浮標(biāo)陀螺穩(wěn)定控制攝像系統(tǒng)的研制”（201410333005Z）

通訊聯(lián)系人：王小英，副教授，工學(xué)博士，研究方向：嵌入式系統(tǒng)、圖形圖像處理、計算機(jī)網(wǎng)絡(luò)等，E-mail：xiaoying_wang@cslg.cn.