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

孫立志

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

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

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

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

a_c= 2×ω×v （1）

式中：ω——動坐標系的角速度;

v——質(zhì)點相對動坐標系的線速度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中，x_k——隨機序列;

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

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

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

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

在無人機控制系統(tǒng)中，由于控制器硬件的限制，以及受限于系統(tǒng)對實時性的要求，上述提及的各種數(shù)字濾波方法并不一定適合應(yīng)用于工程實際中。在實際應(yīng)用中，需要綜合考慮濾波器濾波算法的復(fù)雜程度和濾波效果?？柭鼮V波方法是一種模型清晰，算法簡潔同時濾波效果較好，濾波輸出標準偏差較小的濾波方法，適合在消費級和工業(yè)級無人機系統(tǒng)中應(yīng)用。

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

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

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

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

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

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

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

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

式中C——為量測矩陣。

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

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

則量測矩陣為C=1。

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

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

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

濾波增益為

式中R——為量測噪聲的方差，其值由實驗方法確定。

協(xié)方差陣估計為

狀態(tài)估計為

系統(tǒng)輸出為

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

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

卡爾曼濾波方法實驗

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

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

總結(jié)

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