無人機微機電系統慣性傳感器卡爾曼濾波技術

孫立志

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

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

微機電系統慣性傳感器原理

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

a_c= 2×ω×v （1）

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

v——質點相對動坐標系的線速度。

微機電系統工作時，通過電驅動，使微機械的振動框架達到諧振狀態，而產生一定的速度，此時當微機電系統隨平臺框架旋轉時，即產生科氏力;科氏力與檢測框架發生關聯，即形成檢測信號，經放大輸出得到電信號。據此微機電系統將無人機機體的旋轉角速度信息轉換為與之成比例的電壓信號，通過模數轉換得到角速度的數字信息，將角速度進行積分可得框架的角度信息。

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

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

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

應用于無人機的微機電系統慣性傳感器噪聲特性

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

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

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

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

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

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

微機電系統慣性傳感器隨機噪聲的數學模型

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

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

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

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

式中，x_k——隨機序列;

φ₁——AR（1）模型系數;

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

該AR模型即為系統的狀態方程，其中的系數通過微機電系統實際的參數進行計算。

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

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

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

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

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

式中：φ——為狀態轉移矩陣;

H——為噪聲驅動矩陣，表征噪聲對系統狀態的影響程度。

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

設Vsub>k為量測噪聲，Zsub>k是系統的測量值，即微機電系統輸出信號值，則系統的量測方程為

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

式中C——為量測矩陣。

因系統的測量值為狀態變量和量測噪聲的疊加，即

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

則量測矩陣為C=1。

對于該系統，設Xsub>k為狀態變量Xsub>k的濾波估計，則卡爾曼濾波的遞推過程如下，其中狀態一步預測為

協方差陣一步預測為

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

濾波增益為

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

協方差陣估計為

狀態估計為

系統輸出為

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

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

卡爾曼濾波方法實驗

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

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

總結

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