GPS/INS 組合制導(dǎo)彈藥空中對(duì)準(zhǔn)的初始滾轉(zhuǎn)角估計(jì)新算法

佘浩平，楊樹興，倪慧

(1.北京理工大學(xué) 飛行器動(dòng)力學(xué)與控制教育部重點(diǎn)實(shí)驗(yàn)室，北京100081;2.中國兵器科學(xué)研究院，北京100089)

0 引言

GPS/INS 組合制導(dǎo)彈藥從常規(guī)平臺(tái)發(fā)射過程中，通常需要承受高過載、高轉(zhuǎn)速等惡劣條件。例如，制導(dǎo)迫彈在發(fā)射過程中的最大過載為10 000 g左右，而制導(dǎo)炮彈的最大過載則高達(dá)幾萬個(gè)g.此外，制導(dǎo)彈藥發(fā)射時(shí)通常采用與常規(guī)彈藥一致的彈體高速旋轉(zhuǎn)方案，進(jìn)入有控段后才使彈體傾斜穩(wěn)定或低速旋轉(zhuǎn)。慣性導(dǎo)航系統(tǒng)一般要在發(fā)射結(jié)束后在空中重新進(jìn)行對(duì)準(zhǔn)?？罩袑?duì)準(zhǔn)一般分為兩步:粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)。粗對(duì)準(zhǔn)過程是給慣導(dǎo)系統(tǒng)提供粗略的初始值，為精對(duì)準(zhǔn)作準(zhǔn)備;在此基礎(chǔ)上，組合導(dǎo)航系統(tǒng)運(yùn)用Kalman 濾波器，融合GPS 和INS 數(shù)據(jù)，進(jìn)行精對(duì)準(zhǔn)。

在粗對(duì)準(zhǔn)時(shí)，位置、速度和俯仰角、偏航角等的初值可以方便地從裝定的彈道數(shù)據(jù)或重新捕獲的GPS 測(cè)量數(shù)據(jù)中獲得。但較為特殊的是，由于發(fā)射過程中彈體旋轉(zhuǎn)，即使是粗略的滾轉(zhuǎn)角初始值也不易獲得。因此，需要研究GPS/INS 組合制導(dǎo)彈藥在空中進(jìn)行對(duì)準(zhǔn)時(shí)，獲取彈體初始滾轉(zhuǎn)角的方法。一些制導(dǎo)彈藥上采用地磁測(cè)量元件來獲得滾轉(zhuǎn)角［1-2］，但這樣需要在彈上增加地磁測(cè)量元件。本文研究一種不需要額外增加硬件設(shè)備，直接利用GPS 和角速率陀螺的測(cè)量數(shù)據(jù)來估計(jì)GPS/INS 組合制導(dǎo)彈藥初始滾轉(zhuǎn)角的新算法。

1 估計(jì)算法的基本思路

GPS/INS 組合制導(dǎo)彈藥在發(fā)射過程結(jié)束后，通過滾轉(zhuǎn)控制或氣動(dòng)減旋，使彈體傾斜穩(wěn)定或低速旋轉(zhuǎn)。GPS 接收機(jī)重新進(jìn)行捕獲、跟蹤和定位以獲得GPS 位置和速度測(cè)量數(shù)據(jù)。與彈體捷聯(lián)安裝的3 個(gè)角速率陀螺可分別測(cè)得在各自安裝軸方向上的彈體角速率。

若彈體的滾轉(zhuǎn)角理想地保持為0°，則彈上兩個(gè)橫向角速率陀螺測(cè)量值就分別是彈體的俯仰和偏航角速率。如果彈體的滾轉(zhuǎn)角不為0°，則兩個(gè)橫向角速率陀螺的測(cè)量值是彈體俯仰角速率和偏航角速率的合成，其中還包含了滾轉(zhuǎn)角的信息。對(duì)于采用彈道式飛行方案的制導(dǎo)彈藥，飛行過程中必然存在俯仰姿態(tài)的變化。因此可從角速率陀螺的測(cè)量信息中，辯識(shí)出彈體的滾轉(zhuǎn)角。本文的滾轉(zhuǎn)角估計(jì)算法是依據(jù)這一基本思路提出的。

GPS/INS 組合制導(dǎo)彈藥在飛行中根據(jù)彈體滾轉(zhuǎn)體制的不同，可分為兩種方案:一種是彈體采取傾斜穩(wěn)定控制;另一種是彈體低速旋轉(zhuǎn)。下面分別對(duì)這兩種情況，討論彈體初始滾轉(zhuǎn)角的估計(jì)算法。

2 傾斜穩(wěn)定彈體的滾轉(zhuǎn)角估計(jì)算法

首先討論飛行中彈體傾斜穩(wěn)定的情況。此時(shí)傾斜穩(wěn)定回路將彈體的滾轉(zhuǎn)角控制在某一未知角度附近，需要對(duì)該角度進(jìn)行估計(jì)。

2.1 估計(jì)算法的基本原理

彈箭繞質(zhì)心轉(zhuǎn)動(dòng)運(yùn)動(dòng)的運(yùn)動(dòng)學(xué)方程［3］為

式中:ωxm、ωym、ωzm分別為彈體轉(zhuǎn)動(dòng)角速度ω 在彈體坐標(biāo)系3 個(gè)軸Oxm、Oym、Ozm上的分量;γ 為滾轉(zhuǎn)角;分別為俯仰角、俯仰角速率和偏航角速率。

由(1)式的前兩式解算ωym和ωzm得

結(jié)合(2)式說明估計(jì)算法的基本思路。等式左邊的ωym和ωzm可由兩個(gè)橫向角速率陀螺測(cè)得，若能獲知等式右邊?、和的值，則可計(jì)算得到滾轉(zhuǎn)角γ.由于不同時(shí)刻處計(jì)算得到的γ 值均不相同，考慮采用最小二乘估計(jì)法獲得滾轉(zhuǎn)角的最優(yōu)估計(jì)值.

記

于是可將(2)式寫成如下矩陣形式

設(shè)t 在t0～tn時(shí)段內(nèi)，滾轉(zhuǎn)角為γ，是一個(gè)未知的待估計(jì)值。取t 在t0～tn時(shí)段內(nèi)的測(cè)量數(shù)據(jù)來估計(jì)γ，每一時(shí)刻的角速率ωym、ωzm由角速率陀螺測(cè)得，每一時(shí)刻的利用GPS 速度測(cè)量數(shù)據(jù)進(jìn)行近似估計(jì)(具體方法在2.2 節(jié)中闡述)。將各時(shí)刻的相應(yīng)值代入(5)式，得如下矩陣方程式

式中:

應(yīng)用最小二乘估計(jì)法［4］，得到X 的估計(jì)值為

使得

取極小值。

得到X 即sinγ、cosγ 的估計(jì)值后，就可由(10)式計(jì)算初始滾轉(zhuǎn)角γ，

為確定滾轉(zhuǎn)角在360°范圍內(nèi)的取值，應(yīng)根據(jù)X(1)和X(2)的符號(hào)進(jìn)一步確定γ 的象限，最終得到初始滾轉(zhuǎn)角的估計(jì)值.

2.2 彈體姿態(tài)角參數(shù)的估計(jì)方法

在上述估計(jì)算法中，需知各時(shí)刻的俯仰角及其變化率和偏航角的變化率，下面介紹獲得這些姿態(tài)角參數(shù)的方法。

根據(jù)制導(dǎo)彈藥的特點(diǎn)，彈體在氣動(dòng)力作用下一般是靜穩(wěn)定的，無控時(shí)攻角和側(cè)滑角都很小，即姿態(tài)角均在彈道角附近擺動(dòng)。故用彈道角參數(shù)作為相應(yīng)姿態(tài)角參數(shù)的近似值。即采取如下近似:

式中θ、ψv分別為彈道傾角和彈道偏角。

用GPS 接收機(jī)提供的速度測(cè)量值在地面坐標(biāo)系各軸上的分量vx、vy、vz解算制導(dǎo)彈藥的彈道傾角和彈道偏角，計(jì)算公式如下

GPS 接收機(jī)輸出的速度測(cè)量數(shù)據(jù)是不連續(xù)的，根據(jù)(12)式可以計(jì)算得到輸出點(diǎn)處的彈道傾角和彈道偏角。輸出點(diǎn)之間的數(shù)據(jù)可用曲線擬合方法如最小二乘3 次曲線擬合方法［5］獲得。彈道傾角的變化率和彈道偏角的變化率則可通過計(jì)算擬合曲線在各點(diǎn)處的斜率獲得。

2.3 估計(jì)算法的步驟

為方便起見，將傾斜穩(wěn)定彈體的初始滾轉(zhuǎn)角估計(jì)算法的步驟歸納如下:

1)取t 在t0～tn時(shí)段內(nèi)的GPS 速度測(cè)量值vx、vy和vz，根據(jù)(12)式分別計(jì)算出各時(shí)間點(diǎn)處的彈道傾角θ 和彈道偏角ψv;

2)采用曲線擬合方法對(duì)θ 和ψv進(jìn)行曲線擬合，通過計(jì)算擬合曲線在各點(diǎn)處的斜率獲得和;

4)根據(jù)矩陣(8)式計(jì)算得到X 的估計(jì)值;

5)由(10)式計(jì)算初始滾轉(zhuǎn)角γ，并確定γ 的象限，最終得到初始滾轉(zhuǎn)角的估計(jì)值.

研究工作還考慮了另外兩種算法，為區(qū)別起見，將上面介紹的估計(jì)算法稱為算法A-1，下面介紹的兩種算法分別稱為算法A-2 和算法A-3。

算法A-2 與算法A-1 的不同之處在于僅用到(2)式中的第1 式，即此時(shí)

算法A-3 與算法A-1 的不同之處在于僅用到(2)式中的第2 式，即此時(shí)

3 低速旋轉(zhuǎn)彈體的滾轉(zhuǎn)角估計(jì)算法

再討論飛行中彈體低速旋轉(zhuǎn)的情況。此時(shí)滾轉(zhuǎn)角是連續(xù)變化的，需要估計(jì)特定時(shí)刻的滾轉(zhuǎn)角。

從(1)式的前兩式中解算出sinγ 和cosγ，得

將滾轉(zhuǎn)角γ 寫成角頻率和角相位的形式

又根據(jù)(1)式可得

對(duì)(17)式兩邊積分得

則(15)式的左邊可寫成

結(jié)合(15)式和(19)式分析，式中ωxm、ωym和ωzm可用橫向角速率陀螺測(cè)得，若能獲得?、和的值，則可計(jì)算得到初始滾轉(zhuǎn)角γ0;由于各時(shí)刻處計(jì)算得到的γ0值均不相同，考慮采用最小二乘估計(jì)法來獲得初始滾轉(zhuǎn)角的最優(yōu)估計(jì)值0.

記

則(15)式可寫成如下矩陣形式

注意到(22)式與(5)式具有相同的矩陣形式，只是其中符號(hào)所代表的含義不同?？刹捎门c算法A-1 類似的方法得到低速旋轉(zhuǎn)彈體的滾轉(zhuǎn)角估計(jì)算法。該算法的步驟與算法A-1 的基本相同，只是計(jì)算矩陣C 和Z 時(shí)，應(yīng)根據(jù)(22)式計(jì)算。得到t0時(shí)刻的滾轉(zhuǎn)角估計(jì)值后，進(jìn)一步根據(jù)(18)式可計(jì)算出后續(xù)各時(shí)刻的滾轉(zhuǎn)角。

研究工作還考慮了另外兩種算法，為區(qū)別起見，將上面介紹的算法稱為算法B-1，下面介紹的兩種算法分別稱為算法B-2 和算法B-3。

算法B-2 與算法B-1 的不同之處在于僅用到(15)式中的第1 式，即此時(shí)

算法B-3 與算法B-1 的不同之處在于僅用到(15)式中的第2 式，即此時(shí)

各算法的具體性能如何，下面將結(jié)合算例進(jìn)行仿真和性能分析。

4 估計(jì)算法的仿真和性能分析

4.1 估計(jì)算法的仿真條件

以某制導(dǎo)彈藥的模型參數(shù)為例進(jìn)行仿真分析，采用彈道式飛行方案，發(fā)射角為50°.分別研究飛行中彈體傾斜穩(wěn)定和低速旋轉(zhuǎn)兩種情況，在MATLAB/Simulink 環(huán)境下建立仿真模型，進(jìn)行初始滾轉(zhuǎn)角估計(jì)算法的仿真和性能分析。

假定角速率陀螺在發(fā)射過程中的測(cè)量數(shù)據(jù)不可用，GPS 接收機(jī)重新捕獲到衛(wèi)星信號(hào)后，從t=15 s起輸出速度測(cè)量數(shù)據(jù)。以彈體轉(zhuǎn)動(dòng)角速度在彈體坐標(biāo)系各軸上的分量ωxm、ωym、ωzm的仿真值(或加入相應(yīng)的誤差)，分別作為安裝在彈體3 個(gè)軸上的角速率陀螺的測(cè)量值。以彈的飛行速度在地面坐標(biāo)系各軸上的分量vx、vy、vz的仿真值(或加入相應(yīng)的誤差)，分別作為GPS 接收機(jī)提供的3 個(gè)方向上的速度測(cè)量值。角速率陀螺和GPS 接收機(jī)的數(shù)據(jù)輸出率分別為1 000 Hz 和1 Hz，即每秒獲得1 000 次角速率測(cè)量數(shù)據(jù)和1 次速度測(cè)量數(shù)據(jù)。采用t 在15～25 s時(shí)段內(nèi)的測(cè)量數(shù)據(jù)來估計(jì)滾轉(zhuǎn)角。

在確定滾轉(zhuǎn)角之前，不便對(duì)彈體進(jìn)行俯仰和偏航控制，這里將無控時(shí)的姿態(tài)角和彈道角進(jìn)行比較。仿真計(jì)算表明，無風(fēng)或有隨機(jī)風(fēng)干擾時(shí)，姿態(tài)角均在彈道角附近擺動(dòng)，彈道角體現(xiàn)了姿態(tài)角的平均效果。而有常值風(fēng)作用時(shí)，姿態(tài)角與彈道角之間存在一個(gè)固定的差值，這是因?yàn)閺楏w產(chǎn)生了平衡攻角和側(cè)滑角。圖1中給出無風(fēng)情況下的比較圖。

在多種條件下進(jìn)行了算法的仿真，仿真條件見表1。其中，對(duì)于有誤差的情況，根據(jù)制導(dǎo)彈藥的典型情況，其誤差取值如下:

1)GPS 在3 個(gè)方向上的速度測(cè)量值誤差均為:均值為0，標(biāo)準(zhǔn)偏差為0.1 m/s，按正態(tài)分布;

2)3 個(gè)陀螺的角速率測(cè)量誤差均為:均值為0，標(biāo)準(zhǔn)偏差為10-4rad/s，按正態(tài)分布;

3)隨機(jī)風(fēng)條件下，縱風(fēng)、橫風(fēng)和垂直風(fēng)速均為:均值為0，標(biāo)準(zhǔn)偏差為3 m/s，按正態(tài)分布;

4)常值風(fēng)條件下，縱風(fēng)、橫風(fēng)和垂直風(fēng)速的大小均為20 m/s.

4.2 估計(jì)算法的仿真結(jié)果

圖1 姿態(tài)角與彈道角的比較Fig.1 Comparison between attitude angles and ballistic angles

表1 估計(jì)算法的仿真條件Tab.1 Simulation conditions of estimation algorithm

針對(duì)彈體傾斜穩(wěn)定情況，分別采用算法A-1、算法A-2 和算法A-3 進(jìn)行滾轉(zhuǎn)角估計(jì)。將t=15 s 時(shí)利用估計(jì)算法得到的滾轉(zhuǎn)角與實(shí)際彈道計(jì)算得到的滾轉(zhuǎn)角相比較。其中，滾轉(zhuǎn)角估計(jì)值的取值范圍為(-180°，180°］，而實(shí)際彈道計(jì)算得到的滾轉(zhuǎn)角要減去整周部分(即減去360°的整數(shù)倍部分)，兩者相減得到滾轉(zhuǎn)角估計(jì)誤差。在不同估計(jì)算法和不同仿真條件下，各進(jìn)行100 次滾轉(zhuǎn)角估計(jì)，并求得估計(jì)誤差的平均值和標(biāo)準(zhǔn)偏差，計(jì)算結(jié)果見表2所示。

表2 估計(jì)算法A 的誤差值Tab.2 Estimated errors of algorithm A

針對(duì)彈體低速旋轉(zhuǎn)情況，分別采用算法B-1、算法B-2 和算法B-3 進(jìn)行滾轉(zhuǎn)角估計(jì)，進(jìn)行上述計(jì)算，所得結(jié)果見表3所示。

表3 估計(jì)算法B 的誤差值Tab.3 Estimated errors of algorithm B

4.3 估計(jì)算法的性能分析

通過對(duì)不同滾轉(zhuǎn)角估計(jì)算法和各種仿真條件下的仿真結(jié)果進(jìn)行分析，可得以下結(jié)論:

1)應(yīng)選用算法A-1 作為傾斜穩(wěn)定彈體的滾轉(zhuǎn)角估計(jì)算法。

算法A-1 在各種條件下的估計(jì)精度都比較高，估計(jì)誤差在2°以內(nèi)。而算法A-2 和A-3 在各種條件下的估計(jì)誤差都特別大，因?yàn)楫?dāng)俯仰角速度比偏航角速度大得多時(shí)，這兩種估計(jì)算法中的矩陣C 是一個(gè)病態(tài)矩陣，很容易導(dǎo)致算法發(fā)散。

2)可選用算法B-1 或算法B-2、算法B-3 作為低速旋轉(zhuǎn)彈體的滾轉(zhuǎn)角估計(jì)算法。

算法B-2 和B-3 的估計(jì)精度與算法B-1 的估計(jì)精度都比較高且差別不大，估計(jì)誤差在3°以內(nèi)。其中，算法B-2 和B-3 的計(jì)算量比B-1 要小一些。

3)風(fēng)(包括隨機(jī)風(fēng)和常值風(fēng))、GPS 速度測(cè)量誤差和陀螺角速率測(cè)量誤差均對(duì)估計(jì)精度有影響，其中風(fēng)是造成滾轉(zhuǎn)角估計(jì)誤差的主要因素。

制導(dǎo)彈藥在有風(fēng)條件下飛行時(shí)，彈道傾角、彈道偏角分別與俯仰角、偏航角之間存在由風(fēng)引起的夾角，估計(jì)算法中的近似條件符合性不夠好，從而影響估計(jì)算法的精度。

5 結(jié)束語

本文提出的初始滾轉(zhuǎn)角估計(jì)算法，適用于采用彈道式飛行方案、具有氣動(dòng)靜穩(wěn)定性、飛行中彈體傾斜穩(wěn)定或低速旋轉(zhuǎn)、需要進(jìn)行空中對(duì)準(zhǔn)的GPS/INS組合制導(dǎo)彈藥，可滿足為粗對(duì)準(zhǔn)提供滾轉(zhuǎn)角初值的精度要求。完成粗對(duì)準(zhǔn)后，就可進(jìn)一步利用Kalman濾波器，融合GPS 和INS 數(shù)據(jù)進(jìn)行精對(duì)準(zhǔn)。

References)

［1］ Bradford S Davis.Using low-cost MEMS accelerometers and gyroscopes as strapdown IMUS on rolling projectiles［C］∥IEEE PLANS，Position Location and Navigation Symposium.US:IEEE，1996.

［2］ 高峰，張合.基于基準(zhǔn)角和補(bǔ)償角的常規(guī)彈藥滾轉(zhuǎn)角磁探測(cè)算法研究［J］.探測(cè)與控制學(xué)報(bào)，2008，30(5):11-15.GAO Feng，ZHANG He.Algorithm of roll angle determination of conventional ammunitions based on benchmark angle and compensation angle［J］.Journal of Detection ＆ Control，2008，30(5):11-15.(in Chinese)

［3］ 錢杏芳，林瑞雄，趙亞男.導(dǎo)彈飛行力學(xué)［M］.北京理工大學(xué)出版社，2000.QIAN Xing-fang，LIN Rui-xiong，ZHAO Ya-nan.Missile flight mechanics［M］.Beijing:Beijing Institute of Technology Press，2000.(in Chinese)

［4］ 鐘秋海，付夢(mèng)印.現(xiàn)代控制理論與應(yīng)用［M］.北京:機(jī)械工業(yè)出版社，1997.ZHONG Qiu-hai，F(xiàn)U Meng-yin.Modern control theory and applications［M］.Beijing:China Machine Press，1997.(in Chinese)

［5］ 丁麗娟.數(shù)值計(jì)算方法［M］.北京:北京理工大學(xué)出版社，1997.DING Li-juan.Numerical calculation method［M］.Beijing:Beijing Institute of Technology Press，1997.(in Chinese)