趙嘉梁, 李世中, 趙紫良

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

0 引 言

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

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

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

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

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

(1)

(2)

(3)

(4)

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

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

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

2 EKF算法及改進(jìn)

2.1 EKF算法理論

為適應(yīng)卡爾曼濾波的實(shí)際應(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)

測(cè)量向量為z∈Rm滿足測(cè)量方程

Zk=h(xk,vk),

(6)

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

(7)

(8)

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

(9)

(10)

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

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

(11)

(12)

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

(13)

(14)

式中:εk和ηk表示均值為0的獨(dú)立隨機(jī)變量。

利用一次離散卡爾曼濾波算法, 對(duì)誤差估計(jì)矢量進(jìn)行后驗(yàn)估計(jì), 可得

(15)

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

(16)

(17)

(18)

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

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

(19)

把式(3)代入式(19)中, 不計(jì)噪聲項(xiàng), 展開得到

(20)

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

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

(21)

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

2.3 EKF算法改進(jìn)

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

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

(22)

R=α*R0,

(23)

(24)

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

3 實(shí)驗(yàn)結(jié)果及分析

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

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

3.1 靜態(tài)恒定實(shí)驗(yàn)

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

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

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

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

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

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

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

3.2 動(dòng)態(tài)變化實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 瞬變磁干擾實(shí)驗(yàn)

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

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

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

4 結(jié) 論

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

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

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

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