基于飛行軌跡的三維模型實時姿態(tài)調(diào)整算法

魏五洲，高 峰，李軍明，胡彭輝

（1.解放軍63850 部隊，吉林 白城 137001；2.中國電子信息產(chǎn)業(yè)集團有限公司第六研究所，北京 102209）

0 引言

在三維GIS（地理信息系統(tǒng)）彈道可視化系統(tǒng)中，導彈飛行姿態(tài)的實時、精確調(diào)整是直觀決定導彈測控試驗成功的重要因素之一。目前對高速飛行目標的姿態(tài)測量方法主要分為兩大類：一類是姿態(tài)內(nèi)測［1］，即在飛行目標上掛載測量傳感器采集飛行目標的姿態(tài)數(shù)據(jù)；另一類是姿態(tài)外測［2］，即利用地面測量設備對飛行目標姿態(tài)進行測量。姿態(tài)內(nèi)測的方法目前已經(jīng)比較廣泛地應用于飛機等航空器上，但還存在實時性較弱、測量誤差較大等不足［3］。姿態(tài)外測比較典型的方法是采用高速攝像機和光學經(jīng)緯儀拍攝飛行目標的影像數(shù)據(jù)，然后對這些影像序列進行分析進而得出飛行目標的姿態(tài)。但是由于飛行目標的高速性和遠距離性，用影像序列的分析手段在技術上還存在許多困難［4-6］。

由于雷達設備對飛行目標的軌跡數(shù)據(jù)的采集具有實時性高和準確性好等［7-8］特點。本文在對導彈的飛行姿態(tài)可視化的研究中，為了在三維GIS 彈道可視化系統(tǒng)中能夠實時仿真出平滑、逼真的導彈飛行姿態(tài)，提出一種姿態(tài)外測的方法，即基于飛行軌跡求解導彈姿態(tài)的算法。

1 基于飛行軌跡的導彈姿態(tài)調(diào)整算法

為了驗證該算法實用性，將該算法用于某型號導彈的飛行態(tài)勢仿真，本文在國產(chǎn)麒麟Linux 操作系統(tǒng)下，基于osg3.4 和osgEarth2.8 三維可視化庫，使用建模工具，對基于飛行軌跡的導彈姿態(tài)生成算法進行研究，研究過程中的數(shù)據(jù)響應速度達到了毫秒（ms）量級，實時性非常好，具體算法如圖1 所示。

圖1 基于飛行軌跡的導彈姿態(tài)調(diào)整算法原理圖

首先，使用建模工具創(chuàng)建導彈模型，確定彈體坐標系OXYZ 各軸指向；通過地球坐標系與地心坐標系的變換，將導彈模型加載到三維地球上；再根據(jù)飛行軌跡點以及地面坐標系與彈體坐標系的角度關系求出彈體姿態(tài)角；最后利用四元數(shù)旋轉矩陣實時更新三維地球上導彈的飛行姿態(tài)。

2 算法實現(xiàn)

2.1 構建彈體坐標系

三維空間中各坐標系之間的關系，如圖2 所示。地心坐標系OX0Y0Z0：即原點O 在地球的中心，OZ0軸指向北極方向，OX0軸為赤道面與黃道面的交線且指向春分點，OY0軸垂直O(jiān)X0和OZ0軸所在的平面且符合右手定則。

圖2 各坐標系之間的關系

地球坐標系OX1Y1Z1：即OZ1軸與地心坐標系軸OZ0重合，OX1和OY1軸位于赤道平面且OX1軸指向格林尼治子午線，OY1軸垂直O(jiān)X1軸指向東經(jīng)90°方向。地球附近任意一個坐標位置都可以用經(jīng)度、緯度和高度表示。

地面坐標系OX2Y2Z2：即東北天坐標系，OX2指向東，OY2指向北，OZ2垂直地面指向天空。

彈體坐標系OXYZ，即原點O 位于導彈重心，OX 軸沿彈體橫軸指向右，OY 軸與彈體縱軸重合，指向導彈頭部，OZ 軸沿彈體豎軸指向上。

2.2 加載導彈模型

根據(jù)導彈在地球坐標系下的初始位置點P0（L0，B0，H0）將導彈加載到三維地球上，其中，L0、B0、H0分別表示導彈的經(jīng)度，緯度和海拔高度。

首先使用式（1）把角度制表示的經(jīng)緯度轉化為弧度制表示的經(jīng)緯度，然后使用式（2）將導彈在地球坐標系下的當前位置點轉化為地心坐標系下的點（X，Y，Z）。

其中，R 表示轉化的弧度，A 表示需要轉化的角度。

式（2）中，點（X，Y，Z）表示地心坐標系中的點坐標，B 、L 分別表示弧度制的經(jīng)度、緯度。a 表示地球的長半軸，b 表示地球的短半軸，a 和b 的值為一固定的常量，a=6 378.137 0 km，b=6 356.752 3 km。e 為地球的第一偏心率，N 表示地球曲率半徑，W 為第一輔助系數(shù)，計算公式如式（3）～式（5）。

加載完成后，彈體坐標系與地面坐標系的關系如圖4 所示，彈體坐標系的OY 軸指向正北，即與地面坐標系OY2軸重合；彈體坐標系的OX 軸指向正東，即與地面坐標系OX2軸重合；彈體坐標系的OZ軸垂直地面指向天空，即與地面坐標系OZ2軸重合。

圖4 俯仰角示意圖

俯仰角為繞X 軸旋轉的角度，即彈體坐標系縱軸OY 與地面坐標系OY2之間夾角，角度范圍為［-90°，90°］，導彈抬頭向上為正，向下為負；航向角為繞Z 軸旋轉的角度，即彈體坐標系OY 在地面坐標系水平面的投影與地面坐標系OY2之間夾角，角度范圍［0°，360°］，順時針旋轉為正；橫滾角為繞Y 軸旋轉的角度，即彈體坐標系OX 軸與地面坐標系水平面的夾角，角度范圍［0°，360°］，順時針旋轉為正。

2.3 姿態(tài)角的求解

方位角（Azimuth angle，縮寫為Az）是從某點的指北方向線起依順時針方向到目標方向線之間的水平夾角［5］。假設導彈飛行一個軌跡點P1（L1，B1，H1），相鄰的下一個軌跡點P2（L2，B2，H2），如圖3 所示，將P2點投影在P1所在平面上，P3是與P1同經(jīng)度，同高度，與P2同緯度的點，即P3（L1，B2，H1）。

圖3 方位角示意圖

根據(jù)地球上任意兩點距離公式［6］，d 表示P1和P2兩點間的距離，r0為地球半徑6 378.137 0 km，計算方法如式（6）。

如圖4 所示，P2'是P2在P1平面的投影。根據(jù)P2與P1兩點的高度差h 和P2與P1兩點之間距離d12，則俯仰角按式（7）計算。

橫滾角可根據(jù)導彈的飛行關鍵位置進行動態(tài)設置，如果不需要可以設置為0.0，本算法橫滾角初始化為0.0°，每50 ms 增加0.1°。

2.4 計算四元數(shù)旋轉矩陣

在三維程序設計中，為了避免歐拉角旋轉時萬向鎖的存在且四元數(shù)使用4 個數(shù)值就可以表示旋轉，占用存儲空間更?。?-10］，本文采用四元數(shù)旋轉來調(diào)整三維場景中動態(tài)目標的姿態(tài)。

四元數(shù)是一種高階復數(shù)［9］，四元數(shù)q 按式（8）表示。

歐拉角旋轉矩陣M 為：

再結合如式（10）的半角公式。

容易得出如式（11）的四元數(shù)的旋轉矩陣Q。

最后結合求出的姿態(tài)角以及式（9）和式（11），分別得出繞Z 軸、X 軸、Y 軸的四元數(shù)旋轉矩陣QZ、QX和QY。本算法采用的旋轉順序為Z→X→Y，故得到最終的旋轉矩陣QL=QZQXQY，最后利用QL實時更新三維地球中導彈模型的姿態(tài)即可。

3 算法驗證

3.1 仿真試驗

根據(jù)本文算法的實現(xiàn)方法，將本文算法實際用于某型號導彈測控試驗中姿態(tài)的調(diào)整。

圖5 是采用經(jīng)緯儀和本文算法的導彈飛行姿態(tài)圖，圖5（a）、（c）是導彈上升階段的姿態(tài)，圖5（b）、（d）是導彈下降階段的姿態(tài)，圖片中伴隨模型的線條是實時飛行軌跡線。從圖5（a）、（b）可以直觀看出，導彈的飛行姿態(tài)和實時飛行軌跡線之間存在角度偏差，飛行效果不逼真，這是由于光學經(jīng)緯儀等測量設備的實時性和準確性不高，導致飛行姿態(tài)的數(shù)據(jù)出現(xiàn)延時，誤差大或者缺失等問題。而圖5（c）、（d）中采用本文算法仿真的導彈飛行姿態(tài)和實時飛行軌跡線始終保持一致，逼真度較高，實際動態(tài)效果流暢。

圖5 導彈飛行姿態(tài)圖

3.2 算法的通用性研究

為了驗證本文算法對其他飛行目標姿態(tài)調(diào)整的效果，將本算法用于飛機模型的仿真試驗，飛機坐標系OXYZ，即原點O 位于飛機重心，OX 軸沿彈體橫軸指向右，OY 軸與彈體縱軸重合，指向飛機頭部，OZ 軸沿彈體豎軸指向上。飛機飛行時，本算法計算出的姿態(tài)數(shù)據(jù)不斷地賦值給飛機，飛機在飛行過程中呈現(xiàn)出不同的變化姿態(tài)。

圖6 是對飛機模型使用本文算法的實時態(tài)勢的展示效果圖，可以看出，飛機的飛行態(tài)勢隨著飛行軌跡實時不斷變化，實時性好，效果流暢，飛行姿態(tài)逼真，觀賞性強。

圖6 使用本文算法的飛機姿態(tài)效果圖

4 結論

本文提出一種基于飛行軌跡的三維模型實時姿態(tài)調(diào)整算法，該算法利用實時性和準確性較穩(wěn)定的三維模型飛行軌跡數(shù)據(jù)，自動生成飛行姿態(tài)數(shù)據(jù)，實時調(diào)整三維場景中三維模型的飛行姿態(tài)。將該算法用于在某型號導彈飛行態(tài)勢的三維GIS 仿真模擬試驗中，從導彈呈現(xiàn)出的飛行態(tài)勢以及求出的姿態(tài)數(shù)據(jù)可以看出，該算法自動生成的飛行態(tài)勢精確度高，實際應用效果好。將該算法應用于飛機的飛行姿態(tài)的模擬，仿真結果表明，飛機也呈現(xiàn)出很好的飛行效果，由此得出本文算法通用于動態(tài)目標的飛行姿態(tài)調(diào)整。