基于擴(kuò)展卡爾曼濾波的固定翼無(wú)人機(jī)姿態(tài)解算方法

弋英民,王柯穎,苑易偉,薛向宏,李余興,劉柏均,王燁琛

1(西安理工大學(xué) 自動(dòng)化與信息工程學(xué)院,西安 710048)

2(西安理工大學(xué) 陜西省工業(yè)圖傳與控制應(yīng)用校企聯(lián)合研究中心,西安 710048)

1 引 言

姿態(tài)解算通常是利用數(shù)據(jù)融合算法標(biāo)記傳感器有用信息,按照坐標(biāo)系轉(zhuǎn)換關(guān)系計(jì)算姿態(tài)信息,進(jìn)而進(jìn)行組合導(dǎo)航的.精確、穩(wěn)定的姿態(tài)信息在姿態(tài)控制[1]、目標(biāo)跟蹤[2]和組合導(dǎo)航[3]等眾多領(lǐng)域都發(fā)揮著至關(guān)重要的作用.固定翼無(wú)人機(jī)(Unmanned Aerial Vehicle,UAV)憑借飛行速度快,航程距離遠(yuǎn),運(yùn)載能力大的特點(diǎn),在邊境巡邏、戰(zhàn)術(shù)偵察、災(zāi)情監(jiān)視等領(lǐng)域也發(fā)揮著重要作用.

固定翼無(wú)人機(jī)飛控系統(tǒng)中的慣性測(cè)量單元(Inertial Measurement Unit,IMU)是以多軸組合的傳感器單元,將傳感器的輸出值通過(guò)濾波算法進(jìn)行數(shù)據(jù)融合,可以抑制姿態(tài)誤差的累計(jì),消除噪聲干擾,從而獲得正確的姿態(tài)角.目前在固定翼無(wú)人機(jī)上采用的姿態(tài)解算方法,普遍使用的有卡爾曼濾波器[4]、Mahony互補(bǔ)濾波器[5]和Madgwick梯度下降法[6].本文在擴(kuò)展卡爾曼濾波算法的基礎(chǔ)上,結(jié)合互補(bǔ)濾波器和動(dòng)態(tài)梯度下降法,用于解算固定翼無(wú)人機(jī)姿態(tài)角.

卡爾曼濾波器(Kalman Filter,KF)廣泛用于數(shù)據(jù)融合和狀態(tài)估計(jì),它遵循兩個(gè)通用步驟:預(yù)測(cè)和更新[7].通過(guò)調(diào)節(jié)卡爾曼增益來(lái)最小化誤差,使得估計(jì)值接近真實(shí)值.文獻(xiàn)[8,9]將線性卡爾曼濾波器用于MARG(Magnetic,Angular Rate and Gravity)傳感器方位估計(jì),利用3個(gè)傳感器構(gòu)成兩層結(jié)構(gòu)的Kalman濾波器,最后和Madgwick算法進(jìn)行了對(duì)比驗(yàn)證,根據(jù)靜態(tài)和動(dòng)態(tài)均方根誤差值顯示,Kalman濾波器表現(xiàn)出的性能更加準(zhǔn)確和穩(wěn)定;文獻(xiàn)[10]在考慮到外部加速度的動(dòng)態(tài)條件下設(shè)計(jì)了基于卡爾曼濾波的姿態(tài)估計(jì)算法,該算法能在短期內(nèi)快速動(dòng)態(tài)條件下表現(xiàn)出較高的精度,但一旦存在長(zhǎng)時(shí)間的高外部加速度,該算法的優(yōu)勢(shì)逐漸降低;文獻(xiàn)[11]針對(duì)MARG傳感器系統(tǒng)的姿態(tài)估計(jì)誤差問(wèn)題,采用BP神經(jīng)網(wǎng)絡(luò)嘗試補(bǔ)償姿態(tài)誤差,主要通過(guò)訓(xùn)練傳感器數(shù)據(jù),以補(bǔ)償卡爾曼濾波估計(jì)的姿態(tài),實(shí)驗(yàn)結(jié)果證明融合了神經(jīng)網(wǎng)絡(luò)的KF算法角度誤差有效減小,但改善效果有限.

擴(kuò)展卡爾曼濾波器(Extended Kalman Filter,EKF)是指對(duì)非線性系統(tǒng)進(jìn)行了線性化的狀態(tài)觀測(cè).其動(dòng)態(tài)性能良好,僅依靠參數(shù)矩陣進(jìn)行解算,不影響估計(jì)精度.文獻(xiàn)[12]基于擴(kuò)展卡爾曼濾波理論,選擇陀螺儀的姿態(tài)四元數(shù)和漂移偏差構(gòu)造狀態(tài)方程,利用正交化方法從加速度計(jì)和磁強(qiáng)計(jì)的輸出中獲取姿態(tài)四元數(shù)作為測(cè)量向量,該EKF算法模型能夠準(zhǔn)確表示無(wú)人機(jī)在動(dòng)、靜態(tài)條件下的姿態(tài);文獻(xiàn)[13]采用雙擴(kuò)展卡爾曼濾波(DEKF)并行地對(duì)飛行器的數(shù)據(jù)進(jìn)行預(yù)測(cè)和更新,第一個(gè)EKF負(fù)責(zé)估計(jì)俯仰和橫滾角信息,第2個(gè)EKF用于估計(jì)航向信息,能夠限制磁干擾對(duì)偏航估計(jì)的影響;文獻(xiàn)[14]以姿態(tài)偏差、陀螺儀偏差以及運(yùn)動(dòng)加速度構(gòu)造狀態(tài)向量,采用誤差擴(kuò)展卡爾曼濾波,即乘法EKF,該方法具有一定的可行性和可靠性,但誤差模型的建立依舊比較理想.以上利用EKF算法及其改進(jìn)方法減小線性化誤差有限,并沒(méi)有解決無(wú)人機(jī)在面臨高機(jī)動(dòng)情況下穩(wěn)定性較差的問(wèn)題.

互補(bǔ)濾波器(Complementary Filter,CF)通常起到數(shù)據(jù)融合和的作用,它以計(jì)算量小的顯著優(yōu)勢(shì),成為了姿態(tài)角估計(jì)方面最廣泛的濾波器之一.文獻(xiàn)[15,16]采用互補(bǔ)濾波器的融合方法來(lái)求解姿態(tài)角,該方法能顯著提高解算速度,降低處理器精度;文獻(xiàn)[17,18]分別基于FPGA平臺(tái)和IMU單元設(shè)計(jì)了一種改進(jìn)型互補(bǔ)濾波算法,滿足了無(wú)人機(jī)姿態(tài)解算的精確度和實(shí)時(shí)性需求;文獻(xiàn)[19]提出了一種互補(bǔ)濾波器級(jí)聯(lián)結(jié)構(gòu),分別采用非線性結(jié)構(gòu)修正陀螺儀偏差、線性結(jié)構(gòu)估計(jì)姿態(tài)角.但是互補(bǔ)濾波器在高頻端截止頻率附近衰減較慢,導(dǎo)致處理后的數(shù)據(jù)存在誤差,精確度較低,飛控系統(tǒng)出現(xiàn)滯后現(xiàn)象.

梯度下降算法是四元數(shù)誤差沿著負(fù)梯度方向,通過(guò)不斷迭代誤差函數(shù),直到誤差值小于某一閾值,從而得到該時(shí)刻的最佳估計(jì)值.盧等人在四旋翼飛行器上分別比較了梯度下降和互補(bǔ)濾波融合算法與自適應(yīng)互補(bǔ)濾波算法,最終自適應(yīng)互補(bǔ)濾波算法試驗(yàn)效果更佳[20];文獻(xiàn)[21]在四元數(shù)的EKF姿態(tài)估計(jì)基礎(chǔ)上,利用動(dòng)態(tài)步長(zhǎng)梯度下降算法對(duì)加速度數(shù)據(jù)進(jìn)行補(bǔ)償,對(duì)GPS測(cè)量數(shù)據(jù)的磁畸變進(jìn)行消除,在快速運(yùn)動(dòng)情況下提高了姿態(tài)解算的精度.雖然梯度下降法一定程度上可以消除噪聲干擾,但需要設(shè)計(jì)一個(gè)合適的步長(zhǎng),太大會(huì)導(dǎo)致求解的姿態(tài)角度發(fā)散,太小沒(méi)有收斂效果.

本文提出一種基于擴(kuò)展卡爾曼濾波的固定翼無(wú)人機(jī)姿態(tài)解算方法,在姿態(tài)角保持穩(wěn)定時(shí)采用標(biāo)準(zhǔn)EKF進(jìn)行解算,姿態(tài)角發(fā)生劇烈變化時(shí)采用改進(jìn)EKF方法進(jìn)行解算.改進(jìn)方法在EKF的基礎(chǔ)上加入互補(bǔ)濾波器和動(dòng)態(tài)梯度下降法,分別作用于陀螺儀和加速度計(jì)兩個(gè)傳感器,可有效減小EKF算法線性化誤差,解算結(jié)果穩(wěn)定性更高.首先利用小波包分解原理對(duì)原始數(shù)據(jù)進(jìn)行去噪,然后用改進(jìn)互補(bǔ)濾波之后的陀螺儀測(cè)量數(shù)據(jù)求得初始姿態(tài)作為狀態(tài)變量.用加速度計(jì)測(cè)量數(shù)據(jù)作為量測(cè)變量,用動(dòng)態(tài)梯度下降法對(duì)運(yùn)動(dòng)加速度加以抑制,給定雅克比矩陣,經(jīng)EKF方程組迭代計(jì)算得到當(dāng)前時(shí)刻的姿態(tài)角四元數(shù).該方法減少了傳統(tǒng)方法各自使用時(shí)的局限性,提高了解算精確度和穩(wěn)定性.

2 系統(tǒng)模型

2.1 姿態(tài)描述與歐拉角轉(zhuǎn)換

機(jī)載NED坐標(biāo)系(North-East-Down Coordinate System):通常以地心作為坐標(biāo)的原點(diǎn),N表示地球北向、E表示地球東向、D表示地球地向,故又稱作是北東地坐標(biāo)系,該坐標(biāo)系我們用n系表示.

機(jī)體軸坐標(biāo)系(Body Frame):它是無(wú)人機(jī)慣性導(dǎo)航的基準(zhǔn)坐標(biāo)系,其原點(diǎn)O取在無(wú)人機(jī)的重心位置,X軸指向機(jī)身前進(jìn)方向,Z軸垂直于X軸向下而Y軸與X、Z軸構(gòu)成右手系,由此指向機(jī)身右側(cè),故又稱作是前右下坐標(biāo)系,該坐標(biāo)系我們用b系表示.機(jī)載NED坐標(biāo)系與機(jī)體軸坐標(biāo)系的組合關(guān)系如圖1所示.

圖1 機(jī)載NED坐標(biāo)系與機(jī)體軸坐標(biāo)系的組合關(guān)系Fig.1 Combination between airborne NED coordinate system and airframe axis coordinate system

(1)

姿態(tài)角是無(wú)人機(jī)在機(jī)體軸坐標(biāo)系中參考機(jī)載NED坐標(biāo)系繞固定點(diǎn)旋轉(zhuǎn)三次完成轉(zhuǎn)換的,通常按照Z(yǔ)、Y、X軸的順序產(chǎn)生歐拉角,由此可得Z軸偏航角yaw(記為ψ),Y軸俯仰角pitch(記為θ),以及X軸滾轉(zhuǎn)角roll(記為φ)在三維空間中所對(duì)應(yīng)的旋轉(zhuǎn)矩陣分別為:

(2)

q=q0+q1i+q2j+q3k

(3)

其中,q0,q1,q2,q3是實(shí)數(shù),i,j,k表示虛數(shù)單位.

(4)

(5)

2.2 擴(kuò)展卡爾曼濾波算法

EKF姿態(tài)解算部分主要分為兩個(gè)過(guò)程,預(yù)測(cè)過(guò)程和測(cè)量過(guò)程.對(duì)應(yīng)公式為式(6)和式(7):

(6)

(7)

在時(shí)間預(yù)測(cè)過(guò)程中,將得到的陀螺儀三軸角速度矢量,以四元數(shù)形式通過(guò)一階龍格庫(kù)塔法進(jìn)行首次估計(jì),即:

(8)

(9)

其中wk是近似為白高斯噪聲的狀態(tài)噪聲,狀態(tài)轉(zhuǎn)移矩陣Φ(bw,Δt)是用零階積分計(jì)算的:

(10)

在狀態(tài)更新過(guò)程中,首先對(duì)于一個(gè)三軸加速度計(jì),把任意時(shí)刻測(cè)得的加速度矢量,作為觀測(cè)變量,記作:

z(k)=[abx(k)aby(k)abz(k)]T+vk

(11)

式中abx(k)、aby(k)、abz(k)是在載體坐標(biāo)系中重力加速度分別對(duì)應(yīng)x、y、z軸的方向分量,vk是近似為白高斯噪聲的測(cè)量噪聲.

接著,根據(jù)旋轉(zhuǎn)矩陣進(jìn)行坐標(biāo)轉(zhuǎn)換得到如式(12)所示:

(12)

則系統(tǒng)的觀測(cè)方程可以表示為式(13)所示:

(13)

根據(jù)式(13),h(x(k),k)為非線性函數(shù),需要通過(guò)泰勒級(jí)數(shù)展開(kāi)將非線性函數(shù)在當(dāng)前估計(jì)狀態(tài)的平均值附近線性化,求得雅克比矩陣為:

(14)

則量測(cè)過(guò)程:

(15)

最后為了保持四元數(shù)單位范數(shù)的性質(zhì),在時(shí)間預(yù)測(cè)和狀態(tài)更新階段結(jié)束時(shí),都要采用歸一化步驟對(duì)姿態(tài)估計(jì)值進(jìn)行歸一化處理.

3 改進(jìn)擴(kuò)展卡爾曼濾波的姿態(tài)解算方法

擴(kuò)展卡爾曼濾波在預(yù)測(cè)參數(shù)時(shí)要求模型具有先驗(yàn)性與已知性,然而在工程實(shí)踐中通常難以獲得準(zhǔn)確的數(shù)學(xué)模型,且非線性系統(tǒng)在線性化的同時(shí),也會(huì)帶來(lái)線性化誤差,因此對(duì)非線性依賴嚴(yán)重的模糊系統(tǒng),求解參數(shù)的效果并不明顯.應(yīng)用互補(bǔ)濾波可以有效減輕溫漂誤差,但在進(jìn)行歐拉角與四元數(shù)的轉(zhuǎn)換以及PI積分時(shí),將角度誤差帶入到解算的歐拉角中,導(dǎo)致積分出的四元數(shù)項(xiàng)精度不夠,因此誤差累積也始終影響著互補(bǔ)濾波算法的解算結(jié)果.梯度下降法和互補(bǔ)濾波算法都是通過(guò)向量叉乘并求導(dǎo)得到誤差向量進(jìn)行補(bǔ)償,一般適用于低頻低速運(yùn)動(dòng),但如果是強(qiáng)機(jī)動(dòng)、高速飛行的環(huán)境,就需要和其他算法進(jìn)行結(jié)合以提高解算精度.

針對(duì)3種傳統(tǒng)濾波算法各自使用時(shí)的局限性,且為了提高固定翼無(wú)人機(jī)的環(huán)境適應(yīng)能力和解算過(guò)程的穩(wěn)定性,本文在EKF算法的基礎(chǔ)上,加入動(dòng)態(tài)梯度算法和互補(bǔ)濾波算法,用于無(wú)人機(jī)慣性測(cè)量單元陀螺儀和加速度計(jì)兩個(gè)傳感器的數(shù)據(jù)融合與姿態(tài)解算.

3.1 陀螺儀的角速度補(bǔ)償

慣導(dǎo)單元IMU傳感器在不同頻率范圍內(nèi)有不同的動(dòng)態(tài)響應(yīng)優(yōu)勢(shì),由于陀螺儀與加速度計(jì)又具有頻率互補(bǔ)性,因此可以結(jié)合陀螺儀和加速度計(jì)的性能優(yōu)勢(shì),采用互補(bǔ)濾波算法結(jié)合不同頻的輸出信息,使得表征結(jié)果更加貼合實(shí)際值.主要步驟如下:

1)計(jì)算誤差

(16)

設(shè)加速度計(jì)3個(gè)軸的值分別為ax,ay,az,對(duì)其分別進(jìn)行求模和歸一化處理,如下式所示.

對(duì)求得的gb、ab經(jīng)過(guò)矢量乘積即可得到互補(bǔ)濾波的角速度校正值e,如式(17)所示:

(17)

2)PI誤差補(bǔ)償

由于陀螺儀傳感器在使用過(guò)程當(dāng)中先天性具有受溫度影響數(shù)據(jù)逐漸漂移的不足,因此使用PI控制器(Proportional Integral controller)進(jìn)行誤差補(bǔ)償,可降低傳感器輸出數(shù)據(jù)受自身性能的影響.控制的效果取決于比例可調(diào)增益P和積分增益I這兩個(gè)參數(shù),PI控制器濾波公式如下:

(18)

(19)

3.2 運(yùn)動(dòng)加速度的抑制處理

梯度下降法可以表示為:

xn+1=xn-μ×▽F(xn)

(20)

式中:x表示運(yùn)算前后自變量的值,▽F(xn)表示目標(biāo)函數(shù)的梯度,下標(biāo)n表示搜索次數(shù),負(fù)號(hào)表示沿著梯度的負(fù)方向逼近極點(diǎn),即搜索方向,μ表示在搜索方向上的步長(zhǎng).

設(shè)加速度計(jì)歸一化后在機(jī)體軸坐標(biāo)系中為ab=[axayaz]T,當(dāng)重力向量在機(jī)載NED坐標(biāo)系中為gn=[0 0 1]T時(shí),理論重力加速度與加速度計(jì)測(cè)得值相減得到的誤差函數(shù)用四元數(shù)表示法為:

(21)

(22)

將式(9)簡(jiǎn)單定義為迭代公式:

(23)

則根據(jù)梯度下降法的迭代過(guò)程式(9)可演變?yōu)?

(24)

(25)

其中,α>1,w為陀螺儀的測(cè)量角速度;Ts為系統(tǒng)的采樣時(shí)間,均為正相關(guān).為了增強(qiáng)四元數(shù)估計(jì)的動(dòng)態(tài)跟蹤特性,設(shè)置與物理方向速率相關(guān)的自適應(yīng)步長(zhǎng),如式所示:

(26)

另外,為了降低運(yùn)動(dòng)加速度對(duì)高速狀態(tài)下的飛行器的干擾.定義δk為加速度計(jì)測(cè)量值的可信因子:

(27)

(28)

將以上迭代公式應(yīng)用于狀態(tài)向量姿態(tài)四元數(shù)的估計(jì)中,可以避免線性化誤差,提高估計(jì)精度.

3.3 強(qiáng)機(jī)動(dòng)高速運(yùn)動(dòng)的干擾消除辦法

在無(wú)人機(jī)的高速、高空運(yùn)動(dòng)的作業(yè)環(huán)境下,需要面臨強(qiáng)干擾、高運(yùn)動(dòng)加速度的惡劣問(wèn)題,為了保證飛行的穩(wěn)定性和解算的精確性,并在此基礎(chǔ)上盡量降低處理器計(jì)算的復(fù)雜程度,本文提出一種根據(jù)閾值進(jìn)行算法動(dòng)態(tài)切換的解算方法.

由于EKF算法是基于前一時(shí)刻的姿態(tài)估計(jì)值進(jìn)行更新迭代的,那么當(dāng)飛行姿態(tài)保持穩(wěn)定不變時(shí),標(biāo)準(zhǔn)EKF足以解算出穩(wěn)定姿態(tài)角;當(dāng)飛行姿態(tài)發(fā)生劇烈變化時(shí),由于EKF算法本身的遞歸特性,再加上微分過(guò)程帶來(lái)的線性化誤差問(wèn)題,本文提出一種基于閾值法優(yōu)化改進(jìn)擴(kuò)展卡爾曼濾波的固定翼無(wú)人機(jī)姿態(tài)解算方法,改進(jìn)方法在EKF的基礎(chǔ)上加入互補(bǔ)濾波器和動(dòng)態(tài)梯度下降法,分別作用于陀螺儀和加速度計(jì)兩個(gè)傳感器,可有效減小EKF算法線性化誤差,提高解算過(guò)程穩(wěn)定性和環(huán)境適應(yīng)能力.

在EKF算法解算姿態(tài)角的過(guò)程中,姿態(tài)角的更新主要是通過(guò)四元數(shù)微分方程的求解進(jìn)行的,而姿態(tài)角的劇烈變化會(huì)直接體現(xiàn)在陀螺儀的輸出角速度上,不穩(wěn)定、不準(zhǔn)確的角速度會(huì)導(dǎo)致求解的姿態(tài)角誤差增大.因此本文以陀螺儀的輸出角速度變化值為判定姿態(tài)角劇烈變化的閾值,判斷式為式(29).當(dāng)判定大于某一閾值a時(shí),認(rèn)為此刻姿態(tài)角發(fā)生了變化,需要使用改進(jìn)EKF算法進(jìn)行姿態(tài)解算,否則使用標(biāo)準(zhǔn)EKF算法進(jìn)行姿態(tài)解算.

(29)

最終陀螺儀經(jīng)過(guò)了預(yù)測(cè)過(guò)程,加速度計(jì)經(jīng)過(guò)了測(cè)量過(guò)程,將兩者輸出的結(jié)果通過(guò)迭代合并得到了三軸姿態(tài)角的四元數(shù)形式,并通過(guò)歐拉角反解公式求解出三軸姿態(tài)角.公式如下:

(30)

3.4 姿態(tài)解算的整體流程

本文在EKF的基礎(chǔ)上,加入PI補(bǔ)償?shù)幕パa(bǔ)濾波算法和動(dòng)態(tài)步長(zhǎng)的梯度下降法,整體姿態(tài)解算流程圖如圖2所示.算法步驟為:

圖2 姿態(tài)解算整體流程Fig.2 Flowchart of posture solution

1)采集IMU慣導(dǎo)單元陀螺儀、加速度計(jì)傳感器原始數(shù)據(jù),并對(duì)原始數(shù)據(jù)進(jìn)行小波包降噪;

2)如果判定角速度w1變化值小于閾值a,那么將陀螺儀輸出值計(jì)算的姿態(tài)四元數(shù)作為初始狀態(tài)向量,將加速度計(jì)輸出加速度作為量測(cè)向量,使用標(biāo)準(zhǔn)EKF算法實(shí)現(xiàn)估計(jì)角度和測(cè)量角度的融合,計(jì)算出姿態(tài)四元數(shù)和協(xié)方差矩陣;

3)如果判定角速度w1變化值大于閾值a,那么使用動(dòng)態(tài)步長(zhǎng)的梯度下降法求解加速度計(jì)輸出的誤差函數(shù),并補(bǔ)償給姿態(tài)四元數(shù),使用PI補(bǔ)償?shù)幕パa(bǔ)濾波算法求解陀螺儀輸出角速度的誤差向量,并以補(bǔ)償后的姿態(tài)四元數(shù)和輸出加速度作為狀態(tài)向量和量測(cè)向量,使用EKF算法解算得到最優(yōu)估計(jì)角度;

4)把得到的最優(yōu)估計(jì)角度作為下一次迭代的初始角度,并不斷利用四元數(shù)微分方程進(jìn)行姿態(tài)角的更新,最終得到一組穩(wěn)定的最優(yōu)估計(jì)四元數(shù)角度;

5)通過(guò)反解歐拉角公式最終得到一組最優(yōu)估計(jì)歐拉角度.

4 測(cè)試過(guò)程及結(jié)果分析

本文主要采集傳感器ADIS16488A三軸陀螺儀和加速度計(jì)的數(shù)據(jù),數(shù)據(jù)集共計(jì)六組,靜態(tài)測(cè)試每組13558個(gè),動(dòng)態(tài)測(cè)試每組15636個(gè),頻率為100Hz,即采樣時(shí)間為10ms,每采集一次就進(jìn)行一次數(shù)據(jù)融合和解算.解算算法中使用到的參數(shù)如表1所示.

表1 實(shí)驗(yàn)參數(shù)Table 1 Experiment parameter

為了驗(yàn)證本文采用的閾值法優(yōu)化改進(jìn)擴(kuò)展卡爾曼濾波算法的穩(wěn)定性和準(zhǔn)確性,在進(jìn)行測(cè)試的同時(shí),對(duì)標(biāo)準(zhǔn)的Mahony濾波器、EKF算法以及文獻(xiàn)[18]和文獻(xiàn)[21]中提出的EKF和Mahony濾波器融合算法(Mahony_EKF)、EKF和動(dòng)態(tài)梯度下降法相結(jié)合的算法(DGDA_EKF)進(jìn)行了算法驗(yàn)證,同本文中提出的改進(jìn)EKF算法進(jìn)行了性能比較.

4.1 靜態(tài)測(cè)試

在無(wú)人機(jī)處于靜置狀態(tài)時(shí),使用5種姿態(tài)解算算法對(duì)三軸姿態(tài)角進(jìn)行了估計(jì),如圖3所示.每幅圖中從上往下依次是Mahony濾波器、標(biāo)準(zhǔn)EKF、Mahony_EKF算法、DGDA_EKF算法以及本文提出的改進(jìn)EKF算法的解算結(jié)果,參考值均為0°附近.圖3(a)X軸解算的橫滾角在-0.01°波動(dòng),圖3(b)Y軸解算的俯仰角在-0.1°波動(dòng),圖3(c)Z軸解算的航向角在-1°～1°范圍波動(dòng).

圖3 靜態(tài)測(cè)試三軸姿態(tài)解算結(jié)果圖Fig.3 Results of triaxial attitude calculation in static test

與標(biāo)準(zhǔn)EKF算法相比,單獨(dú)使用Mahony濾波器,三軸解算角度均有一定的漂移誤差,航向角達(dá)到最大誤差2°.而在EKF 的基礎(chǔ)上所進(jìn)行的融合算法中,本文提出的改進(jìn)EKF算法解算橫滾角誤差能保持在0.005°左右,航向角在其他解算算法均解算漂移的情況下誤差保持在0.6°左右.

圖4給出了靜態(tài)測(cè)試下5種解算方法的后驗(yàn)均方根誤差對(duì)比結(jié)果,實(shí)驗(yàn)結(jié)果證明,在5種不同的姿態(tài)解算方法中,單獨(dú)使用Mahony濾波器和EKF算法靜態(tài)誤差較大,在EKF算法的基礎(chǔ)上,加入互補(bǔ)濾波器和梯度下降法的融合算法中,本文提出的基于閾值法改進(jìn)EKF解算算法后驗(yàn)均方根誤差最小,準(zhǔn)確度最高.

圖4 靜態(tài)測(cè)試解算方法誤差對(duì)比圖Fig.4 Error comparison results of the solution methods in static test

4.2 動(dòng)態(tài)測(cè)試

動(dòng)態(tài)實(shí)驗(yàn)是在室外對(duì)固定翼無(wú)人機(jī)進(jìn)行了長(zhǎng)達(dá)10min的試飛實(shí)驗(yàn),飛行過(guò)程解算姿態(tài)角結(jié)果如圖5(a)、圖5(b)、圖5(c)所示.無(wú)人機(jī)一開(kāi)始保持穩(wěn)定在0°,并分別對(duì)俯仰角、橫滾角和航向角進(jìn)行了單獨(dú)測(cè)試,驗(yàn)證了姿態(tài)變化的有效性.在大約過(guò)了5min,迭代265次之后,無(wú)人機(jī)在橫滾角的變化上較為明顯,此時(shí)橫滾角和航向角大約為20°,俯仰角依舊在0°振蕩.無(wú)人機(jī)在變換完姿態(tài)后,保持姿態(tài)角度飛行了一段時(shí)間,在迭代次數(shù)達(dá)到400之后,恢復(fù)為原始角度并最終完成了此次飛行.

圖5 動(dòng)態(tài)測(cè)試三軸姿態(tài)解算結(jié)果圖Fig.5 Results of triaxial attitude calculation in dynamic test

圖5(a)和圖5(b)分別表示慣導(dǎo)單元X軸和Y軸解算的姿態(tài)角結(jié)果,從圖中的解算結(jié)果來(lái)看,本文提出的改進(jìn)EKF算法解算過(guò)程“毛刺”較少,更加平滑,特別是在無(wú)人機(jī)最后飛行階段,其他4種解算方法均出現(xiàn)了角度振蕩,而改進(jìn)EKF算法能夠保持在較為平穩(wěn)的狀態(tài),說(shuō)明本文提出的改進(jìn)EKF算法能夠有效降低慣導(dǎo)單元傳感器的噪聲影響和環(huán)境干擾,提高解算過(guò)程的穩(wěn)定性.圖5(c)是Z軸解算的航向角結(jié)果,在其他4種算法解算角度一定程度上有所漂移的同時(shí),本文提出的改進(jìn)EKF算法解算穩(wěn)定,能夠抑制角度漂移,表現(xiàn)優(yōu)越.

相較于單獨(dú)使用Mahony濾波器,僅使用EKF算法,三軸姿態(tài)角不僅沒(méi)有逐漸漂移,而且能較好的反映真實(shí)姿態(tài)角,但震蕩較多,特別是在角度有一定變化或即將變化的時(shí)間點(diǎn),穩(wěn)定性不強(qiáng),說(shuō)明解算過(guò)程受環(huán)境因素、傳感器自身噪聲等影響很大.在EKF 的基礎(chǔ)上分別結(jié)合了Mahony濾波器和動(dòng)態(tài)梯度下降法之后,穩(wěn)定性有所增強(qiáng),有效改善了單一算法解算姿態(tài)角穩(wěn)定性差的問(wèn)題.而本文提出以擴(kuò)展卡爾曼濾波器為核心解算固定翼無(wú)人機(jī)姿態(tài)角,在此基礎(chǔ)上加入了PI補(bǔ)償?shù)幕パa(bǔ)濾波算法和動(dòng)態(tài)梯度下降法,根據(jù)閾值動(dòng)態(tài)選擇解算算法,三軸姿態(tài)角不僅可以較好的反映真實(shí)姿態(tài)角,而且輸出姿態(tài)角更加平滑,說(shuō)明解算過(guò)程受噪聲影響較小、算法的濾波效果更好.

5種算法解算姿態(tài)角過(guò)程得到的后驗(yàn)均方根誤差圖表如圖6和表2所示.由圖表中的數(shù)據(jù)可以得到,在同一組測(cè)試數(shù)據(jù)的解算過(guò)程中,改進(jìn)EKF算法得到的姿態(tài)角均方誤差在0.01～0.05之間,明顯小于其他方法解算的姿態(tài)角均方誤差,并且姿態(tài)變化范圍小、穩(wěn)定性高.

表2 解算方法誤差對(duì)比表Table 2 Error comparison table of solution methods

圖6 動(dòng)態(tài)測(cè)試解算方法誤差對(duì)比圖Fig.6 Error comparison results of the solution methods in dynamic test

5 結(jié)束語(yǔ)

針對(duì)目前常見(jiàn)的姿態(tài)解算算法中存在的傳感器誤差累積和無(wú)關(guān)變量干擾問(wèn)題,本文在擴(kuò)展卡爾曼濾波算法的基礎(chǔ)上,引入PI補(bǔ)償?shù)幕パa(bǔ)濾波算法和動(dòng)態(tài)梯度下降法,在飛行姿態(tài)發(fā)生變化時(shí)使用融合后的改進(jìn)EKF算法進(jìn)行姿態(tài)解算,飛行姿態(tài)保持穩(wěn)定不變時(shí)使用標(biāo)準(zhǔn)EKF算法進(jìn)行解算.依據(jù)飛行姿態(tài)變化動(dòng)態(tài)切換解算方法,減少3種方法各自使用時(shí)的局限性,同時(shí)也減少傳感器噪聲帶來(lái)的解算干擾.通過(guò)實(shí)驗(yàn)證明,該算法靜態(tài)測(cè)試均方根誤差小于0.1,動(dòng)態(tài)測(cè)試均方根誤差保持在0.05以內(nèi),能有效降低姿態(tài)解算的誤差,提高了解算的準(zhǔn)確性和穩(wěn)定性,增強(qiáng)了解算系統(tǒng)的環(huán)境適應(yīng)力.