董一平, 劉 寧,*, 蘇 中, 王靖驍, 白宏陽

(1. 北京信息科技大學自動化學院, 北京 100192; 2. 北京信息科技大學高動態(tài)導航技術北京市重點實驗室, 北京 100192; 3. 南京理工大學能源與動力工程學院, 南京 210094)

0 引 言

現(xiàn)代軍事中,精確化打擊已經成為主要的發(fā)展方向。高速自旋飛行體在飛行過程中自身高速旋轉產生陀螺力矩,能夠克服由推力偏心、質量偏心、氣動偏心等引起的誤差,具有較強的抗干擾能力。因此,受到世界上各軍事強國的普遍重視。高速自旋是一種繞自身軸線的旋轉速度為10 rev/s (1 rev=360°)以上的運動狀態(tài),廣泛使用于航天、武器、民用等領域中。高速自旋飛行體主要包括:火箭彈類、導彈、彈箭類等,同時具有較高的打擊能力,在增雨防雹、太空探索等民用領域也發(fā)揮重大作用,因此研究高速自旋飛行體的制導問題至關重要。

高速自旋飛行體在制導過程中,需要實時對飛行體的位置坐標、飛行姿態(tài)、飛行速度等參數(shù)進行解算,但存在著一定的困難。文獻[9]中給出火箭彈的滾動角度較大,衛(wèi)星在捕捉信號時難度較大;文獻[10]中指出火箭彈飛行過程中橫滾角過大,需要較大的動態(tài)測量范圍,對陀螺儀的精度要求較高。高速自旋飛行體的滾轉角速度可達上百轉每秒,單純使用捷聯(lián)慣導系統(tǒng)(strapdown inertial navigation system,SINS),則陀螺儀具有較大的測量范圍,但其分辨率會減低,導致測量誤差增大。

針對上述問題,組合導航的方式可以將SINS與北斗衛(wèi)星導航系統(tǒng)二者互補,同時具有精度高,抗干擾能力強等優(yōu)點,更好發(fā)揮優(yōu)勢。文獻[13]提出了全球定位系統(tǒng)(global positioning system, GPS)/慣性測量單元(inertial measurement unit, IMU)組合方式,采用擴展卡爾曼濾波方法(extended Kalman filter, EKF)進行濾波;文獻[9]給出了INS/GPS超緊耦合的組合導航方法。但超緊組合方式對信息限制較多,使用不便捷;文獻[14]給出了采用地磁輔助的微機械(micro electro mechanical system,MEMS)陀螺進行火箭彈姿態(tài)測量;文獻[15]提出了基于MEMS的IMU和北斗衛(wèi)星組合的方式,并通過無跡卡爾曼濾波算法對據(jù)進行融合,提高彈道參數(shù)測量的精度。

組合導航的濾波方式有多種,主要為卡爾曼濾波(Kalman filter, KF)算法和以其為基礎的改進算法。EKF作為估計工具,在控制領域具有一定地位,但在非線系統(tǒng),EKF可能出現(xiàn)偏差。傳統(tǒng)的EKF算法,其系統(tǒng)噪聲協(xié)方差矩陣和量測噪聲協(xié)方差矩陣設定為常值,但實際系統(tǒng)噪聲是隨機的,所以濾波誤差會增大。因此,本文基于自適應EKF(adaptive EKF, AEKF)算法進行數(shù)據(jù)融合,對量測噪聲進行估計,改進AEKF算法。

本文基于北斗/SINS松組合模型,使用改進的AEKF算法對數(shù)據(jù)進行融合,根據(jù)北斗衛(wèi)星是否更新判斷進入量測。仿真結果表明,與未改進的相比,改進的AEKF算法系統(tǒng)濾波誤差更小,系統(tǒng)更加穩(wěn)定,同時具有輕量級的優(yōu)點。

1 AEKF算法

1.1 AEKF算法實現(xiàn)

AEKF算法是在EKF算法的基礎上演變而來,增加了遺忘因子,以對系統(tǒng)的噪聲進行估計,達到減小誤差的效果。EKF算法是解決非線性狀態(tài)估計最為直接且性能較好的方法。由于噪聲的不可知性,對EKF的算法造成影響。AEKF既有EKF適用于非線性系統(tǒng)的優(yōu)點,又能夠對未知的噪聲進行估計,進而適應不斷變化的噪聲情況。因此,向EKF中引入自適應機制。

使用較為廣泛的AEKF算法為Sage-Husa算法,該算法主要通過觀測數(shù)據(jù)進行濾波的同時,使用噪聲統(tǒng)計估計器,能夠實時地估計和修正系統(tǒng)的系統(tǒng)噪聲和量測噪聲,具體引入帶遺忘因子的噪聲統(tǒng)計估計值。

設線性離散化系統(tǒng)方程和量測方程分別為

(1)

式中：是系統(tǒng)時刻的維估計狀態(tài);是系統(tǒng)時刻的維觀測序列;是系統(tǒng)狀態(tài)轉移矩陣;是觀測矩陣;-1,分別是系統(tǒng)噪聲和量測噪聲序列,相互獨立且為正態(tài)白噪聲。則Sage-Husa算法具體描述為

狀態(tài)預測:

(2)

(3)

殘差:

(4)

量測更新:

(5)

式中:為時刻EKF的增益矩陣。

(6)

=[-],-1

(7)

自適應估計器為

(8)

(9)

(10)

(11)

式中：=(1-)(1-+1)(0<<1)為遺忘因子,其作用為加強新數(shù)據(jù)對濾波的作用。

1.2 算法改進

本文在此基礎上,對自適應器的量測噪聲估計公式中添加系數(shù),其中添加系數(shù)的目的是更準確地反映量測噪聲的變化情況,具體為

(12)

(13)

2 組合導航系統(tǒng)

2.1 坐標系選取

本文采用SINS,使用導航坐標系北-東-地坐標系;組合導航系統(tǒng)借助衛(wèi)星信號修正,衛(wèi)星信號為WGS-84國際統(tǒng)一的大地坐標系經度、緯度和高度。為統(tǒng)一計算,將衛(wèi)星信號從大地坐標系轉換至中間地心直角坐標系,再轉換至北-東-地(N-E-D)坐標系。

2.2 數(shù)學模型

傳統(tǒng)的EKF建模方法,對于建模所需的變量易于選取,但在計算過程中需要每次對非線性數(shù)據(jù)采用雅各比矩陣的方式進行線性化,計算量大,對實時性有較大影響。因此本文采用對系統(tǒng)方程和量測方程的非線性函數(shù)作泰勒級數(shù)展開并僅保留線性項,獲得線性模型,提高系統(tǒng)的實時性。

選取系統(tǒng)的誤差量作為狀態(tài)變量:位置誤差Δ、速度誤差Δ、姿態(tài)誤差Δ、加速度計零偏誤差Δ和陀螺零偏誤差Δ,建立狀態(tài)方程如下:

(14)

(15)

(16)

其次,建立量測方程+1=[+1,+1],具體如下式所示:

(17)

2.3 系統(tǒng)實現(xiàn)

本次組合導航系統(tǒng)采用松組合的方式,具有易實現(xiàn)和有冗余度的優(yōu)點。SINS隨時間增加造成誤差積累,北斗衛(wèi)星導航系統(tǒng)具有全天候、實時性高等優(yōu)點,與SINS組合使用,二者能夠互補。結構框圖如圖1所示。

圖1 組合導航系統(tǒng)組成

首先需要對系統(tǒng)進行初始對準,是獲取的高準確性和高實時性是高精度組合導航實現(xiàn)的前提。初始對準目的是獲取載體的速度、位置和姿態(tài)信息。然后對SINS數(shù)據(jù)進行解算,得到飛行體的位置、速度和姿態(tài)信息,并把獲取的北斗衛(wèi)星數(shù)據(jù)進行坐標變換。由于SINS更新速度快于北斗衛(wèi)星數(shù)據(jù),需要判斷北斗衛(wèi)星是否進入量測更新,以保證修正數(shù)據(jù)的準確性。在更新時,用北斗衛(wèi)星數(shù)據(jù)校正SINS數(shù)據(jù),使用AEKF算法,對量測噪聲進行估計;在非量測更新階段,單獨采用SINS對飛行體進行定位。

具體硬件系統(tǒng)包括慣性測量模塊、彈載計算機模塊和數(shù)據(jù)記錄模塊。慣性測量模塊包括陀螺儀、三軸加速度計;彈載計算機采用Cortex-M7+FPGA的架構進行設計;數(shù)據(jù)記錄儀用于數(shù)據(jù)存儲。具體硬件實物如圖2所示。

圖2 硬件實物圖

3 仿真分析

為驗證本次改進算法在高速自旋飛行體上的可行性,利用Matlab軟件對實際載體運動的數(shù)據(jù)進行仿真分析。

載體運動軌跡為拋物線,實地觀測結果為飛行高度約為2 000 m,飛行距離約為15 000 m,飛行總時長為46.5 s,發(fā)射角為25°。SINS的更新速率為1 ms,北斗衛(wèi)星的更新速率為100 ms。仿真參數(shù)的誤差描述如表1所示。實際陀螺儀和加速度計的測量數(shù)據(jù)如圖3和圖4所示。

表1 仿真參數(shù)誤差

圖3 陀螺儀測量數(shù)據(jù)

圖4 加速度計測量數(shù)據(jù)

本文采用組合導航,使用AEKF濾波方法對SINS和北斗的數(shù)據(jù)進行融合,對比未改進與改進的AEKF算法對濾波誤差的影響。

3.1 AEKF算法對量測噪聲估計

本文經過分析,采用AEKF算法對量測噪聲協(xié)方差矩陣進行估計。組合導航軌跡東向-地向平面信息、位置誤差和速度誤差如圖5～圖7所示。遺忘因子取0.9。

圖5 組合導航軌跡東向-地向平面信息

圖6 算法改進前的位置誤差

圖7 算法改進前的速度誤差

由圖5可以看出,SINS信息沒有被修正過來,在后期誤差越來越大,與北斗的觀測數(shù)據(jù)有一定偏差。從圖6和圖7也可以看出,誤差越來越大,沒有趨于穩(wěn)定。位置誤差在30 s時開始不斷上升,地方向上最后下降,整體偏高;速度誤差在20 s時整體上升。因此,以上方法的濾波效果不佳。

3.2 改進的AEKF算法

由上一組為改進的AEKF算法仿真結果可知,組合導航濾波出現(xiàn)明顯偏差,結束點的位置與北斗的量測位置相距1 400 m。因此,采用改進的AEKF算法進行濾波。位置誤差、速度誤差如圖8和圖9所示。遺忘因子取0.9。

圖8 b=0.9時的位置誤差

圖9 b=0.9時的速度誤差

由圖8可以看出,位置誤差漸趨平穩(wěn),且均收斂,雖然在35 s時,有次波動,但很快調節(jié),恢復收斂,與未改進AEKF算法對比明顯,且誤差大范圍下降,最后北向位置誤差保持在3 m 范圍內,東向誤差平均保持在6 m 范圍內。由圖9可知,速度誤差也有較大改善,在北向、東向兩方向上分別收斂于2 m/s 和14 m/s,效果明顯。地向速度改動不明顯。

再次調節(jié)遺忘因子,改善系統(tǒng)濾波能力。經過反復調試,取遺忘因子為0.3,得到位置、速度誤差如圖10和圖11所示。

圖10 b=0.3時的位置誤差

圖11 b=0.3時的速度誤差

經過調節(jié)遺忘因子參數(shù),由圖11可知,速度誤差進一步改善,東向誤差再次下調位置誤差改變不明顯,但位置、速度誤差保持穩(wěn)定。經過此次濾波算法調節(jié),組合導航運動軌跡如圖12所示。

圖12 組合導航運動軌跡圖

組合導航軌跡表明,濾波算法有效,將SINS數(shù)據(jù)在量測更新階段校正過來,避免誤差干擾,與圖5對比,有明顯改善。組合導航軌跡跟隨北斗量測的數(shù)據(jù)進行調整。由上述未改進AEKF算法與不同參數(shù)調整的改進AEKF算法,可以得出,位置誤差由發(fā)散,最終北向位置誤差為 2 m,東向位置誤差為6 m,地向位置誤差7 m。北向、東向速度誤差從發(fā)散調整為收斂,最終北向速度誤差為2 m/s 范圍內,東向速度誤差為 10 m/s。由此表明,本文改進的AEKF濾波算法行之有效,能夠保證組合導航濾波的穩(wěn)定性。

4 結 論

本文研究了基于AEKF算法的高速自旋飛行體組合導航方法,針對EKF算法中系統(tǒng)存在的噪聲問題,引入了自適應器,添加遺忘因子對量測噪聲進行估計,并對EKF的建模做了調整,每次計算過程中不必重復計算雅可比矩陣,提高系統(tǒng)的實時性,采用濾波算法控制,對高速自旋飛行體的組合導航控制中具有輕量級的優(yōu)點。通過AEKF算法與改進的AEKF算法對比,改進的算法能夠明顯降低誤差,并保證誤差漸趨穩(wěn)定,進入收斂。通過本次仿真,得到AEKF算法能夠提高導航精度,但還能夠進一步改善,后續(xù)工作繼續(xù)調整算法,并且采用其他算法進行修正,如無跡卡爾曼算法等。本文僅對系統(tǒng)的量測噪聲進行修正,能夠自適應估計協(xié)方差矩陣,保證了北斗/SINS組合導航的定位精度,并且提高系統(tǒng)的穩(wěn)定性。