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

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

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

0 引言

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

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

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

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

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

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

2 算法實(shí)現(xiàn)

2.1 構(gòu)建彈體坐標(biāo)系

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

圖2 各坐標(biāo)系之間的關(guān)系

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

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

彈體坐標(biāo)系OXYZ，即原點(diǎn)O 位于導(dǎo)彈重心，OX 軸沿彈體橫軸指向右，OY 軸與彈體縱軸重合，指向?qū)楊^部，OZ 軸沿彈體豎軸指向上。

2.2 加載導(dǎo)彈模型

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

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

其中，R 表示轉(zhuǎn)化的弧度，A 表示需要轉(zhuǎn)化的角度。

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

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

圖4 俯仰角示意圖

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

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

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

圖3 方位角示意圖

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

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

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

2.4 計(jì)算四元數(shù)旋轉(zhuǎn)矩陣

在三維程序設(shè)計(jì)中，為了避免歐拉角旋轉(zhuǎn)時(shí)萬向鎖的存在且四元數(shù)使用4 個(gè)數(shù)值就可以表示旋轉(zhuǎn)，占用存儲(chǔ)空間更小［9-10］，本文采用四元數(shù)旋轉(zhuǎn)來調(diào)整三維場(chǎng)景中動(dòng)態(tài)目標(biāo)的姿態(tài)。

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

歐拉角旋轉(zhuǎn)矩陣M 為：

再結(jié)合如式（10）的半角公式。

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

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

3 算法驗(yàn)證

3.1 仿真試驗(yàn)

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

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

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

3.2 算法的通用性研究

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

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

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

4 結(jié)論

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