基于四元数的DR技术

鲁丹丹，翟永翠，周玉芳

(江苏自动化研究所，江苏连云港 222061)

随着计算机仿真技术的飞速发展和硬件性能的不断提高，各国不断增加对模拟训练的重视程度。美军从20世纪80年代开始，就致力于异地分布式模拟训练的研究，取得了许多成果，提高了训练效益[1-2]。异地分布式模拟训练是一种能实现战役、战术、平台、武器多级联动的训练。在带动武器的异地分布式训练系统中包含船舰、飞机、导弹等运动实体。训练中这些运动实体的状态(如位置、姿态等)是不断变化的，仿真节点需将这些变化告知系统中其他相关仿真节点，以便了解这些实体目前的状态。当系统中仿真的运动实体数量庞大时，节点之间大量的状态信息发送将给整个系统造成通信负荷，很难保证训练的实时性。

针对该问题有两种解决办法:一种办法是提高硬件条件、构建高速大容量的通信网络；另一种办法是设法降低训练系统的通信负荷，即减少节点之间交互的信息量，推算定位(Dead Reckoning，简称DR)技术就是降低节点之间信息交互量的一种手段。

1 DR技术

1.1 DR技术基本原理

DR(Dead Reckoning)一词来源于航海。在航海中，为了使船只到达某一地点，需要使船头保持某一方向航行一段时间，当实际航线与期望的方向出现较大的差异时，修正航向，再沿新的航行方向进行[3]。在分布交互仿真中采用DR技术估计仿真实体的位置和姿态，限制仿真节点间实体状态发送频率、减少网络传输数据量。使用DR技术时，对于发送实体，仿真节点维持该实体的一个高精度模型和一个DR模型，当高精度模型计算出的位置、姿态与DR模型计算出的位置、姿态的差值超过设定的值(阈值)时，仿真节点向其相关仿真节点发送该实体的由高精度模型计算出的实际位置和姿态，并用此信息更新该实体的DR模型参数。对于接收实体，仿真节点内部维持其感兴趣的其他实体的DR模型，并使用该模型外推其他实体的位置和姿态，直到接收到状态更新信息，使用接收到的信息更新相关实体的DR模型参数，并继续外推。

1.2 DR模型

表1给出了DIS标准中的9种DR模型(Dead Reckoning Model，简称DRM)。DR模型可表示为DRM(F or R，P or V， W or B)。其中第一个参数表示实体是否有转动运动，F表示无转动运动，R表示有转动运动；第二个参数表示实体是做匀速运动还是变速运动，P表示做匀速运动，V表示做变速运动；第三个参数表示实体运动所选择的坐标系，W表示地心坐标系，B表示实体坐标系。同时第二个参数表明算法是关于位置的一阶算法还是二阶算法。

表1 DR模型

9种DRM中第2、3、4、5模型使用地心坐标系计算，第6、7、8、9模型使用实体坐标系计算。

地心坐标系下的位置外推公式为：

P=P0+V0×Δt

(1)

(2)

式中，P0，V0，A0分别表示地心坐标系下初始仿真时刻t0的位置向量、速度向量以及加速度向量，Δt表示算法外推的时间增量。

实体坐标系下的位置外推公式为：

(3)

(4)

(5)

仿真实体姿态由实体坐标系相对于地心坐标系的欧拉角表示，分别为ψ，θ，φ。9种模型中实体姿态外推公式为

[R]w→b=[DR][R0]w→b

(6)

式中，[DR]表示t0时刻的实体坐标旋转到t0+Δt时刻的实体坐标的旋转矩阵：

(7)

式中，ω=(ωx，ωy，ωz)是实体坐标系下的角速度向量。[R0]w→b表示t0时刻从地心坐标系到实体坐标系的旋转矩阵，可由t0时刻的欧拉角计算得到。[R]w→b表示t0+Δt时刻地心坐标系到实体坐标系的旋转矩阵，若：

则t0+Δt时刻的姿态角为：

θ=arcsin(-A11)

ψ=arccos(A11/cosθ)×sgn(A12)

φ=arccos(A33/cosθ)×sgn(A23)

(8)

2 仿真实体状态外推

仿真训练系统中使用地心坐标系下的DR模型来预测实体的运动状态。当系统中仿真实体做无转动匀速运动时，使用DR(FPW)模型仅对实体位置进行外推；当系统中仿真实体做无转动加速运动时，使用DR(FVW)模型仅对实体位置进行外推。当系统中仿真实体做转动匀速运动时，使用DR(RPW)对实体的位置和姿态进行外推；当系统中仿真实体做转动加速运动时，使用DR(RVW)模型对实体位置和姿态进行外推。在仿真实体的生命周期内，可根据实体的运动状态选取不同的DR模型。同时在仿真系统中使用DR模型对实体状态外推时，需根据仿真系统的精度需求设定仿真实体位置更新阈值和姿态更新阈值。

在给定的标准中[4]，DR技术应用于仿真系统中的基本实体(Base Entity)类，包括飞行器(Aircraft)、水面舰艇(Surface Vessel)、潜航器(Surface Vessel)、车辆(Ground Vehicle)、两栖车辆(Amphibious Vehicle)、航天器(Spacecraft)等平台以及传感器(Sensor)、无线电(Radio)等实体。这些实体的主要属性有姿态、速度、加速度、空间位置、角速度。大多数仿真系统中，仿真实体的姿态角由实体坐标系相对于地理坐标系(北东天坐标系)的欧拉角定义，分别为航向角C，纵摇角φ以及横摇角β。仿真实体位置信息为地理(北东天)坐标系下的速度向量V=(Vx，VyVz)，加速度向量a=(ax，ay，az)，以及空间位置向量p′=(X，Y，Z)。

2.1 实体位置外推

由于系统中仿真实体的位置是在地理坐标系下表示的，因此需将地理坐标系下的量转换为地心坐标系下的量进行计算。

(9)

2.2 实体姿态外推

标准DR模型中，仿真实体的姿态角通过欧拉角法计算得到。由于欧拉角法运算量大，且存在奇异点，可用四元数法求解欧拉角[5]。本文首先对四元数的基本概念进行介绍，再利用四元数方法进行实体姿态外推。

1) 四元数介绍

四元数(Quaternions)是哈密尔顿(爱尔兰物理学家1805-1865)于1843年提出的数学概念，其指一个实数单位和三个虚数单位组成的实元的数，即

Q(q0，q1，q2，q3)=q0+q1i+q2j+q3k

q0是四元数的标量部分，q=q1i+q2j+q3k是四元数的矢量部分。其还具有矢量形式以及矩阵形式:

Q=(q0，q1，q2，q3)τ

(10)

四元数点乘：

qAqB=qA0qB0+qAxqBx+qAyqBy+qAzqBz

(11)

四元数的点乘可用于计算两个四元数之间的转动角。设ρ是四元数qA和qB之间的转动角。

(12)

2) 基于四元数的DR姿态外推

由于连续的旋转可以通过四元数的乘法来表示[7]。因此仿真实体的姿态可通过四元数进行解算。已知仿真实体t0时刻的姿态角以及实体绕轴转动的角度，可得到经Δt时间后实体的姿态角。使用四元数乘法进行姿态外推的公式如下：

qD(t0+t)=qD(t0)qDR

(13)

其中qD(t0)表示地理坐标系到实体坐标系的转动四元数，由t0时刻的姿态角C0，φ0，β0计算得到：

(14)

qDR0=cos(τ)

qDRx=uxsin(τ)

qDRy=uysin(τ)

qDRz=uzsin(τ)

(15)

qD(t0+Δt)表示t0+Δt时刻仿真实体从地理坐标系到实体坐标系的转动四元数。

qD(t0+Δt)=(q0，qx，qy，qz)

根据转动四元数和转动矩阵的关系，可求得t0+Δt时刻实体的三个姿态角为：

φ=arcsin(2(qyqz+q0qx))

(16)

同样进行姿态外推时，外推出的姿态与实际姿态存在一定的误差，随着仿真时间的推进，或者仿真实体的随机机动，这个误差可能会很大。较大的姿态误差会影响仿真效果，因此需预先设定姿态更新阈值。假设K为设定的姿态更新阈值。

对于具有转动运动的仿真实体，节点内部维持两个四元数，一个是使用仿真实体实际姿态角计算得到的四元数qM，另一个是使用公式(13)计算得到的四元数qD。定义α为qM与qD之间的旋转角。

(17)

当外推出的姿态角与实际姿态角的差值超过姿态更新阈值，即α>K时

仿真节点将向外发送该实体的实际状态信息，并更新该实体的DR模型参数,包括位置信息以及姿态信息等。并用当前时刻的四元数qM(t)代替公式(13)中的四元数qD(t)(qA(t)→qD(t))，继续进行外推，直到下次更新。

3 仿真实验及结果分析

“××体系建模与仿真平台”能支持海上方向战役、战术多级对抗仿真，具有模型开发、想定编辑、分析评估等功能。本文采用该仿真平台进行仿真实验，分别使用标准DR姿态外推模型与基于四元数的DR姿态外推模型对仿真实体的姿态进行外推，同时对仿真实体的位置进行了外推。节点内部的DR运行过程如图1所示。

使用该仿真平台编辑飞机飞行任务想定，飞行轨迹为曲线如图2所示。设定飞机的位置更新阈值为100 m，姿态更新阈值为3°，仿真步长为1 s，仿真运行时间为20 min。飞机做转动、变速运动，飞行平均速度为260.12 m/s。

图1 DR技术运行过程图

图2 飞机飞行航迹

下面分别从算法运行时间、飞机姿态角、飞机运行轨迹误差、飞机状态更新时间间隔，来比较两种算法。

两种算法运行时间对比图如图3所示。标准DR姿态外推算法平均运行时间为0.008 466 ms，基于四元数的DR姿态外推算法平均运行时间为0.007 592 ms，较标准姿态外推算法在运行时间上降低了10.32%左右。

图3 姿态外推算法运行时间对比图

两种算法外推出的飞机飞行航向角如图4所示，飞行横摇角如图5所示，飞行纵摇角如图6所示。

图4 航向角对比图

从图4可以看出使用基于四元数的DR外推得到的航向角更接近于实际的航向角。

图5 横摇角对比图

从图5可以看出，仿真过程中飞机的实际横摇角大小变化不明显。标准DR姿态外推方法得到的横滚角大小变化相对明显。基于四元数的DR姿态外推方法得到的横滚角与实际横滚角的大小更加接近。

从图6可以看出，运行的初始阶段飞机飞行的纵摇角出现明显变动，这是由于仿真初始阶段飞机的上升运动而造成的。通过对比可以看出，基于四元数的DR姿态外推方法推算出的纵摇角更符合飞机实际的纵摇角。

通过以上分析可以得到基于四元数的姿态外推算法运行效率更高，且外推出的姿态角的精度更高。下面分析两种方法外推出的轨迹与算法更新频率。

图6 纵摇角对比图

使用基于四元数的DR模型外推得到的飞行轨迹如图7所示，对比图7和图2可以看出，使用该模型能得到较精确的飞行轨迹。

图7 基于四元数的DR外推飞行轨迹

使用标准DR模型外推，得到的飞机飞行轨迹如图8所示。使用该方法也能得到较为准确的飞机飞行轨迹。

图8 标准DR外推飞机轨迹

飞机飞行的位置误差由每一时刻的实际位置与DR模型外推出的位置的差值得到。两种方法位置误差如图9所示。基于四元数的DR外推平均位置误差为32.2 m，标准DR外推平均位置误差为40.1 m。造成这一现象的原因是，基于四元数的外推方法推算出的姿态角更加精确，使得外推出的仿真实体的位置也更加精确。

图9 位置误差对比图

更新时间间隔由当前的更新时刻减去上次更新时刻得到。更新时间间隔越大，说明模型外推效果越好。两种方法的更新时间间隔如图10所示。基于四元数的DR外推方法实现飞机轨迹外推的平均更新时间间隔为18.9 s，标准DR外推方法实现飞机轨迹外推的平均更新时间间隔为12.5 s。基于四元数的DR外推方法在减少仿真实体的状态更新频率上更加有效。

图10 更新时间间隔对比图

以上实验结果表明本文提出的基于四元数的DR外推算法运行时间更少、运行效率更高，姿态和位置外推精度更高。仿真实体更加精确的位置和姿态推算，一方面能有效减少实体状态发送频率，缓解网络通信负荷，另一方面系统的仿真精度更高，更加符合实际情况。这对于构建大规模的分布式模拟训练是具有重要意义的。

4 结束语

DR技术是缓解分布式仿真系统中网络通信量的重要手段，本文为减少标准DR模型姿态外推的计算量，提出了基于四元数的姿态外推方法。该方法在运行时间、外推精度等方面较标准方法有了一定的提升。而DR技术还可以从更新阈值、网络传输延迟方面进行研究。