淡 鵬，王 丹，侯黎強，馬 宏

（1宇航動力學(xué)國家重點實驗室，西安 710043;2西安衛(wèi)星測控中心，西安 710043）

0 引言

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

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

火箭飛行過程可視化的常用參考坐標系有以下幾種:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 多次旋轉(zhuǎn)法

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

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

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

設(shè)在T時刻（T為相對T0的時間）發(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ù)當前姿態(tài)的歐拉角旋轉(zhuǎn)到箭體坐標系下，對3-2-1轉(zhuǎn)序來說，其旋轉(zhuǎn)方法為:1）繞Z軸旋轉(zhuǎn)φ;2）繞Y軸旋轉(zhuǎn)ψ;3）繞X軸旋轉(zhuǎn)γ。用公式可表示為:RX（γ）RY（ψ）RZ（φ）。

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

整個旋轉(zhuǎn)過程用OpenGL語句表示為:

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

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

若能計算出M，則可據(jù)此解算出3個轉(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））

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

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

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

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

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

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

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

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

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

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

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

圖3 原型軟件對火箭飛行繪制效果

4 應(yīng)用效果

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

5 結(jié)束語

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

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

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

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

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

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

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

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