徐 博，趙曉偉，金坤明

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

艦船慣性導(dǎo)航是一種完全自主的導(dǎo)航方式，由于不需要外界的任何信息以及不向外界傳遞電磁波等特點，被廣泛應(yīng)用于多個領(lǐng)域[1]。初始對準(zhǔn)技術(shù)是捷聯(lián)慣導(dǎo)系統(tǒng)關(guān)鍵技術(shù)之一，初始對準(zhǔn)的好壞直接影響到慣導(dǎo)解算的精度[2-3]。為了提高初始對準(zhǔn)的精度，減少初始對準(zhǔn)的時間，近年來，很多學(xué)者圍繞著捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)開展了大量的研究。朱兵等提出了一種利用粒子群算法對羅經(jīng)回路參數(shù)進行尋優(yōu)的方法，該方法相對于傳統(tǒng)的根據(jù)經(jīng)驗以及大量的反復(fù)試驗確定羅經(jīng)回路參數(shù)而言，節(jié)約了大量的時間[4]，謝祖輝等提出了一種基于粒子群優(yōu)化算法的參數(shù)辨識法，該方法解決了基座搖擺運動條件下，用遞推最小二乘參數(shù)辨識法對初始失準(zhǔn)角進行估計時，存在方位失準(zhǔn)角收斂速度慢、估計精度受到北向失準(zhǔn)角估計精度影響等問題[5]。嚴恭敏等也開創(chuàng)性地提出逆向?qū)Ш礁拍?，并結(jié)合羅經(jīng)法，達到了縮短對準(zhǔn)時間的目的[6]。孫立江等針對激光捷聯(lián)慣導(dǎo)晃動基座下受載體運動干擾難以實現(xiàn)粗對準(zhǔn)這一問題，提出了一種慣性系下基于多矢量定姿的粗對準(zhǔn)方法，有效地抑制了干擾運動對對準(zhǔn)精度的影響，建立了更準(zhǔn)確的捷聯(lián)矩陣，同時使對準(zhǔn)結(jié)果具有更好的穩(wěn)定性[7]。

為了對慣性系對準(zhǔn)算法進行深入探索，本文針對使用多矢量定姿的慣性系對準(zhǔn)方法時航向角收斂速度慢的問題，利用粒子群算法對慣性系對準(zhǔn)方法進行優(yōu)化，并進行了轉(zhuǎn)臺試驗驗證，試驗結(jié)果表明所提算法的有效性。文章的結(jié)構(gòu)安排為：第一部分給出坐標(biāo)系的定義并介紹了慣性系多矢量的對準(zhǔn)方法；第二部分首先給出了粒子群算法，然后給出優(yōu)化算法所用到的粒子群適應(yīng)度函數(shù)以及粒子早熟問題的解決方案；第三部分進行轉(zhuǎn)臺搖擺試驗驗證，并對試驗結(jié)果進行分析；第四部分給出結(jié)論。

1 矢量定姿對準(zhǔn)方法

1.1 坐標(biāo)系定義

（1）地球坐標(biāo)系（e系）：坐標(biāo)原點取在地球中心，oxe軸在赤道平面與載體當(dāng)?shù)刈游缑娴慕唤缇€上，oze沿極軸方向，oxe軸、oye軸和oze軸構(gòu)成右手坐標(biāo)系[8]。

（2）選取“東-北-天”坐標(biāo)系為導(dǎo)航坐標(biāo)系（n系）。

（3）定義“右-前-上”坐標(biāo)系為載體坐標(biāo)系（b系）。

（4）地心慣性坐標(biāo)系（i系）：在初始對準(zhǔn)起始時刻，oxi軸在當(dāng)?shù)刈游缑媲移叫杏诔嗟榔矫?，ozi指向地球自轉(zhuǎn)方向，三軸構(gòu)成右手坐標(biāo)系，初始對準(zhǔn)后i系三軸相對慣性空間保持不動。

（5）初始時刻慣性坐標(biāo)系（ib0系）：在起始時刻ib0系重合于b系，初始對準(zhǔn)后ib0系不隨捷聯(lián)慣組轉(zhuǎn)動，即在慣性空間中保持指向不動。

1.2 慣性系多矢量對準(zhǔn)算法

L為載體所在位置的緯度。

ωie為地球自轉(zhuǎn)角速度， Δt=t-t0為距對準(zhǔn)起始時刻的時間間隔。

利用陀螺儀輸出的角速度信息，可由式(4)求得載體坐標(biāo)系相對載體慣性系的旋轉(zhuǎn)矩陣即

捷聯(lián)慣導(dǎo)比力方程如下所示：

對比力方程進行變形得：

為了減低加速度計測量干擾的影響，將式(9)兩邊在 [t0,tk]內(nèi)積分，由于gn= [0 0 -g]T，則重力加速度在地心慣性系下的投影為：

對式(10)進行積分得：

選擇m個時刻的速度vi、vib0作為參考矢量，構(gòu)造目標(biāo)方程得：

整理變形得：

式中，tr(?)表示對矩陣的求跡運算。

其中：

對B進行奇異值分解，得：

式中：U1和U2為正交矩陣；S=diag(s1,s2,s3)，s1≥s2≥s3≥ 0。

使Q取最小值的最優(yōu)解：

此時進行艦船載體坐標(biāo)系對導(dǎo)航坐標(biāo)系姿態(tài)矩陣的更新：

2 優(yōu)化的慣性系對準(zhǔn)算法

2.1 粒子群優(yōu)化算法

粒子群優(yōu)化算法通過事先設(shè)置一群粒子，并讓群體中的每個粒子通過不斷地跟蹤兩個極值和來更新自己的位置和速度，從而降低參數(shù)辨識過程中初始值選取的不恰當(dāng)對收斂速度的影響，最終提高系統(tǒng)初始對準(zhǔn)的速度[10-11]。其中，稱為個體最優(yōu)，是每個粒子自身找到的最優(yōu)解;稱為全局最優(yōu)，是整個種群目前找到的最優(yōu)解。每個粒子的速度和位置迭代計算公式如下:

式中，k= 1,2...G，G表示最大的迭代次數(shù)；i= 1,2...Size，Size表示種群規(guī)模，r1和r2表示0 到1之間的隨機數(shù)；c1表示局部學(xué)習(xí)因子，代表個體學(xué)習(xí)能力，c2表示全局學(xué)習(xí)因子，代表社會學(xué)習(xí)能力，二者的取值范圍通常為[0，2]；ω(t)表示慣性權(quán)重，其值越大越有利于展開全局尋優(yōu)，越小越有利于局部尋優(yōu)，取值范圍通常為[0，1]；Vi表示第i個粒子的速度；Xi表示第i個粒子的位置；pi表示個體最優(yōu)位置；BestSi表示群體最優(yōu)位置[12]。粒子群算法的步驟如下：

（1）設(shè)置粒子群參數(shù)并對其初始化；

（2）計算每個粒子適應(yīng)度函數(shù)值；

（3）更新個體和群體最優(yōu)解；

（4）通過式（19）計算粒子的速度和位置；

（5）重復(fù)步驟（2）-（4）；

（6）達到最大迭代次數(shù)，結(jié)束粒子群算法，輸出待估參數(shù)。

用粒子群算法使適應(yīng)度函數(shù)取得全局最小值從而辨識最優(yōu)的航向角的基本流程如圖1 所示。

圖1 粒子群算法流程圖Fig.1 Flow chart of particle swarm optimization algorithm

2.2 粒子群適應(yīng)度函數(shù)的建立

針對慣性系對準(zhǔn)算法航向收斂速度慢的問題，考慮到粒子群算法估計參數(shù)較多時，對適應(yīng)度函數(shù)要求較高等因素，而且慣性系對準(zhǔn)算法水平姿態(tài)收斂較快，故本文采用慣性系對準(zhǔn)算法對水平姿態(tài)進行估計，將載體的航向作為粒子群算法的待估參數(shù)。由姿態(tài)角計算得到姿態(tài)矩陣由式（20）計算得：

通過vi、vib0和建立誤差方程如下：

選擇速度誤差量的平方和作為粒子群算法適應(yīng)度函數(shù)，適應(yīng)度函數(shù)表示為：

式(23)中，z(1) 、z(2) 、z(3) 為向量z中的元素，由適應(yīng)度函數(shù)可知當(dāng)用粒子群算法估計的航向角越接近真實航向角時，適應(yīng)度函數(shù)值也就越小。所以利用粒子群算法不斷更新載體的航向角，使得適應(yīng)度函數(shù)達到最小值，得到當(dāng)前時刻最優(yōu)估計的航向角。

2.3 粒子早熟收斂問題的解決

對于粒子群中的任意粒子，其最終收斂位置將是整個粒子群找到的全局極值。如果粒子群找到的全局極值只有一個，那么所有粒子都會“聚集”到該位置;如果全局極值不止一個，那么粒子將隨機聚集在這幾個全局極值位置。全局極值是所有粒子在算法運行過程中找到的最佳粒子位置，該位置并不一定就是搜索空間中的全局最優(yōu)點。若該位置為全局最優(yōu)點，則算法達到全局收斂，否則算法陷入早熟收斂。針對艦船不具備較強機動能力的特點，可以通過設(shè)置航向變化角速率閾值來解決粒子早熟問題。對于系泊狀態(tài)下的船舶，由于存在一定規(guī)律的低頻線晃動，所以本文設(shè)置載體最大角速率為0.09 rad/s。

如果當(dāng)前時刻載體的艏搖角速率超過設(shè)定的載體最大角速率，則可以判斷粒子陷入早熟收斂狀態(tài)，此時去除掉當(dāng)前時刻全局最優(yōu)點所求的，利用前一時刻全局最優(yōu)點所求的代替。利用粒子群優(yōu)化的艦船初始對準(zhǔn)流程圖如圖2 所示。

圖2 粒子群優(yōu)化的艦船初始對準(zhǔn)流程圖Fig.2 Particle swarm optimization for initial ship alignment flowchart

3 轉(zhuǎn)臺試驗驗證與分析

3.1 試驗設(shè)備及指標(biāo)

試驗轉(zhuǎn)臺為SGT-8 型三軸慣導(dǎo)測試轉(zhuǎn)臺，附帶一臺轉(zhuǎn)臺控制計算機，轉(zhuǎn)臺具有自動閑置、定位、增量、速率、搖擺等功能。轉(zhuǎn)臺實物圖如圖3 所示，轉(zhuǎn)臺的主要性能指標(biāo)如表1 所示。

圖3 SGT-8 型三軸慣導(dǎo)測試轉(zhuǎn)臺和慣性測量單元Fig.3 SGT-8 triaxial inertial navigation test turntable and inertial measurement unit

表1 SGT-8 型三軸轉(zhuǎn)臺主要性能指標(biāo)Tab.1 Main performance index of SGT-8 triaxial turntable

3.2 試驗結(jié)果與分析

為了驗證粒子群算法優(yōu)化的初始對準(zhǔn)方法的有效性，進行雙軸轉(zhuǎn)臺試驗，即轉(zhuǎn)臺外框固定，內(nèi)、中框做搖擺運動。轉(zhuǎn)臺參數(shù)設(shè)置如下：航向角設(shè)定為225 °；縱搖搖擺幅值設(shè)定為14 °，搖擺周期為5 s；橫搖搖擺幅值設(shè)定為3.7 °，搖擺周期為8 s；運行時間設(shè)定為600 s，當(dāng)?shù)鼐暥葹?5.7755 °，當(dāng)?shù)亟?jīng)度為126.6820 °。所采用的光纖慣性導(dǎo)航系統(tǒng)的器件參數(shù)如下：陀螺漂移為0.01 °/h，加速度計零偏為 1 ×1 0-4g。

粒子群參數(shù)設(shè)置為：種群規(guī)模Size=20，最大迭代次數(shù)G=150，粒子運動的范圍為[0 2π]，粒子運動的速度的范圍為[-0.5 0.5] rad。轉(zhuǎn)臺試驗結(jié)果如圖4～10 所示。

圖4 轉(zhuǎn)臺中框搖擺運動狀態(tài)Fig.4 Swaying motion of the frame in the turntable

圖5 轉(zhuǎn)臺內(nèi)框搖擺運動狀態(tài)Fig.5 The state of motion of the inside frame of a turntable

圖6 陀螺角速度輸出Fig.6 Gyro angular velocity output

圖7 加速度計比力輸出Fig.7 Accelerometer specific force output

圖8 粒子更新速度曲線Fig.8 Particle update speed curve

圖9 粒子更新適應(yīng)度函數(shù)值曲線Fig.9 Particle update fitness function value curve

圖10 對比對準(zhǔn)航向角誤差曲線Fig.10 Contrast the alignment Angle error curve

表2 三種對準(zhǔn)方法用時對比表Tab.2 Time comparison table of three alignment methods

轉(zhuǎn)臺試驗結(jié)果分析如下：

（1）圖4 為轉(zhuǎn)臺中框搖擺運動狀態(tài)圖，從圖可得轉(zhuǎn)臺運動的俯仰角幅值為14 °，運動周期為5 s。圖5為轉(zhuǎn)臺內(nèi)框搖擺運動狀態(tài)圖，從圖可得轉(zhuǎn)臺運動的橫滾角幅值為3.7 °，搖擺周期為8 s。由圖4、圖5 可知轉(zhuǎn)臺的運動狀態(tài)與設(shè)定值保持一致；

（2）圖6 為陀螺儀角速度輸出結(jié)果，結(jié)果顯示x、y、z 三軸陀螺儀角速度輸出幅值分別為0.31 rad/s、0.05 rad/s、0.02 rad/s。圖7 為加速度計比力輸出結(jié)果，結(jié)果顯示x 軸比力輸出為(-0.6～0.6)2m/s ，y 軸比力輸出為(-2.4～2.4) m/s2，z 軸比力輸出為(9.5～9.8) m/s2；

（3）圖8 為粒子更新速度結(jié)果，可以看出粒子群更新速度在粒子運動的速度范圍[-0.5 0.5] rad 內(nèi)不斷減小，直至迭代到第50 代時粒子更新速度趨向于零并保持穩(wěn)定。圖9 為粒子更新適應(yīng)度函數(shù)值的結(jié)果，粒子更新適應(yīng)度函數(shù)值最初為20 m/s，更新到第5 代時適應(yīng)度函數(shù)值減小至1.8 m/s，適應(yīng)度函數(shù)值下降速度非?？臁膱D8、圖9 中可以看出通過粒子群算法只需要迭代50 次就可以找到該時刻的全局最優(yōu)解，粒子群算法能夠快速地找到目標(biāo)函數(shù)值；

（4）為了更加有效地驗證粒子群算法的優(yōu)越性，引入了svd-v 和svd-g 兩種對準(zhǔn)方法進行比對，其中svd-v 是以vi、vib0作為參考矢量的基于多矢量定姿的慣性系對準(zhǔn)方法，svd-g 是以gi、gib0作為參考矢量的基于多矢量定姿的慣性系對準(zhǔn)方法。圖10 為三種初始對準(zhǔn)航向角誤差曲線對比圖，表2 根據(jù)圖10 的誤差曲線列寫了三種初始對準(zhǔn)方法在不同對準(zhǔn)航向角誤差條件下的所用時間?？梢钥闯觯趕vd-v 的初始對準(zhǔn)方法優(yōu)于基于svd-g 的初始對準(zhǔn)方法，當(dāng)使初始對準(zhǔn)航向誤差角＜0.5 °時，svd-v 算法相比svd-g 算法對準(zhǔn)時間縮短了44%，當(dāng)使初始對準(zhǔn)航向誤差角＜0.1 °時，svd-v算法相比svd-g 算法對準(zhǔn)時間縮短了21%。基于粒子群優(yōu)化的慣性系對準(zhǔn)算法明顯優(yōu)越于與其對比的兩種對準(zhǔn)方法，當(dāng)使初始對準(zhǔn)航向誤差角＜0.1 °時，基于粒子群優(yōu)化的慣性系對準(zhǔn)算法相比基于svd-g算法對準(zhǔn)時間縮短了56%。

綜上，三種方法雖然穩(wěn)態(tài)精度相當(dāng)，航向角誤差均小于0.1 °，達到了0.01 °/h 的光纖陀螺精度對應(yīng)的穩(wěn)態(tài)對準(zhǔn)精度。但是基于慣性系的粒子群算法優(yōu)化的初始對準(zhǔn)方法收斂速度明顯優(yōu)于多矢量定姿的慣性系對準(zhǔn)的兩種方法，對于縮短艦船的初始對準(zhǔn)時間具有重要意義。

4 結(jié) 論

艦船初始對準(zhǔn)的結(jié)果對于艦船捷聯(lián)慣導(dǎo)系統(tǒng)后續(xù)的導(dǎo)航定位至關(guān)重要，為了快速、準(zhǔn)確地得到艦船初始對準(zhǔn)結(jié)果，本文所提方法首先通過基于多矢量定姿法對水平姿態(tài)進行對準(zhǔn)，然后利用基于慣性系的粒子群優(yōu)化的初始對準(zhǔn)算法對航向角進行對準(zhǔn)，試驗結(jié)果表明：粒子群優(yōu)化算法能夠快速準(zhǔn)確地得到待估航向角，有效縮短初始對準(zhǔn)時間，達到光纖陀螺對應(yīng)的穩(wěn)態(tài)對準(zhǔn)精度指標(biāo)。