張雙彪，李興城

(1 北京信息科技大學(xué)高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室, 北京 100101; 2 北京理工大學(xué)宇航學(xué)院, 北京 100081)

0 引言

火箭彈、制導(dǎo)炮彈、制導(dǎo)子彈等旋轉(zhuǎn)載體常采用的姿態(tài)解算方法是以歐拉角為基礎(chǔ)的，如歐拉角姿態(tài)解算方法和基于旋轉(zhuǎn)矢量的雙步姿態(tài)解算方法，前者常用于控制系統(tǒng)設(shè)計(jì)，后者常用于定位、定向和導(dǎo)航系統(tǒng)，但兩種方法均受錐形運(yùn)動(dòng)影響而出現(xiàn)不同程度的姿態(tài)解算誤差[1-2]，因此姿態(tài)解算得到了國內(nèi)外廣大學(xué)者的關(guān)注。研究發(fā)現(xiàn)，旋轉(zhuǎn)載體的錐形運(yùn)動(dòng)存在兩種旋轉(zhuǎn)方式，一種是通過繞3個(gè)正交軸旋轉(zhuǎn)表征的形式，如圖1所示；另一種是載體因定軸轉(zhuǎn)動(dòng)而發(fā)生的章動(dòng)和進(jìn)動(dòng)表現(xiàn)的雙軸旋轉(zhuǎn)形式，如圖2所示[3-4]。兩種錐形運(yùn)動(dòng)均會(huì)影響姿態(tài)解算方法，特別是自轉(zhuǎn)姿態(tài)誤差發(fā)散嚴(yán)重。為提高姿態(tài)解算的適應(yīng)性和解算精度，廣大學(xué)者主要采用便于參數(shù)優(yōu)化的雙步姿態(tài)解算方法，通過錐形運(yùn)動(dòng)條件優(yōu)化旋轉(zhuǎn)矢量的計(jì)算參數(shù)，抑制姿態(tài)誤差，并利用旋轉(zhuǎn)矢量計(jì)算姿態(tài)矩陣，得到載體姿態(tài)，而優(yōu)化效果憑借誤差漂移率體現(xiàn)[5-9]。然而，目前該類姿態(tài)誤差并未得到有效消除。

圖1 三軸旋轉(zhuǎn)表征的錐形運(yùn)動(dòng)

圖2 兩軸旋轉(zhuǎn)表征的錐形運(yùn)動(dòng)

為此，提出圓錐姿態(tài)解算方法，并給出基于旋轉(zhuǎn)矢量的雙步姿態(tài)解算過程，通過與傳統(tǒng)的解算方法進(jìn)行仿真對比，證明該方法的優(yōu)越性。

1 常規(guī)的姿態(tài)解算方法

1.1 歐拉角姿態(tài)解算方法

歐拉角姿態(tài)解算方法是傳統(tǒng)的姿態(tài)解算方法，根據(jù)飛行力學(xué)知識(shí)，載體的角運(yùn)動(dòng)模型為[10]：

(1)

(2)

利用獲得的角速度測量值，可以解算載體姿態(tài)。由歐拉角表示的載體坐標(biāo)系Oxbybzb到參考坐標(biāo)系Oxyz的姿態(tài)矩陣為：

(3)

式中:上角標(biāo)i表示目標(biāo)坐標(biāo)系為參考坐標(biāo)系;C11=cos(?)cos(ψ)；C12=-sin(?)cos(ψ)cos(γ)+sin(ψ)sin(γ)；C13=sin(?)cos(ψ)sin(γ)+sin(ψ)·cos(γ)；C21=sin(?)；C22=cos(?)cos(γ)；C23=-cos(?)sin(γ)；C31=-cos(?)sin(ψ)；C32=sin(?)sin(ψ)cos(γ)+cos(ψ)sin(γ)；C33=-sin(?)·sin(ψ)sin(γ)+cos(ψ)cos(γ)。

在錐形運(yùn)動(dòng)環(huán)境下，利用俯仰角?和偏航角ψ可以近似計(jì)算半錐角：

(4)

1.2 雙步姿態(tài)解算方法

雙步姿態(tài)解算方法包括旋轉(zhuǎn)矢量更新和姿態(tài)矩陣更新兩個(gè)過程，具體更新如下[3]：

φl=αl+δφl

(5)

(6)

(7)

式中l(wèi)表示第幾次循環(huán)，為便于數(shù)據(jù)更新，式(7)常被近似為下式：

(8)

式中kij為旋轉(zhuǎn)矢量優(yōu)化參數(shù)。

姿態(tài)矩陣更新如下：

(9)

(10)

(11)

(12)

(13)

利用旋轉(zhuǎn)矢量解算歐拉角時(shí)，俯仰角?、偏航角ψ和滾轉(zhuǎn)角γ分別利用下式計(jì)算：

(14)

(15)

(16)

偏航角ψ和滾轉(zhuǎn)角γ真值根據(jù)表1和表2確定。

表1 ψ取值

2 圓錐姿態(tài)解算方法

圓錐姿態(tài)解算方法借鑒了章動(dòng)原理，通過定義圓錐姿態(tài)角表述錐形運(yùn)動(dòng)，分別為進(jìn)動(dòng)角δ1，章動(dòng)角δ2，自轉(zhuǎn)角δ3，其中δ1取值范圍為[0° 360°]，δ2取值范圍為[0° 90°]，δ3取值范圍為[0° 360°][8]。進(jìn)動(dòng)角表征錐形運(yùn)動(dòng)的旋轉(zhuǎn)過程，章動(dòng)角表征錐形運(yùn)動(dòng)的搖擺幅度，自轉(zhuǎn)角則表征載體在進(jìn)行錐形運(yùn)動(dòng)時(shí)自轉(zhuǎn)過程，見圖2。需要說明的是，當(dāng)Oxbyb面位于鉛垂面內(nèi)時(shí)為進(jìn)動(dòng)角δ1起始位置。與章動(dòng)原理不同的是，文中針對坐標(biāo)系旋轉(zhuǎn)過程進(jìn)行了改進(jìn)，具體為：

根據(jù)旋轉(zhuǎn)順序可以得到通過圓錐姿態(tài)角表示的參考坐標(biāo)系Oxyz與載體坐標(biāo)系Oxbybzb的旋轉(zhuǎn)關(guān)系，如圖3所示。

圖3 錐形運(yùn)動(dòng)姿態(tài)角的兩軸旋轉(zhuǎn)過程

根據(jù)圖3建立旋轉(zhuǎn)載體的角運(yùn)動(dòng)方程為：

(17)

(18)

當(dāng)δ2不為零時(shí)，根據(jù)式(18)可以利用角速度測量值實(shí)時(shí)解算錐形運(yùn)動(dòng)的姿態(tài)角。根據(jù)歐拉姿態(tài)角與圓錐姿態(tài)角的幾何關(guān)系建立如下表達(dá)式：

?=±arcsin|sinδ1sinδ2|

(19)

(20)

二者的符號可以根據(jù)表3和表4選取。

表3 ?符號選取

表4 ψ符號選取

由式(18)可知，當(dāng)δ2為零時(shí)，存在奇異值的問題。為提高圓錐姿態(tài)適應(yīng)性，同樣可以利用雙步姿態(tài)解算結(jié)構(gòu)更新載體的圓錐姿態(tài)，見式(5)～式(10)，載體坐標(biāo)系Oxbybzb到參考坐標(biāo)系Oxyz的圓錐姿態(tài)矩陣可根據(jù)圖3旋轉(zhuǎn)關(guān)系確定，具體為：

(21)

式中:D11=cosδ2；D12=-cos(δ1-δ3)sinδ2；D13=-sin(δ1-δ3)sinδ2；D21=cosδ1sinδ2；D22=sin(δ1-δ3)sinδ1+cos(δ1-δ3)cosδ1cosδ2；D23=-cos(δ1-δ3)sinδ1+sin(δ1-δ3)cosδ1cosδ2;D31=sinδ1sinδ2；D32=-sin(δ1-δ3)cosδ1+cos(δ1-δ3)sinδ1cosδ2；D33=cos(δ1-δ3)cosδ1+sin(δ1-δ3)sinδ1cosδ2。

根據(jù)式(21)，可以建立圓錐姿態(tài)計(jì)算公式：

(22)

δ2=arccosD11

(23)

(24)

需要說明的是，自轉(zhuǎn)角δ3是需要確定進(jìn)動(dòng)角δ1后再計(jì)算。進(jìn)動(dòng)角δ1和自轉(zhuǎn)角δ3的真值可根據(jù)表5和表6所示確定。

表5 δ1取值

表6 δ3取值

3 仿真與分析

圖4 角速度測量值

首先，將歐拉角姿態(tài)解算方法與雙步姿態(tài)解算方法進(jìn)行對比。圖5為旋轉(zhuǎn)矢量方法優(yōu)化后歐拉角姿態(tài)誤差，姿態(tài)角存在振蕩式發(fā)散誤差，誤差量級為10-5～10-4rad。圖6為旋轉(zhuǎn)矢量方法優(yōu)化前后的歐拉角姿態(tài)誤差對比，誤差表現(xiàn)為復(fù)雜的發(fā)散現(xiàn)象，量級為10-14～10-13rad。結(jié)合圖5和圖6，說明雙軸旋轉(zhuǎn)錐形運(yùn)動(dòng)引起的姿態(tài)發(fā)散振蕩誤差，在旋轉(zhuǎn)矢量優(yōu)化后仍然存在，優(yōu)化效果并不明顯，同時(shí)說明利用雙步姿態(tài)解算方法得到歐拉角存在誤差。

圖5 旋轉(zhuǎn)矢量方法優(yōu)化后歐拉角姿態(tài)誤差

圖6 旋轉(zhuǎn)矢量方法優(yōu)化前后的歐拉角姿態(tài)誤差對比

然后，將圓錐姿態(tài)解算方法與雙步姿態(tài)解算方法進(jìn)行對比。圖7為旋轉(zhuǎn)矢量方法優(yōu)化后圓錐姿態(tài)誤差，可見仍存在振蕩式誤差，但并未發(fā)散，章動(dòng)角δ2誤差量級為10-5rad，自轉(zhuǎn)角δ3的誤差量級為10-6rad，精度均高于歐拉角姿態(tài)。圖8為旋轉(zhuǎn)矢量方法優(yōu)化前后圓錐姿態(tài)誤差對比，優(yōu)化前后誤差并未發(fā)散，誤差量級為10-4～10-3rad，且自轉(zhuǎn)角δ3精度未得到提高。分析圖7和圖8可知，旋轉(zhuǎn)矢量方法的優(yōu)化與否對圓錐姿態(tài)自轉(zhuǎn)角的影響并不明顯，同時(shí)也證明雙步姿態(tài)解算方法存在誤差。

圖7 圓錐姿態(tài)解算方法與旋轉(zhuǎn)矢量對比

圖8 旋轉(zhuǎn)矢量方法優(yōu)化前后的圓錐姿態(tài)誤差對比

最后，分別利用基于歐拉角姿態(tài)的解算方法與基于圓錐姿態(tài)的解算方法計(jì)算半錐角α，并進(jìn)行對比。如圖9所示，歐拉角姿態(tài)計(jì)算存在發(fā)散的誤差，圓錐姿態(tài)解算方法無誤差，但其雙步姿態(tài)解算方法存在不發(fā)散的振蕩誤差，具體誤差關(guān)系為：

圖9 半錐角誤差

Δα圓錐姿態(tài)解算<Δα歐拉角姿態(tài)解算<Δα雙步(圓錐姿態(tài))<Δα雙步(歐拉角)

4 結(jié)論

考慮了錐形運(yùn)動(dòng)存在的不同形式，提出了一種圓錐姿態(tài)解算方法，同時(shí)為提高該方法的適應(yīng)性，給出了基于旋轉(zhuǎn)矢量的解算過程，通過與傳統(tǒng)姿態(tài)解算方法對比發(fā)現(xiàn)：

1)相比傳統(tǒng)方法，圓錐姿態(tài)解算方法的精度高于歐拉角姿態(tài)解算方法。

2)基于雙步姿態(tài)解算的圓錐姿態(tài)解算精度高于基于雙步姿態(tài)解算的歐拉角姿態(tài)解算，且常規(guī)優(yōu)化的旋轉(zhuǎn)矢量方法效果并不明顯。

文中提出的圓錐姿態(tài)解算方法更適合旋轉(zhuǎn)載體的雙軸錐形運(yùn)動(dòng)條件，能夠?yàn)樾D(zhuǎn)載體的復(fù)雜角運(yùn)動(dòng)解耦和姿態(tài)誘導(dǎo)誤差抑制的研究工作提供理論參考。未來工作將進(jìn)一步研究雙步姿態(tài)解算方法引起姿態(tài)出現(xiàn)發(fā)散的振蕩誤差的機(jī)理，以及相應(yīng)的誤差抑制方法。