一種基于慣性匹配的船體姿態(tài)基準(zhǔn)傳遞方法

袁二凱，楊功流，于 沛，唐 改

(1．北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院，北京100191;2．中國人民解放軍93716 部隊(duì)，天津301700)

0 引 言

現(xiàn)代艦船都配備有雷達(dá)、導(dǎo)彈發(fā)射架、飛機(jī)等攻防設(shè)備，為保證它們的正常運(yùn)行，需要艦船為其提供一定精度的姿態(tài)、位置等運(yùn)動參數(shù)。艦船上的中心航姿系統(tǒng)是高精度慣導(dǎo)系統(tǒng)或平臺羅經(jīng)，它可以測量并提供這些參數(shù)［1］。但是由于船體變形角的存在，設(shè)備安裝點(diǎn)和中心航姿系統(tǒng)之間存在角偏差，直接影響到設(shè)備的使用精度。消除船體變形的影響，獲取船體局部高精度姿態(tài)信息成為近年來國內(nèi)外研究的一個(gè)熱點(diǎn)問題。目前部分艦船采用光學(xué)方法測量主慣導(dǎo)與設(shè)備之間的船體變形，在中心主慣導(dǎo)高精度姿態(tài)基準(zhǔn)上補(bǔ)償變形量以獲取戰(zhàn)位點(diǎn)處的高精度姿態(tài)信息，但光學(xué)測量系統(tǒng)成本高昂，結(jié)構(gòu)復(fù)雜，應(yīng)用并不廣泛。在各用戶設(shè)備處安裝高精度航姿系統(tǒng)成本很高，而且俄羅斯的早期實(shí)踐證明這一方法并不完善［2－4］。

本文提出一種基于慣性姿態(tài)匹配法實(shí)時(shí)估計(jì)船體變形以實(shí)現(xiàn)姿態(tài)基準(zhǔn)傳遞的方法。在對光學(xué)設(shè)備測得的船體變形角數(shù)據(jù)進(jìn)行頻域分析的基礎(chǔ)上，對船體變形角高精度建模。通過在用戶設(shè)備旁邊安裝低精度的捷聯(lián)慣導(dǎo)系統(tǒng)，利用中心主慣導(dǎo)與捷聯(lián)慣導(dǎo)2 個(gè)慣性測量單元(IMU)輸出的姿態(tài)信息作為觀測量，建立卡爾曼濾波方程，實(shí)現(xiàn)船體變形角的實(shí)時(shí)最優(yōu)估計(jì)，在主慣導(dǎo)輸出姿態(tài)信息的基礎(chǔ)上補(bǔ)償變形量，將高精度姿態(tài)基準(zhǔn)傳遞給用戶設(shè)備。

1 姿態(tài)匹配算法

1.1 坐標(biāo)定義

高精度姿態(tài)基準(zhǔn)傳遞方法中各系統(tǒng)的安裝位置及坐標(biāo)定義如圖1所示。IMU1 為船體的中心主慣導(dǎo)，是高精度慣導(dǎo)系統(tǒng)或平臺羅經(jīng)，全船的姿態(tài)信息均以IMU1 的姿態(tài)信息為基準(zhǔn)，oy 軸沿著船體的縱軸，oz 軸與甲板面相垂直并指向上方，ox 軸與oy、oz 軸構(gòu)成右手直角坐標(biāo)系，即為右前上坐標(biāo)系。IMU2 安裝在用戶設(shè)備附近，與IMU1 近似對準(zhǔn)，為較低精度捷聯(lián)慣導(dǎo)，坐標(biāo)系為ox′y′z′。姿態(tài)基準(zhǔn)傳遞方案利用IMU1 和IMU2 的姿態(tài)輸出，估計(jì)二者之間的實(shí)時(shí)船體變形角，將IMU1 的高精度姿態(tài)信息傳遞到IMU2 所在戰(zhàn)位點(diǎn)。定義ox 方向的船體變形為縱撓角，oy 方向的船體變形為橫撓角，oz 方向的船體變形為首撓角。

圖1 系統(tǒng)的安裝位置及坐標(biāo)定義Fig.1 The position of the system and the coordinate definition

對算法中各坐標(biāo)系定義如下:

定義IMU1 確立的載體坐標(biāo)系為b1，IMU2 確立的載體坐標(biāo)系為b2。IMU2 為低精度慣導(dǎo)，由于陀螺漂移等因素的影響，t 時(shí)刻IMU2 的計(jì)算坐標(biāo)系會慢慢偏離b2，定義t 時(shí)刻IMU2 的計(jì)算坐標(biāo)系b′2。

1.2 姿態(tài)匹配算法原理

根據(jù)坐標(biāo)系的定義和坐標(biāo)變換原理可得:

式(1)中左右兩邊狀態(tài)轉(zhuǎn)移矩陣對應(yīng)的歐拉角相等。記t 時(shí)刻2 個(gè)IMU 之間的船體變形角為φ對應(yīng)的姿態(tài)歐拉角為θε，則對應(yīng)的歐拉角可近似為φ+θε。

記

可得

由式(5)可見，2 個(gè)IMU 輸出姿態(tài)信息，實(shí)時(shí)變形角與IMU2 姿態(tài)誤差角之間存在線性關(guān)系，可以此為基礎(chǔ)設(shè)計(jì)卡爾曼濾波器實(shí)現(xiàn)對船體變形角的實(shí)時(shí)最優(yōu)估計(jì)，從而實(shí)現(xiàn)高精度姿態(tài)基準(zhǔn)的傳遞。

2 船體變形角的高精度建模

研究表明［1］，船體變形的頻率集中在0.3 Hz 以內(nèi)，由變化較快的動態(tài)變形θ 和變化緩慢的靜態(tài)變形Φ 兩部分組成。記t 時(shí)刻2 個(gè)IMU 之間的船體變形角為φ，可得:

為了實(shí)現(xiàn)對船體變形角的準(zhǔn)確建模，本文選取一段光學(xué)設(shè)備采集的船體變形實(shí)測數(shù)據(jù)(理論精度達(dá)到1″，可近似視為船體的實(shí)際變形)進(jìn)行了頻域分析。

縱撓角的頻譜如圖2所示。圖中頻率低于0.3 Hz 的2 個(gè)峰值體現(xiàn)的是船體變形角的頻率特性，高于0.4 Hz 的峰值反映的是由于設(shè)備震動產(chǎn)生的噪聲，可以在不影響變形角成分的情況下將其與高頻白噪聲一起濾除。而且從頻率上看，靜態(tài)變形和動態(tài)變形分布界限明顯:靜態(tài)變形頻率集中在0.05 Hz 以內(nèi)，動態(tài)變形集中在0.1 ～0.3 Hz，因此在頻域內(nèi)將2 種變形分離并建模可行。濾除高頻噪聲和分離2 類變形都可以通過小波分析法實(shí)現(xiàn)［7］。

圖2 縱撓角的頻譜圖Fig.2 The spectrum curve of the pitching deformation

其中，σ 為靜態(tài)變形角速率ωΦi的標(biāo)準(zhǔn)差，作為白噪聲的驅(qū)動系數(shù)，可以在分離靜態(tài)變形角后求得。

動態(tài)變形θ 可視為二階Markov 過程，其對應(yīng)的變形濾波器方程可表示為:

其中，μ 為動態(tài)變形不規(guī)則系數(shù) (相關(guān)時(shí)間的倒數(shù));λ 為動態(tài)變形的支配頻率;D 為動態(tài)變形的方差;b2=μ2+λ2;w(t)為均值為0、方差為1的高斯白噪聲。動態(tài)變形的支配頻率λ 即為圖2中0.1 ～0.3 Hz 范圍內(nèi)峰值對應(yīng)的頻率。相關(guān)時(shí)間和方差可以在分離動態(tài)變形后通過數(shù)學(xué)方法解算得到。

通過在頻域內(nèi)分離靜態(tài)變形和動態(tài)變形，式(7)和式(8)可以實(shí)現(xiàn)對船體變形角的精確建模，并作為狀態(tài)量在卡爾曼濾波器中實(shí)現(xiàn)最優(yōu)估計(jì)。

3 卡爾曼濾波器模型的建立

3.1 狀態(tài)方程的建立

IMU2 的姿態(tài)誤差角可表示為:

卡爾曼濾波器狀態(tài)向量取如下18 維形式:

其中:Φ 為靜態(tài)變形角;ωΦ為靜態(tài)變形角速度;θ為動態(tài)變形角;˙θ 為動態(tài)變形角速度;θε為IMU2 陀螺漂移造成的姿態(tài)失準(zhǔn)角;ε 為IMU2 的陀螺漂移。

系統(tǒng)狀態(tài)方程的矩陣形式如下:

其中F 為18 ×18 維的稀疏矩陣，由式(8) ～式(10)可得:

G 為系統(tǒng)噪聲系數(shù)矩陣，是18 ×18 維的對角陣，其中非零元素為:

3.2 量測方程的建立

卡爾曼濾波量測方程的矩陣形式為:

Z 的定義如式(4)，由式(6)、式(7)和式(10)得:

4 仿真驗(yàn)證與結(jié)果分析

4.1 仿真條件

1)船體繞x，y，z 三軸以正弦規(guī)律做搖擺運(yùn)動，繞x，y，z 三軸的搖擺周期分別為8 s，7 s，6 s;搖擺幅度分別為4°，5°，3°;初始水平姿態(tài)角為0;航向角為30°;仿真時(shí)間50 h。

2)IMU1 作為姿態(tài)基準(zhǔn)，姿態(tài)輸出不含陀螺漂移等誤差信息;IMU2 的姿態(tài)輸出是在船體姿態(tài)角的基礎(chǔ)上，加入陀螺漂移和船體變形角。IMU2 的x，y，z 三軸陀螺漂移分別為0.005°/h，0.005°/h，0.01°/h;船體變形角將實(shí)測船體變形角速度加到IMU2 陀螺的角速度輸出中參與姿態(tài)解算。

4.2 仿真結(jié)果分析

利用小波分析法濾除高頻噪聲分離靜態(tài)和動態(tài)變形結(jié)果如圖3所示。計(jì)算可得動態(tài)變形模型參數(shù)為:μ=［0.1，0.0625，0.0743］，1/s，λ=［0.13，0.1，0.13］，Hz，D=［9.6 ×10－7，2.25 ×10－5，2.2 ×10－7］，deg;靜態(tài)變形模型參數(shù)為:σ=［3.7×10－6，2.7 ×10－6，7.3 ×10－7］，deg。

圖3 小波分析法分離靜態(tài)和動態(tài)變形結(jié)果Fig.3 Separation result of static and dynamic deformation

卡爾曼濾波估計(jì)結(jié)果如圖4所示，算法實(shí)現(xiàn)了對船體靜態(tài)變形和動態(tài)變形的分離估計(jì)，IMU2 的x，y，z 三個(gè)軸向陀螺漂移分別收斂于0.005°/h，0.005°/h，0.01°/h，與仿真設(shè)定值一致。總體變形角的估計(jì)結(jié)果與理論值對比曲線和估計(jì)誤差曲線如圖5所示，船體變形角的估計(jì)結(jié)果與理論值一致，x，y，z 三個(gè)軸向的變形角估計(jì)誤差均方差分別為2.3512″，4.565″，3.1242″。姿態(tài)匹配算法有效抑制了捷聯(lián)慣導(dǎo)陀螺漂移的影響，實(shí)現(xiàn)了對船體變形角的高精度實(shí)時(shí)估計(jì)。捷聯(lián)慣導(dǎo)所在局部位置的理想姿態(tài)為船體中心姿態(tài)和局部變形量之和，實(shí)現(xiàn)了船體變形角的高精度估計(jì)，在中心主慣導(dǎo)高精度測量結(jié)果的基礎(chǔ)上補(bǔ)償變形量就能得到設(shè)備所在位置的姿態(tài)信息，所以局部位置姿態(tài)信息的精度取決于中心主慣導(dǎo)的姿態(tài)測量精度和船體變形角的估計(jì)精度。

捷聯(lián)慣導(dǎo)的姿態(tài)測量誤差和姿態(tài)基準(zhǔn)傳遞法得到的姿態(tài)誤差對比如圖6所示。捷聯(lián)慣導(dǎo)由于陀螺漂移的影響，姿態(tài)測量誤差發(fā)散較快，姿態(tài)匹配算法可以實(shí)現(xiàn)船體變形角的實(shí)時(shí)高精度估計(jì)，當(dāng)中心主慣導(dǎo)的姿態(tài)測量結(jié)果為理想值時(shí)，由于變形角估計(jì)均方誤差在5″以內(nèi)，所以基準(zhǔn)傳遞精度可以達(dá)到5″以內(nèi)。

圖4 卡爾曼濾波估計(jì)結(jié)果Fig.4 Estimation result of the Kalman filter

圖5 變形估計(jì)結(jié)果與理論值對比曲線及誤差曲線Fig.5 Comparison of simulation result and the initial measurement data and the estimation error curve

圖6 姿態(tài)測量誤差和姿態(tài)傳遞誤差曲線Fig.6 Curves of attitude measurement error and the attitude reference transfer error

5 結(jié) 語

本文提出一種基于慣性姿態(tài)匹配法實(shí)時(shí)估計(jì)船體變形以實(shí)現(xiàn)姿態(tài)基準(zhǔn)傳遞的方法。姿態(tài)匹配法利用中心主慣導(dǎo)和用戶設(shè)備處捷聯(lián)慣導(dǎo)的姿態(tài)輸出構(gòu)建卡爾曼濾波方程，在對船體變形高精度建模的基礎(chǔ)上，可以實(shí)現(xiàn)船體變形角的實(shí)時(shí)高精度估計(jì)，將主慣導(dǎo)高精度的姿態(tài)基準(zhǔn)傳遞給用戶設(shè)備。仿真驗(yàn)證表明，算法能夠有效抑制低精度捷聯(lián)慣導(dǎo)陀螺漂移因素對船體變形估計(jì)結(jié)果的影響，中心主慣導(dǎo)的姿態(tài)測量結(jié)果為理想值時(shí)，姿態(tài)基準(zhǔn)傳遞精度在5″以內(nèi)，說明船體變形角建模方法和姿態(tài)匹配算法的可行性，為船體局部高精度姿態(tài)信息獲取提供了理論參考。

［1］MOCHALOV A V．A system for measuring deformation of large-sized objects［C］．RTO/ NATO，F(xiàn)rance，1999，15:1－15．

［2］MOCHALOV A V，KAZANTSEV．Use of the ring laser units for measurement of the moving object deformation［C］．Proceedings of SPIE，2002(4680):85－92．

［3］朱昀炤，汪順亭，繆玲娟，等．船體變形測量技術(shù)綜述［J］．船舶工程，2007，29(6):58－61．

ZHU Yun-zhao，WANG Shun-ting，MIAO Ling-juan，et al．Review of measuring technique for ship deformation［J］．Ship Engineering，2007，29(6):58－61．

［4］TITTERTON D H，WESTON J L．Strapdown inertial navigation technology(Second edition)［M］．The Institution of Electrical Engineers，2004．

［5］鄭佳興，秦石喬，王省書．基于姿態(tài)匹配的船體形變測量方法［J］．中國慣性技術(shù)學(xué)報(bào)，2010，18(2):175－180．

ZHENG Jia-xing，QIN Shi-qiao，WANG Xing-shu．Ship hull angular deformation measurement taking slow-varying quasi-static component into account［J］．Journal of Chinese Inertial Technology，2010，18(2):175－180．

［6］SCHNEIDER A M，Kalman filter formulations for transfer alignment of strapdown inertial units［J］．Journal of the Institute of Navigation，1983，30(1):72－89．

［7］WEN M．Spectrum denoise by wavelet analyze and its realize in Matlab［J］．Modern Electronics Technique，2003，26(24):47－49．