一種基于SVDCKF的無人機動態自適應航姿算法

楊躍, 劉小雄, 章衛國, 劉旭航, 郭一聰

(西北工業大學 自動化學院, 陜西 西安 710129)

近些年來,無人機應用在許多領域,如空中監視[1]、三維地圖重構[2]、搜索救援[3]等。其中,航姿參考系統(attitude and heading reference systems,AHRS)作為無人機自主飛行系統的關鍵部分,主要由航姿傳感器(陀螺儀、加速度計、磁力計)和微處理器組成,利用傳感器融合算法解算無人機航姿,并且無需其他傳感器輔助。本文重點是對低成本航姿傳感器的小型無人機航姿解算進行研究。

目前,小型無人機航姿解算算法主要包括互補濾波[4]、卡爾曼濾波[5]等。其中,互補濾波依靠傳感器之間的頻率互補特性,融合傳感器數據來計算航姿。梯度下降濾波通過一種迭代的方式尋找最優值,具體的實現過程是根據目標函數斜率的負方向搜索解。不過這兩種方法都沒有考慮低成本航姿傳感器噪聲對航姿解算精度的影響。相較于互補濾波和梯度下降濾波,卡爾曼濾波是將帶有噪聲的多種傳感器進行融合的線性最小方差估計方法,同時在建模過程中也會考慮噪聲的影響。通常在高斯噪聲分布系統中,卡爾曼濾波被看作是一種最佳估計器,經常用作處理線性系統模型。不過在實際應用中,無人機航姿系統是非線性模型。此時卡爾曼濾波處理非線性模型會導致航姿解算精度下降,甚至發散,因此一些改進的非線性卡爾曼濾波[6]逐漸被用作無人機航姿解算。針對無人機航姿解算問題,文獻[7]提出了一種擴展卡爾曼四元數航姿算法,使用擴展卡爾曼濾波對建立的非線性航姿模型進行處理,并且將四元數和陀螺儀隨機偏差作為狀態估計參數。文獻[8]利用多級可變擴展卡爾曼濾波算法,使用低成本慣性器件、磁力計以及GPS導航系統等多種傳感器,設計出一種可以實時估計無人機航姿的濾波估計器。 擴展卡爾曼濾波是一種對非線性函數進行一階泰勒展開的近似方法,這樣會使得模型存在舍入誤差。所以一些基于確定采樣型的非線性卡爾曼濾波相繼被提出,主要包括無跡卡爾曼濾波[9]、容積卡爾曼濾波[10]、粒子濾波[11]等。文獻[12]提出了一種基于自適應無跡卡爾曼濾波的四元數航姿濾波算法,相比于擴展卡爾曼濾波,該算法可以將非線性航姿模型精確到二階及二階以上的精度,并且無需對模型進行雅可比矩陣顯式計算。容積卡爾曼濾波采用容積加權采樣點,對非線性函數模型的逼近可以達到三階及三階以上,精度較無跡卡爾曼濾波進一步提高。粒子濾波是一種基于貝葉斯采樣估計的重要性序列采樣濾波方法,雖然與無跡卡爾曼濾波和容積卡爾曼濾波相比精度更高,但是粒子濾波計算量大,實時性較差。同時在濾波過程中,容易出現粒子退化問題,導致濾波器發散。

實際上,當無人機在進行飛行時,航姿模型統計特性會受到一些不確定因素的影響,導致航姿解算的魯棒性下降。文獻[13]提出一種新的卡爾曼航姿濾波用于估計姿態四元數,同時設計了一種自適應濾波方法用于減少系統噪聲特性中的模型誤差,提高了濾波抗擾性。文獻[14]設計了自適應增益方法來調節航姿測量噪聲方差,并且消除了不確定性誤差的影響,解算出可靠的無人機航姿。

本文首先建立了無人機非線性航姿系統模型,將姿態四元數和航姿傳感器隨機偏差作為狀態向量估計參數。針對航姿模型系統的非線性和濾波過程中協方差矩陣出現的非正定問題,設計了奇異值容積卡爾曼濾波,提高了航姿解算精度。考慮到飛行過程中三軸加速度對無人機航姿解算的影響,設計了一種動態自適應調節因子,改善了加速度測量噪聲方差。最后通過仿真和實驗結果表明該算法可以提高無人機航姿解算的精度和魯棒性。

1 航姿數學模型

1.1 四元數姿態更新模型

在無人機捷聯姿態表示方法中,通常用單位四元數進行姿態更新和剛體轉換的快速計算。

(1)

式中：qw是四元數實部；qv是四元數虛部且有qv=qxi+qyj+qzk；θ是旋轉角；e是旋轉軸且e=[ex,ey,ez]T。

四元數連續乘法運算可以定義如下

(2)式和(3)式中,?表示四元數乘法算子。q-1是q的可逆四元數,同時四元數必須滿足(4)式的正交原則。

(4)

本文利用單位四元數進行無人機姿態更新,對四元數姿態微分方程使用一階畢卡迭代法進行求解,并給出離散模型。

q(tk)=q(tk-1)?q(Δt)

(5)

式中：q(tk)和q(tk-1)分別是在時間點tk-1和tk的單位四元數；q(Δt)由陀螺儀三軸角速度進行更新。

(6)

式中,Ωw是陀螺儀三軸角速度的反對稱矩陣,具體形式如下

(7)

式中,w=[wx,wy,wz]T是陀螺儀在機體系下的三軸角速度。

(8)

1.2 航姿傳感器模型

航姿解算離不開航姿傳感器的測量值。本文使用低成本航姿傳感器,主要包括陀螺儀,加速度計和磁力計。這些傳感器固聯在小型無人機機體重心,并且傳感器的三軸之間在理想條件下相互正交。

(9)

1.3 航姿問題表述

無人機航姿模型在實際情況中是非線性系統,因此本文建立了高斯離散狀態下航姿非線性系統模型。

(10)

式中:xk是航姿狀態估計參數;f(·)是非線性動力學函數;vk-1是航姿傳感器輸入參數;zk是航姿觀測參數,h(·)是非線性觀測函數。其中,mk-1和nk分別是系統動態噪聲和觀測噪聲,假設兩者都是零均值高斯白噪聲且互不相關。

1.4 非線性航姿模型

對于無人機航姿解算,建立無人機航姿模型是非常有必要的。根據對四元數姿態更新、航姿傳感器模型以及航姿問題的分析,本文設計了一種非線性四元數航姿模型系統。(11)式是航姿系統動力學方程,其中航姿狀態參數x(tk)包括四元數q(tk)以及航姿傳感器偏差[bω(tk),ba(tk),bm(tk)]T。

(11)

式中,mq(tk-1),mw(tk-1),ma(tk-1)和mm(tk-1)是系統過程噪聲且滿足以下條件

在系統觀測模型中,對于加速度計和磁力計數據采樣頻率不一致的問題,可以考慮單獨設計相應航姿傳感器系統觀測模型。(12)式是觀測傳感器為加速度計的非線性觀測方程。

(12)

在解算無人機偏航角時,使用磁力計測量當地磁場強度數值,并且將計算得到的航向角作為觀測值。(13)式和(14)式為偏航角非線性觀測方程。

(13)

zm(tk)=atan2(mx,my)+nm(tk)

(14)

2 非線性航姿SVDCKF濾波

針對非線性航姿濾波問題,本文采用CKF濾波方法,較之于EKF和UKF有更好的解算精度。利用Cholesky 分解狀態協方差矩陣P且P=UTU,U是三角矩陣。不過使用Cholesky分解會出現一些問題:

1) Cholesky分解狀態協方差矩陣P時,P必須滿足正定或者對稱正定的性質,這樣做限制了P的取值范圍,導致航姿解算不穩定;

2) 狀態協方差矩陣在航姿濾波算法運行期間可能會成為稀疏矩陣,破壞了Cholesky分解對P的要求。

因此本文利用奇異值分解(singular value decomposition,SVD)來代替Cholesky分解,使得狀態協方差矩陣P可以克服上述問題。

P=USVT

(15)

(15)式為奇異值分解,其中P為m×m維的任意矩陣,U和V是單位正交矩陣。S是對角矩陣,并且S矩陣對角線上的元素稱為“奇異值”。

(16)

將SVD奇異值分解和CKF濾波結合,設計一種航姿SVDCKF濾波算法,圖1是非線性航姿濾波框圖,具體步驟如下:

(17)

(18)

式中:n是濾波狀態維數;m是cubature采樣點個數。本文航姿狀態維數n=13,cubature采樣點個數m=26。

step 2 航姿濾波狀態預測。

(19)

計算cubature采樣點Xi,k-1|k-1(i=1,2,…,m)。

(20)

將cubature采樣點Xi,k-1|k-1通過非線性航姿系統動力學方程進行傳遞預測。

(21)

計算航姿狀態預測值和狀態協方差預測值。

(22)

step 3 航姿濾波狀態修正。

(23)

計算cubature采樣點Xi,k|k-1(i=1,2,…,m)。

(24)

將cubature采樣點Zi,k|k-1通過非線性航姿觀測方程進行更新修正。

Zi,k|k-1=h(Xi,k|k-1)

(25)

(26)

計算航姿濾波增益矩陣Kk。

(27)

計算航姿狀態更新值和狀態協方差更新值。

(28)

圖1 非線性航姿濾波框圖

3 動態自適應調節因子

無人機在不同的飛行條件下,加速度計的三軸加速度會發生很大變化,特別是某些有害的加速度或異常測量值可能會對加速度數值產生影響。此外,在無人機飛行期間,機體抖動和氣流擾動也會使加速度數值具有不確定性。因此本文在自適應調節傳感器噪聲方差的基礎上,設計了一種動態自適應調節因子來改善加速度計觀測噪聲方差,并且將無人機的飛行條件分為靜態條件、低動態條件和高動態條件。圖2是動態自適應SVDCKF濾波框圖,其中動態加速度因子可以由(29)式定義。

(29)

式中,[afx,afy,afz]T是加速度計在機體系的三軸加速度,g是當地重力加速度。

圖2 動態自適應SVDCKF濾波框圖

3.1 靜態條件

無人機起飛之前,在水平地面的飛行狀態可以假定為靜態條件。此時三軸加速度僅受當地重力加速度和機體輕微抖動的影響。由于本文使用低成本慣性器件,器件本身帶有很大的傳感器噪聲。

(30)

(31)

3.2 低動態條件

無人機在飛行過程中,受到機體抖動和氣流擾動的影響,傳遞到加速度計中產生有害加速度,這些有害加速度會污染三軸加速度數值,進而導致無人機姿態解算失敗。

(32)

如果動態加速度因子?滿足(32)式,那么無人機加速度處于低動態條件,閾值Ta通過實驗測試進行設定。加速度噪聲方差可以通過(33)式進行調節。

(33)

3.3 高動態條件

無人機在飛行過程中可能會受到一些糟糕的因素干擾,例如強風、亂流以及鳥類。這些突然且急劇的變化都會造成三軸加速度無法使用。

?>Ta

(34)

如果動態加速度因子滿足(34)式,那么無人機加速度處于高動態條件,此時加速度測量噪聲需要設置成較大的數值,這樣可以消除一些異常的加速度對無人機姿態解算的影響。

(35)

4 仿真和實驗結果分析

4.1 實驗平臺

本文使用圖3的實驗平臺采集無人機航姿傳感器數據。其中航姿傳感器包括MPU6500慣性測量單元以及HMC5893磁力計。實驗過程中,使用圖3a)旋翼無人機采集靜態條件和低動態條件的航姿傳感器數據,使用圖3b)固定翼無人機采集高動態條件的航姿傳感器數據。

圖3 無人機航姿實驗平臺

4.2 實驗結果分析

為了更好地驗證所提出的航姿濾波算法的性能,利用采集到的航姿傳感器數據,在仿真軟件上對算法進行分析,并且與EKF和CKF航姿濾波進行對比。

圖4 靜態條件下的三軸加速度數值變化圖

·靜態條件

圖4描述了無人機在靜態條件下的三軸加速度變化,其中X軸的變化范圍為[-0.05,0.6]m/s2,Y軸的變化范圍為[-0.55,0.5]m/s2,Z軸的變化范圍為[-10.5,-9.5]m/s2。從圖5至圖7可以看出,相比于EKF和CKF,所提出的航姿濾波算法的姿態誤差最小。由于靜態條件下三軸加速度變化比較穩定,因此測量噪聲對航姿精度的影響較小。此時精確的非線性航姿模型和高維的非線性航姿濾波算法將影響航姿精度的解算。本文設計了一種13維航姿估計的模型和高維奇異值容積卡爾曼濾波,提高了航姿濾波精度,同時減少了一些不確定因素的干擾。

圖5 靜態條件下的滾轉角誤差圖 圖6 靜態條件下的俯仰角誤差圖 圖7 靜態條件下的偏航角誤差圖

為了更加直觀地比較3種濾波算法的航姿解算精度,表1提供了無人機靜態條件下EKF、CKF和SVDCKF航姿精度的平均絕對誤差(mean absolute errors,MAE)、標準偏差(standard deviation,STD)以及均方根誤差(root mean square errors,RMSE)。從表1中可以看出,對于低成本航姿傳感器姿態解算精度而言,本文所提出的算法在航姿解算精度方面優于EKF和CKF,可以更好地處理無人機非線性航姿模型。

表1 EKF、CKF以及SVDCKF航姿精度對比

·低動態條件

圖8描述了無人機在低動態條件下的三軸加速度變化,其中X軸的變化范圍為[-2,4]m/s2,Y軸的變化范圍為[-2,2]m/s2,Z軸的變化范圍為[-10.5,-8.5]m/s2。

圖8 低動態條件下的三軸加速度數值變化圖

從圖9至圖11中可以看出,在低動態條件下,EKF航姿濾波姿態誤差最大,主要是因為EKF對非線性航姿模型的一階截斷描述引入了舍入誤差,進而傳遞到航姿解算,放大了航姿解算的誤差。雖然CKF航姿解算誤差要小于EKF,但是在低動態飛行過程中,三軸加速度測量值會受到一些不確定因素的干擾,此時加速度量測噪聲將會不斷變化。CKF由于設置的是恒定的加速度量測噪聲方差,無法消除這些干擾帶來的影響。因此本文設計了動態自適應調節因子?,通過?來不斷地調節加速度噪聲方差,減少了航姿誤差。

圖9 低動態條件下的滾轉角誤差圖 圖10 低動態條件下的俯仰角誤差圖圖11 低動態條件下的偏航角誤差圖

表2提供了無人機低動態條件下EKF、CKF和SVDCKF航姿精度的平均絕對誤差、標準偏差以及均方根誤差。從表2中可以看出,帶有動態自適應調節因子的SVDCKF航姿濾波算法精度要優于EKF和CKF,提高了算法的魯棒性和抗擾性。

表2 EKF、CKF以及SVDCKF航姿精度對比

·高動態條件

圖12描述了無人機在高動態條件下的三軸加速度變化,其中X軸的變化范圍為[-10,10]m/s2,Y軸的變化范圍為[-6,1]m/s2,Z軸的變化范圍為[-10.5,-4.5]m/s2。本文使用固定翼無人機采集高動態飛行條件下的航姿傳感器數據。與旋翼無人機相比,固定翼無人機飛行速度快、機動靈活。從圖13至圖15中可以看出,EKF航姿濾波的姿態誤差在40 s后急劇變化,這是因為高動態條件下,航姿模型的非線性程度增強,引起EKF一階截斷帶來的舍入誤差越來越大。在高動態條件下,帶有動態自適應調節因子的SVDCKF相比于CKF,對加速度量測噪聲的處理效果更好,消除了非加速度對航姿解算的影響,提高了解算精度。

圖12 高動態條件下的三軸加速度數值變化圖

圖13 高動態條件下的滾轉角誤差圖 圖14 高動態條件下的俯仰角誤差圖圖15 高動態條件下的偏航角誤差圖

表3提供了無人機高動態條件下EKF、CKF和SVDCKF航姿精度的平均絕對誤差、標準偏差以及均方根誤差。從表3中可以看出,本文所提出的航姿濾波算法在高動態條件下通過調節動態自適應因子,降低了航姿解算誤差,提高了航姿解算的魯棒性。

表3 EKF、CKF以及SVDCKF航姿精度對比

5 結 論

本文提出了一種帶有動態自適應調節因子的SVDCKF非線性航姿濾波算法,以小型無人機為研究對象,對無人機飛行時的航姿解算需求進行了分析和設計,提高了無人機航姿解算精度和魯棒性。相比于其他航姿算法,本文提出的航姿算法主要有以下優點:

1) 設計了一種航姿狀態的非線性航姿系統模型,將陀螺儀、加速度計以及磁力計隨機偏差作為狀態估計參數,消除了航姿傳感器隨機誤差對航姿解算精度的影響。

2) 針對無人機航姿模型的非線性問題,以及在濾波過程中狀態協方差出現的矩陣非正定問題,用奇異值分解代替Cholesky分解,然后與容積卡爾曼濾波結合,對非線性航姿模型進行處理,提高了航姿解算精度。

3) 對于無人機在復雜條件飛行時,三軸加速度的變化對航姿解算的影響,設計了一種動態自適應調節因子對加速度量測噪聲方差進行處理,提高了航姿濾波的魯棒性和抗擾性。