基于擴展卡爾曼濾波器的無人機狀態(tài)估計

王文麗 何博

摘? 要：在無人機飛行過程中，由于低成本無人機對狀態(tài)反饋不準確和外部干擾對傳感器的影響，導致飛行時間一長會出現(xiàn)漂移現(xiàn)象，使得無人機產(chǎn)生浮動等不良問題。為了解決這些問題，提出一種適用于小型無人機狀態(tài)估計的四元數(shù)擴展卡爾曼濾波數(shù)據(jù)融合算法，該算法通過建立四元數(shù)系統(tǒng)觀測模型和傳感器量測模型，解決了低成本傳感器對無人機位置和姿態(tài)直接測量的低精度和易被外界干擾的問題，有效提高了狀態(tài)估計的精度。仿真實驗驗證了算法的有效性。

關鍵詞：無人機;四元數(shù);擴展卡爾曼濾波器;狀態(tài)估計;數(shù)據(jù)融合

中圖分類號：TP183? ? ?文獻標識碼：A文章編號：2096-4706（2022）04-0118-05

UAV State Estimation Based on Extended Kalman Filter

WANG Wenli， HE Bo

（Department of Aerospace Science and Technology， Aerospace Engineering University of the PLA Strategic Support Force， Beijing? 101416， China）

Abstract： During the flight of the UAV， due to the inaccurate state feedback of the low-cost UAV and the influence of external interference on the sensor， the drift phenomenon will occur over a long flight time， causing the UAV to float and other undesirable problems. In order to solve these problems， a quaternion extended Kalman filter data fusion algorithm suitable for small UAV state estimation is proposed. The algorithm solves the problems of low accuracy and easy interference from the outside world in the direct measurement of the position and attitude of the UAV for the low-cost UAV， by establishing the quaternionic system observation model and the sensor measurement model. It effectively improves the accuracy of the state estimation. The simulation experiments demonstrate the effectiveness of the algorithm.

Keywords： UAV; quaternion; extended Kalman filter; state estimation; data fusion

0? 引? 言

隨著無人機技術(shù)的發(fā)展，固定翼無人機由于其成本低、飛行速度快、高度高、可按規(guī)劃航跡自主飛行等特點，廣泛應用于軍事和民用領域，進而對無人機飛行時完整的狀態(tài)反饋提出了更高的要求。但同時，大多數(shù)無人機配置著由低成本微機電系統(tǒng)、磁力計、GPS及氣壓傳感器組成的低成本飛控系統(tǒng)，這些傳感器在測量過程中會產(chǎn)生持續(xù)的噪聲和漂移，從而造成嚴重后果。且利用這樣的飛控系統(tǒng)研究無人機自主飛行控制問題，雖可以明顯降低其研究成本，但隨著時間的增加，傳感器的測量誤差被疊加，嚴重影響無人機的控制精度。因此本文從低成本傳感器入手，研究固定翼無人機自主飛行控制問題。

由于無人機系統(tǒng)的內(nèi)部動態(tài)規(guī)律無法直接測量，狀態(tài)估計作為傳感器測量與控制器解算的中間層，以可獲取的測量數(shù)據(jù)作為輸入估算出無人機當前狀態(tài)，反饋至控制器形成閉環(huán)，其準確性直接影響無人機的定位精度和控制效果。本文關心狀態(tài)估計應用于低成本傳感器對誤差的影響。

本文針對無人機狀態(tài)反饋問題，結(jié)合無人機模型預測控制算法探索無人機飛行過程中良好的運動估計方法。擴展卡爾曼濾波是通過這采樣時刻點附近線性化獲得近似線性模型，然后采用經(jīng)典卡爾曼濾波方法進行狀態(tài)估計，以解決非線性動態(tài)過程的狀態(tài)估計問題，獲得可以不直接測量的無人機狀態(tài)，為解決低成本傳感器帶來的漂移現(xiàn)象和控制精度等問題提供思路[1]。

1? 無人機建模及問題描述

1.1? 坐標系之間的轉(zhuǎn)換

固定翼無人機通過調(diào)節(jié)升降舵、副翼和方向舵的角度來改變無人機的受力大小，進而調(diào)整無人機的姿態(tài)，無人機的推力用于控制飛行速度。無人機運動估計涉及慣性坐標系FI、機體坐標系FB，其二者之間的轉(zhuǎn)換關系如圖1所示。

其中慣性坐標系（NED軸系統(tǒng)）原點在地球上某一點處，N軸指向北，并且和地球表面相切;E軸沿著一條恒定緯度指向東方，并且和地球表面相切;D軸指向地球的中心。在本文中，慣性坐標系隨著經(jīng)度和緯度的變化而變化。

機體坐標系FB原點位于無人機質(zhì)心，XB平行于機身軸線，指向機頭方向為正;YB垂直于對稱面，指向右翼為正;ZB在飛機對稱面內(nèi)垂直于XB，指向下方為正。

無人機的位置、速度和姿態(tài)系統(tǒng)為非線性系統(tǒng)，其離散時間的狀態(tài)空間模型為：

（1）

圖1? 無人機坐標示意圖

其中，x表示系統(tǒng)狀態(tài)變量，分別為速度分量V，α，β，角速度分量p，q，r，四元數(shù)表示的姿態(tài)q1，q2，q3，q4，實時位置N，E，h，推力T;y為系統(tǒng)觀測向量;f（x，u）、h（x）均為非線性向量函數(shù)或矩陣;ω，υ分別為系統(tǒng)方程和量測方程中不可避免的過程噪聲和量測噪聲。

1.2? 四元數(shù)描述姿態(tài)角

由于傳感器所測量的值是相對于機體坐標系的，因此常在機體坐標系中描述無人機數(shù)學模型。參考方程（1）首先在慣性坐標系中無人機繞ZB軸轉(zhuǎn)動偏航角ψ度，在此基礎上，繞YB軸旋轉(zhuǎn)俯仰角θ度，然后，繞XB軸旋轉(zhuǎn)滾轉(zhuǎn)角φ 度，此時坐標系與無人機機體坐標系重合。

（2）

三個歐拉角（ψ，θ，φ）和旋轉(zhuǎn)順序Z-Y-X完全定義了姿態(tài)。以歐拉角表示的旋轉(zhuǎn)矩陣為：

（3）

由方程（3）可解算出無人機的三個姿態(tài)角為：

（4）

由于歐拉角包含大量的三角運算，計算時間較長，且可能出現(xiàn)奇異現(xiàn)象;而四元數(shù)法僅含四個未知量且易編程實現(xiàn)，故中本文所研究的項目中使用四元數(shù)法對無人機的姿態(tài)進行描述。在慣性參考系中，四元數(shù)的常規(guī)表示形式為，其中實部與旋轉(zhuǎn)角μ有關，虛部與旋轉(zhuǎn)軸i，j，k有關，ex，ey，ez是XI，YI，ZI軸的單位分向量，如下式[2]（5）;

（5）

在方程（5）中，矢量q可描述無人機的姿態(tài)，前三個參數(shù)包含旋轉(zhuǎn)矢量信息，第四個參數(shù)僅包含旋轉(zhuǎn)角度信息。ex，ey，ez可為旋轉(zhuǎn)瞬軸和旋轉(zhuǎn)方向。

機體坐標系經(jīng)過三次順序旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣T（q）可用四元數(shù)表示為：

（6）

由方程（6）可得方程（4）中的無人機的偏航角ψ、俯仰角θ和滾轉(zhuǎn)角φ也可以表示為：

（7）

即用四元數(shù)表示姿態(tài)角為：

（8）

2? 擴展卡爾曼濾波狀態(tài)估計

擴展卡爾曼濾波器利用含噪聲的傳感器使隨機系統(tǒng)的均方估計誤差最小化，在采樣點附近將非線性系統(tǒng)一階線性化后忽略高階項以滿足近似線性，是高斯白噪聲情況下的最小方差次優(yōu)濾波器。估計周期可分為預測和校正兩大部分，流程圖[3]如圖2所示。

2.1? 引入地心地固坐標系

為了更好地描述無人機的飛行位置和姿態(tài)并對其進行測量和估計，引入地心地固坐標系（ECEF）[4]，該坐標系如圖3所示，假設一個恒定半徑為R的圓形地球模型，坐標原點與地球質(zhì)心重合，z軸與地軸平行指向北極點;x軸指向赤道與本初子午線的交點;y軸在赤道平面上與x軸和z軸構(gòu)成右手笛卡爾坐標系。引用λ表示緯度，指向北半球為正，在±90°之間取值;φ表示經(jīng)度，本初子午線東邊為正，在±180°之間取值;h表示高度，是地球表面和無人機的距離，指向地心為正。

2.2? 建立預測方程模型

分析圖3表示的經(jīng)度、緯度、高度的時間導數(shù)，并以NED速度分量的形式表示為：

（9）

各個分量的時間導數(shù)為：

（10）

其中，方程（10）中的aN，aE，aD是在機體坐標系中獲得的加速度測量值aX，aY，aZ經(jīng)四元數(shù)逆變換到慣性參考系中獲得的相應的加速度測量值，是補償未測量的重力加速度。

（11）

結(jié)合方程（6）和方程（11）可得到速度分量的時間導數(shù)為：

（12）

GPS中收到信號解算過程延遲時間較長，如果不被建模，估計出的狀態(tài)向量可能會出現(xiàn)較大誤差，因此用一階帕德近似對連續(xù)時間延遲建模[5]：

（13）

下標d表示延遲狀態(tài)。將方程（13）寫成微分方程的形式，并且應用于方程（9）和方程（12）以獲得延遲位置和速度的模型為：

（14）

（15）

至此，定義出預測模型中的狀態(tài)向量為：

（16）

2.3? 建立量測模型

本文中使用GPS系統(tǒng)提供延遲位置和速度的直接測量值。定義如下：

（17）

磁力計提供和姿態(tài)有關的測量值，假設磁場是恒定的，通過四元數(shù)轉(zhuǎn)換矩陣定義磁力計測量值為：

（18）

非線性測量向量由GPS和磁力計測量值組成，定義如下：

h（x）=[h1，h2，h3，h4，h5，h6，h7，h8，h9]T （19）

2.4? 飛行狀態(tài)估計

在k采樣時刻，擴展卡爾曼濾波具體計算方法[6]如下：

Step1：設立系統(tǒng)模型，

（20）

方程（20）中，過程噪聲和量測噪聲的協(xié)方差矩陣可表達為：

E[ωωT]=QC，E[υυT]=RC（21）

計算狀態(tài)矩陣和量測矩陣的雅可比矩陣對擴展卡爾曼濾波器進行線性化;

（22）

利用離散采樣時間Ts將方程（22）轉(zhuǎn)換到離散時域為：

（23）

并且，由于連續(xù)過程噪聲直接映射到狀態(tài)向量上，因此將過程噪聲協(xié)方差矩陣QC離散化為：

（24）

Step2：設定采樣時刻真實狀態(tài)向量和估計狀態(tài)向量之間的差值和誤差協(xié)方差矩陣為：

（25）

Step3：設定初始狀態(tài)和誤差協(xié)方差P（0）;

Step4：k+1時刻的狀態(tài)變量的先驗估計;

（26）

Step5：k+1時刻的誤差協(xié)方差的先驗估計;

P-（k+1）=Φ（k）P（k）Φ（k）T+Q（k）（27）

Step6：k時刻卡爾曼濾波增益計算;

Kk=P（k+1）H（k+1）T[H（k+1）

P（k+1）H（k+1）T+R（k+1）]-1? （28）

Step7：k+1時刻狀態(tài)量測變量的更新;

（29）

Step8：k+1時刻誤差協(xié)方差的更新;

P（k+1）=[I-K（k+1）H（k+1）]P-（k+1）[I-K（k+1）

H（k+1）]T+K（k+1）R（k+1）K（k+1）T? ?（30）

在k+1時刻返回step1，循環(huán)往復，直至飛行任務結(jié)束。

3? 仿真實驗與分析

本文采用Matlab中的Simulink建立了固定翼無人機的控制系統(tǒng)模型，對某小型固定翼無人機進行整機控制律設計，在此基礎上，對無人機位置、姿態(tài)進行相應的估計，驗證本文所述的擴展卡爾曼濾波算法。其中，無人機參數(shù)為：m=5 kg，

g=9.81 ms-2，l=0.38 m，Jx=0.2 kgm2，Jy=0.36 kgm2，Jz=0.525 kgm2，量測數(shù)據(jù)通過捷聯(lián)傳感器獲得，傳感器模型如2.3節(jié)所示，控制器采用模型預測控制算法。

由于傳感器的測量值易被白噪聲破壞，因此將過程噪聲協(xié)方差建模并參考文獻[7]賦值為：

量測噪聲協(xié)方差建模并參考文獻[7]賦值為：

如圖4所示的飛行航跡，姿態(tài)圖顯示了無人機在剛開始就向下俯沖，在大約5 s時反向上爬升直至收斂在一個穩(wěn)定的高度。

仿真結(jié)果證明，濾波器在大約10 s后收斂并且?guī)缀醴€(wěn)定在穩(wěn)定解。隨后，當濾波器在穩(wěn)定狀態(tài)下工作時，狀態(tài)誤差協(xié)方差的增量被各自元素的量測值連續(xù)抵消。

如圖5、圖6和圖7所示，分別是EKF對偏航角、俯仰角和滾轉(zhuǎn)角的估計效果。由圖可知，加入擴展卡爾曼濾波器后，無人機在控制器的作用下，大概在10 s后收斂并趨于穩(wěn)定，只是偏航角的變化較大，如圖8所示的狀態(tài)誤差圖可看出只是在飛行初始階段會有較大的變化，待飛行平穩(wěn)后便會逐漸收斂并于某一小值穩(wěn)定。

4? 結(jié)? 論

本文研究基于擴展卡爾曼濾波算法應用在固定翼無人機飛行控制中，解決因低成本傳感器的漂移等不良問題，具體是將測量數(shù)據(jù)和估計數(shù)據(jù)進行數(shù)據(jù)融合，從而修正陀螺儀產(chǎn)生的隨機漂移誤差，解決狀態(tài)誤差累計造成更嚴重后果的問題。通過實驗，發(fā)現(xiàn)采用擴展卡爾曼濾波不僅可以有效解決上述問題，還可以幫助提高測量精度。

參考文獻：

[1] GREWAL M S，ANDREWS A P.卡爾曼濾波理論與實踐（MATLAB版）：第4版 [M].北京：電子工業(yè)出版社，2017.

[2] BEARD R W，MCLAIN T W.Small Unmanned Aircraft：Theory and Practice [M].Princeton：Princeton University Press，2012.

[3] 楊兆，沈作軍.基于擴展卡爾曼濾波的小型固定翼無人機姿態(tài)估計方法分析 [J].航空科學技術(shù)，2017，28（11）：15-21.

[4] 金舒燦，胡越黎，張賀.基于EKF的四旋翼姿態(tài)解算仿真與設計 [J].電子技術(shù)應用，2017，43（9）：127-131+136.

[5] 丁磊，吳蔚劼，方挺.基于擴展卡爾曼濾波的多旋翼飛行器融合姿態(tài)解算算法 [J].安徽工業(yè)大學學報（自然科學版），2018，35（3）：240-248.

[6] 石宇芃，馬宏軍，陳豹.基于四元數(shù)的四旋翼無人機擴展卡爾曼濾波算法 [J].控制工程，2021，28（11）：2131-2135.

[7] HOUGH W J.Autonomous aerobatic flight of a fixed wing unmanned aerial vehicle [J].Stellenbosch University of Stellenbosch，2007.

作者簡介：王文麗（1996.11—），女，漢族，甘肅酒泉人，碩士研究生在讀，研究方向：無人機自主飛行控制。