基于擴展卡爾曼濾波的無人機航姿算法

趙嘉梁, 李世中, 趙紫良

(中北大學(xué) 機電工程學(xué)院, 山西 太原 030051)

0 引 言

近年來, 無人機被廣泛應(yīng)用于航空偵察、立體地圖重構(gòu)、搜救等方面。其中, 姿態(tài)航向參考系統(tǒng)(Attitude and Heading Reference System, AHRS)解算的精度和速度關(guān)乎到無人機操控計算的穩(wěn)定性、可信度和實現(xiàn)的難度, 它可以提供載體俯仰角θ、橫滾角φ和航向角ψ的信息, 是無人機飛行控制的先決條件[1-2]。

國內(nèi)外已有不少學(xué)者在無人機航姿算法方面進行了大量研究, 文獻[3]提出了一種基于卡爾曼濾波求解姿態(tài)的方法, 它是將具有信噪比的多種信號加以集成的線性最小方差預(yù)測技術(shù), 在建模時需要考慮噪音的干擾, 然而在現(xiàn)實應(yīng)用中, 由于小型無人機航姿模型為非線性, 此時卡爾曼濾波算法對其的錯誤處理會使航姿解算準(zhǔn)確度下降, 甚至發(fā)散。文獻[4]中提出了四元數(shù)卡爾曼濾波器, 但未提供傳感器和硬件設(shè)計信息, 也未在工程上實現(xiàn)。文獻[5]提出的微機械捷聯(lián)式AHRS的四元數(shù)卡爾曼濾波器具有較高的靜態(tài)精度, 但未考慮加速度對航姿解算的影響, 具有一定的局限性。文獻[6-7]均采用了四元數(shù)卡爾曼濾波方法, 但四元數(shù)協(xié)方差陣出現(xiàn)奇異, 導(dǎo)致發(fā)散, 且此問題未被解決。

本文主要對小型無人機航姿算法進行研究, 提出的AEKF(Adaptive Extended Kalman Filter)算法可有效解決上述問題, 且經(jīng)三軸轉(zhuǎn)臺實驗測試結(jié)果表明此算法可以提高無人機的航姿解算精度。

1 航姿系統(tǒng)工作原理

首先采集陀螺儀角速度信息, 將信號進行預(yù)處理后更新姿態(tài)角[8-9], 然后利用加速度計解算俯仰角θ、橫滾角φ。

(1)

(2)

(3)

(4)

ψM為磁航向角,α為當(dāng)?shù)卮牌? 則載體航向角為ψ=ψM±α。

系統(tǒng)總體流程如圖1 所示。

圖1 航姿系統(tǒng)總體流程

2 EKF算法及改進

2.1 EKF算法理論

為適應(yīng)卡爾曼濾波的實際應(yīng)用需求, 處理非線性系統(tǒng)的濾波問題, EKF需要將當(dāng)前狀態(tài)量與協(xié)方差線性化[10-12]。非線性系統(tǒng)狀態(tài)向量為x∈Rn, 狀態(tài)方程[13]為

xk=f(xk-1,uk-1,wk-1)。

(5)

測量向量為z∈Rm滿足測量方程

Zk=h(xk,vk),

(6)

式中:xk和xk-1分別表示k和k-1時刻的狀態(tài)量;uk-1為外界輸入;h表示k時刻xk與Zk之間的關(guān)系;wk表示過程噪聲,vk表示測量噪聲, 令它們相互獨立且滿足正態(tài)分布:p(w)～N(0,Q),p(v)～N(0,R),Q、R為噪聲協(xié)方差, 假設(shè)它們恒定。實際過程中, 隨機變量wk和vk為未知量, 假定其數(shù)值為0, 則狀態(tài)向量和測量向量的估計值為

(7)

(8)

線性化狀態(tài)方程和測量方程[14]為

(9)

(10)

式中:A,W分別為f對x,w偏導(dǎo)的雅可比矩陣。H,V分別為h對x,v偏導(dǎo)的雅可比矩陣。

預(yù)估誤差與觀測誤差分別為

(11)

(12)

誤差的表達(dá)式可表示為

(13)

(14)

式中:εk和ηk表示均值為0的獨立隨機變量。

利用一次離散卡爾曼濾波算法, 對誤差估計矢量進行后驗估計, 可得

(15)

則卡爾曼濾波的表達(dá)式為

(16)

(17)

(18)

2.2 EKF模型的構(gòu)建

構(gòu)建無人機航姿系統(tǒng)的EKF數(shù)學(xué)模型, 其狀態(tài)方程[15]為

(19)

把式(3)代入式(19)中, 不計噪聲項, 展開得到

(20)

通過式(1), 式(2)和式(4), 從加速度計和磁傳感器的數(shù)據(jù)中計算出量測向量

Z(k)=X(k)+v(k),

(21)

式中:v(k)表示量測噪聲。根據(jù)式(9)和式(10), 取定初始值并優(yōu)化相應(yīng)濾波參數(shù), 按式(17)和式(18)進行迭代運算。

2.3 EKF算法改進

當(dāng)實際應(yīng)用環(huán)境中存在瞬變磁干擾時, 量測航向誤差較大, 卡爾曼濾波無法迅速減少這種誤差, 導(dǎo)致濾波發(fā)散[16-17]。自適應(yīng)EKF算法(AEKF)主要用于解決此類問題, 其基本原理如下:

具體用以下公式表達(dá)

(22)

R=α*R0,

(23)

(24)

式中:R為觀測噪聲矩陣,R0為矩陣初始值。

3 實驗結(jié)果及分析

實驗使用的主控芯片采用STM32F103, MEMS傳感器為GY953, 其內(nèi)部集成了加速度計、陀螺儀及磁力計, 精度高且穩(wěn)定性好。

本文分別通過靜態(tài)恒定、動態(tài)變化及瞬變磁干擾3種實驗, 驗證EKF及其改進算法的性能。

3.1 靜態(tài)恒定實驗

將無人機航姿系統(tǒng)置于轉(zhuǎn)臺, 對轉(zhuǎn)臺3個軸的角度進行調(diào)節(jié), 并對θ,φ,ψ3個姿態(tài)角誤差依次進行評估, 具體如下:

1)將轉(zhuǎn)臺維持為θ與φ示值相同的狀態(tài), 當(dāng)轉(zhuǎn)臺處于-60°,-30°, 0°, 30°和 60°時保持靜態(tài)恒定, 得到θ和φ的誤差如圖2 所示。

圖2 靜態(tài)恒定時的俯仰角和橫滾角誤差

從圖2 可以看出, 不同傾角時, 俯仰角θ和橫滾角φ的誤差均不超過0.25°。

2)變化θ與φ, 將輸出信號每隔30°左右記錄一次, 得到誤差如圖3 所示。

圖3 靜態(tài)恒定時的航向角誤差

從圖3 可以看出, 當(dāng)傾角增大時, 航向角ψ的誤差也變大, 誤差最大不超過0.5°。

3.2 動態(tài)變化實驗

旋轉(zhuǎn)轉(zhuǎn)臺某軸, 使θ,φ,ψ中的兩個角保持恒定, 另外的一個角為動態(tài)變化。采集這兩個恒定角的姿態(tài)信息輸出, 與對應(yīng)的轉(zhuǎn)臺示值進行比較, 得出不同動態(tài)條件下的航姿誤差。分別改變θ,φ和ψ, 利用3組試驗結(jié)果來評估系統(tǒng)動態(tài)性能。

1)保持ψ和φ不變, 改變θ, 使其保持動態(tài)變化。記錄ψ,φ輸出和轉(zhuǎn)臺示值, 結(jié)果如圖4 和圖5 所示。圖中, 恒定未變的參考值為轉(zhuǎn)臺示值, 值隨時間變化的曲線中, 幅度較大的為量測值, 未融合傳感器數(shù)據(jù), 幅度較小的為EKF結(jié)果。

圖4 俯仰角動態(tài)變化時的航向角濾波效果

圖5 俯仰角動態(tài)變化時的橫滾角濾波效果

由圖4 和圖5 可以看出, 量測值誤差較大,ψ最大誤差約為 2.5°,φ誤差小于1.25°; EKF融合傳感器數(shù)據(jù)后,ψ誤差不超過 1.5°,φ誤差不超過0.5°。

2)保持ψ和θ不變, 改變φ, 使其保持動態(tài)變化。記錄ψ,θ的輸出和轉(zhuǎn)臺示值, 結(jié)果如圖6 和圖7 所示。

圖6 橫滾角動態(tài)變化時的航向角濾波效果

圖7 橫滾角動態(tài)變化時的俯仰角濾波效果

由圖6 和圖7 可知,φ處于動態(tài)變化時, 量測值誤差較大,ψ最大誤差約為 7°,θ誤差小于1°; EKF融合后,ψ最大誤差降低到約2°,θ誤差不超過0.25°。

3)保持φ和θ不變, 改變ψ, 使其動態(tài)變化。記錄系統(tǒng)的φ和θ的輸出和轉(zhuǎn)臺示值, 結(jié)果如圖8 和圖9。

圖8 航向角動態(tài)變化時的俯仰角濾波效果

圖9 航向角動態(tài)變化時的橫滾角濾波效果

由圖8 和圖9 可以看出,ψ動態(tài)變化的情況下, 量測θ和φ的誤差最大值均約為2.5°, EKF融合傳感器數(shù)據(jù)后最大誤差均降低為約0.5°。

系統(tǒng)的3種動態(tài)試驗結(jié)果表明:

1)當(dāng)θ動態(tài)變化時,ψ誤差為±1.5°,φ誤差為±0.5°。

2)當(dāng)φ動態(tài)變化時,ψ誤差為±2°,θ誤差為±0.25°。

3)當(dāng)ψ動態(tài)變化時,θ和φ誤差均為±0.5°。結(jié)果表明, EKF 算法在不同動態(tài)變化情況時都能獲得良好的濾波效果。

3.3 瞬變磁干擾實驗

因為以上試驗中沒有周圍環(huán)境的磁干擾, 所以EKF與AEKF的結(jié)果一致。為檢驗AEKF 算法在磁干擾環(huán)境下的性能, 進行如下試驗: 將航姿系統(tǒng)置于轉(zhuǎn)臺, 因為僅航向角會受磁干擾的影響, 故只記錄航向角讀數(shù)。令轉(zhuǎn)臺保持靜止, 手持一小塊磁石反復(fù)移動, 持續(xù)數(shù)秒, 并對最終結(jié)果進行評估。圖10 中, 恒定不變的為參考值, 曲線幅度變化最大的為量測值, 磁干擾時段航向角量測值出現(xiàn)的誤差大至約80°; 幅度變化次之的為EKF算法情況下的濾波效果, 最大誤差達(dá)到約70°, 對磁干擾的抑制效果有限; 變化幅度最小的為AEKF, 姿態(tài)和真實值之間的偏差在不同的時間段都很小, 被限制到約5°內(nèi)。

圖10 瞬變磁干擾影響下的AEKF算法濾波效果

由此可見, AEKF 算法下的輸出誤差受限制性能良好, 且在磁干擾消失時也能很快收斂, 具有高可靠性和良好的抗磁干擾性能。

4 結(jié) 論

本文設(shè)計的微型無人機航姿系統(tǒng), 利用EKF方法進行了數(shù)據(jù)融合, 通過靜態(tài)、動態(tài)和磁干擾試驗, 得出如下結(jié)論:

1)EKF 算法融合傳感器數(shù)據(jù)后, 靜態(tài)誤差不超過0.5°, 動態(tài)誤差不超過2°, 有效地提高了姿態(tài)角解算精度。

2)對EKF算法進行自適應(yīng)改進, 對加速度量測噪聲方差進行處理, 瞬變磁干擾最大誤差從約70°下降到5°, 極大地提高了抗磁干擾能力。

3)系統(tǒng)能滿足小型無人機的常規(guī)飛行控制需求, 適應(yīng)應(yīng)用環(huán)境, 具有較為廣闊的發(fā)展空間。