基于擴展卡爾曼濾波的固定翼無人機姿態解算方法

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

1(西安理工大學 自動化與信息工程學院,西安 710048)

2(西安理工大學 陜西省工業圖傳與控制應用校企聯合研究中心,西安 710048)

1 引 言

姿態解算通常是利用數據融合算法標記傳感器有用信息,按照坐標系轉換關系計算姿態信息,進而進行組合導航的.精確、穩定的姿態信息在姿態控制[1]、目標跟蹤[2]和組合導航[3]等眾多領域都發揮著至關重要的作用.固定翼無人機(Unmanned Aerial Vehicle,UAV)憑借飛行速度快,航程距離遠,運載能力大的特點,在邊境巡邏、戰術偵察、災情監視等領域也發揮著重要作用.

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

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

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

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

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

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

2 系統模型

2.1 姿態描述與歐拉角轉換

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

機體軸坐標系(Body Frame):它是無人機慣性導航的基準坐標系,其原點O取在無人機的重心位置,X軸指向機身前進方向,Z軸垂直于X軸向下而Y軸與X、Z軸構成右手系,由此指向機身右側,故又稱作是前右下坐標系,該坐標系我們用b系表示.機載NED坐標系與機體軸坐標系的組合關系如圖1所示.

圖1 機載NED坐標系與機體軸坐標系的組合關系Fig.1 Combination between airborne NED coordinate system and airframe axis coordinate system

(1)

姿態角是無人機在機體軸坐標系中參考機載NED坐標系繞固定點旋轉三次完成轉換的,通常按照Z、Y、X軸的順序產生歐拉角,由此可得Z軸偏航角yaw(記為ψ),Y軸俯仰角pitch(記為θ),以及X軸滾轉角roll(記為φ)在三維空間中所對應的旋轉矩陣分別為:

(2)

q=q0+q1i+q2j+q3k

(3)

其中,q0,q1,q2,q3是實數,i,j,k表示虛數單位.

(4)

(5)

2.2 擴展卡爾曼濾波算法

EKF姿態解算部分主要分為兩個過程,預測過程和測量過程.對應公式為式(6)和式(7):

(6)

(7)

在時間預測過程中,將得到的陀螺儀三軸角速度矢量,以四元數形式通過一階龍格庫塔法進行首次估計,即:

(8)

(9)

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

(10)

在狀態更新過程中,首先對于一個三軸加速度計,把任意時刻測得的加速度矢量,作為觀測變量,記作:

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

(11)

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

接著,根據旋轉矩陣進行坐標轉換得到如式(12)所示:

(12)

則系統的觀測方程可以表示為式(13)所示:

(13)

根據式(13),h(x(k),k)為非線性函數,需要通過泰勒級數展開將非線性函數在當前估計狀態的平均值附近線性化,求得雅克比矩陣為:

(14)

則量測過程:

(15)

最后為了保持四元數單位范數的性質,在時間預測和狀態更新階段結束時,都要采用歸一化步驟對姿態估計值進行歸一化處理.

3 改進擴展卡爾曼濾波的姿態解算方法

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

針對3種傳統濾波算法各自使用時的局限性,且為了提高固定翼無人機的環境適應能力和解算過程的穩定性,本文在EKF算法的基礎上,加入動態梯度算法和互補濾波算法,用于無人機慣性測量單元陀螺儀和加速度計兩個傳感器的數據融合與姿態解算.

3.1 陀螺儀的角速度補償

慣導單元IMU傳感器在不同頻率范圍內有不同的動態響應優勢,由于陀螺儀與加速度計又具有頻率互補性,因此可以結合陀螺儀和加速度計的性能優勢,采用互補濾波算法結合不同頻的輸出信息,使得表征結果更加貼合實際值.主要步驟如下:

1)計算誤差

(16)

設加速度計3個軸的值分別為ax,ay,az,對其分別進行求模和歸一化處理,如下式所示.

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

(17)

2)PI誤差補償

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

(18)

(19)

3.2 運動加速度的抑制處理

梯度下降法可以表示為:

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

(20)

式中:x表示運算前后自變量的值,▽F(xn)表示目標函數的梯度,下標n表示搜索次數,負號表示沿著梯度的負方向逼近極點,即搜索方向,μ表示在搜索方向上的步長.

設加速度計歸一化后在機體軸坐標系中為ab=[axayaz]T,當重力向量在機載NED坐標系中為gn=[0 0 1]T時,理論重力加速度與加速度計測得值相減得到的誤差函數用四元數表示法為:

(21)

(22)

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

(23)

則根據梯度下降法的迭代過程式(9)可演變為:

(24)

(25)

其中,α>1,w為陀螺儀的測量角速度;Ts為系統的采樣時間,均為正相關.為了增強四元數估計的動態跟蹤特性,設置與物理方向速率相關的自適應步長,如式所示:

(26)

另外,為了降低運動加速度對高速狀態下的飛行器的干擾.定義δk為加速度計測量值的可信因子:

(27)

(28)

將以上迭代公式應用于狀態向量姿態四元數的估計中,可以避免線性化誤差,提高估計精度.

3.3 強機動高速運動的干擾消除辦法

在無人機的高速、高空運動的作業環境下,需要面臨強干擾、高運動加速度的惡劣問題,為了保證飛行的穩定性和解算的精確性,并在此基礎上盡量降低處理器計算的復雜程度,本文提出一種根據閾值進行算法動態切換的解算方法.

由于EKF算法是基于前一時刻的姿態估計值進行更新迭代的,那么當飛行姿態保持穩定不變時,標準EKF足以解算出穩定姿態角;當飛行姿態發生劇烈變化時,由于EKF算法本身的遞歸特性,再加上微分過程帶來的線性化誤差問題,本文提出一種基于閾值法優化改進擴展卡爾曼濾波的固定翼無人機姿態解算方法,改進方法在EKF的基礎上加入互補濾波器和動態梯度下降法,分別作用于陀螺儀和加速度計兩個傳感器,可有效減小EKF算法線性化誤差,提高解算過程穩定性和環境適應能力.

在EKF算法解算姿態角的過程中,姿態角的更新主要是通過四元數微分方程的求解進行的,而姿態角的劇烈變化會直接體現在陀螺儀的輸出角速度上,不穩定、不準確的角速度會導致求解的姿態角誤差增大.因此本文以陀螺儀的輸出角速度變化值為判定姿態角劇烈變化的閾值,判斷式為式(29).當判定大于某一閾值a時,認為此刻姿態角發生了變化,需要使用改進EKF算法進行姿態解算,否則使用標準EKF算法進行姿態解算.

(29)

最終陀螺儀經過了預測過程,加速度計經過了測量過程,將兩者輸出的結果通過迭代合并得到了三軸姿態角的四元數形式,并通過歐拉角反解公式求解出三軸姿態角.公式如下:

(30)

3.4 姿態解算的整體流程

本文在EKF的基礎上,加入PI補償的互補濾波算法和動態步長的梯度下降法,整體姿態解算流程圖如圖2所示.算法步驟為:

圖2 姿態解算整體流程Fig.2 Flowchart of posture solution

1)采集IMU慣導單元陀螺儀、加速度計傳感器原始數據,并對原始數據進行小波包降噪;

2)如果判定角速度w1變化值小于閾值a,那么將陀螺儀輸出值計算的姿態四元數作為初始狀態向量,將加速度計輸出加速度作為量測向量,使用標準EKF算法實現估計角度和測量角度的融合,計算出姿態四元數和協方差矩陣;

3)如果判定角速度w1變化值大于閾值a,那么使用動態步長的梯度下降法求解加速度計輸出的誤差函數,并補償給姿態四元數,使用PI補償的互補濾波算法求解陀螺儀輸出角速度的誤差向量,并以補償后的姿態四元數和輸出加速度作為狀態向量和量測向量,使用EKF算法解算得到最優估計角度;

4)把得到的最優估計角度作為下一次迭代的初始角度,并不斷利用四元數微分方程進行姿態角的更新,最終得到一組穩定的最優估計四元數角度;

5)通過反解歐拉角公式最終得到一組最優估計歐拉角度.

4 測試過程及結果分析

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

表1 實驗參數Table 1 Experiment parameter

為了驗證本文采用的閾值法優化改進擴展卡爾曼濾波算法的穩定性和準確性,在進行測試的同時,對標準的Mahony濾波器、EKF算法以及文獻[18]和文獻[21]中提出的EKF和Mahony濾波器融合算法(Mahony_EKF)、EKF和動態梯度下降法相結合的算法(DGDA_EKF)進行了算法驗證,同本文中提出的改進EKF算法進行了性能比較.

4.1 靜態測試

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

圖3 靜態測試三軸姿態解算結果圖Fig.3 Results of triaxial attitude calculation in static test

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

圖4給出了靜態測試下5種解算方法的后驗均方根誤差對比結果,實驗結果證明,在5種不同的姿態解算方法中,單獨使用Mahony濾波器和EKF算法靜態誤差較大,在EKF算法的基礎上,加入互補濾波器和梯度下降法的融合算法中,本文提出的基于閾值法改進EKF解算算法后驗均方根誤差最小,準確度最高.

圖4 靜態測試解算方法誤差對比圖Fig.4 Error comparison results of the solution methods in static test

4.2 動態測試

動態實驗是在室外對固定翼無人機進行了長達10min的試飛實驗,飛行過程解算姿態角結果如圖5(a)、圖5(b)、圖5(c)所示.無人機一開始保持穩定在0°,并分別對俯仰角、橫滾角和航向角進行了單獨測試,驗證了姿態變化的有效性.在大約過了5min,迭代265次之后,無人機在橫滾角的變化上較為明顯,此時橫滾角和航向角大約為20°,俯仰角依舊在0°振蕩.無人機在變換完姿態后,保持姿態角度飛行了一段時間,在迭代次數達到400之后,恢復為原始角度并最終完成了此次飛行.

圖5 動態測試三軸姿態解算結果圖Fig.5 Results of triaxial attitude calculation in dynamic test

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

相較于單獨使用Mahony濾波器,僅使用EKF算法,三軸姿態角不僅沒有逐漸漂移,而且能較好的反映真實姿態角,但震蕩較多,特別是在角度有一定變化或即將變化的時間點,穩定性不強,說明解算過程受環境因素、傳感器自身噪聲等影響很大.在EKF 的基礎上分別結合了Mahony濾波器和動態梯度下降法之后,穩定性有所增強,有效改善了單一算法解算姿態角穩定性差的問題.而本文提出以擴展卡爾曼濾波器為核心解算固定翼無人機姿態角,在此基礎上加入了PI補償的互補濾波算法和動態梯度下降法,根據閾值動態選擇解算算法,三軸姿態角不僅可以較好的反映真實姿態角,而且輸出姿態角更加平滑,說明解算過程受噪聲影響較小、算法的濾波效果更好.

5種算法解算姿態角過程得到的后驗均方根誤差圖表如圖6和表2所示.由圖表中的數據可以得到,在同一組測試數據的解算過程中,改進EKF算法得到的姿態角均方誤差在0.01～0.05之間,明顯小于其他方法解算的姿態角均方誤差,并且姿態變化范圍小、穩定性高.

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

圖6 動態測試解算方法誤差對比圖Fig.6 Error comparison results of the solution methods in dynamic test

5 結束語

針對目前常見的姿態解算算法中存在的傳感器誤差累積和無關變量干擾問題,本文在擴展卡爾曼濾波算法的基礎上,引入PI補償的互補濾波算法和動態梯度下降法,在飛行姿態發生變化時使用融合后的改進EKF算法進行姿態解算,飛行姿態保持穩定不變時使用標準EKF算法進行解算.依據飛行姿態變化動態切換解算方法,減少3種方法各自使用時的局限性,同時也減少傳感器噪聲帶來的解算干擾.通過實驗證明,該算法靜態測試均方根誤差小于0.1,動態測試均方根誤差保持在0.05以內,能有效降低姿態解算的誤差,提高了解算的準確性和穩定性,增強了解算系統的環境適應力.