基于復(fù)合卡爾曼的低成本手機(jī)陀螺儀降噪研究

王輝, 解迎剛, 邢婧瑜

北京信息科技大學(xué) 信息與通信工程學(xué)院，北京 100101

智能手機(jī)中的MEMS傳感器可用于室內(nèi)定位等功能。在利用手機(jī)進(jìn)行聯(lián)合定位的時(shí)候，陀螺儀會(huì)起到很大的作用，而加速度計(jì)只起到輔助的作用。陀螺儀的誤差關(guān)乎整個(gè)定位系統(tǒng)的精度[1]。但是智能手機(jī)中的慣性傳感器成本低，質(zhì)量普遍較差，所輸出的數(shù)據(jù)往往夾雜著大量噪聲。如何在不增加硬件成本的同時(shí)有效降低低成本MEMS陀螺儀的噪聲具有很大的工程意義。MEMS陀螺儀的噪聲主要有確定噪聲和漂移噪聲兩類(lèi)。確定噪聲可以通過(guò)特定補(bǔ)償進(jìn)行消除，本研究不作深入探討。國(guó)內(nèi)外對(duì)陀螺儀的漂移誤差進(jìn)行了大量研究，主要是先建立陀螺儀的誤差漂移模型，然后使用濾波進(jìn)行降噪處理；這里的誤差漂移模型建模方法主要有小波分析、時(shí)間序列分析和神經(jīng)網(wǎng)絡(luò)等[2-5]。濾波技術(shù)主要有非線性濾波、Kalman濾波和這些濾波方法的組合改進(jìn)等[6-11]。在本研究中，單獨(dú)利用加速度計(jì)輸出的原始數(shù)據(jù)可以直接計(jì)算出俯仰角和橫滾角，但會(huì)受到響應(yīng)時(shí)間的制約，在運(yùn)動(dòng)時(shí)相應(yīng)的數(shù)據(jù)傳輸會(huì)滯后從而形成誤差。盡管陀螺儀的動(dòng)態(tài)性能好，由陀螺儀的原始數(shù)據(jù)經(jīng)過(guò)積分可以得到載體轉(zhuǎn)過(guò)的角度，但積分運(yùn)算會(huì)使得原始的誤差積累起來(lái)，最后誤差越積越多，無(wú)限發(fā)散。采用卡爾曼濾波方法融合加速度計(jì)和陀螺儀的測(cè)量信息，可以根據(jù)它們各自的優(yōu)缺點(diǎn)提高姿態(tài)角的測(cè)量精度，為手機(jī)聯(lián)合定位提供準(zhǔn)確的基本信息。

1 基于卡爾曼濾波器的信息融合

本文的核心方法是使用卡爾曼濾波器對(duì)解算出的姿態(tài)角進(jìn)行濾波降噪處理。原始數(shù)據(jù)是加速度計(jì)測(cè)出的加速度和陀螺儀測(cè)出的角速度。將兩種信息經(jīng)過(guò)濾波器進(jìn)行解算、融合，得到更加準(zhǔn)確的姿態(tài)角信息。通過(guò)加速度計(jì)可以測(cè)得配備該傳感器的物體的運(yùn)動(dòng)方向。一般情況下加速度計(jì)是用于計(jì)算位移量的，但是也可以用于修正角度[12]，這也是本文研究的內(nèi)容。

1.1 姿態(tài)角解算

圖1顯示了手機(jī)中陀螺儀的工作原理，使用其獲取的角速度值直接積分就可獲得角度值，但是噪聲非常顯著，獲得的結(jié)果精度較差，因此需要利用加速度計(jì)的數(shù)據(jù)對(duì)其進(jìn)行修正。

圖1 陀螺儀傳感器可測(cè)量的角度量

根據(jù)經(jīng)典力學(xué)公式，如果載體同時(shí)發(fā)生空間平移姿態(tài)變化，單純根據(jù)3個(gè)軸的加速度不能求解姿態(tài)角變化量。但是一般運(yùn)動(dòng)控制系統(tǒng)中平移加速控制和姿態(tài)角控制是分離的，因此在一定區(qū)間內(nèi)，可以假定載體勻速運(yùn)動(dòng)的，這時(shí)就能利用加速度計(jì)的數(shù)據(jù)對(duì)角加速度做線性估計(jì)[13]。需要說(shuō)明的是這種估計(jì)不是十分精確，但是可以滿足一般應(yīng)用的需求，具體分析和計(jì)算如下：

將圖1中的俯仰角定義為θ，橫滾角定義為γ。加速度計(jì)獲取的3軸數(shù)據(jù)用fx,fy,fz表示。假定載體在當(dāng)前運(yùn)動(dòng)區(qū)間內(nèi)做勻速運(yùn)動(dòng)，那么物體姿態(tài)發(fā)生變化時(shí)，加速度計(jì)數(shù)值的變化就可以認(rèn)為是由角度變化引起的[14]，此時(shí)可以按照下式近似計(jì)算出俯仰角和橫滾角：

(1)

1.2 低成本陀螺儀零漂誤差分析

在進(jìn)行手機(jī)聯(lián)合定位的時(shí)候，陀螺儀會(huì)起到很大的作用，而加速度計(jì)只起到輔助作用。陀螺儀誤差關(guān)乎整個(gè)定位系統(tǒng)的精度。陀螺儀的誤差主要是靜態(tài)誤差，這個(gè)靜態(tài)誤差一般被稱為零點(diǎn)漂移。

陀螺儀有兩個(gè)工作狀態(tài)，分別是開(kāi)環(huán)工作狀態(tài)和閉環(huán)工作狀態(tài)[15]。陀螺儀處于開(kāi)環(huán)工作狀態(tài)時(shí)信噪比小，漂移噪聲較為嚴(yán)重，此時(shí)陀螺儀的漂移誤差會(huì)隨著時(shí)間逐漸積累，最終造成嚴(yán)重的零點(diǎn)漂移。陀螺儀處于閉環(huán)工作狀態(tài)中時(shí)信噪比雖然會(huì)增大，但是噪聲的漂移現(xiàn)象仍然會(huì)在一段時(shí)間后出現(xiàn)。在干擾力矩作用下自轉(zhuǎn)軸相對(duì)慣性空間的偏差角變化速度被稱作陀螺漂移率(即測(cè)量角的偏差在單位時(shí)間內(nèi)的變化率)。它是衡量陀螺儀精度的主要性能指標(biāo)，漂移率越小，陀螺儀精度越高。

1.3 卡爾曼濾波原理

卡爾曼濾波方法最大優(yōu)勢(shì)就是在測(cè)量方差已知的情況下能夠從一系列存在測(cè)量噪聲的數(shù)據(jù)中估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)，卡爾曼濾波器的實(shí)現(xiàn)需要使用到兩組數(shù)據(jù)，這兩組數(shù)據(jù)命名為系統(tǒng)的狀態(tài)量和系統(tǒng)的觀測(cè)量。使用加速度計(jì)測(cè)量的姿態(tài)角數(shù)值作為觀測(cè)值，以陀螺儀的零偏作為狀態(tài)值.并建立狀態(tài)方程和量測(cè)方程[16]：

當(dāng)系統(tǒng)初始狀態(tài)以及對(duì)應(yīng)的協(xié)方差矩陣已知后相應(yīng)的Kalman濾波系統(tǒng)可以迭代運(yùn)行，對(duì)連續(xù)過(guò)程中系統(tǒng)的任一時(shí)刻系統(tǒng)的狀態(tài)進(jìn)行最優(yōu)估計(jì)，達(dá)到降噪的效果。

1.4 低成本陀螺儀降噪建模

陀螺儀輸出的角速度存在固定偏差[17]，設(shè)觀測(cè)角度值φ對(duì)應(yīng)的偏差為b，那么該系統(tǒng)根據(jù)角度和角速度可以獲得如下方程：

(2)

式中:ω為由陀螺儀獲取的角速度值；n為其對(duì)應(yīng)噪聲誤差。

設(shè)系統(tǒng)采樣周期為T(mén)，式(2)可重寫(xiě)為

設(shè)狀態(tài)向量:

X(k)=[φ(k)b(k)]T

則可得系統(tǒng)的狀態(tài)方程為：

系統(tǒng)的觀測(cè)方程基于加速度計(jì)建立，根據(jù)前述計(jì)算方法由式(1)可以直接利用3個(gè)加速度值計(jì)算出角度值，因此觀測(cè)方程為：

式中m為加速度計(jì)噪聲誤差。參照卡爾曼濾波的實(shí)現(xiàn)原理，只需再獲取2個(gè)噪聲的協(xié)方差陣Q和R即可進(jìn)行濾波。在本文中，Q和R分別為：

式中Q和R矩陣中的參數(shù)通過(guò)元器件的給定參數(shù)確定[18]。

以上是針對(duì)單個(gè)角度計(jì)算，當(dāng)需要同時(shí)計(jì)算俯仰角θ和橫滾角γ時(shí)，對(duì)各向量進(jìn)行擴(kuò)展，系統(tǒng)方程涉及的矩陣A、B、Q分別為：

狀態(tài)向量X和控制向量U分別為

觀測(cè)方程設(shè)計(jì)的矩陣H和R以及觀測(cè)值向量Z分別為：

2 濾波模型的程序?qū)崿F(xiàn)

根據(jù)上述模型編寫(xiě)了濾波程序文件。濾波程序分為：1)讀取實(shí)驗(yàn)獲取的數(shù)據(jù)；2)由加速度算出姿態(tài)角，并作為觀測(cè)值；3)進(jìn)行卡爾曼濾波；4)得到狀態(tài)預(yù)估值4個(gè)步驟。

圖2為卡爾曼濾波的子程序流程。

圖2 卡爾曼濾波子程序流程

3 濾波模型在智能手機(jī)上的實(shí)現(xiàn)

3.1 數(shù)據(jù)收集

MATLAB可以從Android設(shè)備中獲取傳感器數(shù)據(jù)。需要在安卓系統(tǒng)上安裝能夠測(cè)量傳感器數(shù)據(jù)的應(yīng)用程序sensorUDP，其界面如圖3所示，同時(shí)要求運(yùn)行MATLAB的計(jì)算機(jī)與安裝sensorUDP應(yīng)用程序的Android設(shè)備處于同一個(gè)WiFi環(huán)境下。 SensorUDP是一款由Takashi SASAKI開(kāi)發(fā)的APP,允許用戶獲得手機(jī)傳感器的各種原始數(shù)據(jù)如加速度、角速度、線速度和經(jīng)緯度等。

圖3 sensorUDP程序界面

sensorUDP需要和sensorgroup組件配合使用，該組件可以從MATLAB的官網(wǎng)下載，將其解壓至安裝目錄下的toolbox文件夾中。sensorgroup.m文件提供了一組函數(shù)可供調(diào)用，用戶可以利用這些函數(shù)讀取相應(yīng)的傳感器數(shù)據(jù)。

圖4 提示設(shè)置界面

在使用時(shí)，首先在計(jì)算機(jī)上設(shè)置無(wú)線網(wǎng)絡(luò)，在手機(jī)端連接創(chuàng)建好的無(wú)線網(wǎng)絡(luò)。保證兩個(gè)設(shè)備在同一個(gè)網(wǎng)絡(luò)環(huán)境下，MATLAB就能接收到手機(jī)發(fā)出的UDP數(shù)據(jù)包，并通過(guò)函數(shù)命令解析數(shù)據(jù)包。函數(shù)命令可以由mathwork官網(wǎng)獲得并保存在MATLAB工作路徑中。函數(shù)命令obj=sensorgroup(deviceType)會(huì)提示用戶進(jìn)行相關(guān)設(shè)置從而建立通信,如圖4所示。需要在dest.host對(duì)應(yīng)編輯框中輸入要接收數(shù)據(jù)的電腦的IP地址(10.3.136.90)，端口值一般設(shè)置為默認(rèn)值50000。

3.2 靜止條件下手機(jī)陀螺儀誤差分析

將手機(jī)平放在地面上，保持靜止?fàn)顟B(tài)，收集陀螺儀和加速度計(jì)的原始數(shù)據(jù)(陀螺儀原始數(shù)據(jù)為載體的三軸角速度，加速度計(jì)的原始數(shù)據(jù)為載體的三軸加速度)。

由圖1所示，手機(jī)的X軸和Y軸具有相似性，所以只需要單獨(dú)分析其中一個(gè)軸的靜態(tài)誤差即可。以陀螺儀Y軸為例進(jìn)行誤差分析。

圖5 靜止?fàn)顟B(tài)下陀螺儀Y軸角速率數(shù)據(jù)采集結(jié)果

先將靜止?fàn)顟B(tài)下的數(shù)據(jù)進(jìn)行濾波仿真，圖6和圖7中虛線為濾波前結(jié)果，實(shí)線為濾波后結(jié)果。

(a)靜止時(shí)俯仰角濾波結(jié)果

(b)靜止時(shí)橫滾角濾波結(jié)果圖6 靜止?fàn)顟B(tài)下濾波結(jié)果

(a)運(yùn)動(dòng)時(shí)俯仰角濾波結(jié)果

(b)運(yùn)動(dòng)時(shí)橫滾角濾波結(jié)果圖7 運(yùn)動(dòng)狀態(tài)下濾波結(jié)果

由Y軸靜止?fàn)顟B(tài)下的濾波結(jié)果可得表1。

表1 陀螺儀Y軸靜態(tài)漂移誤差

在單獨(dú)使用陀螺儀原始數(shù)據(jù)解算姿態(tài)角時(shí)，100 s后由積分運(yùn)算造成的漂移誤差達(dá)到大約28.3°(動(dòng)態(tài)漂移率為0.28°/s)，隨著時(shí)間推移誤差積累，數(shù)據(jù)逐漸發(fā)散，最后所得的結(jié)果不能正常使用?？梢钥闯鐾勇輧x數(shù)據(jù)的漂移問(wèn)題嚴(yán)重。由圖6和表1可以看出濾波很好地抑制了陀螺儀靜止時(shí)的零點(diǎn)漂移噪聲，且解算出的姿態(tài)角平穩(wěn)，可供一般應(yīng)用直接使用。

3.3 動(dòng)態(tài)條件下手機(jī)陀螺儀濾波降噪

將手機(jī)按照依次繞X軸Y軸旋轉(zhuǎn)90°的順序進(jìn)行數(shù)據(jù)收集。將數(shù)據(jù)保存為mat文件，和編寫(xiě)的濾波程序放置到同一目錄下，然后運(yùn)行濾波程序，所得結(jié)果如圖7所示。分析陀螺儀運(yùn)動(dòng)狀態(tài)下的相關(guān)數(shù)據(jù)可得表2(其中誤差計(jì)算方式以旋轉(zhuǎn)到90°時(shí)的數(shù)據(jù)為標(biāo)準(zhǔn))。

表2 陀螺儀動(dòng)態(tài)誤差

分析濾波前的姿態(tài)角信息可知在動(dòng)態(tài)條件下：俯仰角在200 s的漂移誤差約為28.6°(動(dòng)態(tài)漂移率約為0.143°/s)，橫滾角在200 s內(nèi)的漂移誤差大約為34.4°(動(dòng)態(tài)漂移率約為0.17°/s)。

對(duì)比分析濾波前后數(shù)據(jù)可明顯發(fā)現(xiàn)，濾波前的角度存在顯著誤差，已經(jīng)無(wú)法使用。而經(jīng)濾波后，姿態(tài)角數(shù)據(jù)平穩(wěn)，誤差大小控制在4%左右，誤差在可接受范圍內(nèi)，綜合降噪效果在70%左右，濾波結(jié)果可以在一般的手機(jī)應(yīng)用中直接使用。

4 結(jié)論

利用手機(jī)陀螺儀輸出的角速度數(shù)據(jù)對(duì)其在靜止?fàn)顟B(tài)下的零漂做出了定量分析，實(shí)驗(yàn)表明零漂造成的誤差嚴(yán)重影響了姿態(tài)角的測(cè)量精度。當(dāng)載體處于運(yùn)動(dòng)狀態(tài)時(shí)，將誤差較大的陀螺儀原始數(shù)據(jù)和由加速度計(jì)解算出的動(dòng)態(tài)響應(yīng)較差的姿態(tài)角數(shù)據(jù)進(jìn)行信息融合、濾波降噪處理。實(shí)驗(yàn)證明當(dāng)載體處于運(yùn)動(dòng)狀態(tài)時(shí)，采用卡爾曼濾波能有效降低陀螺儀的漂移誤差、提高姿態(tài)角測(cè)量的精度。此方法達(dá)到了預(yù)期的降噪目標(biāo)，并且可以將經(jīng)過(guò)濾波處理后的姿態(tài)角信息用于手機(jī)的一些簡(jiǎn)單應(yīng)用如定位和平衡類(lèi)小游戲等。但本文只涉及了平面方向的六軸自由度，在未來(lái)的工作中可以將航向角信息和磁力計(jì)信息加以利用，實(shí)現(xiàn)在空間九軸上的姿態(tài)角測(cè)量。