艦載武器SINS非線性傳遞對準模型及濾波算法

李 娜, 王新龍

?

艦載武器SINS非線性傳遞對準模型及濾波算法

李 娜, 王新龍

(北京航空航天大學宇航學院, 北京, 100191)

為了滿足艦載武器初始對準高精度和快速性的要求, 更好地解決艦載武器在大失準角情況下的傳遞對準問題, 提出了一種結合基于四元數的非線性傳遞對準模型與非線性無跡卡爾曼濾波(UKF)算法的方法, 推導并建立了艦載武器捷聯慣導系統(SINS)的非線性誤差模型。該模型采用姿態四元數表示姿態誤差, 以提高姿態解算時的快速性和精度, 選用速度加姿態作為量測量, 以提高系統的可觀測性, 采用奇異值分解(SVD)方法解決了方差陣的病態問題, 以確保算法的魯棒性, 仿真結果表明, 該方法不僅解決了艦載武器在大失準角情況下的傳遞對準問題, 而且能夠有效提高傳遞對準的精度和快速性, 其計算精度和對準時間滿足系統設計要求。

艦載武器; 捷聯慣導系統; 非線性; 四元數; 無跡卡爾曼濾波; 傳遞對準

0 引言

為了提高艦載武器的快速反應能力, 對捷聯慣導系統(strap-down inertial navigation system, SINS)對準的快速性和精確性提出了越來越高的要求。傳遞對準是解決SINS動基座對準問題的主要方法。它是通過引入高精度的主慣導信息, 以此為基準運用卡爾曼濾波器來估計子慣導系統的速度誤差、姿態誤差和慣性器件誤差等狀態變量的值, 從而對子慣導信息進行補償。

由于艦載武器的多樣化以及低成本、小型化慣性器件的飛速發展, 在艦載武器傳遞對準過程中, 子慣導系統慣性器件的精度較低, 這會導致慣導計算產生較大的誤差, 并且主、子慣導之間的安裝誤差、桿臂效應、彈性變形等因素的影響也會產生較大的姿態誤差, 從而影響傳遞對準的精度。

捷聯慣導系統的精對準一般需在完成粗對準的基礎上進行, 通過艦載武器SINS陀螺儀和加速度計的輸出值, 與對準點已知的重力加速度和地球自轉角速度的矢量值構造方程, 計算出載體的姿態矩陣, 從而完成粗對準。而艦船在搖擺狀態下, 搖擺引起的干擾角速度遠大于地球自轉角速度, 陀螺輸出的信噪比很低, 且干擾角速度具有很寬的頻帶, 無法從陀螺輸出中提取地球自轉角速度, 粗對準實現起來很困難, 因此工程上往往希望可以在初始失準角較大的情況下直接進行精對準。此時如果仍然使用適用于小角度傳遞對準的線性模型進行估計, 將會導致很大的對準誤差。姿態四元數作為描述剛體旋轉運動的方法之一, 在許多領域得到了廣泛應用。與傳統的歐拉方法相比, 四元數對姿態轉動的描述更簡潔, 且四元數方程是線性微分方程, 具有線性化程度高、求解速度快、精度高及方程無奇異等優點。

無跡卡爾曼濾波(unscented Kalman filter, UKF)是Juliter等人提出的基于UT (unscented transformation)的卡爾曼濾波方法, 它是利用選取好的一組確定的采樣點對狀態變量的概率密度函數進行線性化, 然后再利用卡爾曼濾波的框架進行遞推濾波。UKF方法不需要計算雅可比矩陣, 不需要對狀態方程和量測方程線性化, 因此也就不像廣義卡爾曼濾波（extended Kalman filter, EKF）線性化處理時存在較大的截斷誤差。為了保證UKF算法的魯棒性, 采用奇異值分解的方法解決濾波過程中的方差陣的病態問題。

本文推導并建立了一種適用于艦載武器的基于四元數的非線性傳遞對準模型, 并采用速度加姿態的匹配方式與改進的UKF濾波算法相結合, 以提高艦載武器初始對準的精度和快速性。

1 非線性傳遞對準模型的建立

1.1 系統狀態方程的建立

1.1.1 桿臂效應誤差補償

桿臂效應是由于慣性測量組件安裝位置與載體中心不重合而引起加速度輸出中的干擾加速度, 它相當于加速度計的附加測量誤差。設安裝在載體上的慣性測量組件其加速度計偏離搖擺中心的距離(桿臂長度)為, 則由桿臂效應引起的干擾加速度為

對式(2)求導, 并將式(1)代入, 得到桿臂效應速度微分方程

1.1.2 速度誤差方程

由于安裝誤差及陀螺漂移等各種因素的影響, SINS數學平臺系與導航坐標系之間存在平臺失準角矢量,而導航系相對數學平臺系的姿態四元數可表示為

(5)

主慣導系統的速度誤差微分方程表示為

子慣導系統的速度誤差微分方程表示為

(7)

若子慣導的速度誤差定義為

對上式兩端求導, 并將式(3)、式(5)~式(7)代入得

(9)

1.1.3 姿態誤差方程

假設主慣導系統的慣性元器件不存在誤差, 則主慣導系統的姿態四元數微分方程表示為

(11)

子慣導系統的姿態四元數微分方程為

(14)

基于姿態四元數的快速傳遞對準模型的狀態方程形式表示為

假設子慣導系統的陀螺漂移和加速度計偏差為常值、主子慣導的相對位置固定不變, 并且認為, 則可以得到系統的狀態方程表達式

1.2 系統量測方程的建立

由于“速度+姿態角”匹配能同時兼顧計算參數和測量參數2種匹配方式的優點, 因此這里采用“速度+姿態角”匹配方式, 以主、子INS的速度和姿態角輸出差值作為量測量。那么, 基于四元數的傳遞對準模型量測方程為

(18)

2 三軸搖擺運動模型及可觀測性分析

捷聯慣性系統在三軸搖擺運動時的條件為載體以正弦規律繞航向軸、縱搖軸和橫搖軸搖擺, 其數學模型為

(20)

(21)

在設計卡爾曼濾波器之前, 通常先進行系統的可觀測性分析, 以確定濾波器的估計效果。由于艦載武器捷聯慣導是一個時變系統, 所以其可觀測性分析比定常系統復雜的多。如果用分段線性定常系統 (piece-wise constant system, PWCS)代替時變系統, 利用Meskin和Itzhack的可觀測矩陣(stripped observable matrix, SOM)代替總的可觀測陣(total observability matrix, TOM)來分析, 可使分析過程大為簡化。雖然這種分段定常系統可觀測性分析方法可定性地分析系統的可觀測性, 但無法定量地描述各狀態變量的可觀測程度。因此本文采用了一種能夠定量分析系統狀態可觀測度的奇異值分解方法。這種方法不需要事先進行濾波運算, 就可直接簡單地實現系統的可觀測度分析。它根據每個狀態所對應的奇異值的大小, 即可判斷狀態變量的可觀測度。較大的奇異值對應的狀態變量可以獲得較好的估計。反之, 當奇異值非常小或為零時, 狀態變量不可觀測。

根據艦船搖擺運動特點, 采用“速度+二軸搖擺”和“速度+三軸搖擺”運動模型在60 s的時間段內進行可觀測度分析。對于艦載武器非線性傳遞對準模型(16)和(18), 首先將非線性系統模型線性化得到系統的可觀測陣, 然后利用分析線性系統的奇異值分析方法得到各狀態變量的可觀測度, 如圖1所示(這里取艦船繞航向軸、縱搖軸和橫搖軸的搖擺角度幅值,,搖擺周期分別為,,初始航向為初始相位都為零, 艦船速度為)。

分析表明, “速度+二軸搖擺”時不考慮方位軸的艏搖, 可觀測矩陣的秩為13, 其中三軸速度誤差、主慣導載體坐標系和子慣導解算的載體坐標系之間的姿態誤差四元數、主慣導載體坐標系和子慣導真實載體坐標系之間四元數、東向和北向陀螺漂移以及2個軸向的桿臂長度, 這13個狀態變量的可觀測度較好, 其余狀態變量的奇異值很小,可認為狀態不可觀測; 而“速度+三軸搖擺”運動增加了方位軸向的角速度運動, 與“速度+二軸搖擺”運動相比, “速度+三軸搖擺”運動在可觀測度增加的同時, 不可觀測量也變得可觀測。因此, 本文選了“速度+三軸搖擺”運動模型, 這時卡爾曼濾波器可對系統狀態進行很好的估計, 從而使艦載武器SINS對準時可以獲得更好的對準性能。

圖1 狀態變量可觀測度分析結果

3 UKF算法特點與分析

EKF是一種應用廣泛的非線性系統濾波方法, 應用時首先需要將系統非線性模型進行泰勒展開, 保留1階項而忽略高階項以實現模型的線性化處理, 因此線性化處理后的系統模型存在較大的截斷誤差, 從而影響了模型的精確性, 降低了濾波的精度。而且, EKF將狀態方程中的模型誤差作為過程噪聲來處理, 且假設該噪聲為高斯白噪聲, 這與實際艦載武器SINS的實際噪聲情況并不相符。

近年來出現了一種基于UT變換的UKF非線性濾波算法。UKF算法的核心思想是: 首先, 在原狀態分布中取一些點, 使這些點的均值和協方差等于原狀態分布的均值和協方差; 再將這些點代入非線性函數中, 得到非線性函數值點集, 通過該點集求取變換后的均值和協方差; 最后, 根據正交投影法則導出遞推公式。對于線性系統來說, 它的濾波性能與卡爾曼濾波相當, 但對于非線性系統, 它的性能則明顯優于EKF濾波算法。

對于如下一個連續非線性系統

UKF算法的主要步驟概括為以下方面。

1）首先, 采用4階Runge Kutta法以數值積分的方法將系統模型進行離散化, 得到一個離散非線性系統

2）進行初始化

計算采樣點的權重

(26)

5) 時間更新

狀態向量的一步預測值為

(29)

一步預測均方誤差為

量測值的一步預測為

(31)

6) 量測更新

量測值的估計均方差方程

濾波器增益方程

狀態變量估計值的更新

(36)

狀態變量估計均方誤差方程

從上述UKF算法的主要步驟可看出, 這種UKF算法不再局限于系統或量測噪聲為高斯分布的情形, 不需要計算雅可比矩陣, 也不需要對狀態方程和量測方程進行線性化處理, 因此, 也就不像EKF線性化處理時存在較大的截斷誤差。

4 仿真結果與分析

艦船三軸搖擺運動模型參數同第2節進行系統可觀測性分析時是一致的。其他參數設置如下。

7) 估計誤差均方差初值為

8) 系統噪聲方差陣為

量測噪聲方差陣為

選取地理坐標系為導航坐標系, 載體坐標系為右、前、上方向。利用以上參數, 分別用EKF方法和UKF方法對所建立的傳遞對準非線性模型(16)和(18)進行狀態估計, 結果如圖2~ 8所示。其中, 圖8為基于EKF和UKF兩種非線性濾波算法估計出的四元數計算出的相對姿態角。直觀來看, 兩種濾波算法估計出的主、子慣導真實載體坐標系間姿態誤差四元數在20 s左右都能收斂到真實值附近, 且精度也較高, 三軸速度誤差也都能在10 s左右收斂到0值附近。

圖2 基于EKF算法的主子慣導真實載體坐標系姿態誤差四元數估計值

圖3 基于EKF算法的主子慣導真實載體坐標系姿態誤差四元數估計均方差曲線

圖4 基于EKF算法主子慣導速度誤差估計均方差曲線

圖5 基于UKF算法的主子慣導真實載體坐標系姿態誤差四元數估計值

圖6 基于UKF算法的主子慣導真實載體坐標系姿態誤差四元數估計均方差曲線

圖7 基于UKF算法主子慣導速度誤差估計均方差曲線

表1為應用這2種濾波方法所得主、子慣導真實載體坐標系之間姿態誤差四元數估計值的標準差, 表2為2種濾波算法估計出的主、子慣導真實載體坐標系之間的姿態誤差角的估計值。

圖8 基于2種濾波算法的主子慣導真實載體坐標系姿態誤差角估計值

表1 基于2種濾波算法的主子慣導真實載體坐標系之間姿態誤差四元數估計標準差

表2 基于2種濾波算法的主子慣導真實載體坐標系之間姿態誤差角估計值

由表1看出, 應用UKF算法進行傳遞對準時, 主、子慣導真實載體坐標系之間的姿態誤差四元數,,估計值的標準差均小于應用EKF方法進行傳遞對準時估計值的標準差, 而對于的估計值來說, 兩種算法的估計標準差相差很小。

由表2分析得到, 基于UKF方法的姿態角估計值分別為俯仰角4.991 4°, 航向角10.685 2°, 橫滾角10.021 4°, 而基于EKF方法的姿態角估計值分別為俯仰角5.468 2°, 航向角10.984 2°, 橫滾角10.379 6°, 與標稱值(5°,10°,10°)相比, 基于UKF算法的估計值更接近于真實值, 3個角的估計精度都大幅提高。而EKF算法由于線性化時忽略高階項產生的誤差較大而降低了濾波估計精度。從收斂性來說, 基于EKF和UKF估計的俯仰角都在大約16 s時收斂到真實值附近, 航向角在大約20 s時收斂到真實值附近, 而對于橫滾角, EKF濾波在大約20 s時收斂到真實值附近, 而UKF在10 s時就收斂到真實值附近。因此, 對于艦載武器SINS傳遞對準非線性模型(16)和(18), 無論是收斂速度還是估計精度, UKF的濾波方法都優于EKF的濾波方法。

5 結束語

由于基于四元數的非線性傳遞對準模型具有計算量小、精度高、非奇異性和全姿態工作以及大角度傳遞對準系統設計要求等特點, 具有很強的實用性。而采用速度加姿態角的傳遞對準匹配方式, 只需要載體進行簡單的搖擺運動即可快速完成對準, 這種匹配方式為大型載體在機動條件下實現快速對準提供了可能。因此, 對于艦載武器捷聯慣導系統, 速度+姿態角匹配的傳遞對準方式不失為一種理想的方式, 它不需要做特別的機動動作, 只需利用艦船的自然搖擺運動就可以提高系統的可觀測性, 從而提高對艦載武器SINS姿態誤差角的估計速度及精度。

另外, 由于UKF非線性濾波算法與傳統的EKF非線性濾波算法相比, 直接使用系統的非線性模型, 不需要對模型進行線線化處理, 也就不含有高端截斷誤差, 而且, UKF算法也不局限于系統或量測噪聲為高斯分布的情形。仿真結果表明, 對于速度誤差的估計, 2種濾波方法都能在10 s左右收斂, 且估計精度較高; 對于姿態誤差估計, 無論在收斂速度還是估計精度上, UKF方法都具有較高的估計精度和較快的收斂速度。因此, 對艦載武器SINS進行傳遞對準時, UKF是一種值得推廣應用的濾波方法。

[1] 王新龍, 申亮亮, 馬閃. 搖擺基座SINS快速精確傳遞對準方法[J]. 北京航空航天大學學報, 2009, 35(6): 728-731. Wang Xin-long, Shen Liang-liang, Ma Shan. Transfer Align- ment of Strapdown Inertial Navigation Systems on Rolling Bases[J]. Journal of Beijing University of Aeronautics and Astronautics, 2009, 35(6): 728-731.

[2] 秦永元, 嚴恭敏, 顧東晴, 等. 搖擺基座上基于信息的捷聯慣導粗對準研究[J]. 西北工業大學學報, 2005, 23(5): 681-684.

Qin Yong-yuan, Yan Gong-min, Gu Dong-qing, et al. A Clear Way of SINS Coarse Alignment Design Rockong Ship[J]. Journal of Northwestern Polytechnical University, 2005, 23(5): 681-684.

[3] Julier S J, Uhlmann J K. A New Approach for Filtering Nonlinear System[C]//Proceedings of the 1995 American Control Conference, 1995: 1628-1632.

[4] 申亮亮, 王新龍, 陳濤. 艦載武器SINS速度+姿態匹配傳遞對準建模與仿真[J]. 魚雷技術, 2008,16(5): 22-26.

Shen Liang-liang, Wang Xin-long, Chen Tao. Modeling and Simulation of Velocity and Attitude Matching Transfer Ali- gnment of Strapdown Inertial Navigation System for Ship- borne Weapons[J], Torpedo Technology, 2008, 16(5): 22-26.

[5] 熊芝蘭, 郝燕玲, 孫楓. 基于四元數的慣導系統快速匹配對準算法[J]. 哈爾濱工程大學學報, 2008, 29(1): 28-34. Xiong Zhi-lan, Hao Yan-ling, Sun Feng. Rapid Matching Alignment Algorithm of Inertial Navigation System Based on Quaternion[J]. Jornal of Harbin Engineering University, 2008, 29(1): 28-34.

[6] 萬德鈞, 房建成. 慣性導航初始對準[M]. 南京: 東南大學出版社, 1999: 174.

[7] 程向紅, 萬德鈞, 仲巡. 捷聯慣導系統的可觀測性和可觀測度研究[J]. 東南大學學報, 1997, 27(6): 6-10. Cheng Xiang-hong, Wan De-jun, Zhong Xun. Study on Observability and Its Degree of Strapdown Inertial Navi- gation System[J]. Jouanal of Southeast University, 1997, 27(6): 6-10.

[8] Efebvre T L, Bruyninckx H, Schutter J De. Comment on “A New Method for the Nonlinear Transform of Means and Covariances in Filters and Estimators” [J]. IEEE Transactions on utomation Control, 2002, 47(8): 1406-1408.

[9] 孫昌躍, 鄧正隆. 大方位失準角的艦載武器INS對準[J]. 中國慣性技術學報, 2008, 16(5): 534-537.

Sun Chang-yue, Deng Zheng-long. Alignment of Shipborne Weapon INS with Large Inertial Azimuth Uncertainty[J]. Jornal of Chinese Inertial Technology, 2008, 16(5): 534-537.

[10] Bertozzi M, Binelli E, Roggi A.Stereo Vision-based Appro- hes for Pedestrian Detection[J]. Proceeding of 2005 IEEE Comter Society Conference on Computer Vision and Pattern Recognition, IEEE, 2005, 25(25): 16-23.

[11] 鄧勝利, 邵雷, 吳平. SINS/GPS全姿態組合導航系統的UKF算法研究[J]. 電光與控制, 2010, 17(12): 24-28.

Deng Sheng-li, Shao Lei, Wu Ping. The UKF Algorithm Used in All-Attitude Intergrated Navigation System of SINS/ GPS[J]. Electronics Optics & Control, 2010, 17(12): 24-28.

(責任編輯: 楊力軍)

Nonlinear Transfer Alignment Model and Filtering Algorithm for Strapdown Inertial Navigation System of Shipborne Weapons

LI Na, WANG Xin-long

(School of Astronautics, Beihang University of Aeronautics and Astronautics, Beijing 100191, China)

To satisfy the requirement of fast reaction and high precision of initial alignment for shipborne weapons, and to enhance the transfer alignment for shipborne weapons with large misalignment, we present a method of combining the rapid transfer alignment model based on quaternion and the nonlinear unscented Kalman filter (UKF) algorithm, and establish a nonlinear error model for strapdown inertial navigation system(SINS) of shipborne weapons. This error model introduces quaternion to describe the attitude errors in order to enhance the speed and accuracy in calculating attitude. We select a velocity and attitude matching approach to improve the observability of the system, and solve the ill-condition of covariance matrix of the traditional nonlinear UKF filter by using singular value decomposition(SVD) to ensure the algorithm robustness. The simulation results show that the proposed method can effectively improve the precision and speed of transfer alignment for shipborne weapons with large misalignment, and its calculation accuracy and alignment time meet the design requirements of the system.

shipborne weapons; strapdown inertial navigation system(SINS); nonlinear; quaternion; unscented Kalman filter(UKF); transfer alignment

TJ630.33; TJ765.3

A

1673-1948(2011)06-0443-08

2011-05-03;

2011-06-10.

李 娜(1983-), 女, 在讀碩士, 研究方向為慣性導航技術.