單玉浩 施建禮 彭文輝

(海軍潛艇學(xué)院 青島 266041)

?

潛射戰(zhàn)術(shù)導(dǎo)彈水彈道測(cè)量中的算法應(yīng)用研究*

單玉浩 施建禮 彭文輝

(海軍潛艇學(xué)院 青島 266041)

現(xiàn)有潛射戰(zhàn)術(shù)導(dǎo)彈射擊訓(xùn)練中,對(duì)于導(dǎo)彈彈道的監(jiān)控只有出水之后的空中彈道,而水中段尚無(wú)有效的監(jiān)測(cè)手段。而在訓(xùn)練打靶過(guò)程中,水中段是最薄弱環(huán)節(jié),導(dǎo)彈最易出現(xiàn)故障且故障原因難以確定,水彈道測(cè)量將為這些問(wèn)題的解決提供依據(jù)。論文將MEMS慣性傳感器應(yīng)用于潛射戰(zhàn)術(shù)導(dǎo)彈水彈道測(cè)量中,以捷聯(lián)慣導(dǎo)姿態(tài)更新中最惡劣的工作環(huán)境——錐運(yùn)動(dòng)作為仿真環(huán)境,分析四階龍格庫(kù)塔法和畢卡算法的優(yōu)劣,并以梯形公式計(jì)算畢卡算法中的角增量,仿真得出的結(jié)論有很好的工程實(shí)用價(jià)值。

潛射武器; 捷聯(lián)算法; 畢卡算法; 四階龍格庫(kù)塔算法

Class Number TJ630.33

1 引言

隨著現(xiàn)代戰(zhàn)爭(zhēng)的需要,高精度制導(dǎo)武器迅速發(fā)展起來(lái),潛射戰(zhàn)術(shù)導(dǎo)彈作為潛艇的主戰(zhàn)武器,其射程已達(dá)到幾百甚至上千公里,因此要求其具有相當(dāng)高的制導(dǎo)精度。慣導(dǎo)技術(shù)從產(chǎn)生到發(fā)展,已廣泛應(yīng)用到軍事領(lǐng)域,其不受外界環(huán)境的限制,不需要接收外界數(shù)據(jù),可靠性高。而且隨著靜電陀螺、光纖陀螺等高精度陀螺的產(chǎn)生,慣導(dǎo)系統(tǒng)正朝著小型化、高精度的方向發(fā)展[1]。

潛射戰(zhàn)術(shù)導(dǎo)彈打靶監(jiān)控手段較多,諸如差分GPS技術(shù)、靶場(chǎng)光電經(jīng)緯儀交會(huì)測(cè)量技術(shù),但是這只是針對(duì)導(dǎo)彈出水后空中段彈道的測(cè)量,對(duì)于其在水中段的監(jiān)控目前尚未形成有效手段,考慮導(dǎo)彈內(nèi)部空間小,水下段時(shí)間短的問(wèn)題,傳統(tǒng)的GPS等手段已不能滿足測(cè)量需求,本文考慮將捷聯(lián)式慣導(dǎo)系統(tǒng)應(yīng)用于潛射戰(zhàn)術(shù)導(dǎo)彈水彈道測(cè)量上。捷聯(lián)式慣導(dǎo)系統(tǒng)以數(shù)學(xué)平臺(tái)代替物理平臺(tái),其體積小、質(zhì)量輕、易于安裝,因此廣泛應(yīng)用于潛射戰(zhàn)術(shù)導(dǎo)彈上。這類慣導(dǎo)系統(tǒng)雖然略去了復(fù)雜的物理平臺(tái),但其相應(yīng)的算法就變得相當(dāng)復(fù)雜,優(yōu)化捷聯(lián)算法可以保證數(shù)學(xué)平臺(tái)的求解精度和快速性。捷聯(lián)慣導(dǎo)中陀螺輸出的解算是整個(gè)系統(tǒng)運(yùn)動(dòng)參數(shù)解算的基礎(chǔ)[2],其精度決定了整個(gè)系統(tǒng)的精度,因此研究系統(tǒng)的姿態(tài)解算算法可以很大程度提高整個(gè)武器系統(tǒng)的反應(yīng)能力和精度。

MEMS技術(shù)應(yīng)用在慣性技術(shù)上就產(chǎn)生了各種MEMS慣性傳感器,其中比較突出的體現(xiàn)就是硅微加速度計(jì)和硅微陀螺,這種慣性器件體積小、質(zhì)量輕、成本低、可靠性高、易于安裝[3],被廣泛應(yīng)用到民航及戰(zhàn)術(shù)導(dǎo)彈領(lǐng)域。潛射戰(zhàn)術(shù)導(dǎo)彈水下發(fā)射時(shí)間短,因此要求運(yùn)動(dòng)參數(shù)解算速度足夠快且慣導(dǎo)系統(tǒng)體積質(zhì)量足夠小,不會(huì)影響導(dǎo)彈的力學(xué)環(huán)境和其他設(shè)備的正常工作,因此選用MEMS慣性測(cè)量組件作為水彈道測(cè)量元件。

2 姿態(tài)解算算法

武器系統(tǒng)制導(dǎo)精度不僅取決于慣導(dǎo)系統(tǒng)的硬件精度,而且很大程度上更取決于算法的精度。姿態(tài)陣的計(jì)算是捷聯(lián)算法中最重要的部分[4],捷聯(lián)式慣導(dǎo)系統(tǒng)的姿態(tài)解算算法主要包括方向余弦法、歐拉法、四元數(shù)法和旋轉(zhuǎn)矢量法,其中四元數(shù)法算法簡(jiǎn)單,計(jì)算量比較小,易于實(shí)現(xiàn),因此在工程上較為實(shí)用。

2.1 四元數(shù)微分方程

表征地理坐標(biāo)系至載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)微分方程可表示為[5]

(1)

其中

2.2 定時(shí)采樣的畢卡算法

畢卡算法是通過(guò)角增量來(lái)計(jì)算四元數(shù)的一種算法,但MEMS陀螺的輸出一般為角速度信息,因此必須用合適的方法將角速度轉(zhuǎn)換為角增量。

采用定時(shí)采樣法,采樣時(shí)間間隔一定,tk+1時(shí)刻旋轉(zhuǎn)四元數(shù)的解為[1]

(2)

其中

在實(shí)際計(jì)算中,由于指數(shù)形式和三角運(yùn)算的計(jì)算復(fù)雜,因此為減少運(yùn)算的復(fù)雜程度,對(duì)eA進(jìn)行泰勒級(jí)數(shù)展開:

(3)

所以

代入式(3)可得四元數(shù)的各階近似算法。

一階近似算法為

Q(tk+1)=(I+A)Q(tk)

二階近似算法為

三階近似算法為

四階近似算法為

角速度與角度之間存在如下關(guān)系:

(4)

由上式可得角增量的近似值為

Δθ(tk)=h·ω(tk)

(5)

其中h為更新周期,h=tk+1-tk。通過(guò)各時(shí)刻的角速度信息的輸出,利用歐拉式(5)即可得到各時(shí)刻的角增量信息。

通過(guò)角速度得到角增量也可以通過(guò)數(shù)值積分來(lái)完成,對(duì)微分方程(4)兩端在區(qū)間[tk,tk+1]上積分,得

(6)

若用梯形公式計(jì)算式(6)右端,即

Δθ(tk) =Δθ(tk+1)-Δθ(tk)

(7)

2.3 四階龍格庫(kù)塔算法

假設(shè)姿態(tài)更新時(shí)間間隔為h,用四階龍格庫(kù)塔法解式(1),得遞推形式為

(8)

四元數(shù)與坐標(biāo)變換矩陣及姿態(tài)角的關(guān)系可表示為[6]

通過(guò)求解四元數(shù)微分方程,可以不斷得到姿態(tài)四元數(shù),進(jìn)而可以確定姿態(tài)矩陣,結(jié)合真值表就可以計(jì)算出航向角、俯仰角和橫滾角的真實(shí)值[7～8]。

3 圓錐運(yùn)動(dòng)下算法的可靠性驗(yàn)證

對(duì)捷聯(lián)陀螺來(lái)說(shuō),圓錐運(yùn)動(dòng)是最惡劣的工作環(huán)境,它會(huì)引起數(shù)學(xué)平臺(tái)的嚴(yán)重漂移[9],因此,如果能保證在圓錐運(yùn)動(dòng)條件下算法的可靠性,那么其他環(huán)境下算法也是可靠的。

在實(shí)現(xiàn)過(guò)程中,將圓錐運(yùn)動(dòng)的姿態(tài)四元數(shù)的解析表達(dá)式作為真值,分別檢驗(yàn)采用梯形公式的四階畢卡算法和四階龍格庫(kù)塔法的精度。

1) 仿真條件取圓錐運(yùn)動(dòng)的半錐角為a=1*pi/180;旋轉(zhuǎn)角速度為w=pi/3(rad/s),姿態(tài)更新周期為h=0.01s

當(dāng)轉(zhuǎn)軸在yoz平面時(shí),地理坐標(biāo)系至載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)為

角速度為

得到兩種算法的姿態(tài)角誤差分別如圖1～圖6所示。

圖1 畢卡算法的橫滾角誤差

圖2 畢卡算法的偏航角誤差

圖3 畢卡算法的俯仰角誤差

圖4 龍格庫(kù)塔法的橫滾角誤差

圖5 龍格庫(kù)塔法的偏航角誤差

圖6 龍格庫(kù)塔法的俯仰角誤差

當(dāng)轉(zhuǎn)軸在xoy平面上的圓錐運(yùn)動(dòng),地理坐標(biāo)系至載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)為

角速度為

得到兩種算法的姿態(tài)角誤差分別如圖7～圖12所示。

圖7 畢卡算法的橫滾角誤差

圖8 畢卡算法的偏航角誤差

圖9 畢卡算法的俯仰角誤差

圖10 四階龍格庫(kù)塔算法的橫滾角誤差

圖11 四階龍格庫(kù)塔算法的橫滾角誤差

圖12 四階龍格庫(kù)塔算法的橫滾角誤差

當(dāng)轉(zhuǎn)軸在xoz平面上,地理坐標(biāo)系至載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)為

角速度為

得到兩種算法的姿態(tài)角誤差分別如圖13～圖18所示。

圖13 畢卡算法的橫滾角誤差

圖14 畢卡算法的偏航角誤差

圖15 畢卡算法的俯仰角誤差

圖16 四階龍格庫(kù)塔法的橫滾角誤差

圖17 四階龍格庫(kù)塔法的偏航角誤差

圖18 四階龍格庫(kù)塔法的俯仰角誤差

可以看出,在取角速度為w=pi/3(rad/s),姿態(tài)更新周期為0.01s時(shí),60s內(nèi)錐運(yùn)動(dòng)的龍格庫(kù)塔法和畢卡算法的姿態(tài)計(jì)算誤差都在10-7數(shù)量級(jí)上,針對(duì)錐運(yùn)動(dòng)的旋轉(zhuǎn)軸不同可以看出,繞轉(zhuǎn)軸的姿態(tài)角計(jì)算誤差會(huì)逐漸發(fā)散,算法產(chǎn)生嚴(yán)重漂移,且畢卡算法的發(fā)散速度更快。

2) 上述條件仿真出的結(jié)果能滿足精度要求,考慮增大圓錐運(yùn)動(dòng)的半錐角[10],轉(zhuǎn)軸在yoz平面內(nèi),半錐角為a=20*pi/180時(shí),旋轉(zhuǎn)角速度為w=pi/3(rad/s),姿態(tài)更新周期為0.01s。

隨著錐運(yùn)動(dòng)的半錐角的增大,兩種算法的漂移也相應(yīng)地增大,俯仰角的漂移最快,橫滾角和偏航角的誤差也逐漸發(fā)散,但龍格庫(kù)塔法的算法漂移相對(duì)畢卡算法的速度較慢,在60s的時(shí)間內(nèi)兩種仿真精度在一個(gè)數(shù)量級(jí)上。

圖19 畢卡算法的橫滾角誤差

圖20 畢卡算法的偏航角誤差

圖21 畢卡算法的俯仰角誤差

圖22 龍格庫(kù)塔法的橫滾角誤差

圖23 龍格庫(kù)塔法的偏航角誤差

圖24 龍格庫(kù)塔法的俯仰角誤差

4 結(jié)語(yǔ)

當(dāng)振動(dòng)比較小,運(yùn)動(dòng)較平穩(wěn)時(shí),四元數(shù)法中的畢卡算法和四階龍格庫(kù)塔法的精度相當(dāng),但隨著振動(dòng)加劇,環(huán)境條件更加惡劣時(shí),四階龍格庫(kù)塔法精度要優(yōu)于畢卡算法,算法漂移率較小,從運(yùn)算精度上來(lái)講,可選擇四階龍格庫(kù)塔算法,但其在每步更新過(guò)程中需多次計(jì)算四元數(shù)微分方程式(1)右端的值,算法實(shí)現(xiàn)比較復(fù)雜,快速性和實(shí)時(shí)性略次于畢卡算法。Matlab仿真過(guò)程中使用tic、toc函數(shù)監(jiān)測(cè)算法運(yùn)行時(shí)間,四階龍格庫(kù)塔算法和畢卡算法的平均計(jì)算時(shí)間分別為1.0703e-04s和5.9295e-05s,畢卡算法的效率明顯高于四階龍格庫(kù)塔算法,從運(yùn)算速度上來(lái)講,可選擇畢卡算法。

對(duì)于潛射戰(zhàn)術(shù)導(dǎo)彈來(lái)說(shuō),其水下發(fā)射環(huán)境惡劣,水下彈道時(shí)間短,對(duì)其彈道測(cè)量來(lái)講,實(shí)時(shí)性和精度都是至關(guān)重要的,基于硬件系統(tǒng)考慮,在高采樣率下要求同時(shí)進(jìn)行捷聯(lián)解算,這就需要算法相對(duì)簡(jiǎn)單,同時(shí)從本文的結(jié)論發(fā)現(xiàn)四元數(shù)法中的四階龍格庫(kù)塔法和畢卡算法的精度在同一數(shù)量級(jí)上,因此考慮使用畢卡算法作為姿態(tài)更新算法。由于捷聯(lián)慣導(dǎo)器件直接固聯(lián)在載體上,受載體的角振動(dòng)影響大,引起誤差發(fā)散很快,而且對(duì)于導(dǎo)彈這種高速運(yùn)動(dòng)體,振動(dòng)環(huán)境惡劣,由高過(guò)載產(chǎn)生的非線性誤差大且不易補(bǔ)償,提高采樣頻率和姿態(tài)更新頻率是保證解算精度行之有效的方法。

[1] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2006:1-4.

[2] 賀元軍,盧曉東,呂春紅.基于MEMS捷聯(lián)慣導(dǎo)系統(tǒng)的解算與誤差修正方法[J].計(jì)算機(jī)測(cè)量與控制,2010,18(6):1364-1366.

[3] 李榮冰,劉建業(yè),曾慶華,等.基于MEMS技術(shù)的微型慣性導(dǎo)航系統(tǒng)的發(fā)展現(xiàn)狀[J].中國(guó)慣性技術(shù)學(xué)報(bào),2004,12(6):89-94.

[4] 孫麗,秦永元.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法比較[J].中國(guó)慣性技術(shù)學(xué)報(bào),2006,14(3):6-10.

[5] Qamar A, Shams, Michael Moniuszko, John C, Ingham. Applying MEMS technology to field[J]. Flight and Space Deployable Systems, IEEE,2001:246-255.

[6] 張榮輝,賈宏光,陳濤,等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程,2008,16(10):1964-1970.

[7] 楊淑潔.懸翼式微小飛行器姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)[D].哈爾濱:哈爾濱工業(yè)大學(xué)學(xué)位論文,2011:9-14.

[8] 趙鵬,沈庭芳,單寶堂.基于MEMS的微型無(wú)人機(jī)姿態(tài)儀的設(shè)計(jì)[J].太原:火力與指揮控制,2009,34(6):164-167.

[9] 季超,楊曉東.INS/GPS組合導(dǎo)航系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)研究[J].艦船電子工程,2012,32(55-57):164-167.

[10] Sameh Nassar, Klaus Peter Achwarz, Naserel Shemimy. Modeling Inertial Sensor Errors Using Autoregressive(AR) Models[J]. Navigation,2004,51(24):259-268.

Algorithm Application of Water Trajectory of Submarine-launched Missiles

SHAN Yuhao SHI Jianli PENG Wenhui

(Navy Submarine Academy, Qingdao 266041)

Aerial trajectory can be measured merely in the launch trainings of submarine-launched missiles, and underwater trajectory is not yet measured at present. Underwater trajectory is the most vulnerable in the whole trajectory and the breakdown reason is difficult to confirm. Measure of the underwater trajectory provides the possibility of breakdown analysis. The paper applies MEMS inertial sensors in underwater trajectory measurement, considering conical motion as attitude updating simulation environment. Trapezoid formula is used to calculate angle increment of Pirkanmaa algorithm, and analyse advantages and disadvantages of fourth order Ronge-Kutta method and Pirkanmaa algorithm. Ultimately conclusion by simulation is commendable to engineering application.

submarine-launched weapons, strapdown algorithm, Pirkanmaa algorithm, four order Ronge-Kutta

2015年3月1日,

2015年4月23日

單玉浩,男,碩士研究生,研究方向:圖形圖像處理。施建禮,男,副教授,碩士生導(dǎo)師,研究方向:維修與檢測(cè)。彭文輝,男,講師,研究方向:硬件設(shè)計(jì)與實(shí)現(xiàn)。

TJ630.33

10.3969/j.issn.1672-9730.2015.09.035