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

王文麗 何博

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

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

中圖分類號(hào)：TP183? ? ?文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：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? 引? 言

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

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

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

1? 無人機(jī)建模及問題描述

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

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

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

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

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

（1）

圖1? 無人機(jī)坐標(biāo)示意圖

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

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

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

（2）

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

（3）

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

（4）

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

（5）

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

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

（6）

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

（7）

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

（8）

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

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

2.1? 引入地心地固坐標(biāo)系

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

2.2? 建立預(yù)測(cè)方程模型

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

（9）

各個(gè)分量的時(shí)間導(dǎo)數(shù)為：

（10）

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

（11）

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

（12）

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

（13）

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

（14）

（15）

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

（16）

2.3? 建立量測(cè)模型

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

（17）

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

（18）

非線性測(cè)量向量由GPS和磁力計(jì)測(cè)量值組成，定義如下：

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

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

在k采樣時(shí)刻，擴(kuò)展卡爾曼濾波具體計(jì)算方法[6]如下：

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

（20）

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

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

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

（22）

利用離散采樣時(shí)間Ts將方程（22）轉(zhuǎn)換到離散時(shí)域?yàn)椋?/p>

（23）

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

（24）

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

（25）

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

Step4：k+1時(shí)刻的狀態(tài)變量的先驗(yàn)估計(jì);

（26）

Step5：k+1時(shí)刻的誤差協(xié)方差的先驗(yàn)估計(jì);

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

Step6：k時(shí)刻卡爾曼濾波增益計(jì)算;

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時(shí)刻狀態(tài)量測(cè)變量的更新;

（29）

Step8：k+1時(shí)刻誤差協(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時(shí)刻返回step1，循環(huán)往復(fù)，直至飛行任務(wù)結(jié)束。

3? 仿真實(shí)驗(yàn)與分析

本文采用Matlab中的Simulink建立了固定翼無人機(jī)的控制系統(tǒng)模型，對(duì)某小型固定翼無人機(jī)進(jìn)行整機(jī)控制律設(shè)計(jì)，在此基礎(chǔ)上，對(duì)無人機(jī)位置、姿態(tài)進(jìn)行相應(yīng)的估計(jì)，驗(yàn)證本文所述的擴(kuò)展卡爾曼濾波算法。其中，無人機(jī)參數(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，量測(cè)數(shù)據(jù)通過捷聯(lián)傳感器獲得，傳感器模型如2.3節(jié)所示，控制器采用模型預(yù)測(cè)控制算法。

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

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

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

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

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

4? 結(jié)? 論

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

參考文獻(xiàn)：

[1] GREWAL M S，ANDREWS A P.卡爾曼濾波理論與實(shí)踐（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] 楊兆，沈作軍.基于擴(kuò)展卡爾曼濾波的小型固定翼無人機(jī)姿態(tài)估計(jì)方法分析 [J].航空科學(xué)技術(shù)，2017，28（11）：15-21.

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

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

[6] 石宇芃，馬宏軍，陳豹.基于四元數(shù)的四旋翼無人機(jī)擴(kuò)展卡爾曼濾波算法 [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.

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