基于擴展卡爾曼濾波的小型固定翼無人機姿態(tài)估計方法分析

楊兆，沈作軍

北京航空航天大學 航空科學與工程學院，北京 100191

隨著現(xiàn)代科技的發(fā)展，無人機成為一種越來越受重視的新興飛行器，由于其低風險、低成本、可自主飛行和可重復利用的特點，被廣泛應用于軍事和民用領域。針對小型無人機，由于其制作成本低、體積較小，在配備導航系統(tǒng)時，主要選擇重量輕、成本低且體積小、易于集成且功耗小的微機電系統(tǒng)（Micro Electro Mechanical System，MEMS）式慣性導航系統(tǒng)。慣導系統(tǒng)的最大缺點是隨著時間的推移，其誤差隨著時間的積分會愈來愈大。相對于高成本高精度的慣導系統(tǒng)，MEMS的精度較低，這會導致單純的依靠MEMS進行姿態(tài)估計得到的誤差越來越大并且不能得到修正。衛(wèi)星定位導航系統(tǒng)可以提供不隨時間積累誤差的導航信息，因此，將這兩種導航系統(tǒng)的數(shù)據(jù)進行融合，從而給出一個較為精確的姿態(tài)信息。不準確的姿態(tài)信息會對無人機的飛行控制產(chǎn)生巨大的干擾，不僅會使得無法完成規(guī)劃的飛行任務，嚴重時還會導致控制失穩(wěn)，造成無人機墜毀等事故。

本文基于擴展卡爾曼濾波理論（Extended Kalman Filter，EKF），結合MEMS的慣性導航系統(tǒng)和衛(wèi)星定位導航系統(tǒng)的測量信息，研究一種針對小型無人機的姿態(tài)估計方法，并對其進行仿真分析。

1 卡爾曼濾波理論

卡爾曼濾波[1]是一種對系統(tǒng)進行最優(yōu)估計的算法，其原理是利用線性系統(tǒng)狀態(tài)方程，通過對系統(tǒng)輸入，輸出觀測數(shù)據(jù)。由于所得觀測數(shù)據(jù)包括系統(tǒng)本身所含噪聲以及外界干擾的影響，因此，最優(yōu)估計的過程也被視作濾波過程。卡爾曼濾波具有以下幾個特點：

（1）利用狀態(tài)空間方程在時域內(nèi)設計一種遞推的濾波器，可以用于估計多維的隨機過程。

（2）卡爾曼濾波理論采用狀態(tài)方程去描述被估計量的動態(tài)變化規(guī)律。被估計的狀態(tài)量其動態(tài)信息由兩方面確定，一是激勵白噪聲的統(tǒng)計信息，二是動力學方程。在動力學方程已知并且激勵白噪聲是一個平穩(wěn)過程的情況下，被估計的狀態(tài)量既可以是平穩(wěn)的，也可以是非平穩(wěn)的。

（3）雖然是在時域內(nèi)設計的一種遞推的濾波器算法，但是卡爾曼濾波有連續(xù)性和離散型兩類算法，離散型算法方便在數(shù)字計算機上實現(xiàn)。

卡爾曼濾波理論克服了以往濾波理論的局限性，因此在工程上得到了較為廣泛的應用，在航空航天領域的導航制導控制（GNC）方向以及通信領域都起到了重大的作用[2]。

1.1 離散型卡爾曼濾波

由于離散型的卡爾曼濾波算法通過對時間進行離散，可以在數(shù)字計算機上進行編程計算，所以該方法在對信號的線性估計和處理方面有著廣泛的應用。

假設在tk時刻，系統(tǒng)噪聲序列Wk-1驅動被估計的狀態(tài)量Xk，式（1）描述了驅動的原理。式中：Φk，k-1是由tk-1的狀態(tài)進入到tk時刻狀態(tài)的一步轉移陣，Γk-1是系統(tǒng)噪聲驅動陣、Wk-1是系統(tǒng)噪聲序列。

Zk為tk時刻狀態(tài)量Xk的量測量，它們之間有線性關系由量測方程表示：

式中：Hk為量測陣；Vk為量測噪聲序列。式（1）和式（2）中的噪聲序列滿足下式：

式中：Qk和Rk分別是系統(tǒng)噪聲序列的方差陣和量測噪聲序列的方差陣，在這假定Qk是非負定矩陣，Rk是正定矩陣。根據(jù)線性最小方差的原理推導出離散型卡爾曼濾波方程。

卡爾曼濾波過程其實是一個預測ü校正的過程，根據(jù)系統(tǒng)方程利用上一個時刻的狀態(tài)估計值進行預測，從而計算得出當前時刻的一步狀態(tài)估計值，而當前時間點的狀態(tài)最優(yōu)估計值是根據(jù)質(zhì)量優(yōu)劣參數(shù)和量測方程以及利用量測信息校正狀態(tài)量的一步預測估計值這三個量得到的。

1.2 擴展卡爾曼濾波

對于一般的線性系統(tǒng)而言，離散型卡爾曼濾波基本方程都可以解決，但是對于工程應用中的實際系統(tǒng)而言，大部分都是非線性的，所以要求系統(tǒng)方程和量測方程是非線性的，對于非線性系統(tǒng)，一般用式（4）表示：

式中：f表示系統(tǒng)的非線性動力學函數(shù)，該函數(shù)與時間、輸入量以及系統(tǒng)的狀態(tài)量相關；G用來表示該系統(tǒng)的噪聲輸入。h表示系統(tǒng)的非線性量測函數(shù)，該函數(shù)與系統(tǒng)的狀態(tài)和時間相關；w和v是兩組零均值白噪聲序列，彼此相互獨立，其初始狀態(tài)不受系統(tǒng)的初始狀態(tài)X(0)影響，它們具有如下特性：

如式（5）所描述的非線性系統(tǒng)，離散型卡爾曼濾波的基本方程將不再能解決該類問題。對于非線性較弱的系統(tǒng)，通過對式（4）中的非線性部分進行泰勒展開，保留一次項，省略掉高階項，從而得到線性系統(tǒng)。用這種方法可以繼續(xù)使用卡爾曼濾波基本方程對該線性系統(tǒng)狀態(tài)進行估計。但是在對系統(tǒng)進行線性化時，需要找到一個系統(tǒng)的參照點，也就是一條參考的標稱軌跡。假如選擇的參考軌跡是系統(tǒng)噪聲為0時所解算得到的軌跡，便會出現(xiàn)以下兩個問題：其一，參考軌跡的狀態(tài)量，量測值的參考值都預先存儲在計算機中，需要較大的存儲單元，在工程應用上，特別是對小型固定翼無人機而言，實現(xiàn)起來十分麻煩；其二，在實際的工程系統(tǒng)中，由于受到多方面的影響，隨著時間的積累，實際軌跡與參考軌跡之間的誤差將越來越大，此時如果還使用預存的參考軌跡，那么系統(tǒng)的非線性部分誤差將十分大，從而難以忽略，因此會直接影響到系統(tǒng)的準確性。

在工程應用中，需要對系統(tǒng)進行線性化時，通常采用的參考軌跡是最優(yōu)狀態(tài)估計軌跡，把這種基于最優(yōu)估計進行線化的軌跡，再采用卡爾曼濾波進行估計的方法稱為擴展卡爾曼濾波[3]。

1.3 離散型擴展卡爾曼濾波方程

真實軌跡和標稱軌跡之間的偏差定義用式（6）和式（7）表示：

式中為標稱量測值，表達式為：

從式（8）可以看出，在遞推的過程中，若要求得到狀態(tài)量估計值 ，那么必須已知量測值 ，但是量測值又需要從狀態(tài)量估計值所計算得到，為了解決以上問題，采取上一時刻的狀態(tài)量估計值來求解量測量的標稱值：

到標稱軌跡后，則可以對系統(tǒng)進行線化，線化后的標稱軌跡有：

式中：F(t)和H(t)為雅可比矩陣，表達式為：

在對系統(tǒng)完成線化后，還需要對系統(tǒng)進行離散，以便于在計算機上實現(xiàn)，離散后的系統(tǒng)為：

其中：

式中：T為時間步長。根據(jù)離散系統(tǒng)方程和上一節(jié)所提到的離散型卡爾曼濾波基本方程，可以推導得出擴展離散性卡爾曼濾波方程：

（1）狀態(tài)一步預測：

（2）一步預測均方差計算：

（3）卡爾曼增益計算：

（4）狀態(tài)偏差及狀態(tài)估計：

（5）估計均方誤差：

2 傳感器誤差模型

在實際的工程應用中，由于環(huán)境的溫度、濕度，地球地磁效應、周圍電磁環(huán)境以及傳感器本身的測量誤差和轉換誤差等影響，在仿真的過程中需要對傳感器建模過程中加入誤差模型，使得仿真過程更加與實際工程應用相一致，才能在此基礎上設計出更合適的算法來對這些系統(tǒng)的誤差進行補償，從而得到一個更為準確的測量和估計。

2.1 MEMS慣導模型

慣性導航系統(tǒng)器件在實際應用中會受到很多環(huán)境因素的影響，例如，加速度計和陀螺儀在實際應用中會受到類似測量噪聲、偏差等因素的影響。針對陀螺儀，一種常見的陀螺儀誤差模型[4]可用式（21）表示：

在式（21）中，陀螺儀量測得到的角速率分別為[pmqmrm]；實際的角速率分別為[p q r]；加入的誤差項[bpbqbr]包含了陀螺儀接通電源后所產(chǎn)生的常值漂移b0和隨機漂移bR

[5]，可用式（22）表示；高斯白噪聲分別為 [εpεqεr]，其中[p q r]分別代表飛行器飛行過程中的滾轉角速度、俯仰角速度、偏航角速度。

陀螺儀在每次啟動時都會產(chǎn)生一個常值漂移和隨機漂移，常值漂移一般用b0表示，而隨機漂移一般用一階馬爾科夫[6]隨機過程來描述，用來表示陀螺的誤差在運行過程中隨時間的變化，用式（23）表示，τ為高斯一階馬爾科夫過程中的時間常數(shù)，εbR為加入的高斯白噪聲。

加速度計相比于陀螺儀產(chǎn)生的誤差較小，所以加速度計產(chǎn)生的誤差用式（24）表示，只考慮高斯白噪聲。式中加度計量測值用[AxmAymAzm]表示，實際的加速度信息用[AxAyAz]表示，高斯白噪聲用 [εAxεAyεAz]表示，其中[AxAyAz]表示飛行器三個方向的加速度。

2.2 衛(wèi)星定位系統(tǒng)模型

目前，衛(wèi)星定位系統(tǒng)有美國全球定位系統(tǒng)（Global Position System，GPS）、俄羅斯的格洛納斯系統(tǒng)、中國的北斗定位系統(tǒng)。本文中采用的仿真模型是GPS，GPS有兩大功能應用比較廣泛，一是位置測量，一是速度測量。GPS根據(jù)偽距、多普勒頻移和載波相位這三種原始測量值通過不同的計算方式派生出單點、相位差分、偽距差分等多種定位方式。GPS速度測量可以通過多普勒頻移獲取，由于和衛(wèi)星距離無關，所以GPS的測速精度遠遠高于定位精度，精確度可以達到0.01m/s[7，8]。GPS的誤差包含多種，如衛(wèi)星速度誤差、衛(wèi)星軌道誤差、對流層和電離層時延變化率等。為了保證誤差的一般性并且方便仿真的進行，本文根據(jù)式（25）建立了以下模型，假設速度信息無漂移，只受到一個高斯白噪聲的影響。

GPS本身并不具備有測量載體加速度的功能，但是我們可以通過速度差分來計算得出加速度信息，通過這種方法對載體加速度進行計算，在參考文獻[9]和參考文獻[10]中已經(jīng)得到了應用，而且其計算精度能達到0.01m/s2。該種方法不受重力、溫度等影響。利用式（26）可以對GPS測得的速度信息進行差分便可以得到加速度信息：

式中：k表示GPS的運行過程中第k步的可用速度信息；δt表示速度更新時間間隔。

3 構建擴展卡爾曼濾波過程

卡爾曼濾波方程構建包括預測和校正兩個過程，校正過程是根據(jù)量測數(shù)據(jù)對預測得到的數(shù)據(jù)進行校正。下面主要介紹根據(jù)卡爾曼濾波算法進行姿態(tài)估計的系統(tǒng)方程建立過程和測量方程的推導過程。

飛行器的姿態(tài)一般用兩種方式描述，即姿態(tài)角和四元數(shù)。在本文中選擇使用姿態(tài)角，也就是繞大地坐標系形成的三個歐拉角（滾轉角、俯仰角、偏航角）。其微分方程的關系可在飛行力學相關書籍的運動學方程建立中找出，將式（21）代入得到：

為了能夠在陀螺儀啟動時可以估計出常值漂移，因此在選定卡爾曼濾波方程的狀態(tài)量時，選擇三個姿態(tài)角和三個陀螺儀的誤差量作為系統(tǒng)的狀態(tài)矢量：

針對非線性系統(tǒng)方程，在擴展卡爾曼濾波中要對其進行線化，從而得到線化點的雅可比矩陣，利用該雅可比矩陣進行狀態(tài)量的一步迭代預測計算。根據(jù)GPS計算得到的加速度信息和MEMS測量的加速度信息相結合，可以得到飛行器的姿態(tài)信息。具體過程如下：

（1）在固定翼飛行器飛行過程中，為了保證飛行安全，一般選擇在側滑角較小或者為0的情況下飛行，因此本文針對側滑角為0的情況下，根據(jù)GPS測量的速度信息計算偏航角：

（2）慣導系統(tǒng)中加速度計是測量體軸系下的加速度A，但是式（26）中，由GPS所得到的加速度aGPS是大地坐標系的加速度，關系如下：

式中：g為重力加速度，DCMgb為體軸系和大地坐標系之間的轉換矩陣。根據(jù)式（31）中已經(jīng)計算好的偏航角，可以將DCMgb改為下述形式：

令：

（3）進一步推導得出俯仰角θ、滾轉角φ的表達式：

（4）根據(jù)上述的三個計算順序依次求出飛行器的三個姿態(tài)角的表達式，通過加速度計和GPS可以聯(lián)合求出姿態(tài)信息，將該信息作為卡爾曼濾波方程中的量測值。

（5）由于通過上述計算可以直接得到姿態(tài)角信息，所以卡爾曼濾波方程中的量測方程可以得到很大的簡化，其量測方程如式（37）所示：

4 仿真算例

在第三小節(jié)中建立了卡爾曼濾波的系統(tǒng)方程和量測方程，本文基于離散型擴展卡爾曼濾波的基本方程，建立姿態(tài)估計的遞推算法，利用Matlab數(shù)學仿真軟件，對Cessna182飛機進行氣動建模并且進行全機的控制律設計，在此基礎上，對該算法進行仿真驗證。飛行軌跡設計包含對稱和非對稱運動。為了更好地模擬真實的飛行數(shù)據(jù)，在虛擬傳感器信息中需要加入噪聲和漂移量，見表1，根據(jù)參考文獻[11]和參考文獻[12]選擇出適合的噪聲量和漂移量。

表1 傳感器噪聲和偏移量Table 1 Sensor noise and offset

則基于傳感器的噪聲誤差參數(shù)可以得到系統(tǒng)噪聲方差陣。由于量測信息是由解析計算得到，所以噪聲方差沒有直接的計算依據(jù)，本文中假設量測信息即姿態(tài)歐拉角的標準差為0.5°，在R取上述值時，估計器有較好的收線效果。

圖1 三維飛行軌跡圖Fig.1 3D trajectory

圖1是飛行器的三維飛行軌跡，圖2是加速度計和GPS加入白噪聲和漂移量后，飛行器未加入卡爾曼濾波時的姿態(tài)角變化的真實值和估計值，圖3是飛行器未加入卡爾曼濾波時姿態(tài)角真實值和估計值的誤差。從圖3可以看出，當反饋的姿態(tài)角信息有誤差時，隨著時間的推移，控制器不能很好地消除誤差，導致姿態(tài)角的誤差越來越大，滾轉角最大誤差達到20°、俯仰角誤差達到5°、偏航角的較大誤差使得飛行器已經(jīng)脫離了飛行任務的航跡。

圖2 未經(jīng)過卡爾曼濾波的姿態(tài)角變化Fig.2 Attitude variation without EKF

圖3 未加入濾波姿態(tài)角真實值與估計值間的誤差Fig.3 Attitude deviation without EKF between estimation and true value

圖4是加入卡爾曼濾波后的姿態(tài)估計值和真實值，圖5是加入濾波姿態(tài)角真實值與估計值間的誤差。從圖5中可以看出，在飛行過程中誤差比較小，滾轉角的誤差在2°以內(nèi)，而俯仰角的誤差也在2.5°以內(nèi)，偏航角在有最大2°左右的誤差后逐漸收斂為0。

對比之后可以看出，利用離散型擴展卡爾曼濾波對姿態(tài)角進行估計的算法，對飛行器導航系統(tǒng)的準確性有了較大的提升。

圖4 加入濾波姿態(tài)角變化圖Fig.4 Attitude variation with EKF

圖5 加入濾波姿態(tài)角真實值與估計值間的誤差Fig.5Attitude deviation with EKF between estimation and true value

5 結束語

本文針對低成本的小型固定翼無人機，利用擴展卡爾曼濾波理論，結合MEMS慣導系統(tǒng)和GPS導航系統(tǒng)，建立了進行姿態(tài)角估計的卡爾曼濾波方程，根據(jù)MEMS和GPS的誤差模型建立系統(tǒng)方程，根據(jù)加速度計和GPS速度差分計算得到的加速度計算姿態(tài)角得到量測方程，并將該方法帶入飛行器的數(shù)學仿真中，進行閉環(huán)仿真驗證。可以看出，加入濾波比不加入濾波姿態(tài)角的誤差要小一個量級，取得了很好的姿態(tài)估計精度結果，說明該姿態(tài)估計方法是針對成本較低的固定翼無人機是可靠的。

[1] Kalman R E. A new approach to linear filtering and prediction problems[J]. Journal of Basic Eng （ASME）， 1960（82D）： 35-46.

[2] Schmidt S F. The Kalman filter： Its recognition and development for aerospace application[J]. Journal of Guidance and Control，1981， 4（1）： 4-7.

[3] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理[M].西安：西北工業(yè)大學出版社，2015：33-35.QIN Yongyuan， ZHANG Hongyue， WANG Shuhua. Kalman filtering and combined navigation principle[M]. Xi’an： Northwestern Polytechnical University Press， 2015：33-35.（in Chinese）

[4] Farrenkopf R L. Analytic steady-state accuracy solution for two common spacecraft attitude estimators[J]. Journal of Guidance and Control， 1978， 1（4）： 282-284.

[5] Gleason S， Egziabher G. GNSS application and methods[M].Boston： Artech House Publishers， 2009： 105-107.

[6] John L C， Markley F L， Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance， Control and Dnamics， 2007（30）： 14-16.

[7] Luis S， Donghyun K， Ricichard B L. A GPS velocity sensor：How accurate can it be?-a first look[C]//ION NTM 2004， San Diego， 2004：875-885.

[8] 王甫紅，張小紅，黃勁松. GPS單點測速的誤差分析及精度評價 [J].武漢大學學報，2007（6）：515-519.WANG Fuhong， ZHANG Xiaohong， HUANG Jinsong. Error analysis and accuracy evaluation of GPS single point velocity measurement[J]. Journal of Wuhan University， 2007（6）： 515-519.（in Chinese）

[9] Psiaki M L， Powell S P， Kintner P M. The accuracy of the GPS-derived acceleration vector， a novel attitude reference[C]//Proceedings of Guidance， Navigation， and Control Conference，Portland， 1999：751-760.

[10] Pinder S D， Crowe T G， Nikiforuk P N. Application of the global positioning system in determination of vehicular acceleration[J]. AIAA Aircraft， 2001（38）： 856-859.

[11] Li Y， Dempster A， Li B， et al. A low-cost attitude heading reference system by combination of GPS and magnetometers and MEMS inertial sensors for mobile applications[J]. Journal of Global Positioning System， 2006（6）： 88-95.

[12] Godha S. Performance evaluation of low cost MEMS-based IMU integrated with GPS for land vehicle navigation application[D].Canada： The University of Calgary， 2006.