基于四元數(shù)的DR技術(shù)

魯?shù)さぃ杂来洌苡穹?

(江蘇自動(dòng)化研究所，江蘇連云港 222061)

隨著計(jì)算機(jī)仿真技術(shù)的飛速發(fā)展和硬件性能的不斷提高，各國(guó)不斷增加對(duì)模擬訓(xùn)練的重視程度。美軍從20世紀(jì)80年代開始，就致力于異地分布式模擬訓(xùn)練的研究，取得了許多成果，提高了訓(xùn)練效益[1-2]。異地分布式模擬訓(xùn)練是一種能實(shí)現(xiàn)戰(zhàn)役、戰(zhàn)術(shù)、平臺(tái)、武器多級(jí)聯(lián)動(dòng)的訓(xùn)練。在帶動(dòng)武器的異地分布式訓(xùn)練系統(tǒng)中包含船艦、飛機(jī)、導(dǎo)彈等運(yùn)動(dòng)實(shí)體。訓(xùn)練中這些運(yùn)動(dòng)實(shí)體的狀態(tài)(如位置、姿態(tài)等)是不斷變化的，仿真節(jié)點(diǎn)需將這些變化告知系統(tǒng)中其他相關(guān)仿真節(jié)點(diǎn)，以便了解這些實(shí)體目前的狀態(tài)。當(dāng)系統(tǒng)中仿真的運(yùn)動(dòng)實(shí)體數(shù)量龐大時(shí)，節(jié)點(diǎn)之間大量的狀態(tài)信息發(fā)送將給整個(gè)系統(tǒng)造成通信負(fù)荷，很難保證訓(xùn)練的實(shí)時(shí)性。

針對(duì)該問題有兩種解決辦法:一種辦法是提高硬件條件、構(gòu)建高速大容量的通信網(wǎng)絡(luò)；另一種辦法是設(shè)法降低訓(xùn)練系統(tǒng)的通信負(fù)荷，即減少節(jié)點(diǎn)之間交互的信息量，推算定位(Dead Reckoning，簡(jiǎn)稱DR)技術(shù)就是降低節(jié)點(diǎn)之間信息交互量的一種手段。

1 DR技術(shù)

1.1 DR技術(shù)基本原理

DR(Dead Reckoning)一詞來源于航海。在航海中，為了使船只到達(dá)某一地點(diǎn)，需要使船頭保持某一方向航行一段時(shí)間，當(dāng)實(shí)際航線與期望的方向出現(xiàn)較大的差異時(shí)，修正航向，再沿新的航行方向進(jìn)行[3]。在分布交互仿真中采用DR技術(shù)估計(jì)仿真實(shí)體的位置和姿態(tài)，限制仿真節(jié)點(diǎn)間實(shí)體狀態(tài)發(fā)送頻率、減少網(wǎng)絡(luò)傳輸數(shù)據(jù)量。使用DR技術(shù)時(shí)，對(duì)于發(fā)送實(shí)體，仿真節(jié)點(diǎn)維持該實(shí)體的一個(gè)高精度模型和一個(gè)DR模型，當(dāng)高精度模型計(jì)算出的位置、姿態(tài)與DR模型計(jì)算出的位置、姿態(tài)的差值超過設(shè)定的值(閾值)時(shí)，仿真節(jié)點(diǎn)向其相關(guān)仿真節(jié)點(diǎn)發(fā)送該實(shí)體的由高精度模型計(jì)算出的實(shí)際位置和姿態(tài)，并用此信息更新該實(shí)體的DR模型參數(shù)。對(duì)于接收實(shí)體，仿真節(jié)點(diǎn)內(nèi)部維持其感興趣的其他實(shí)體的DR模型，并使用該模型外推其他實(shí)體的位置和姿態(tài)，直到接收到狀態(tài)更新信息，使用接收到的信息更新相關(guān)實(shí)體的DR模型參數(shù)，并繼續(xù)外推。

1.2 DR模型

表1給出了DIS標(biāo)準(zhǔn)中的9種DR模型(Dead Reckoning Model，簡(jiǎn)稱DRM)。DR模型可表示為DRM(F or R，P or V， W or B)。其中第一個(gè)參數(shù)表示實(shí)體是否有轉(zhuǎn)動(dòng)運(yùn)動(dòng)，F(xiàn)表示無轉(zhuǎn)動(dòng)運(yùn)動(dòng)，R表示有轉(zhuǎn)動(dòng)運(yùn)動(dòng)；第二個(gè)參數(shù)表示實(shí)體是做勻速運(yùn)動(dòng)還是變速運(yùn)動(dòng)，P表示做勻速運(yùn)動(dòng)，V表示做變速運(yùn)動(dòng)；第三個(gè)參數(shù)表示實(shí)體運(yùn)動(dòng)所選擇的坐標(biāo)系，W表示地心坐標(biāo)系，B表示實(shí)體坐標(biāo)系。同時(shí)第二個(gè)參數(shù)表明算法是關(guān)于位置的一階算法還是二階算法。

表1 DR模型

9種DRM中第2、3、4、5模型使用地心坐標(biāo)系計(jì)算，第6、7、8、9模型使用實(shí)體坐標(biāo)系計(jì)算。

地心坐標(biāo)系下的位置外推公式為：

P=P0+V0×Δt

(1)

(2)

式中，P0，V0，A0分別表示地心坐標(biāo)系下初始仿真時(shí)刻t0的位置向量、速度向量以及加速度向量，Δt表示算法外推的時(shí)間增量。

實(shí)體坐標(biāo)系下的位置外推公式為：

(3)

(4)

(5)

仿真實(shí)體姿態(tài)由實(shí)體坐標(biāo)系相對(duì)于地心坐標(biāo)系的歐拉角表示，分別為ψ，θ，φ。9種模型中實(shí)體姿態(tài)外推公式為

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

(6)

式中，[DR]表示t0時(shí)刻的實(shí)體坐標(biāo)旋轉(zhuǎn)到t0+Δt時(shí)刻的實(shí)體坐標(biāo)的旋轉(zhuǎn)矩陣：

(7)

式中，ω=(ωx，ωy，ωz)是實(shí)體坐標(biāo)系下的角速度向量。[R0]w→b表示t0時(shí)刻從地心坐標(biāo)系到實(shí)體坐標(biāo)系的旋轉(zhuǎn)矩陣，可由t0時(shí)刻的歐拉角計(jì)算得到。[R]w→b表示t0+Δt時(shí)刻地心坐標(biāo)系到實(shí)體坐標(biāo)系的旋轉(zhuǎn)矩陣，若：

則t0+Δt時(shí)刻的姿態(tài)角為：

θ=arcsin(-A11)

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

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

(8)

2 仿真實(shí)體狀態(tài)外推

仿真訓(xùn)練系統(tǒng)中使用地心坐標(biāo)系下的DR模型來預(yù)測(cè)實(shí)體的運(yùn)動(dòng)狀態(tài)。當(dāng)系統(tǒng)中仿真實(shí)體做無轉(zhuǎn)動(dòng)勻速運(yùn)動(dòng)時(shí)，使用DR(FPW)模型僅對(duì)實(shí)體位置進(jìn)行外推；當(dāng)系統(tǒng)中仿真實(shí)體做無轉(zhuǎn)動(dòng)加速運(yùn)動(dòng)時(shí)，使用DR(FVW)模型僅對(duì)實(shí)體位置進(jìn)行外推。當(dāng)系統(tǒng)中仿真實(shí)體做轉(zhuǎn)動(dòng)勻速運(yùn)動(dòng)時(shí)，使用DR(RPW)對(duì)實(shí)體的位置和姿態(tài)進(jìn)行外推；當(dāng)系統(tǒng)中仿真實(shí)體做轉(zhuǎn)動(dòng)加速運(yùn)動(dòng)時(shí)，使用DR(RVW)模型對(duì)實(shí)體位置和姿態(tài)進(jìn)行外推。在仿真實(shí)體的生命周期內(nèi)，可根據(jù)實(shí)體的運(yùn)動(dòng)狀態(tài)選取不同的DR模型。同時(shí)在仿真系統(tǒng)中使用DR模型對(duì)實(shí)體狀態(tài)外推時(shí)，需根據(jù)仿真系統(tǒng)的精度需求設(shè)定仿真實(shí)體位置更新閾值和姿態(tài)更新閾值。

在給定的標(biāo)準(zhǔn)中[4]，DR技術(shù)應(yīng)用于仿真系統(tǒng)中的基本實(shí)體(Base Entity)類，包括飛行器(Aircraft)、水面艦艇(Surface Vessel)、潛航器(Surface Vessel)、車輛(Ground Vehicle)、兩棲車輛(Amphibious Vehicle)、航天器(Spacecraft)等平臺(tái)以及傳感器(Sensor)、無線電(Radio)等實(shí)體。這些實(shí)體的主要屬性有姿態(tài)、速度、加速度、空間位置、角速度。大多數(shù)仿真系統(tǒng)中，仿真實(shí)體的姿態(tài)角由實(shí)體坐標(biāo)系相對(duì)于地理坐標(biāo)系(北東天坐標(biāo)系)的歐拉角定義，分別為航向角C，縱搖角φ以及橫搖角β。仿真實(shí)體位置信息為地理(北東天)坐標(biāo)系下的速度向量V=(Vx，VyVz)，加速度向量a=(ax，ay，az)，以及空間位置向量p′=(X，Y，Z)。

2.1 實(shí)體位置外推

由于系統(tǒng)中仿真實(shí)體的位置是在地理坐標(biāo)系下表示的，因此需將地理坐標(biāo)系下的量轉(zhuǎn)換為地心坐標(biāo)系下的量進(jìn)行計(jì)算。

(9)

2.2 實(shí)體姿態(tài)外推

標(biāo)準(zhǔn)DR模型中，仿真實(shí)體的姿態(tài)角通過歐拉角法計(jì)算得到。由于歐拉角法運(yùn)算量大，且存在奇異點(diǎn)，可用四元數(shù)法求解歐拉角[5]。本文首先對(duì)四元數(shù)的基本概念進(jìn)行介紹，再利用四元數(shù)方法進(jìn)行實(shí)體姿態(tài)外推。

1) 四元數(shù)介紹

四元數(shù)(Quaternions)是哈密爾頓(愛爾蘭物理學(xué)家1805-1865)于1843年提出的數(shù)學(xué)概念，其指一個(gè)實(shí)數(shù)單位和三個(gè)虛數(shù)單位組成的實(shí)元的數(shù)，即

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

q0是四元數(shù)的標(biāo)量部分，q=q1i+q2j+q3k是四元數(shù)的矢量部分。其還具有矢量形式以及矩陣形式:

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

(10)

四元數(shù)點(diǎn)乘：

qAqB=qA0qB0+qAxqBx+qAyqBy+qAzqBz

(11)

四元數(shù)的點(diǎn)乘可用于計(jì)算兩個(gè)四元數(shù)之間的轉(zhuǎn)動(dòng)角。設(shè)ρ是四元數(shù)qA和qB之間的轉(zhuǎn)動(dòng)角。

(12)

2) 基于四元數(shù)的DR姿態(tài)外推

由于連續(xù)的旋轉(zhuǎn)可以通過四元數(shù)的乘法來表示[7]。因此仿真實(shí)體的姿態(tài)可通過四元數(shù)進(jìn)行解算。已知仿真實(shí)體t0時(shí)刻的姿態(tài)角以及實(shí)體繞軸轉(zhuǎn)動(dòng)的角度，可得到經(jīng)Δt時(shí)間后實(shí)體的姿態(tài)角。使用四元數(shù)乘法進(jìn)行姿態(tài)外推的公式如下：

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

(13)

其中qD(t0)表示地理坐標(biāo)系到實(shí)體坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)，由t0時(shí)刻的姿態(tài)角C0，φ0，β0計(jì)算得到：

(14)

qDR0=cos(τ)

qDRx=uxsin(τ)

qDRy=uysin(τ)

qDRz=uzsin(τ)

(15)

qD(t0+Δt)表示t0+Δt時(shí)刻仿真實(shí)體從地理坐標(biāo)系到實(shí)體坐標(biāo)系的轉(zhuǎn)動(dòng)四元數(shù)。

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

根據(jù)轉(zhuǎn)動(dòng)四元數(shù)和轉(zhuǎn)動(dòng)矩陣的關(guān)系，可求得t0+Δt時(shí)刻實(shí)體的三個(gè)姿態(tài)角為：

φ=arcsin(2(qyqz+q0qx))

(16)

同樣進(jìn)行姿態(tài)外推時(shí)，外推出的姿態(tài)與實(shí)際姿態(tài)存在一定的誤差，隨著仿真時(shí)間的推進(jìn)，或者仿真實(shí)體的隨機(jī)機(jī)動(dòng)，這個(gè)誤差可能會(huì)很大。較大的姿態(tài)誤差會(huì)影響仿真效果，因此需預(yù)先設(shè)定姿態(tài)更新閾值。假設(shè)K為設(shè)定的姿態(tài)更新閾值。

對(duì)于具有轉(zhuǎn)動(dòng)運(yùn)動(dòng)的仿真實(shí)體，節(jié)點(diǎn)內(nèi)部維持兩個(gè)四元數(shù)，一個(gè)是使用仿真實(shí)體實(shí)際姿態(tài)角計(jì)算得到的四元數(shù)qM，另一個(gè)是使用公式(13)計(jì)算得到的四元數(shù)qD。定義α為qM與qD之間的旋轉(zhuǎn)角。

(17)

當(dāng)外推出的姿態(tài)角與實(shí)際姿態(tài)角的差值超過姿態(tài)更新閾值，即α>K時(shí)

仿真節(jié)點(diǎn)將向外發(fā)送該實(shí)體的實(shí)際狀態(tài)信息，并更新該實(shí)體的DR模型參數(shù),包括位置信息以及姿態(tài)信息等。并用當(dāng)前時(shí)刻的四元數(shù)qM(t)代替公式(13)中的四元數(shù)qD(t)(qA(t)→qD(t))，繼續(xù)進(jìn)行外推，直到下次更新。

3 仿真實(shí)驗(yàn)及結(jié)果分析

“××體系建模與仿真平臺(tái)”能支持海上方向戰(zhàn)役、戰(zhàn)術(shù)多級(jí)對(duì)抗仿真，具有模型開發(fā)、想定編輯、分析評(píng)估等功能。本文采用該仿真平臺(tái)進(jìn)行仿真實(shí)驗(yàn)，分別使用標(biāo)準(zhǔn)DR姿態(tài)外推模型與基于四元數(shù)的DR姿態(tài)外推模型對(duì)仿真實(shí)體的姿態(tài)進(jìn)行外推，同時(shí)對(duì)仿真實(shí)體的位置進(jìn)行了外推。節(jié)點(diǎn)內(nèi)部的DR運(yùn)行過程如圖1所示。

使用該仿真平臺(tái)編輯飛機(jī)飛行任務(wù)想定，飛行軌跡為曲線如圖2所示。設(shè)定飛機(jī)的位置更新閾值為100 m，姿態(tài)更新閾值為3°，仿真步長(zhǎng)為1 s，仿真運(yùn)行時(shí)間為20 min。飛機(jī)做轉(zhuǎn)動(dòng)、變速運(yùn)動(dòng)，飛行平均速度為260.12 m/s。

圖1 DR技術(shù)運(yùn)行過程圖

圖2 飛機(jī)飛行航跡

下面分別從算法運(yùn)行時(shí)間、飛機(jī)姿態(tài)角、飛機(jī)運(yùn)行軌跡誤差、飛機(jī)狀態(tài)更新時(shí)間間隔，來比較兩種算法。

兩種算法運(yùn)行時(shí)間對(duì)比圖如圖3所示。標(biāo)準(zhǔn)DR姿態(tài)外推算法平均運(yùn)行時(shí)間為0.008 466 ms，基于四元數(shù)的DR姿態(tài)外推算法平均運(yùn)行時(shí)間為0.007 592 ms，較標(biāo)準(zhǔn)姿態(tài)外推算法在運(yùn)行時(shí)間上降低了10.32%左右。

圖3 姿態(tài)外推算法運(yùn)行時(shí)間對(duì)比圖

兩種算法外推出的飛機(jī)飛行航向角如圖4所示，飛行橫搖角如圖5所示，飛行縱搖角如圖6所示。

圖4 航向角對(duì)比圖

從圖4可以看出使用基于四元數(shù)的DR外推得到的航向角更接近于實(shí)際的航向角。

圖5 橫搖角對(duì)比圖

從圖5可以看出，仿真過程中飛機(jī)的實(shí)際橫搖角大小變化不明顯。標(biāo)準(zhǔn)DR姿態(tài)外推方法得到的橫滾角大小變化相對(duì)明顯?；谒脑獢?shù)的DR姿態(tài)外推方法得到的橫滾角與實(shí)際橫滾角的大小更加接近。

從圖6可以看出，運(yùn)行的初始階段飛機(jī)飛行的縱搖角出現(xiàn)明顯變動(dòng)，這是由于仿真初始階段飛機(jī)的上升運(yùn)動(dòng)而造成的。通過對(duì)比可以看出，基于四元數(shù)的DR姿態(tài)外推方法推算出的縱搖角更符合飛機(jī)實(shí)際的縱搖角。

通過以上分析可以得到基于四元數(shù)的姿態(tài)外推算法運(yùn)行效率更高，且外推出的姿態(tài)角的精度更高。下面分析兩種方法外推出的軌跡與算法更新頻率。

圖6 縱搖角對(duì)比圖

使用基于四元數(shù)的DR模型外推得到的飛行軌跡如圖7所示，對(duì)比圖7和圖2可以看出，使用該模型能得到較精確的飛行軌跡。

圖7 基于四元數(shù)的DR外推飛行軌跡

使用標(biāo)準(zhǔn)DR模型外推，得到的飛機(jī)飛行軌跡如圖8所示。使用該方法也能得到較為準(zhǔn)確的飛機(jī)飛行軌跡。

圖8 標(biāo)準(zhǔn)DR外推飛機(jī)軌跡

飛機(jī)飛行的位置誤差由每一時(shí)刻的實(shí)際位置與DR模型外推出的位置的差值得到。兩種方法位置誤差如圖9所示?；谒脑獢?shù)的DR外推平均位置誤差為32.2 m，標(biāo)準(zhǔn)DR外推平均位置誤差為40.1 m。造成這一現(xiàn)象的原因是，基于四元數(shù)的外推方法推算出的姿態(tài)角更加精確，使得外推出的仿真實(shí)體的位置也更加精確。

圖9 位置誤差對(duì)比圖

更新時(shí)間間隔由當(dāng)前的更新時(shí)刻減去上次更新時(shí)刻得到。更新時(shí)間間隔越大，說明模型外推效果越好。兩種方法的更新時(shí)間間隔如圖10所示?；谒脑獢?shù)的DR外推方法實(shí)現(xiàn)飛機(jī)軌跡外推的平均更新時(shí)間間隔為18.9 s，標(biāo)準(zhǔn)DR外推方法實(shí)現(xiàn)飛機(jī)軌跡外推的平均更新時(shí)間間隔為12.5 s?；谒脑獢?shù)的DR外推方法在減少仿真實(shí)體的狀態(tài)更新頻率上更加有效。

圖10 更新時(shí)間間隔對(duì)比圖

以上實(shí)驗(yàn)結(jié)果表明本文提出的基于四元數(shù)的DR外推算法運(yùn)行時(shí)間更少、運(yùn)行效率更高，姿態(tài)和位置外推精度更高。仿真實(shí)體更加精確的位置和姿態(tài)推算，一方面能有效減少實(shí)體狀態(tài)發(fā)送頻率，緩解網(wǎng)絡(luò)通信負(fù)荷，另一方面系統(tǒng)的仿真精度更高，更加符合實(shí)際情況。這對(duì)于構(gòu)建大規(guī)模的分布式模擬訓(xùn)練是具有重要意義的。

4 結(jié)束語(yǔ)

DR技術(shù)是緩解分布式仿真系統(tǒng)中網(wǎng)絡(luò)通信量的重要手段，本文為減少標(biāo)準(zhǔn)DR模型姿態(tài)外推的計(jì)算量，提出了基于四元數(shù)的姿態(tài)外推方法。該方法在運(yùn)行時(shí)間、外推精度等方面較標(biāo)準(zhǔn)方法有了一定的提升。而DR技術(shù)還可以從更新閾值、網(wǎng)絡(luò)傳輸延遲方面進(jìn)行研究。