基于飞行轨迹的三维模型实时姿态调整算法

魏五洲，高 峰，李军明，胡彭辉

（1.解放军63850 部队，吉林 白城 137001；2.中国电子信息产业集团有限公司第六研究所，北京 102209）

0 引言

在三维GIS（地理信息系统）弹道可视化系统中，导弹飞行姿态的实时、精确调整是直观决定导弹测控试验成功的重要因素之一。目前对高速飞行目标的姿态测量方法主要分为两大类：一类是姿态内测［1］，即在飞行目标上挂载测量传感器采集飞行目标的姿态数据；另一类是姿态外测［2］，即利用地面测量设备对飞行目标姿态进行测量。姿态内测的方法目前已经比较广泛地应用于飞机等航空器上，但还存在实时性较弱、测量误差较大等不足［3］。姿态外测比较典型的方法是采用高速摄像机和光学经纬仪拍摄飞行目标的影像数据，然后对这些影像序列进行分析进而得出飞行目标的姿态。但是由于飞行目标的高速性和远距离性，用影像序列的分析手段在技术上还存在许多困难［4-6］。

由于雷达设备对飞行目标的轨迹数据的采集具有实时性高和准确性好等［7-8］特点。本文在对导弹的飞行姿态可视化的研究中，为了在三维GIS 弹道可视化系统中能够实时仿真出平滑、逼真的导弹飞行姿态，提出一种姿态外测的方法，即基于飞行轨迹求解导弹姿态的算法。

1 基于飞行轨迹的导弹姿态调整算法

为了验证该算法实用性，将该算法用于某型号导弹的飞行态势仿真，本文在国产麒麟Linux 操作系统下，基于osg3.4 和osgEarth2.8 三维可视化库，使用建模工具，对基于飞行轨迹的导弹姿态生成算法进行研究，研究过程中的数据响应速度达到了毫秒（ms）量级，实时性非常好，具体算法如图1 所示。

图1 基于飞行轨迹的导弹姿态调整算法原理图

首先，使用建模工具创建导弹模型，确定弹体坐标系OXYZ 各轴指向；通过地球坐标系与地心坐标系的变换，将导弹模型加载到三维地球上；再根据飞行轨迹点以及地面坐标系与弹体坐标系的角度关系求出弹体姿态角；最后利用四元数旋转矩阵实时更新三维地球上导弹的飞行姿态。

2 算法实现

2.1 构建弹体坐标系

三维空间中各坐标系之间的关系，如图2 所示。地心坐标系OX0Y0Z0：即原点O 在地球的中心，OZ0轴指向北极方向，OX0轴为赤道面与黄道面的交线且指向春分点，OY0轴垂直OX0和OZ0轴所在的平面且符合右手定则。

图2 各坐标系之间的关系

地球坐标系OX1Y1Z1：即OZ1轴与地心坐标系轴OZ0重合，OX1和OY1轴位于赤道平面且OX1轴指向格林尼治子午线，OY1轴垂直OX1轴指向东经90°方向。地球附近任意一个坐标位置都可以用经度、纬度和高度表示。

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

弹体坐标系OXYZ，即原点O 位于导弹重心，OX 轴沿弹体横轴指向右，OY 轴与弹体纵轴重合，指向导弹头部，OZ 轴沿弹体竖轴指向上。

2.2 加载导弹模型

根据导弹在地球坐标系下的初始位置点P0（L0，B0，H0）将导弹加载到三维地球上，其中，L0、B0、H0分别表示导弹的经度，纬度和海拔高度。

首先使用式（1）把角度制表示的经纬度转化为弧度制表示的经纬度，然后使用式（2）将导弹在地球坐标系下的当前位置点转化为地心坐标系下的点（X，Y，Z）。

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

式（2）中，点（X，Y，Z）表示地心坐标系中的点坐标，B 、L 分别表示弧度制的经度、纬度。a 表示地球的长半轴，b 表示地球的短半轴，a 和b 的值为一固定的常量，a=6 378.137 0 km，b=6 356.752 3 km。e 为地球的第一偏心率，N 表示地球曲率半径，W 为第一辅助系数，计算公式如式（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 姿态角的求解

方位角（Azimuth angle，缩写为Az）是从某点的指北方向线起依顺时针方向到目标方向线之间的水平夹角［5］。假设导弹飞行一个轨迹点P1（L1，B1，H1），相邻的下一个轨迹点P2（L2，B2，H2），如图3 所示，将P2点投影在P1所在平面上，P3是与P1同经度，同高度，与P2同纬度的点，即P3（L1，B2，H1）。

图3 方位角示意图

根据地球上任意两点距离公式［6］，d 表示P1和P2两点间的距离，r0为地球半径6 378.137 0 km，计算方法如式（6）。

如图4 所示，P2'是P2在P1平面的投影。根据P2与P1两点的高度差h 和P2与P1两点之间距离d12，则俯仰角按式（7）计算。

横滚角可根据导弹的飞行关键位置进行动态设置，如果不需要可以设置为0.0，本算法横滚角初始化为0.0°，每50 ms 增加0.1°。

2.4 计算四元数旋转矩阵

在三维程序设计中，为了避免欧拉角旋转时万向锁的存在且四元数使用4 个数值就可以表示旋转，占用存储空间更小［9-10］，本文采用四元数旋转来调整三维场景中动态目标的姿态。

四元数是一种高阶复数［9］，四元数q 按式（8）表示。

欧拉角旋转矩阵M 为：

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

容易得出如式（11）的四元数的旋转矩阵Q。

最后结合求出的姿态角以及式（9）和式（11），分别得出绕Z 轴、X 轴、Y 轴的四元数旋转矩阵QZ、QX和QY。本算法采用的旋转顺序为Z→X→Y，故得到最终的旋转矩阵QL=QZQXQY，最后利用QL实时更新三维地球中导弹模型的姿态即可。

3 算法验证

3.1 仿真试验

根据本文算法的实现方法，将本文算法实际用于某型号导弹测控试验中姿态的调整。

图5 是采用经纬仪和本文算法的导弹飞行姿态图，图5（a）、（c）是导弹上升阶段的姿态，图5（b）、（d）是导弹下降阶段的姿态，图片中伴随模型的线条是实时飞行轨迹线。从图5（a）、（b）可以直观看出，导弹的飞行姿态和实时飞行轨迹线之间存在角度偏差，飞行效果不逼真，这是由于光学经纬仪等测量设备的实时性和准确性不高，导致飞行姿态的数据出现延时，误差大或者缺失等问题。而图5（c）、（d）中采用本文算法仿真的导弹飞行姿态和实时飞行轨迹线始终保持一致，逼真度较高，实际动态效果流畅。

图5 导弹飞行姿态图

3.2 算法的通用性研究

为了验证本文算法对其他飞行目标姿态调整的效果，将本算法用于飞机模型的仿真试验，飞机坐标系OXYZ，即原点O 位于飞机重心，OX 轴沿弹体横轴指向右，OY 轴与弹体纵轴重合，指向飞机头部，OZ 轴沿弹体竖轴指向上。飞机飞行时，本算法计算出的姿态数据不断地赋值给飞机，飞机在飞行过程中呈现出不同的变化姿态。

图6 是对飞机模型使用本文算法的实时态势的展示效果图，可以看出，飞机的飞行态势随着飞行轨迹实时不断变化，实时性好，效果流畅，飞行姿态逼真，观赏性强。

图6 使用本文算法的飞机姿态效果图

4 结论

本文提出一种基于飞行轨迹的三维模型实时姿态调整算法，该算法利用实时性和准确性较稳定的三维模型飞行轨迹数据，自动生成飞行姿态数据，实时调整三维场景中三维模型的飞行姿态。将该算法用于在某型号导弹飞行态势的三维GIS 仿真模拟试验中，从导弹呈现出的飞行态势以及求出的姿态数据可以看出，该算法自动生成的飞行态势精确度高，实际应用效果好。将该算法应用于飞机的飞行姿态的模拟，仿真结果表明，飞机也呈现出很好的飞行效果，由此得出本文算法通用于动态目标的飞行姿态调整。