無(wú)人機(jī)微機(jī)電系統(tǒng)慣性傳感器卡爾曼濾波技術(shù)

孫立志

在消費(fèi)級(jí)和工業(yè)級(jí)無(wú)人機(jī)中，微機(jī)電系統(tǒng)慣性傳感器是其飛控系統(tǒng)十分重要的傳感器。微機(jī)電系統(tǒng)信號(hào)噪聲的存在降低了傳感器的精度，對(duì)此本文提出了利用Allan方差估計(jì)分析噪聲特性的方法，研究了基于卡爾曼濾波的MEMS信號(hào)降噪方法，解決了在無(wú)人機(jī)飛控系統(tǒng)中某些去噪方法計(jì)算復(fù)雜的問(wèn)題，通過(guò)實(shí)驗(yàn)驗(yàn)證了該方法抑制微機(jī)電系統(tǒng)噪聲的有效性。

微機(jī)電系統(tǒng)（Micro-ElectroMechanical System，MEMS）是指尺寸在毫米級(jí)別甚至更小的傳感器系統(tǒng)。微機(jī)電系統(tǒng)傳感器廣泛配備于多種無(wú)人機(jī)系統(tǒng)，特別是消費(fèi)級(jí)和工業(yè)級(jí)無(wú)人機(jī)，這得益于微機(jī)電系統(tǒng)傳感器所具有的重量輕、功耗低、集成度高同時(shí)成本低等一系列優(yōu)點(diǎn)。微機(jī)電系統(tǒng)傳感器包括運(yùn)動(dòng)傳感器、壓力傳感器、光傳感器等，本文涉及的微機(jī)電系統(tǒng)慣性傳感器是運(yùn)動(dòng)傳感器的一種，集成了三軸陀螺儀和三軸加速度計(jì)，是該級(jí)別無(wú)人機(jī)飛控系統(tǒng)最重要的傳感器之一，應(yīng)用十分廣泛。但微機(jī)電系統(tǒng)慣性傳感器自身也有不可避免的缺點(diǎn)，即輸出信號(hào)噪聲成分多，若不進(jìn)行濾波處理則無(wú)法使用，因此本文提出一種基于卡爾曼濾波的微機(jī)電系統(tǒng)慣性傳感器信號(hào)降噪方法，使該類(lèi)傳感器應(yīng)用于無(wú)人機(jī)時(shí)具有較高的精度。

微機(jī)電系統(tǒng)慣性傳感器原理

微機(jī)電系統(tǒng)以哥氏效應(yīng)為基本原理，是一種振動(dòng)式的角速率傳感器。哥氏效應(yīng)是指在一個(gè)作旋轉(zhuǎn)運(yùn)動(dòng)的坐標(biāo)系中，當(dāng)某一點(diǎn)相對(duì)該動(dòng)坐標(biāo)系作直線運(yùn)動(dòng)時(shí)，該動(dòng)點(diǎn)即具有哥氏加速度，對(duì)此可得哥氏加速度的表達(dá)式：

a_c= 2×ω×v （1）

式中：ω——?jiǎng)幼鴺?biāo)系的角速度;

v——質(zhì)點(diǎn)相對(duì)動(dòng)坐標(biāo)系的線速度。

微機(jī)電系統(tǒng)工作時(shí)，通過(guò)電驅(qū)動(dòng)，使微機(jī)械的振動(dòng)框架達(dá)到諧振狀態(tài)，而產(chǎn)生一定的速度，此時(shí)當(dāng)微機(jī)電系統(tǒng)隨平臺(tái)框架旋轉(zhuǎn)時(shí)，即產(chǎn)生科氏力;科氏力與檢測(cè)框架發(fā)生關(guān)聯(lián)，即形成檢測(cè)信號(hào)，經(jīng)放大輸出得到電信號(hào)。據(jù)此微機(jī)電系統(tǒng)將無(wú)人機(jī)機(jī)體的旋轉(zhuǎn)角速度信息轉(zhuǎn)換為與之成比例的電壓信號(hào)，通過(guò)模數(shù)轉(zhuǎn)換得到角速度的數(shù)字信息，將角速度進(jìn)行積分可得框架的角度信息。

本文采用MPU6050慣性傳感器，其內(nèi)部集成三軸陀螺儀和三軸加速度計(jì)，MPU6050的參考坐標(biāo)系為右手坐標(biāo)系，圖中標(biāo)注了3個(gè)測(cè)量軸的正向旋轉(zhuǎn)方向，符合右手螺旋定則。當(dāng)MPU6050圍繞其測(cè)量軸旋轉(zhuǎn)時(shí)，即可輸出相應(yīng)軸向的角速度信息。

微機(jī)電系統(tǒng)的性能指標(biāo)主要有刻度因數(shù)、偏差、分辨率和隨機(jī)漂移，其中分辨率是選擇和使用微機(jī)電系統(tǒng)時(shí)重點(diǎn)關(guān)注的參數(shù)指標(biāo)，它指的是微機(jī)電系統(tǒng)所能敏感到的最小角速度增量。

不考慮微機(jī)電系統(tǒng)的結(jié)構(gòu)形式和制造工藝的不同，微機(jī)電系統(tǒng)的系統(tǒng)模型由物理模型、誤差模型以及量測(cè)模型三部分組成。其中微機(jī)電系統(tǒng)信號(hào)的零點(diǎn)漂移和隨機(jī)噪聲可由誤差模型和量測(cè)模型分析。誤差模型中包括對(duì)確定性誤差和隨機(jī)噪聲的分析，確定性誤差是由模型中的參數(shù)變化和微機(jī)電系統(tǒng)所在的工作環(huán)境決定，隨機(jī)噪聲則是由于其他不確定的因素引起，典型的隨機(jī)誤差包括速度隨機(jī)游走、白化速度噪聲等;量測(cè)模型中包括對(duì)測(cè)量哚聲的分析。

應(yīng)用于無(wú)人機(jī)的微機(jī)電系統(tǒng)慣性傳感器噪聲特性

微機(jī)電系統(tǒng)的噪聲可以分為確定性漂移和隨機(jī)噪聲兩類(lèi)。其中前者理論上能夠完全補(bǔ)償，但需要通過(guò)復(fù)雜的實(shí)驗(yàn)方法實(shí)現(xiàn)，本文不做深入研究;對(duì)于隨機(jī)噪聲，一般不能完全消除，而只能通過(guò)濾波的手段進(jìn)行抑制。

應(yīng)用于無(wú)人機(jī)時(shí)，微機(jī)電系統(tǒng)中的漂移和噪聲主要來(lái)源于兩個(gè)方面。一是微機(jī)電系統(tǒng)自身原理、結(jié)構(gòu)和制造工藝帶來(lái)的缺陷，即來(lái)源于微機(jī)電系統(tǒng)內(nèi)部;二是外部因素，主要指微機(jī)電系統(tǒng)受到環(huán)境因素的影響而產(chǎn)生噪聲。外部因素包括溫濕度、無(wú)人機(jī)結(jié)構(gòu)振動(dòng)、空域內(nèi)電磁輻射等。具體來(lái)說(shuō)，當(dāng)溫度變化時(shí)，微機(jī)電系統(tǒng)的材料發(fā)生對(duì)溫度敏感的變化，影響內(nèi)部振動(dòng)元件的狀態(tài);當(dāng)外界的電磁環(huán)境較惡劣時(shí)，微機(jī)電系統(tǒng)內(nèi)部電信號(hào)的轉(zhuǎn)化、傳輸受到影響。

分析微機(jī)電系統(tǒng)的隨機(jī)漂移，目前主要有Allan方差法功率譜密度法和1σ方法等方法，其中因Allan方法對(duì)隨機(jī)噪聲的各成分有較強(qiáng)的辨識(shí)度，更利于對(duì)各噪聲成分進(jìn)行分析和提取，故更常用于微機(jī)電系統(tǒng)的隨機(jī)噪聲分析。Allan方法用于分析隨機(jī)噪聲成分，其目的是對(duì)隨機(jī)誤差的各個(gè)誤差項(xiàng)進(jìn)行辨識(shí)并確定出各誤差項(xiàng)的系數(shù)，對(duì)微機(jī)電系統(tǒng)的各項(xiàng)性能指標(biāo)進(jìn)行估計(jì);本文的AR模型用于建立隨機(jī)漂移的數(shù)學(xué)模型，將非平穩(wěn)的隨機(jī)序列處理后進(jìn)行AR建模，進(jìn)而設(shè)計(jì)卡爾曼濾波器。

隨著制造工藝的進(jìn)步、安裝精度的提高，微機(jī)電系統(tǒng)噪聲信號(hào)的抑制程度也隨之提高。但是受限于微機(jī)電系統(tǒng)自身的結(jié)構(gòu)原理和生產(chǎn)工藝水平，且考慮微機(jī)電系統(tǒng)的成本等因素，單從微機(jī)電系統(tǒng)本身考慮噪聲抑制是遠(yuǎn)遠(yuǎn)不夠的，因此基于軟件濾波的方法，即數(shù)字濾波方法更要結(jié)合應(yīng)用。基于軟件濾波的方法中，先建立微機(jī)電系統(tǒng)噪聲的數(shù)學(xué)模型，再根據(jù)模型的特點(diǎn)進(jìn)行數(shù)字濾波器的設(shè)計(jì)不失為一種較好地思路。但是考慮到微機(jī)電系統(tǒng)噪聲的隨機(jī)性、非線性等因素，建立較為準(zhǔn)確、精確的數(shù)學(xué)模型有困難。因此，對(duì)微機(jī)電系統(tǒng)輸出信號(hào)進(jìn)行不過(guò)分依賴其信號(hào)模型的直接數(shù)字濾波較為方便。

數(shù)字濾波器是隨機(jī)數(shù)字信號(hào)處理技術(shù)中的常用方法，其具有靈活性強(qiáng)、可靠性高等優(yōu)點(diǎn)，廣泛應(yīng)用于工程實(shí)際中。根據(jù)數(shù)字濾波的理論，數(shù)字信號(hào)的輸入序列和輸出序列之間滿足線性組合的關(guān)系，即任意時(shí)刻的輸出值可由該時(shí)刻的輸入值和過(guò)去時(shí)刻的輸入和輸出值表示，其差分方程為

數(shù)字濾波器的類(lèi)型可以分為非遞歸型和遞歸型兩種，其類(lèi)型根據(jù)信號(hào)輸出序列的系數(shù)值分類(lèi)。a_j（j≥1）的取值均為0時(shí)式（2）表示的是非遞歸型數(shù)字濾波器，其特點(diǎn)是當(dāng)前時(shí)刻輸出序列與過(guò)去時(shí)刻的輸入值無(wú)關(guān)，只和當(dāng)前時(shí)刻的輸入值有關(guān)。反之則式（2）表示遞歸型數(shù)字濾波器。針對(duì)隨機(jī)誤差，目前研究較多的濾波算法有低通濾波、卡爾曼濾波、小波去哚、維納濾波等。其中小波分析方法因其模型階數(shù)較高、算法復(fù)雜，并不適合在嵌入式處理器中應(yīng)用。

微機(jī)電系統(tǒng)慣性傳感器隨機(jī)噪聲的數(shù)學(xué)模型

微機(jī)電系統(tǒng)隨機(jī)哚聲的成分是非平穩(wěn)的有色噪聲，因其主要受工作環(huán)境的影響，所以沒(méi)有確定的誤差模型。時(shí)間序列法和相關(guān)函數(shù)估計(jì)是用于建立隨機(jī)噪聲的模型常用的方法，其中相關(guān)函數(shù)法容易引入額外誤差，而時(shí)間序列法對(duì)信號(hào)的模型要求不高，常用于微機(jī)電系統(tǒng)信號(hào)的處理。

所謂時(shí)間序列法，是指利用統(tǒng)計(jì)的手段對(duì)隨機(jī)過(guò)程的隨機(jī)序列進(jìn)行分析的方法，其目的是根據(jù)測(cè)得的隨機(jī)數(shù)據(jù)，進(jìn)行相應(yīng)的分析，得出其數(shù)學(xué)模型及相關(guān)的參數(shù)。時(shí)間序列分析法的適用范圍是帶限的白噪聲，因此需要對(duì)微機(jī)電系統(tǒng)具有非平穩(wěn)特性的隨機(jī)噪聲進(jìn)行預(yù)處理。所謂平穩(wěn)過(guò)程，是一種在工程中常見(jiàn)的隨機(jī)過(guò)程，如通信中常見(jiàn)的高斯白噪聲、無(wú)人機(jī)受到空氣湍流而產(chǎn)生的波動(dòng)。平穩(wěn)過(guò)程的特點(diǎn)是其統(tǒng)計(jì)特性不僅與現(xiàn)在的狀態(tài)有關(guān)，還與過(guò)去時(shí)間的狀態(tài)有關(guān)。預(yù)處理的方法一般是一階差分法或二階差分法。一般經(jīng)過(guò)一階差分處理后，微機(jī)電系統(tǒng)的隨機(jī)噪聲序列即成為具有平穩(wěn)性、非周期性、正態(tài)性的隨機(jī)差分序列，可以對(duì)其進(jìn)行模型估計(jì)。

對(duì)于這樣的平穩(wěn)隨機(jī)序列，可以利用的自回歸模型（AR（p）模型）進(jìn)行分析，這種模型具有建模簡(jiǎn)單的優(yōu)點(diǎn)，且在工程應(yīng)用中其模型的階次不高，適合處理器運(yùn)算。利用自回歸模型對(duì)隨機(jī)噪聲建模后，測(cè)量某時(shí)刻的數(shù)據(jù)是該時(shí)刻之前p個(gè)時(shí)刻數(shù)據(jù)的線性擬合與噪聲的疊加。因此本文建立AR（1）模型，即一階的AR模型，其結(jié)構(gòu)見(jiàn)式（3）。

x_k=φ₁x_k-1+a_k（3）

式中，x_k——隨機(jī)序列;

φ₁——AR（1）模型系數(shù);

aφ_k——噪聲序列，且aφ₁～。

該AR模型即為系統(tǒng)的狀態(tài)方程，其中的系數(shù)通過(guò)微機(jī)電系統(tǒng)實(shí)際的參數(shù)進(jìn)行計(jì)算。

基于卡爾曼濾波的微機(jī)電系統(tǒng)慣性傳感器信號(hào)降噪方法

在無(wú)人機(jī)控制系統(tǒng)中，由于控制器硬件的限制，以及受限于系統(tǒng)對(duì)實(shí)時(shí)性的要求，上述提及的各種數(shù)字濾波方法并不一定適合應(yīng)用于工程實(shí)際中。在實(shí)際應(yīng)用中，需要綜合考慮濾波器濾波算法的復(fù)雜程度和濾波效果。卡爾曼濾波方法是一種模型清晰，算法簡(jiǎn)潔同時(shí)濾波效果較好，濾波輸出標(biāo)準(zhǔn)偏差較小的濾波方法，適合在消費(fèi)級(jí)和工業(yè)級(jí)無(wú)人機(jī)系統(tǒng)中應(yīng)用。

作為工程應(yīng)用中常用的濾波方法，卡爾曼濾波方法因適用于對(duì)隨機(jī)過(guò)程的估計(jì)，而成為微機(jī)電系統(tǒng)隨機(jī)噪聲抑制方法中最常用的一種。但在設(shè)計(jì)卡爾曼濾波器的過(guò)程中，首先要求建立系統(tǒng)噪聲的數(shù)學(xué)模型。為了解決卡爾曼濾波器對(duì)噪聲模型依賴程度高的問(wèn)題，需要對(duì)非平穩(wěn)的隨機(jī)噪聲進(jìn)行模型估計(jì)。

卡爾曼濾波是一種遞歸型數(shù)字濾波器，它適用于線性動(dòng)態(tài)系統(tǒng)，在應(yīng)用時(shí)，需要分析系統(tǒng)的狀態(tài)方程和量測(cè)方程，設(shè)X_k為系統(tǒng)在k時(shí)刻的狀態(tài)，Wsub>k為過(guò)程噪聲，則系統(tǒng)的狀態(tài)方程為

Xsub>k=φXsub>k-1+HWsub>k （4）

式中：φ——為狀態(tài)轉(zhuǎn)移矩陣;

H——為噪聲驅(qū)動(dòng)矩陣，表征噪聲對(duì)系統(tǒng)狀態(tài)的影響程度。

因隨機(jī)哚聲的模型建立為AR（1）模型，即一階自回歸模型，則狀態(tài)方程（4）中的各矩陣降階為各個(gè)數(shù)字，由式（2）和式（4）可得，狀態(tài)轉(zhuǎn)移矩陣φ=φsub>1=1，噪聲驅(qū)動(dòng)矩陣H=1，Xsub>k=xsub>k，Wsub>k=ω（k）（k=0，1，2，3…）。

設(shè)Vsub>k為量測(cè)噪聲，Zsub>k是系統(tǒng)的測(cè)量值，即微機(jī)電系統(tǒng)輸出信號(hào)值，則系統(tǒng)的量測(cè)方程為

Zsub>k=CXsub>k+ Vsub>k（k=0，1，2，3…） （5）

式中C——為量測(cè)矩陣。

因系統(tǒng)的測(cè)量值為狀態(tài)變量和量測(cè)噪聲的疊加，即

Zsub>k=Xsub>k+Vsub>k （6）

則量測(cè)矩陣為C=1。

對(duì)于該系統(tǒng)，設(shè)Xsub>k為狀態(tài)變量Xsub>k的濾波估計(jì)，則卡爾曼濾波的遞推過(guò)程如下，其中狀態(tài)一步預(yù)測(cè)為

協(xié)方差陣一步預(yù)測(cè)為

式中Q——為系統(tǒng)噪聲的方差，其值由實(shí)驗(yàn)方法確定。

濾波增益為

式中R——為量測(cè)噪聲的方差，其值由實(shí)驗(yàn)方法確定。

協(xié)方差陣估計(jì)為

狀態(tài)估計(jì)為

系統(tǒng)輸出為

可以看出，卡爾曼濾波由增益計(jì)算和濾波計(jì)算兩部分組成，式（7）～（12）即為離散卡爾曼濾波器的計(jì)算過(guò)程，其中式（7）、（11）為濾波計(jì)算部分，由此可得卡爾曼濾波器濾波計(jì)算部分具體結(jié)構(gòu)，式（8）、（9）、（10）為增益計(jì)算部分;其縱向可分為時(shí)間更新部分和測(cè)量更新部分，時(shí)間更新部分為式（7）、（8），測(cè)量更新部分為式（9）、（10）、（11）。其中濾波增益Ksub>k是影響濾波器性能的關(guān)鍵參數(shù)，系統(tǒng)哚聲方差Q和量測(cè)噪聲方差R對(duì)濾波器的影響即通過(guò)Ksub>k體現(xiàn)，正因?yàn)榇耍?dāng)系統(tǒng)噪聲方差和量測(cè)噪聲方差不易通過(guò)實(shí)驗(yàn)確定時(shí)，可直接對(duì)濾波增益進(jìn)行估計(jì)，其范圍為0k<1。

當(dāng)給出初始狀態(tài)時(shí)，即X和Psub>0確定時(shí)，即可根據(jù)時(shí)刻k的量測(cè)值Zsub>k遞推得到時(shí)刻k的狀態(tài)值Xsub>k。

卡爾曼濾波方法實(shí)驗(yàn)

針對(duì)MPU6050，將上述卡爾曼濾波方法應(yīng)用于其輸出信號(hào)的去哚。在驗(yàn)證濾波算法去哚效果的實(shí)驗(yàn)中，為了便于觀察和對(duì)比實(shí)驗(yàn)波形，需要將微機(jī)電系統(tǒng)置于相對(duì)靜止的實(shí)驗(yàn)環(huán)境中，因此利用光學(xué)平臺(tái)來(lái)隔離地基的振動(dòng)。實(shí)驗(yàn)平臺(tái)選用常用于無(wú)人機(jī)飛控系統(tǒng)的嵌入式處理器對(duì)微機(jī)電系統(tǒng)數(shù)據(jù)進(jìn)行濾波處理，以觀察該濾波算法在嵌入式控制系統(tǒng)中的處理效率。

微機(jī)電系統(tǒng)處于靜態(tài)時(shí)，將其輸出作為卡爾曼濾波器的輸入，驗(yàn)證其濾波效果。上圖是微機(jī)電系統(tǒng)靜態(tài)時(shí)原始輸出，其噪聲信號(hào)達(dá)到0.040°/s，經(jīng)卡爾曼濾波器處理，可得下圖中輸出信號(hào)，對(duì)比可以看出微機(jī)電系統(tǒng)靜態(tài)輸出信號(hào)波動(dòng)范圍在±0.025°/s內(nèi)，對(duì)隨機(jī)噪聲信號(hào)起到一定的抑制作用。同時(shí)對(duì)利用上位機(jī)接收的數(shù)據(jù)處理結(jié)果進(jìn)行分析，證明了該濾波算法適于在本課題的嵌入式控制系統(tǒng)中應(yīng)用，計(jì)算效率較高。

總結(jié)

本文分析了微機(jī)電系統(tǒng)慣性傳感器輸出信號(hào)的噪聲特性，利用Allan方差估計(jì)方法對(duì)噪聲建立了數(shù)學(xué)模型。在此基礎(chǔ)上引入了基于卡爾曼濾波的陀螺去噪方法，設(shè)計(jì)了適用于無(wú)人機(jī)飛控系統(tǒng)的卡爾曼濾波器，對(duì)微機(jī)電系統(tǒng)的輸出信號(hào)進(jìn)行了濾波處理。通過(guò)實(shí)驗(yàn)驗(yàn)證，該方法應(yīng)用于無(wú)人機(jī)的控制器中時(shí)獲得了良好的濾波效果，為無(wú)人機(jī)的高精度飛行奠定了基礎(chǔ)。