李 娜, 王新龍

?

艦載武器SINS非線性傳遞對準(zhǔn)模型及濾波算法

李 娜, 王新龍

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

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

艦載武器; 捷聯(lián)慣導(dǎo)系統(tǒng); 非線性; 四元數(shù); 無跡卡爾曼濾波; 傳遞對準(zhǔn)

0 引言

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

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

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

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

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

1 非線性傳遞對準(zhǔn)模型的建立

1.1 系統(tǒng)狀態(tài)方程的建立

1.1.1 桿臂效應(yīng)誤差補償

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

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

1.1.2 速度誤差方程

由于安裝誤差及陀螺漂移等各種因素的影響, SINS數(shù)學(xué)平臺系與導(dǎo)航坐標(biāo)系之間存在平臺失準(zhǔn)角矢量,而導(dǎo)航系相對數(shù)學(xué)平臺系的姿態(tài)四元數(shù)可表示為

(5)

主慣導(dǎo)系統(tǒng)的速度誤差微分方程表示為

子慣導(dǎo)系統(tǒng)的速度誤差微分方程表示為

(7)

若子慣導(dǎo)的速度誤差定義為

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

(9)

1.1.3 姿態(tài)誤差方程

假設(shè)主慣導(dǎo)系統(tǒng)的慣性元器件不存在誤差, 則主慣導(dǎo)系統(tǒng)的姿態(tài)四元數(shù)微分方程表示為

(11)

子慣導(dǎo)系統(tǒng)的姿態(tài)四元數(shù)微分方程為

(14)

基于姿態(tài)四元數(shù)的快速傳遞對準(zhǔn)模型的狀態(tài)方程形式表示為

假設(shè)子慣導(dǎo)系統(tǒng)的陀螺漂移和加速度計偏差為常值、主子慣導(dǎo)的相對位置固定不變, 并且認(rèn)為, 則可以得到系統(tǒng)的狀態(tài)方程表達式

1.2 系統(tǒng)量測方程的建立

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

(18)

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

捷聯(lián)慣性系統(tǒng)在三軸搖擺運動時的條件為載體以正弦規(guī)律繞航向軸、縱搖軸和橫搖軸搖擺, 其數(shù)學(xué)模型為

(20)

(21)

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

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

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

圖1 狀態(tài)變量可觀測度分析結(jié)果

3 UKF算法特點與分析

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

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

對于如下一個連續(xù)非線性系統(tǒng)

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

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

2）進行初始化

計算采樣點的權(quán)重

(26)

5) 時間更新

狀態(tài)向量的一步預(yù)測值為

(29)

一步預(yù)測均方誤差為

量測值的一步預(yù)測為

(31)

6) 量測更新

量測值的估計均方差方程

濾波器增益方程

狀態(tài)變量估計值的更新

(36)

狀態(tài)變量估計均方誤差方程

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

4 仿真結(jié)果與分析

艦船三軸搖擺運動模型參數(shù)同第2節(jié)進行系統(tǒng)可觀測性分析時是一致的。其他參數(shù)設(shè)置如下。

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

8) 系統(tǒng)噪聲方差陣為

量測噪聲方差陣為

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

圖2 基于EKF算法的主子慣導(dǎo)真實載體坐標(biāo)系姿態(tài)誤差四元數(shù)估計值

圖3 基于EKF算法的主子慣導(dǎo)真實載體坐標(biāo)系姿態(tài)誤差四元數(shù)估計均方差曲線

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

圖5 基于UKF算法的主子慣導(dǎo)真實載體坐標(biāo)系姿態(tài)誤差四元數(shù)估計值

圖6 基于UKF算法的主子慣導(dǎo)真實載體坐標(biāo)系姿態(tài)誤差四元數(shù)估計均方差曲線

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

表1為應(yīng)用這2種濾波方法所得主、子慣導(dǎo)真實載體坐標(biāo)系之間姿態(tài)誤差四元數(shù)估計值的標(biāo)準(zhǔn)差, 表2為2種濾波算法估計出的主、子慣導(dǎo)真實載體坐標(biāo)系之間的姿態(tài)誤差角的估計值。

圖8 基于2種濾波算法的主子慣導(dǎo)真實載體坐標(biāo)系姿態(tài)誤差角估計值

表1 基于2種濾波算法的主子慣導(dǎo)真實載體坐標(biāo)系之間姿態(tài)誤差四元數(shù)估計標(biāo)準(zhǔn)差

表2 基于2種濾波算法的主子慣導(dǎo)真實載體坐標(biāo)系之間姿態(tài)誤差角估計值

由表1看出, 應(yīng)用UKF算法進行傳遞對準(zhǔn)時, 主、子慣導(dǎo)真實載體坐標(biāo)系之間的姿態(tài)誤差四元數(shù),,估計值的標(biāo)準(zhǔn)差均小于應(yīng)用EKF方法進行傳遞對準(zhǔn)時估計值的標(biāo)準(zhǔn)差, 而對于的估計值來說, 兩種算法的估計標(biāo)準(zhǔn)差相差很小。

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

5 結(jié)束語

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

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

[1] 王新龍, 申亮亮, 馬閃. 搖擺基座SINS快速精確傳遞對準(zhǔn)方法[J]. 北京航空航天大學(xué)學(xué)報, 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] 秦永元, 嚴(yán)恭敏, 顧東晴, 等. 搖擺基座上基于信息的捷聯(lián)慣導(dǎo)粗對準(zhǔn)研究[J]. 西北工業(yè)大學(xué)學(xué)報, 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速度+姿態(tài)匹配傳遞對準(zhǔn)建模與仿真[J]. 魚雷技術(shù), 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] 熊芝蘭, 郝燕玲, 孫楓. 基于四元數(shù)的慣導(dǎo)系統(tǒng)快速匹配對準(zhǔn)算法[J]. 哈爾濱工程大學(xué)學(xué)報, 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] 萬德鈞, 房建成. 慣性導(dǎo)航初始對準(zhǔn)[M]. 南京: 東南大學(xué)出版社, 1999: 174.

[7] 程向紅, 萬德鈞, 仲巡. 捷聯(lián)慣導(dǎo)系統(tǒng)的可觀測性和可觀測度研究[J]. 東南大學(xué)學(xué)報, 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] 孫昌躍, 鄧正隆. 大方位失準(zhǔn)角的艦載武器INS對準(zhǔn)[J]. 中國慣性技術(shù)學(xué)報, 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全姿態(tài)組合導(dǎo)航系統(tǒng)的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.

(責(zé)任編輯: 楊力軍)

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-), 女, 在讀碩士, 研究方向為慣性導(dǎo)航技術(shù).