火箭飛行姿態(tài)的可視化繪制方法*

淡 鵬，王 丹，侯黎強(qiáng)，馬 宏

（1宇航動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710043;2西安衛(wèi)星測(cè)控中心，西安 710043）

0 引言

在運(yùn)載火箭（或?qū)棧┑娜S飛行視景可視化繪制中，為了更加真實(shí)地反映飛行過(guò)程中火箭與飛行速度、地平面、太陽(yáng)等目標(biāo)或矢量之間的關(guān)系，以及對(duì)火箭大角度調(diào)姿、火箭起旋過(guò)程等進(jìn)行真實(shí)感展示，就需要對(duì)其飛行姿態(tài)數(shù)據(jù)進(jìn)行可視化繪制。為此，文中基于OpenGL圖形庫(kù)和Visual C++編程技術(shù)，對(duì)姿態(tài)的獲取及繪制方法進(jìn)行了研究與實(shí)現(xiàn)。

1 火箭姿態(tài)常用參考坐標(biāo)系

火箭飛行過(guò)程可視化的常用參考坐標(biāo)系有以下幾種:

1）發(fā)射系與發(fā)射慣性系:發(fā)射系坐標(biāo)原點(diǎn)與發(fā)射點(diǎn)固連，OX軸在發(fā)射點(diǎn)水平面內(nèi)并指向發(fā)射瞄準(zhǔn)方向，OY軸垂直于發(fā)射點(diǎn)水平面指向上方，OZ軸為右手法則確定的方向。發(fā)射慣性系起飛瞬時(shí)與發(fā)射系重合，起飛后原點(diǎn)及各軸在慣性空間保持不動(dòng)。

2）箭體坐標(biāo)系:箭體坐標(biāo)系［1－2］為火箭固連坐標(biāo)系，隨火箭一起運(yùn)動(dòng)，其原點(diǎn)為火箭質(zhì)心，X軸沿著火箭體軸方向，并指向火箭的頭部，Y軸在火箭縱對(duì)稱面內(nèi)，垂直于縱軸，指向第Ⅲ象限。

3）DX-2地心固聯(lián)坐標(biāo)系:DX-2地心固聯(lián)坐標(biāo)系（簡(jiǎn)稱地固系），其坐標(biāo)原點(diǎn)在地球質(zhì)心，X軸在真赤道面內(nèi)指向零子午圈;Z軸指向國(guó)際習(xí)用原點(diǎn)（CIO）;Y軸與Z軸、X軸垂直構(gòu)成右手坐標(biāo)系。

4）J2000地心慣性系:J2000平春分點(diǎn)平赤道地心慣性系，其坐標(biāo)原點(diǎn)為地球質(zhì)心，基本平面為J2000.0地球平赤道面，X軸在基本平面內(nèi)由原點(diǎn)指向歷元平春分點(diǎn)，Z軸過(guò)坐標(biāo)原點(diǎn)指向北極，Y軸由右手規(guī)則確定。

2 姿態(tài)數(shù)據(jù)獲取與處理方法

火箭姿態(tài)確定所用數(shù)據(jù)來(lái)自于火箭遙測(cè)，由于火箭飛行制導(dǎo)系統(tǒng)測(cè)量坐標(biāo)系建立在慣性坐標(biāo)系下，因而從火箭遙測(cè)獲取的姿態(tài)參數(shù)一般為發(fā)射慣性系下的數(shù)據(jù)（有時(shí)為姿態(tài)導(dǎo)航系，可通過(guò)平移變換到發(fā)射慣性系）。根據(jù)火箭類型不同，其姿態(tài)參數(shù)傳輸?shù)念愋团c個(gè)數(shù)也可能不同，具體可使用箭遙姿態(tài)數(shù)據(jù)中的姿態(tài)程序角、姿態(tài)角偏差以及火箭飛行的理論姿態(tài)角等綜合計(jì)算出當(dāng)前歷元時(shí)刻的姿態(tài)角（俯仰角、偏航角、滾動(dòng)角）數(shù)據(jù)［2］（需要根據(jù)箭遙參數(shù)種類等進(jìn)行取舍）。

當(dāng)有程序角與姿態(tài)角偏差時(shí)，可使用程序角加上偏差的方法得到當(dāng)前姿態(tài)角，用公式表示為:

姿態(tài)角=程序角+姿態(tài)角偏差

當(dāng)只有姿態(tài)角偏差時(shí)，可使用理論姿態(tài)角與偏差相加的方法得到當(dāng)前姿態(tài)角，用公式表示為:

姿態(tài)角=理論姿態(tài)角+姿態(tài)角偏差

在對(duì)姿態(tài)數(shù)據(jù)的三維繪制過(guò)程中，當(dāng)需要使用實(shí)測(cè)姿態(tài)數(shù)據(jù)時(shí)，由于姿態(tài)測(cè)量系統(tǒng)不可避免的存在各種噪聲及野值數(shù)據(jù)，就必須對(duì)所取得的歐拉角參數(shù)進(jìn)行平滑與剔野處理，以免顯示過(guò)程中箭體方向出現(xiàn)較大幅度抖動(dòng)。

關(guān)于火箭姿態(tài)野值的剔除問(wèn)題，此類方法較多，常用的方法有合理范圍法、差分檢測(cè)法和多項(xiàng)式外推殘差判斷法等。合理范圍法就是給定角度α的一個(gè)合理范圍時(shí)認(rèn)為是野值。差分檢測(cè)法通過(guò)對(duì)前后兩點(diǎn)角度進(jìn)行差分來(lái)計(jì)算角度變化率，當(dāng)變化率不滿足給定閥值時(shí)認(rèn)為是野值，也可進(jìn)一步通過(guò)判斷變化率的變化率進(jìn)行更嚴(yán)格的限制。

多項(xiàng)式外推殘差判斷法需要對(duì)多點(diǎn)角度數(shù)據(jù)進(jìn)行多項(xiàng)式擬合計(jì)算，如采用二次或三次多項(xiàng)式，其構(gòu)造方法常用有最小二乘擬合法、樣條函數(shù)法、拉格朗日插值法等。以最小二乘二次多項(xiàng)式為例，設(shè)角度α對(duì)時(shí)標(biāo)t的擬合多項(xiàng)式為α=at2+bt+c（其中，a、b、c為待估系數(shù)），通過(guò)將多點(diǎn)測(cè)量值代入解算函數(shù)極值的方法可解算出系數(shù)，進(jìn)而可估計(jì)出t時(shí)刻的估計(jì)值，當(dāng)t時(shí)刻測(cè)量值αt與估計(jì)值差，以及門限值δ滿足δ時(shí)，認(rèn)為 αt為正常值，被接受，否則使用估計(jì)值代替αt。

需要注意的是，上面這幾種方法使用中需要合理選擇剔野的門限值。另外在姿態(tài)平滑及剔野中使用較多的是卡爾曼濾波方法，此類算法較多，如文獻(xiàn)［3］給出的弱機(jī)動(dòng)及強(qiáng)機(jī)動(dòng)狀態(tài)下的姿態(tài)EKF濾波算法，文獻(xiàn)［4］給出的狀態(tài)切換UKF濾波姿態(tài)確定算法，文獻(xiàn)［5］使用的QCDKF算法等。

3 姿態(tài)三維繪制方法

火箭或?qū)楋w行過(guò)程三維繪制［6－7］時(shí)，主要涉及位置及姿態(tài)的繪制，當(dāng)保持基準(zhǔn)坐標(biāo)系不動(dòng)時(shí)，可先將火箭3D模型加載到視場(chǎng)中后，根據(jù)姿態(tài)數(shù)據(jù)進(jìn)行模型旋轉(zhuǎn)，然后再平移到火箭所在位置。

在火箭位置及姿態(tài)繪制中常采用地心固連系或J2000地心慣性系作為基本參考系（由于慣性系坐標(biāo)與時(shí)間相關(guān)，故更多的使用地固系），對(duì)于地固系下位置矢量recf與J2000慣性系下位置矢量reci，兩者間的轉(zhuǎn)換關(guān)系為:

其中:PR為歲差矩陣，NR為章動(dòng)矩陣，ER為地球自轉(zhuǎn)矩陣，EP為極移矩陣。

考慮到火箭三維繪制中不需要過(guò)多精確的計(jì)算，則對(duì)于這兩者之間的轉(zhuǎn)換，可只考慮ER自轉(zhuǎn)矩陣，即只考慮當(dāng)前時(shí)刻的格林尼治恒星時(shí)角的差別。此時(shí)在三維繪制中，地心慣性系到固連系之間可通過(guò)繞Z軸旋轉(zhuǎn)一個(gè)恒星時(shí)角的方法實(shí)現(xiàn)。

圖1 火箭在姿態(tài)參考系的初始指向

火箭位置顯示比較簡(jiǎn)單，下面只探討姿態(tài)的繪制方法。設(shè)不考慮火箭位置的情況下，火箭模型加載到視景中的初始方向沿著姿態(tài)參考系的X軸方向（如圖1所示，由于模型視圖變換方法多樣，此參考系X軸與OpenGL的世界坐標(biāo)系X軸可能不同）。此時(shí)，姿態(tài)的三維繪制可通過(guò)下面介紹的幾種方法進(jìn)行實(shí)現(xiàn)。

3.1 多次旋轉(zhuǎn)法

將火箭飛行T時(shí)刻發(fā)射慣性系3個(gè)歐拉角記為俯仰角φ、偏航角ψ、滾動(dòng)角γ，發(fā)射系下對(duì)應(yīng)歐拉角分別記為 φf(shuō)、ψf、γf，設(shè)此歐拉角遵循 3-2-1 轉(zhuǎn)序。起飛時(shí)間記為T0，發(fā)射方位角為A，發(fā)射工位地理經(jīng)度記為λ，地理緯度為δ，起飛時(shí)刻發(fā)射工位格林尼治恒星時(shí)角記為ST0，則其赤經(jīng)為α=λ+ST0，赤緯為β（可用δ近似）。

圖2 地固到發(fā)慣的旋轉(zhuǎn)關(guān)系

當(dāng)使用地固系作為參考系時(shí)，一種較直觀的姿態(tài)可視化方法是借助OpenGL提供的強(qiáng)大的三維坐標(biāo)旋轉(zhuǎn)功能，先將地固系下的火箭初始模型根據(jù)發(fā)射工位的天文坐標(biāo)（三維繪制中可使用大地經(jīng)度、大地緯度近似）及發(fā)射方位角（或稱為射向）旋轉(zhuǎn)到發(fā)射慣性系下。

設(shè)在T時(shí)刻（T為相對(duì)T0的時(shí)間）發(fā)射慣性系下的發(fā)射工位在地球固連系下經(jīng)度為λ－wT（w為地球自轉(zhuǎn)角速度）。則旋轉(zhuǎn)方法為:1）繞Z軸旋轉(zhuǎn)λwT;2）繞Y軸旋轉(zhuǎn) －δ－π/2;3）繞X軸旋轉(zhuǎn) －π/2;4）繞Y軸旋轉(zhuǎn)A。用公式可表示為:

然后再根據(jù)當(dāng)前姿態(tài)的歐拉角旋轉(zhuǎn)到箭體坐標(biāo)系下，對(duì)3-2-1轉(zhuǎn)序來(lái)說(shuō)，其旋轉(zhuǎn)方法為:1）繞Z軸旋轉(zhuǎn)φ;2）繞Y軸旋轉(zhuǎn)ψ;3）繞X軸旋轉(zhuǎn)γ。用公式可表示為:RX（γ）RY（ψ）RZ（φ）。

當(dāng)以地心慣性系作為參考系時(shí)，發(fā)慣系工位起飛時(shí)刻在地慣系下赤經(jīng)為α=λ+ST0，多次旋轉(zhuǎn)方法與上面地固系多次旋轉(zhuǎn)僅在第一步不同，改為繞Z軸旋轉(zhuǎn)α角即可。

整個(gè)旋轉(zhuǎn)過(guò)程用OpenGL語(yǔ)句表示為:

3.2 將姿態(tài)角轉(zhuǎn)換到地固系或地慣系

常用的火箭姿態(tài)以發(fā)慣系（或發(fā)射系）作為參考系，為了方便三維顯示的使用，也可直接將姿態(tài)參考系建立在地固系或地慣系下。此時(shí)的3個(gè)歐拉角為地固（或地慣）到箭體坐標(biāo)系的旋轉(zhuǎn)角度，設(shè)3-2-1轉(zhuǎn)序下3個(gè)旋轉(zhuǎn)角為:繞Z軸轉(zhuǎn)角為ψ，繞Y軸轉(zhuǎn)角為θ，繞X軸轉(zhuǎn)角為φ（此處3個(gè)歐拉角無(wú)明顯的滾動(dòng)、俯仰、偏航含義，故此處稱為繞某軸的轉(zhuǎn)角）。則此時(shí)參考系到箭體系的轉(zhuǎn)換矩陣M為:

若能計(jì)算出M，則可據(jù)此解算出3個(gè)轉(zhuǎn)角:

繞 Z 軸轉(zhuǎn)角為 ψ =arctan2（M（1，2），M（1，1））

繞Y軸轉(zhuǎn)角為θ=arcsin（－M（1，3））

繞 X 軸轉(zhuǎn)角為 φ =arctan2（M（2，3），M（3，3））

需要說(shuō)明的是，上面式子中借助了VC編程中的反正切函數(shù)arctan 2進(jìn)行表示。

而根據(jù)火箭姿態(tài)角，可得到發(fā)慣系到箭體系轉(zhuǎn)換矩陣Mfb，又由參考系到發(fā)慣系的旋轉(zhuǎn)關(guān)系可得到參考系到發(fā)慣系的旋轉(zhuǎn)矩陣Mcf，則參考系到箭體系轉(zhuǎn)換矩陣為M=McfMfb。

這樣就可計(jì)算出地固或地慣參考系下的歐拉轉(zhuǎn)角，即可方便的實(shí)現(xiàn)姿態(tài)數(shù)據(jù)的繪制。此算法的旋轉(zhuǎn)過(guò)程用 OpenGL語(yǔ)句表示為:glRotatef（ψ，0.0f，0.0f，1.0f）;glRotatef（θ，0.0f，1.0f，0.0f）;glRotatef（φ ，1.0f，0.0f，0.0f）;

3.3 不考慮滾動(dòng)時(shí)的赤經(jīng)赤緯方法

如不需要考慮火箭的滾動(dòng)角度，則對(duì)姿態(tài)的繪制也可通過(guò)計(jì)算火箭縱軸方向在地慣系下的赤經(jīng)、赤緯的方式來(lái)實(shí)現(xiàn)。

由火箭的俯仰角φ、偏航角ψ、滾動(dòng)角γ，可得到火箭縱對(duì)稱軸在發(fā)射慣性系下的矢量方向?yàn)閄=（cosφcosψ，sinφcosψ，－sinψ），又由T0時(shí)刻發(fā)射工位的赤經(jīng)可計(jì)算出地慣系到發(fā)慣系的轉(zhuǎn)換矩陣M，則火箭縱對(duì)稱軸在地慣系的方向矢量為XI=MX。即可解算出縱對(duì)稱軸的赤經(jīng)為α =arctan（XI［2］/XI［1］），赤緯為 β =arcsin（XI［3］）。

此時(shí)，火箭體軸方向在J2000地心慣性系下的繪制可通過(guò)兩次旋轉(zhuǎn)得到:1）繞Z軸旋轉(zhuǎn)α角;2）繞Y軸旋轉(zhuǎn)β角。

這兩次旋轉(zhuǎn)用OpenGL語(yǔ)句表示為:

glRotatef（α ，0.0f，0.0f，1.0f）;

glRotatef（β ，0.0f，1.0f，0.0f）;

對(duì)地固系參考系，將赤經(jīng)、赤緯替換為火箭縱對(duì)稱軸的地心經(jīng)度、地心緯度即可。其地心經(jīng)度為λ=α－ST（ST為當(dāng)前時(shí)刻的格林尼治恒星時(shí)角），地心緯度可用β近似。

圖3 原型軟件對(duì)火箭飛行繪制效果

4 應(yīng)用效果

采用VC++開(kāi)發(fā)工具及OpenGL圖形庫(kù)進(jìn)行了火箭飛行過(guò)程視景顯示系統(tǒng)的原型軟件實(shí)現(xiàn)，飛行過(guò)程的可視化效果如圖3所示。

5 結(jié)束語(yǔ)

文中總結(jié)了火箭姿態(tài)數(shù)據(jù)的獲取及計(jì)算方法，給出了其可視化繪制的三種方法，并使用文中方法對(duì)火箭飛行過(guò)程進(jìn)行了繪制實(shí)現(xiàn)，取得了較好的可視化效果，證明了方法的可行性。應(yīng)該看到，文中給出的三種火箭姿態(tài)繪制方法各有優(yōu)缺點(diǎn):方法1不需要做復(fù)雜的計(jì)算，可直接使用發(fā)慣系的姿態(tài)角;方法2需要將姿態(tài)角轉(zhuǎn)換為地固系轉(zhuǎn)角;方法3只需兩次旋轉(zhuǎn)即可完成變換，但沒(méi)有考慮滾動(dòng)角。實(shí)際使用中可根據(jù)情況靈活選擇。

［1］王志剛，施志佳.遠(yuǎn)程火箭與衛(wèi)星軌道力學(xué)基礎(chǔ)［M］.西安:西北工業(yè)大學(xué)出版社，2006.

［2］王玉祥，張忠華，何劍偉.用箭遙數(shù)據(jù)計(jì)算三軸穩(wěn)定衛(wèi)星初始姿態(tài)［J］.遙測(cè)遙控，2007，9（5）:65－68.

［3］張凱，單甘霖，吉兵，等.改進(jìn)的變維濾波實(shí)現(xiàn)運(yùn)動(dòng)目標(biāo)姿態(tài)的跟蹤［J］.電光與控制，2012，19（3）:40 －43.

［4］吳勃，徐歡，喬相偉.狀態(tài)切換UKF的飛行器姿態(tài)確定算法［J］.電機(jī)與控制學(xué)報(bào)，2012，16（6）:98－105.

［5］董欣遠(yuǎn)，王蘇峰.QCDKF算法研究及其在小衛(wèi)星姿態(tài)確定中的應(yīng)用［J］.計(jì)算機(jī)工程與應(yīng)用，2012，48（S2）:141－146.

［6］蘇躍斌，辛長(zhǎng)范，郭本亮，等.三維比例導(dǎo)引彈道的可視化仿真研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2010，30（4）:57－60.

［7］戴雪峰，金連文.基于OpenGL實(shí)現(xiàn)火箭彈道及衛(wèi)星軌道三維可視化［J］.測(cè)控技術(shù)，2006，25（1）:17－19.