基于加速度修正模型的無人機姿態解算算法

劉旭航, 劉小雄, 章衛國, 楊躍

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

穩定的姿態角輸出是無人機完成飛行任務的前提[1]，局限于無人機的成本和體積，目前低成本無人機采用由微機電系統(micro-electromechanical system,MEMS)傳感器構成的導航系統獲得姿態信息[2]，由于低成本傳感器性能的限制以及以互補濾波為代表的常規姿態解算算法對非重力加速度的敏感性[3]，動態環境下，無人機導航系統輸出的姿態角與實際的姿態角相比有很大誤差。因此，如何利用低成本傳感器，實現穩定可靠的姿態輸出是無人機導航領域的一個研究熱點[4]。

針對導航系統中低成本傳感器噪聲較大和易受干擾等問題，文獻[5]使用方向余弦矩陣更新姿態，根據陀螺儀和輔助傳感器的噪聲所處頻段互補的特點，應用互補濾波進行信息融合，提升系統的姿態和測量精度，但文獻[5]中的方法不適用于存在非重力加速度的動態環境中。文獻[6]在互補濾波的基礎上，設計非重力加速度抑制函數，利用加速度計與陀螺儀數據對非重力加速度進行抑制，該算法對非重力加速度的抗干擾能力優于互補濾波，但并沒有對實現非重力加速度的精確估計。文獻[7]在水下航行器中應用擴展卡爾曼濾波算法獲得姿態信息，分別采用了全球定位系統(global positioning system,GPS)和多普勒計程儀(doppler velocity log,DVL)輸出的速度信息減弱非重力加速度對姿態解算的影響，該算法在存在外部干擾的情況下可以準確估計水下航行器的姿態和航向，但在無人機上還缺乏應用案例。文獻[8]提出一種對非重力加速度自適應估計與校正的方法，通過對非重力加速度建模，自適應估計和校正干擾加速度，該算法能夠在短時間保持姿態估計的精度。文獻[9]利用GPS和空速計的速度信息對非重力加速度進行修正，然后通過2套歐拉角之間的滯環轉換，實現了全飛行狀態下比較精確的姿態表示，但由于GPS和空速計更新頻率較低，導致動態環境下該系統輸出姿態角的精度下降。文獻[10]采用GPS和慣性導航系統(inertial navigation system,INS)構建基于擴展卡爾曼濾波(extended Kalman filter,EKF)的組合導航模型，為載體提供實時的位置信息和姿態信息。但該算法對GPS精度和穩定性要求較高，通常需要使用成本較高的差分GPS來獲得精度達到厘米級的位置信息，低成本無人機導航系統中使用的單點GPS精度較低，且更新頻率慢。因此，在低成本無人機導航系統中，通過GPS/INS組合導航算法獲得的姿態角依然存在較大誤差。

本文針對動態環境下低成本無人機導航系統姿態解算這一問題，提出一種基于加速度修正模型的無人機姿態解算算法。首先，建立加速度修正模型，該模型通過求取估計非重力加速度和外部非重力加速度，實現對非重力加速度的準確估計。其次，由非重力加速度獲得修正加速度，減弱動態環境下非重力加速度對姿態解算的影響。然后，建立基于卡爾曼濾波的姿態解算模型，將加速度修正值和磁力計輸出值計算的姿態角作為系統量測信息進行濾波解算，抑制加速度計和陀螺儀中逐漸發散的累積誤差對姿態角解算結果的干擾，實現姿態的準確輸出。最后，利用固定翼無人機采集數據對本算法進行實驗驗證。

1 加速度修正模型

1.1 模型結構

首先設定系統坐標系,選取“北-東-地”地理坐標系為導航系,記為n系,選取“前-右-下”坐標系作為機體系,記為b系。

建立加速度修正模型修正加速度計輸出值,模型結構如圖1所示。

圖1 加速度修正模型結構圖

將由濾波器估計的非重力加速度誤差求得的非重力加速度記為估計非重力加速度,由GPS速度求得的非重力加速度記為外部非重力加速度。估計非重力加速度與加速度計的更新頻率相同,但只能在短時間內保持穩定輸出,誤差的累積會導致估計非重力加速度精度逐漸下降。外部非重力加速度穩定可靠,可以抑制估計非重力加速度誤差累積對系統姿態角解算結果的干擾,但外部非重力加速度輸出頻率較低,不能及時修正加速度計輸出值。因此,本文設計的加速度修正模型同時利用估計非重力加速度和外部非重力加速度修正加速度計輸出值。

1.2 估計非重力加速度

選取姿態角誤差,非重力加速度誤差作為狀態量,系統狀態量如下

(1)

Xε,k=Φk,k-1Xε,k-1+wk

(2)

式中:Φk,k-1表示狀態轉移矩陣;wk為過程噪聲。

在加速度修正模型中,非重力加速度誤差和姿態角誤差在當前時刻的狀態量中已經得到了修正。相鄰2個時刻的狀態量不相關,狀態轉移矩陣可以表示為Φk,k-1=06×6。因此,卡爾曼濾波方程中的狀態一步預測方程和狀態估計方程簡化形式如下

(3)

(4)

式中:Zε,k為當前時刻的量測信息;Hk為量測矩陣;Kk為當前時刻的卡爾曼濾波增益陣。

(5)

(6)

(7)

通過上述推導,(7)式可以展開為如下形式

(8)

式中,ΔR()表示小角度旋轉矩陣。

系統量測方程可以表示為

(9)

式中:vε,k為量測噪聲;其方差記為Rk;量測矩陣Hε,k可以表示如下

(10)

濾波增益陣

Pk/k-1=Φk,k-1Pk-1Φk,k-1+Qk=Qk

(11)

一步預測均方差

(12)

估計均方誤差

Pk=(I-KkHε,k)Pk/k-1

(13)

(14)

1.3 外部非重力加速度

外部非重力加速度由GPS提供的速度信息求得,通過外部非重力加速度對加速度計原始值進行修正。

(15)

(16)

GPS的周期T2與加速度計的周期T1通常成倍數關系,記為T2=nT1。

對周期T2內加速度原始值進行積分,以求取該時間段內加速度計原始值的平均值。

(17)

(18)

2 基于加速度修正模型的姿態解算算法

2.1 姿態解算模型

選取姿態角誤差θε,陀螺儀零偏誤差εb作為姿態角解算模型的濾波狀態量。

(19)

低成本無人機導航系統中,陀螺儀精度大于0.1°/s,由于陀螺儀精度過低,對地球自轉信息不敏感[7]。因此可以將簡化形式姿態誤差方程應用在姿態解算模型中,簡化形式的姿態誤差方程如下

(20)

通常情況下,陀螺常值漂移可用隨機常數來描述。

(21)

系統狀態方程為

(22)

對(22)式進行離散化處理,得到系統狀態方程。

Xk=f(Xk-1)+Wk

(23)

將第一節中計算出的修正加速度記為g,修正加速度的取值表達式如下

(24)

式中,t為陀螺儀角速率所對應的時間戳數值。

由修正加速度獲得滾轉角和俯仰角的方法如下式

(25)

(26)

式中

(27)

將由(25)式和(26)式獲得的姿態角與由姿態解算模型獲得姿態角之間的差值作為量測量。量測方程形式如下

Zk=HkXk+vk

(28)

式中

(29)

2.2 算法結構

基于加速度修正模型的姿態解算算法的結構框圖如圖2所示。

圖2 基于加速度修正模型的姿態解算算法結構圖

由圖2可知,基于加速度修正模型的姿態解算算法從陀螺儀中獲得狀態信息,利用修正重力加速度g和磁力計計算值m獲得量測姿態角,進而獲得量測信息,然后通過構建姿態解算模型進行卡爾曼濾波。濾波輸出值包含陀螺儀零偏誤差εb和姿態角誤差θε,陀螺儀零偏誤差對陀螺儀輸出值進行修正,姿態角誤差可以用來解算姿態角。

3 實驗與分析

3.1 實驗數據采集

實驗數據通過搭載于固定翼無人機的Ellipse-N系統采集,Ellipse-N系統集成了陀螺儀、加速度計、磁力計和GPS,可以采集原始的陀螺儀、加速度計、磁力計和GPS速度信息。同時,Ellipse-N系統輸出的滾轉角誤差和俯仰角誤差的均方根均小于0.2°,偏航角誤差均方根小于0.5°,因此該系統輸出的姿態數據可以作為姿態對比的參考基準。GPS接收機輸出的速度精度為0.1 m/s,更新頻率為10 Hz。Ellipse-N慣性導航系統中的傳感器參數如表1所示。

表1 Ellipse-N傳感器參數

圖3 Ellipse-N慣性導航系統

在固定翼無人機飛行過程中采集時長為105 s的實驗數據,該段時間內無人機起飛后盤旋飛行。無人機初始滾轉角為-2.6°,俯仰角為6.3°,偏航角為276.8°。

3.2 加速度修正模型的驗證

選取時長為3 s的實驗數據進行對比試驗,驗證加速度修正模型的修正效果。將加速度計輸出原始加速度的模值記為G1,僅由外部非重力加速度修正后的得到的加速度模值記為G2,僅由估計非重力加速度修正后得到的加速度模值記為G3,最后將由加速度修正模型獲得的加速度模值記為G4。G1,G2,G3和G4同標準重力加速度的模值Gideal進行對比,對比曲線如圖4所示:

圖4 重力加速度的模值對比圖

通過圖4可以看出,加速度計原始模值G1相對標準重力加速度模值Gideal偏差程度最大,波動范圍為8～13 m/s2,說明非重力加速度不為0,無人機在該段時間內做變加速運動。比較G2,G3和G4的偏離程度,由于GPS更新頻率低,G2數值跳變劇烈,G3曲線在初始階段與G4曲線重合,但G3的誤差在1 s后逐漸大于1 m/s2,說明估計非重力加速度僅能在短時間內修正非重力加速度的干擾,G4的偏離程度最小,對非重力加速度估計最為準確。

表2 加速度模值的統計特性

表2采用平均絕對誤差(mean absolute error,MAE)和均方根誤差(root mean squared error,RMSE)來衡量觀測值同真值之間的偏差,通過表2可以看出,G4的MAE值和RMSE值最小。

通過上述分析可知,包含外部非重力加速度和估計非重力加速度的加速度修正模型可以有效還原重力加速度,避免動態環境下非重力加速度對姿態解算結果的干擾。

3.3 基于加速度修正模型的姿態解算算法驗證

選取時長為105 s的實驗數據對本文所設計的基于加速度修正模型的無人機姿態解算算法進行驗證,并與參考姿態角進行對比。參考姿態角記為αref,本文所設計算法輸出姿態角記為αProp。加速度計原始模值如圖5所示,姿態角對比圖如圖6所示。

根據圖5可以看出,該段時間內加速度計輸出模值與標準重力加速度數值偏差較大,即該段時間內無人機做變加速運動,非重力加速度不為0。觀察圖6,所設計算法輸出的滾轉角和俯仰角與參考滾轉角和參考俯仰角之間誤差小于1°,偏航角與參考偏航角之間誤差小于2°。說明在存在非重力加速度干擾的動態環境中,本文所設計算法能夠穩定輸出姿態角。

圖5 加速度計原始模值

圖6 姿態角對比圖

3.4 不同算法性能比較

采用文獻[5]中的互補濾波算法和文獻[10]中的基于EKF的GPS/INS組合導航算法,以及本文所設計的基于加速度修正模型的姿態解算算法對同一段時長為40 s的實驗數據進行對比實驗,將互補濾波算法的解算結果記為αCF,GPS/INS組合導航算法的解算結果記為αEKF,本文所設計算法的解算結果仍記為αProp。加速度計原始模值如圖7所示,2種算法輸出的姿態角誤差對比圖如圖8所示。

圖7 加速度計原始模值

由圖7可以看出，該段時間內加速度計輸出模值與標準重力加速度數值偏差較大，即該段時間內無人機做變加速運動，非重力加速度數值不為0。觀察圖8，在存在非重力加速度干擾的情況下，基于加速度修正模型的無人機姿態解算算法的俯仰角誤差和滾轉角誤差均控制在1以內，偏航角誤差控制在2 以內。互補濾波算法由于受到非重力加速度的干擾，俯仰角和滾轉角均逐漸發散。同時，由于偏航角量測值由俯仰角和滾轉角計算獲得，因此互補濾波輸出的偏航角誤差值也呈現發散狀態。受限于GPS速度的低精度和低頻率，基于EKF的組合導航算法俯仰角誤差和滾轉角誤差均大于1 ，偏航角誤差峰值大于3。

根據表3中姿態角誤差的統計特性，基于加速度修正模型的無人機姿態解算算法的平方絕對誤差和均方根誤差均小于互補濾波算法和基于EKF的組合導航算法的平方絕對誤差和均方根誤差，即所設計算法性能優于互補濾波算法和基于EKF的組合導航算法，其姿態輸出具有更高的精度。

表3 姿態角誤差的統計特性

4 結 論

本文針對低成本無人機導航系統中，非重力加速度對姿態解算存在干擾的問題，提出基于加速度修正模型的無人機姿態解算算法。該算法首先利用加速度計修正模型對加速度計輸出值進行修正，避免非重力加速度對姿態輸出的干擾。然后利用修正加速度和磁力計構建基于卡爾曼濾波的姿態解算模型，實現準確的姿態輸出。實驗結果表明，該算法能夠有效提高低成本無人機導航系統姿態解算的精度和抗干擾能力。