船用指北平臺(tái)式慣導(dǎo)模擬器中軌跡發(fā)生器設(shè)計(jì)

張夢(mèng)得，胡柏青，陳 琪，張會(huì)東

(1.海軍工程大學(xué)導(dǎo)航工程教研室，湖北武漢430033；2.武警海警總隊(duì)山東支隊(duì)青島大隊(duì)，山東青島266033)

0 引言

平臺(tái)式慣性導(dǎo)航系統(tǒng)是自主式導(dǎo)航系統(tǒng)，能夠?yàn)榕灤_導(dǎo)航，是現(xiàn)代艦船上不可或缺的重要裝備[1–5]。慣性導(dǎo)航系統(tǒng)維保人員必須具備熟練的技能，才能高質(zhì)量完成導(dǎo)航系統(tǒng)的日常維保、維修操作。由于平臺(tái)式慣導(dǎo)系統(tǒng)實(shí)裝操作、維修等訓(xùn)練存在著風(fēng)險(xiǎn)大、成本高、周期長(zhǎng)、設(shè)備壽命降低等方面的問題，還容易受到氣象、海況等條件的限制[6–9]。因此，研制成本較低的平臺(tái)式慣性導(dǎo)航系統(tǒng)模擬器用于維保人員的訓(xùn)練，可大幅度降低人員訓(xùn)練成本，提高訓(xùn)練效率。通過模擬器對(duì)學(xué)員進(jìn)行操作、維修訓(xùn)練和考核，不但可以彌補(bǔ)理論與實(shí)踐環(huán)節(jié)在銜接過程中脫節(jié)的問題，還可以使學(xué)員充分掌握實(shí)際操作程序和方法，大大地提高訓(xùn)練效果。模擬器以船用平臺(tái)式慣導(dǎo)系統(tǒng)為對(duì)象，要求具備不亞于實(shí)裝體驗(yàn)的真實(shí)的系統(tǒng)基本操作和常見及特殊故障診斷排除方面的教學(xué)、訓(xùn)練以及考核等基本功能，為學(xué)員提供真實(shí)體驗(yàn)。

軌跡發(fā)生器通過對(duì)艦船航行軌跡的模擬，可以為慣導(dǎo)模擬器提供慣性測(cè)量器件（Inertial M easurement Unit，IMU）的輸出，是整個(gè)慣導(dǎo)模擬器軟件部分的關(guān)鍵。一個(gè)能夠模擬艦船操舵使用情況下真實(shí)航路的軌跡發(fā)生器，是實(shí)現(xiàn)模擬器后續(xù)進(jìn)行組合導(dǎo)航模式工作，故障檢測(cè)工作模擬的基礎(chǔ)。目前比較常見的軌跡發(fā)生器分為：數(shù)值式軌跡發(fā)生器與解析式軌跡發(fā)生器2種。嚴(yán)恭敏等[10]為捷聯(lián)慣導(dǎo)設(shè)計(jì)了一種基于數(shù)值式軌跡發(fā)生器的，通過輸入運(yùn)動(dòng)時(shí)間，運(yùn)動(dòng)類型以及所需的加速度，角速度等信息，可以得到相對(duì)應(yīng)的IMU參數(shù)，是非常經(jīng)典的軌跡發(fā)生器。文獻(xiàn)[11]為更好的對(duì)飛行器用的捷聯(lián)慣導(dǎo)進(jìn)行仿真，提高仿真精度，根據(jù)需要定義運(yùn)動(dòng)軌跡特性用以計(jì)算軌跡期間載體的速度和角速度變化量，采用數(shù)字積分算法的逆過程分步計(jì)算軌跡過程中的姿態(tài)、位置、速度以及慣性傳感器的積分增量等導(dǎo)航參數(shù)，取得較好效果。文獻(xiàn)[12]從飛機(jī)的運(yùn)動(dòng)特性出發(fā)，根據(jù)飛機(jī)常見的機(jī)動(dòng)模式，模擬了飛機(jī)的俯仰機(jī)動(dòng)、平飛以及協(xié)調(diào)轉(zhuǎn)彎等情況，利用慣性導(dǎo)航算法對(duì)軌跡進(jìn)行了重構(gòu)，得到了較為符合飛行實(shí)際情況的軌跡與與之對(duì)應(yīng)的IMU參數(shù)。文獻(xiàn)[13]指出了數(shù)值式軌跡發(fā)生器會(huì)積累誤差的問題，導(dǎo)致軌跡發(fā)生器精度難以滿足高精度導(dǎo)航系統(tǒng)的要求，提出了高精度的全解析式飛行軌跡生成方法。

當(dāng)前大部分軌跡發(fā)生器的研究都是針對(duì)捷聯(lián)慣導(dǎo)與飛行器展開的，對(duì)于艦船使用的指北方位平臺(tái)式慣導(dǎo)的軌跡發(fā)生器研究不多。設(shè)計(jì)一種能夠應(yīng)用于船用指北方位平臺(tái)式慣導(dǎo)模擬器的軌跡發(fā)生器是一個(gè)亟待解決的問題。本文將基于艦船航行的實(shí)際，結(jié)合指北方位平臺(tái)式慣導(dǎo)的特點(diǎn)，為模擬器設(shè)計(jì)一種解析式軌跡發(fā)生器軟件，操作人員只需要設(shè)置起點(diǎn)與終點(diǎn)，以及必要的參數(shù)就可以生成與之相對(duì)應(yīng)的軌跡以及IMU參數(shù)。在實(shí)驗(yàn)部分，通過在模擬器中設(shè)置位置點(diǎn)生成IMU參數(shù)并通過慣導(dǎo)解算模塊得到慣導(dǎo)自主、組合模式下的導(dǎo)航參數(shù)，證明設(shè)計(jì)方法的有效性。

1 解析式軌跡方程

艦船在大海中航行不同于飛機(jī)，不但運(yùn)動(dòng)速度相比于飛機(jī)較慢，加速度也難以達(dá)到較大數(shù)值，而且也無法像飛機(jī)一樣在較短的時(shí)間內(nèi)完成轉(zhuǎn)彎等大航向改變的機(jī)動(dòng)，也不存在較大幅度無規(guī)律的縱橫搖。因此，在軌跡發(fā)生器的設(shè)計(jì)時(shí)，整個(gè)航行軌跡以直線航行為主，若遇到起點(diǎn)與終點(diǎn)航向不一致的情況，則進(jìn)行轉(zhuǎn)向機(jī)動(dòng)以調(diào)整艦船的航向。這樣就將整個(gè)軌跡分解為了勻速直線運(yùn)動(dòng)，勻變速直線運(yùn)動(dòng)與勻速圓周運(yùn)動(dòng)3種情況的組合。因此，只需要規(guī)定每一小段中的起點(diǎn)與終點(diǎn)，運(yùn)動(dòng)時(shí)間以及必要的運(yùn)動(dòng)參數(shù)就可以生成相對(duì)應(yīng)的軌跡方程。下面將對(duì)這3種運(yùn)動(dòng)模式的模型進(jìn)行介紹。各式中，為緯度L，經(jīng)度λ，起點(diǎn)坐標(biāo)為(L0,λ0) ，終點(diǎn)坐標(biāo)為(L1,λ1)。

1.1 勻速直線運(yùn)動(dòng)

勻速直線運(yùn)動(dòng)是艦船航行過程中最為常見的一種航行模式，通常的經(jīng)濟(jì)航速為10～20 kn，為了提高軌跡發(fā)生器的精度，不能將地球表面認(rèn)為是平面，使用平面上的兩點(diǎn)間距離公式，而是使用球面的距離公式

式中：s是起止位置的距離，R表示地球半徑。

為了降低對(duì)軌跡設(shè)置的要求，防止出現(xiàn)無法在設(shè)置時(shí)間內(nèi)從起點(diǎn)到達(dá)終點(diǎn)導(dǎo)致軌跡發(fā)生器出現(xiàn)錯(cuò)誤的情況，選擇通過操作人員輸入起始點(diǎn)，根據(jù)式（1）計(jì)算的距離與前段軌跡結(jié)束時(shí)的速度，后臺(tái)計(jì)算運(yùn)動(dòng)時(shí)間，根據(jù)式t=s/v可以得到勻速直線運(yùn)動(dòng)航行時(shí)間，式中v表示合速度。

通過航向?qū)纤俣冗M(jìn)行分解可以得到東向與北向速度。

式中：L(t′)與 λ(t′) 表示t′時(shí)刻艦船的緯度與經(jīng)度。

這樣就實(shí)現(xiàn)通過輸入起點(diǎn)坐標(biāo)為 (L0,λ0)與終點(diǎn)坐標(biāo)為 (L1,λ1)進(jìn)行勻速直線運(yùn)動(dòng)軌跡的產(chǎn)生。

1.2 勻變速直線運(yùn)動(dòng)

與勻速直線運(yùn)動(dòng)類似，為防止出現(xiàn)由于設(shè)置勻變速運(yùn)動(dòng)時(shí)間與設(shè)置的起止點(diǎn)的距離不合理，使得加速度過大導(dǎo)致失真的問題。在勻變速直線運(yùn)動(dòng)模型中，通過設(shè)置加速度大小的方式來求解速度與運(yùn)動(dòng)時(shí)間。

式中：vL0為前一段軌跡終點(diǎn)的東向速度；vλ0為前一段軌跡終點(diǎn)的北向速度。

這樣就實(shí)現(xiàn)通過輸入起點(diǎn)坐標(biāo)為 (L0,λ0)，終點(diǎn)坐標(biāo)為 (L1,λ1)與加速度進(jìn)行勻變速直線運(yùn)動(dòng)軌跡的產(chǎn)生。

1.3 勻速圓周運(yùn)動(dòng)

艦船的圓周運(yùn)動(dòng)主要有2種情況，一種情況是由于地形或者航線要求等原因需要圓周運(yùn)動(dòng)或者通過圓周運(yùn)動(dòng)進(jìn)行避障，另一種是由于航向的改變，需要通過一定圓心角的圓周運(yùn)動(dòng)來調(diào)整。

1）勻速圓周機(jī)動(dòng)

勻速圓周機(jī)動(dòng)將軌跡起止點(diǎn)作為圓弧的起止點(diǎn)，由起點(diǎn)航向與起止點(diǎn)位置計(jì)算相關(guān)參數(shù)。由幾何關(guān)系可以知道，圓心一定位于起點(diǎn) (L0,λ0)航向的垂線上，由終點(diǎn) (L1,λ1)向起點(diǎn)航向的垂線方向做垂線，根據(jù)平面幾何中點(diǎn)斜式方程的性質(zhì)，兩線相交必可以求得圓心位置 (L2,λ2)，如圖1所示。根據(jù)式（1）可以求得轉(zhuǎn)彎的半(徑長(zhǎng))度r，根據(jù)等腰三角形的性質(zhì)，由式α=可以求得圓心角α的大小，式中l(wèi)為起止點(diǎn)間的距離，由式（1）求得。

圖 1勻速圓周機(jī)動(dòng)幾何關(guān)系示意圖Fig.1 Geometric relationship of uniform circular motion

圖中，B點(diǎn)與A點(diǎn)是圓周運(yùn)動(dòng)的起始點(diǎn)，O點(diǎn)為根據(jù)起始點(diǎn)位置推算出的圓心位置。根據(jù)等腰三角形的性質(zhì)與圓的幾何性質(zhì)，可以得到相對(duì)應(yīng)的解析式方程。

這樣就實(shí)現(xiàn)通過輸入起點(diǎn)坐標(biāo)為 (L0,λ0)，終點(diǎn)坐標(biāo)為 (L1,λ1)通過起點(diǎn)處速度進(jìn)行勻速圓周機(jī)動(dòng)軌跡的產(chǎn)生。

2）調(diào)整航向的勻速圓周運(yùn)動(dòng)

調(diào)整航向的勻速圓周運(yùn)動(dòng)與圓周運(yùn)動(dòng)類似，只是圓周運(yùn)動(dòng)的起始點(diǎn)不是軌跡的起止點(diǎn)。此外，根據(jù)經(jīng)驗(yàn)以及方便簡(jiǎn)化計(jì)算，調(diào)整航向的圓周運(yùn)動(dòng)設(shè)計(jì)轉(zhuǎn)彎半徑r為100m，假設(shè)軌跡段1的起點(diǎn) (L0,λ0)與終點(diǎn)(L1,λ1) ，軌跡段2的起點(diǎn) (L1,λ1) 與終點(diǎn) (L2,λ2)。通過計(jì)算兩段軌跡起止點(diǎn)連線的斜率K1，K2，可以計(jì)算出轉(zhuǎn)彎的圓心角角度α，如下式：

對(duì)于轉(zhuǎn)彎的起止點(diǎn)問題，起點(diǎn)即位于 (L0,λ0)與(L1,λ1)的連線上，也位于圓心與該連線的垂線上，這樣就可以得到轉(zhuǎn)彎起點(diǎn)的坐標(biāo)。同理也可以得到終點(diǎn)的坐標(biāo)，這樣就將整個(gè)從(L0,λ0) 到 (L2,λ2)的 運(yùn) 動(dòng) 過程，分解成了2個(gè)直線運(yùn)動(dòng)與一段勻速圓周運(yùn)動(dòng)的組合。為了方便說明，此處做一個(gè)仿真圖。給出4個(gè)經(jīng)緯度點(diǎn)作為軌跡點(diǎn)（1，1），（1.01，1.01），（1.022，1.015），（1.034，1.007）。

圖2 調(diào)整航向的勻速圓周運(yùn)動(dòng)軌跡Fig.2 Uniform circular motion track of course adjustment

圖中星點(diǎn)為給出的軌跡點(diǎn)，可以清楚看到，在點(diǎn)（1.01，1.01）與（1.022，1.015）處，由于軌跡點(diǎn)導(dǎo)致的航向調(diào)整，使得軌跡在十分逼近軌跡點(diǎn)的位置有一個(gè)轉(zhuǎn)彎，使得航向調(diào)整得到軌跡下一點(diǎn)的航向。

這樣，對(duì)于復(fù)雜海洋中航行的艦船的軌跡規(guī)劃，就可以分解為以上幾種解析式方程的組合。

2 IMU參數(shù)計(jì)算

軌跡發(fā)生器除了具有航路規(guī)劃的作用，在平臺(tái)式慣導(dǎo)模擬器中，由于平臺(tái)式慣導(dǎo)模擬器并不存在陀螺儀和加速度計(jì)，還有一項(xiàng)重要的作用就是模擬IMU的輸出。無論是慣導(dǎo)的對(duì)準(zhǔn)、解算、組合都需要IMU的參數(shù)作為基礎(chǔ)。因此，在軌跡發(fā)生器的設(shè)計(jì)中，除了要給出軌跡的解析式方程，還需要給出與之相對(duì)應(yīng)的IMU參數(shù)計(jì)算方法。指北方位平臺(tái)式慣導(dǎo)由3個(gè)單自由度積分陀螺儀作為平臺(tái)角運(yùn)動(dòng)敏感控制元件，從而使得平臺(tái)系跟蹤地理系。平臺(tái)上還安裝有東向、北向加速度計(jì)。

2.1 加速度計(jì)算

慣導(dǎo)的解算是通過加速度計(jì)測(cè)得的比力，通過積分得到速度與位置。軌跡發(fā)生器為了能夠生成東向與北向的加速度計(jì)參數(shù)，需要對(duì)速度與位置微分得到相對(duì)應(yīng)的比力，下面給出東向與北向的加速度計(jì)算方法。

首先得到速度的計(jì)算公式如下：

式中：RM為 子午圈半徑，RN為卯酉圈半徑。

這樣通過對(duì)速度進(jìn)行微分就可以計(jì)算出加速度計(jì)提供的不含加速度計(jì)零偏的輸出信息：

式中：ωie為 地球自轉(zhuǎn)角速度；RN為卯酉圈半徑。

2.2 姿態(tài)角計(jì)算

航向角：

為了更真實(shí)模擬船用指北方位平臺(tái)式慣導(dǎo)的情況，加入符合正弦曲線的橫搖，以模擬海浪的艦船姿態(tài)上的作用與影響。

3 實(shí)驗(yàn)分析

為檢驗(yàn)軌跡發(fā)生器設(shè)計(jì)的合理性與實(shí)用性?，F(xiàn)將設(shè)計(jì)的船用解析式軌跡發(fā)生器嵌入到指北方位平臺(tái)式慣導(dǎo)模擬器軟件系統(tǒng)中。

通過對(duì)該軟件系統(tǒng)輸入?yún)?shù)，就可以讓操作人員實(shí)現(xiàn)對(duì)某型船用指北方位平臺(tái)式慣導(dǎo)的操作，維護(hù)與故障診斷方面的訓(xùn)練。

根據(jù)設(shè)置的位置點(diǎn)，軌跡發(fā)生器生成相對(duì)應(yīng)的軌跡與軌跡相對(duì)應(yīng)的加速度計(jì)輸出信息以及姿態(tài)角信息。以此為基礎(chǔ)，通過慣導(dǎo)解算模塊，輸出艦船的實(shí)時(shí)姿態(tài)，位置與速度。

圖3 為通過設(shè)置幾個(gè)位置坐標(biāo)，并選擇航行模式由船用指北方位平臺(tái)式慣導(dǎo)模擬器中的軌跡發(fā)生器生成的一段航行軌跡，模擬從武漢長(zhǎng)江出發(fā)的航行，為了更有效說明軌跡發(fā)生器設(shè)計(jì)的有效性，在軌跡點(diǎn)的設(shè)計(jì)與選取中，要求軌跡盡可能的復(fù)雜，包含多種情況。輸入的幾個(gè)坐標(biāo)位置是（29°，113°），（29.04°，113.06°），（29.12°，113.13°），（29.15°，113.21°），（29.17°，113.28°），（29.27°，113.34°），（29.36°，113.28°），（29.49°，113.28°）。其中在（29°，113°）至（29.04°，113.06°）設(shè)置為勻加速直線運(yùn)動(dòng)，將航速從0加速至12 kn。（29.04°，113.06°）至（29.12°，113.13°）設(shè)置為勻速圓周運(yùn)動(dòng)模擬長(zhǎng)江地理流向。（29.12°，113.13°）至（29.15°，113.21°）為勻速直線運(yùn)動(dòng)，（29.15°，113.21°）至（29.17°，113.28°）段為勻速圓周運(yùn)動(dòng)。（29.17°，113.28°）至（29.27°，113.34°）至（29.36°，113.28°）至（29.49°，113.28°）是2.3節(jié)中提到的需要調(diào)整航向而進(jìn)行勻速圓周運(yùn)動(dòng)勻速直線運(yùn)動(dòng)的情況，以此為依據(jù)生成IMU參數(shù)。

圖3 軌跡發(fā)生器生成軌跡Fig.3 Track generator generating track

在自主工作模式下，某型平臺(tái)式慣導(dǎo)的陀螺漂移為0.003°/h，加速度計(jì)零偏為0.005 g，輸出頻率100 Hz。在組合工作模式下，加入GPS系統(tǒng)組成組合導(dǎo)航系統(tǒng)，輸出頻率為10 Hz，水平位置誤差<10m，采用松組合的方式，使用卡爾曼濾波進(jìn)行計(jì)算。噪聲設(shè)置如下：

自主工作模式，組合工作模式與軌跡發(fā)生器的軌跡圖像如圖4所示?？梢钥吹浇M合模式與軌跡發(fā)生器的軌跡基本重合，自主工作模式由于陀螺儀與加速度計(jì)的漂移與零偏使得在全程11 550 s的航行中出現(xiàn)了一定程度的發(fā)散，而組合工作模式通過引入GPS作為外部觀測(cè)量，較好校正了平臺(tái)式慣導(dǎo)的誤差。為了便于更加準(zhǔn)確的觀察、分析與對(duì)比，圖5和圖6分別為自主、組合工作模式下的經(jīng)緯度誤差可以看到自主工作模式下的位置誤差高達(dá)幾百米甚至上千米，而組合導(dǎo)航的位置精度只有幾米，仿真結(jié)果符合實(shí)際，證明了軌跡發(fā)生器設(shè)計(jì)的合理性。

圖4 三種模式生成軌跡示意圖Fig.4 Three modes to generate trace diagram

圖5 自主工作模式下的位置誤差Fig.5 Position error in autonomous mode

圖6 組合工作模式下的位置誤差Fig.6 Position error in integrated navigation mode

4 結(jié) 語

本文對(duì)船用指北平臺(tái)式慣導(dǎo)模擬器中軌跡發(fā)生器進(jìn)行了設(shè)計(jì)?；谂灤叫械膶?shí)際，結(jié)合指北方位平臺(tái)式慣導(dǎo)的特點(diǎn)，為模擬器設(shè)計(jì)一種解析式軌跡發(fā)生器軟件，操作人員只需要設(shè)置起點(diǎn)與終點(diǎn)以及必要的運(yùn)動(dòng)參量，就可以符合要求的軌跡以及IMU參數(shù)，并通過仿真實(shí)驗(yàn)驗(yàn)證了算法設(shè)計(jì)的有效性。