基于約束粒子群優(yōu)化的海底地形輔助慣性導(dǎo)航定位方法

周 玲，程向紅

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 運(yùn)城學(xué)院 物理與電子工程系，運(yùn)城 044000）

基于約束粒子群優(yōu)化的海底地形輔助慣性導(dǎo)航定位方法

周 玲1,2，程向紅1

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 運(yùn)城學(xué)院 物理與電子工程系，運(yùn)城 044000）

針對(duì)水下自主航行器長(zhǎng)時(shí)間航行后慣性導(dǎo)航系統(tǒng)位置誤差積累的問(wèn)題，提出一種關(guān)于約束粒子群優(yōu)化的海底地形輔助慣性導(dǎo)航定位方法。通過(guò)在等值域內(nèi)進(jìn)行粒子群初始化，改善粒子“早熟”問(wèn)題。采用慣性導(dǎo)航和水深測(cè)量序列計(jì)算相鄰測(cè)量點(diǎn)水平距離、航跡水平轉(zhuǎn)向角和水深變化量三個(gè)匹配參數(shù)，同時(shí)利用待匹配航跡上的粒子群計(jì)算以上三個(gè)參數(shù)，將航跡水平轉(zhuǎn)向角和水深變化量轉(zhuǎn)換為各自等效的水平相對(duì)距離，最后采用對(duì)應(yīng)于相同匹配參數(shù)的等效水平相對(duì)距離的平均絕對(duì)差之和構(gòu)建適應(yīng)度函數(shù)，采用約束粒子群優(yōu)化進(jìn)行地形匹配，輔助慣導(dǎo)系統(tǒng)準(zhǔn)確定位。通過(guò)某海圖內(nèi)海底地形匹配進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明當(dāng)航行器初始位置誤差較大時(shí)：在地形變化明顯區(qū)，ICCP的定位精度為200 m，約束粒子群優(yōu)化的定位精度提高到20 m以?xún)?nèi)；在地形變化平坦區(qū)，ICCP無(wú)法有效定位的情況下，約束粒子群優(yōu)化的定位精度約為250 m。

地形匹配；約束粒子群優(yōu)化；地形輔助導(dǎo)航；水下地形導(dǎo)航；水下自主航行器

長(zhǎng)航時(shí)、高精度、自主性、隱蔽性等性能是水下自主航行器(AUV)對(duì)導(dǎo)航系統(tǒng)提出的基本要求。近年來(lái)，隨著海洋測(cè)繪技術(shù)的發(fā)展，海底地形輔助慣性導(dǎo)航逐漸成為研究熱點(diǎn)[1]。海洋水深的分布情況反映了海底地形的變化程度，水深與水平地理位置存在著對(duì)應(yīng)關(guān)系，因此通過(guò)AUV裝備的深度計(jì)和測(cè)深儀測(cè)量水深值，與預(yù)先存儲(chǔ)的海圖水深數(shù)據(jù)庫(kù)進(jìn)行匹配，可以實(shí)現(xiàn)地形輔助慣性導(dǎo)航系統(tǒng)的定位。

地形匹配定位算法可以分為批處理和濾波處理兩大類(lèi)[2]。批處理方法將一系列水深測(cè)量數(shù)據(jù)累積起來(lái)處理，根據(jù)水深測(cè)量序列和待匹配航跡上的海圖水深序列的相關(guān)度，確定匹配航跡。最早的批處理算法有地形輪廓匹配算法(TERCOM)[3]，其主要用于巡航導(dǎo)彈和飛機(jī)。由于水下環(huán)境和AUV的特殊性，在批處理算法中，基于等值線(xiàn)圖形匹配(ICCP)算法得到廣泛研究。但I(xiàn)CCP適于初始位置誤差較小的情況，當(dāng)初始位置誤差較大時(shí)，存在計(jì)算量大，易陷入局部最優(yōu)的缺點(diǎn)。用于海洋地球物理匹配導(dǎo)航的ICCP改進(jìn)算法中，采用滑動(dòng)窗口技術(shù)[4]減少最近點(diǎn)計(jì)算量，采用混沌優(yōu)化算法[5]、三角形約束模型[6]、BP神經(jīng)網(wǎng)絡(luò)[7]、粒子群優(yōu)化[8,9]、基于誤差平方和價(jià)值函數(shù)最優(yōu)的等值線(xiàn)匹配算法[10]等改善算法局部收斂性和實(shí)時(shí)性。當(dāng)初始位置誤差較大時(shí)，采用TERCOM粗匹配輔助ICCP精匹配的方式[11]。雖然上述方法從多個(gè)方面改進(jìn)了ICCP算法，并取得一定效果，但是在克服ICCP易陷入局部最優(yōu)的缺陷方面仍然有限。

粒子群優(yōu)化(PSO)是智能群優(yōu)化方法的典型代表，本文在以上算法研究的基礎(chǔ)上，充分利用粒子群優(yōu)化快速的搜索特性，結(jié)合地形特征和慣導(dǎo)短時(shí)間導(dǎo)航精度高的特點(diǎn)，解決水下地形輔助慣性導(dǎo)航系統(tǒng)快速、準(zhǔn)確定位問(wèn)題。

1 約束粒子群優(yōu)化

實(shí)際應(yīng)用中，PSO存在粒子“早熟”的問(wèn)題，即搜索過(guò)程中有陷入局部最小值的可能。針對(duì)這一問(wèn)題，采用一種基于約束粒子群優(yōu)化(CPSO)的搜索方法。粒子群的位置參數(shù)通過(guò)等值域進(jìn)行約束，即要求粒子群的水平位置(x,y)滿(mǎn)足以下約束條件：

式中：(xc, yc)為雙線(xiàn)性插值法獲取的水深數(shù)據(jù)等值線(xiàn)上插值點(diǎn)的水平位置；ζ是服從均值為零的高斯白噪聲，其方差根據(jù)初始粒子群位于水深等值域內(nèi)的原則確定。

假設(shè)一個(gè)由N個(gè)粒子組成的群體在D維的搜索空間以一定的速度運(yùn)動(dòng)，粒子i在t時(shí)刻的狀態(tài)屬性設(shè)置如下：

其中，1≤d≤D，1≤i≤N。

則粒子在t+1時(shí)刻的位置通過(guò)下式更新獲得：

式中：慣性權(quán)重ω=ωstart-(ωstart-ωend)ttmax，tmax為最終時(shí)刻（或稱(chēng)為最大迭代次數(shù)），ωstart、ωend分別為初始慣性權(quán)重和終止慣性權(quán)重；r1、r2為均值分布在(0,1)區(qū)間的隨機(jī)數(shù)；c1、c2稱(chēng)為學(xué)習(xí)因子，通常取c1=c2=2。

地形匹配是一個(gè)數(shù)據(jù)相關(guān)問(wèn)題，可用相關(guān)性準(zhǔn)則來(lái)定義適應(yīng)度函數(shù)。算法中選用平均絕對(duì)差(MAD)準(zhǔn)則，即定義

采用SINS和水深測(cè)量序列S計(jì)算相鄰測(cè)量點(diǎn)的航跡水平轉(zhuǎn)向角和水深變化量?jī)蓚€(gè)匹配參數(shù)，同時(shí)利用待匹配航跡T上水深等值域內(nèi)粒子群計(jì)算這兩個(gè)參數(shù)，并將匹配參數(shù)轉(zhuǎn)換為各自等效的水平相對(duì)距離。然后，類(lèi)似于公式(4)，得到航跡水平轉(zhuǎn)向角和水深變化量對(duì)應(yīng)的水平相對(duì)距離的MAD，分別記為E2和E3。最后，將Ei作為CPSO算法的適應(yīng)度函數(shù)，當(dāng)適應(yīng)值最小時(shí)對(duì)應(yīng)搜索的最優(yōu)解。

2 基于CPSO的海底地形匹配方法設(shè)計(jì)

基于CPSO的地形匹配方法流程圖如圖1所示。具體的地形匹配方法的設(shè)計(jì)如下：

① 在水深測(cè)量點(diǎn)的等值域約束下初始化一群二維粒子，分別對(duì)應(yīng)初始水平位置；

② 計(jì)算待匹配航跡上的粒子群與水深測(cè)量序列之間的適應(yīng)度函數(shù)值；

③ 對(duì)每個(gè)粒子，將它的適應(yīng)值和它經(jīng)歷過(guò)的個(gè)體最優(yōu)適應(yīng)值作比較，如果較小，則將其對(duì)應(yīng)的位置作為當(dāng)前個(gè)體最優(yōu)位置；

④ 對(duì)每個(gè)粒子，將它的適應(yīng)值和全局粒子所經(jīng)歷過(guò)的最優(yōu)適應(yīng)值作比較，如果較小，則將其對(duì)應(yīng)的位置作為當(dāng)前全局最優(yōu)位置；

⑤ 根據(jù)式(2)和(3)改變粒子的速度和位置；

⑥ 設(shè)置最大迭代次數(shù)，判斷是否達(dá)到預(yù)設(shè)的最大迭代次數(shù)，如未達(dá)到，回到步驟②，否則，輸出全局最優(yōu)值。航跡均從A到B，兩種方法定位結(jié)果基本相同，都較好地實(shí)現(xiàn)了地形匹配。

圖1 基于CPSO的地形匹配方法流程圖Fig.1 Flowchart of terrain matching method based on CPSO

當(dāng)慣性導(dǎo)航系統(tǒng)初始位置誤差較大時(shí)，東向位置誤差為800 m，北向位置誤差為10 m，如圖3所示，其他條件的設(shè)置同圖2。仿真結(jié)果顯示：CPSO時(shí)長(zhǎng)8.69 s，航跡從A到B；ICCP運(yùn)行時(shí)長(zhǎng)28.62 s，航跡從C到D。與圖2相比較，ICCP算法受初始位置誤差的影響較大，定位速度和精度均變差，而CPSO基本不受初始位置誤差的影響，定位效率和效果均明顯優(yōu)于ICCP算法。

在地形平坦區(qū)航行時(shí)，如圖4所示。仿真的真實(shí)航跡為起始水平位置 (2220 m, 110 m)，北偏西60°直

圖2 在初始位置誤差較小情況下地形變化明顯區(qū)CPSO/ICCP地形匹配結(jié)果Fig.2 CPSO/ICCP simulation results under the condition of less initial position errors in a rough terrain

3 仿真及分析

仿真實(shí)驗(yàn)在MATLAB7.0平臺(tái)下進(jìn)行，環(huán)境Inter(R) Core(TM) （3.3 GHz CPU, RAM 2 GB）。仿真產(chǎn)生海底地形，東向位置范圍為0～3000 m，北向位置范圍為0～1120 m，海圖分辨率約為5 m×5 m，水深量程范圍為-200～0 m。AUV以4 kn勻速航行，速度誤差0.1 m/s，SINS陀螺隨機(jī)常值為0.05 (°)/h，加速度計(jì)零偏為0.1 mg，初始航向誤差為0.3°。此外，通過(guò)設(shè)置初始位置誤差以仿真AUV在進(jìn)入地形匹配區(qū)域時(shí)已經(jīng)累積的位置誤差。

仿真實(shí)驗(yàn)中，測(cè)量?jī)煞N不同地形的水深序列。CPSO參數(shù)設(shè)置為：待匹配航跡上各水深值的粒子個(gè)數(shù)為50，學(xué)習(xí)因子c1=2.1，c2=2.0，慣性權(quán)重ω在0.9～0.4范圍線(xiàn)性遞減，最大迭代次數(shù)設(shè)為20；水深測(cè)量間隔為2 min。

圖3 在初始位置誤差較大情況下地形變化明顯區(qū)CPSO/ICCP地形匹配結(jié)果Fig.3 CPSO/ICCP simulation results under the condition of larger initial position errors in a rough terrain

當(dāng)慣性導(dǎo)航系統(tǒng)初始位置誤差較小時(shí)，如圖2所示。仿真的真實(shí)航跡為起始水平位置(555 m, 20 m)，北偏東40°直航，東向位置誤差為100 m，北向位置誤差為10 m。地形熵為0.54，為地形起伏較明顯區(qū)。仿真結(jié)果顯示ICCP運(yùn)行時(shí)長(zhǎng)7.7 s ，CPSO時(shí)長(zhǎng)8.9 s。航，地形熵為0.77。慣性導(dǎo)航系統(tǒng)初始東向位置誤差500 m，北向位置誤差10 m。仿真結(jié)果顯示：CPSO時(shí)長(zhǎng)13.98 s，航跡從A到B；ICCP運(yùn)行時(shí)長(zhǎng)18.8 s，航跡從C到D。與地形變化明顯區(qū)的圖2和圖3比較，兩種方法在地形平坦區(qū)的定位精度均下降，需要更長(zhǎng)時(shí)間和更多水深測(cè)量進(jìn)行定位，但兩種方法相比較，ICCP更易陷入局部最優(yōu)。

圖4 地形變化平坦區(qū)CPSO/ICCP地形匹配結(jié)果Fig.4 CPSO/ICCP simulation results in flat area

當(dāng)AUV在水下作拐彎運(yùn)動(dòng)，如圖5所示。起始水平位置為（555 m, 110 m），初始航向北偏東60°，地形熵為0.68。慣性導(dǎo)航系統(tǒng)初始東向位置誤差800 m，北向位置誤差100 m。仿真結(jié)果顯示：CPSO時(shí)長(zhǎng)10.48 s，航跡從A到B；ICCP運(yùn)行時(shí)長(zhǎng)16.68 s，航跡從C到D。兩種方法比較，ICCP受初始位置誤差的影響較大，且易陷入局部最優(yōu)；CPSO適用范圍更廣，且定位精度明顯優(yōu)于ICCP。

圖5 拐彎運(yùn)動(dòng)下CPSO/ICCP地形匹配結(jié)果Fig.5 CPSO/ICCP simulation results in turning movement

基于CPSO中粒子群的隨機(jī)分布，對(duì)兩種不同地形特征下的兩條水深序列進(jìn)行50次仿真實(shí)驗(yàn)，分析待匹配航跡上各水深測(cè)量點(diǎn)位置的平均圓概率誤差（CEP 50%），如表1所示。表1中，地形特征明顯時(shí)，定位時(shí)間短，定位精度高；地形平坦時(shí)，定位時(shí)間長(zhǎng)，定位精度低，這與仿真圖中的結(jié)果相一致。

從以上仿真情況可以看出：當(dāng)?shù)匦巫兓黠@時(shí)，兩種方法所需的水深測(cè)量次數(shù)較少，匹配精度較高；當(dāng)?shù)匦巫兓教箷r(shí)，待匹配的水深測(cè)量次數(shù)較多，匹配精度較低，匹配時(shí)間較長(zhǎng)。與CPSO相比，ICCP算法更容易陷入局部最優(yōu)。為提高匹配效率和優(yōu)化匹配效果，可根據(jù)地形特征，如地形熵和地形方差，從測(cè)量數(shù)據(jù)中選取待匹配的水深值進(jìn)行匹配。最終，將地形匹配獲取的當(dāng)前時(shí)刻的位置信息對(duì)慣導(dǎo)系統(tǒng)進(jìn)行位置重置以實(shí)現(xiàn)SINS位置誤差的修正。

表1 兩種地形下各水深測(cè)量值地形匹配的CEP 50%值比較Tab.1 CEP 50% comparison of each sample point for terrain matching on two kinds of terrains

4 結(jié) 論

基于CPSO的海底地形輔助慣性導(dǎo)航定位方法，通過(guò)在水深測(cè)量值的等值域內(nèi)進(jìn)行粒子群初始化，極大的提高了CPSO的搜索效率，改善了粒子“早熟”問(wèn)題。結(jié)合地形特征和慣導(dǎo)短時(shí)間導(dǎo)航精度高的特點(diǎn)，輔助慣導(dǎo)系統(tǒng)準(zhǔn)確定位。通過(guò)某海圖內(nèi)海底地形匹配仿真實(shí)驗(yàn)表明，在A(yíng)UV經(jīng)過(guò)長(zhǎng)時(shí)間航行累積了較大位置誤差后，進(jìn)入海圖覆蓋區(qū)域，CPSO地形匹配結(jié)果明顯優(yōu)于ICCP算法，具有效率高、魯棒性強(qiáng)的特點(diǎn)，地形匹配的結(jié)果可作為AUV長(zhǎng)航時(shí)后慣性導(dǎo)航系統(tǒng)的“位置初始對(duì)準(zhǔn)”。此外，再結(jié)合非線(xiàn)性濾波算法可實(shí)現(xiàn)地形輔助慣性導(dǎo)航的實(shí)時(shí)定位。

[1] Hagen O K， ?nonsen K B. Using terrain navigation to improve marine vessel navigation systems[J]. Marine Technology Society Journal， 2014， 48(2): 45-58.

[2] Donovan G T. Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter[J]. IEEE Journal of Oceanic Engineering， 2012， 37(3): 431-445.

[3] Golden J. Terrain contour matching (TERCOM): A cruise missile guidance aid[J]. Image Processing for Missile Guidance， 1980， 238: 10-18.

[4] 童余德， 邊少鋒， 蔣東方， 等. 實(shí)時(shí)ICCP算法重力匹配仿真[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2011， 19(3): 340-343. Tong Yu-de， Bian Shao-feng， Jiang Dong-fang， et al. Gravity matching simulation of real-time ICCP algorithm[J]. Journal of Chinese Inertial Technology， 2011， 19(3): 340-343.

[5] 張濤， 徐曉蘇， 李佩娟. 混沌優(yōu)化水下地形匹配算法研究[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2009， 17(2): 156-158.

Zhang Tao， Xu Xiao-su， LI Pei-juan. Underwater terrain matching algorithm based on chaotic optimization[J]. Journal of Chinese Inertial Technology， 2009， 17(2): 156-158.

[6] Yang Z L， Zhu Z S， Zhao W G， et al. A triangle matching algorithm for gravity-aided navigation for underwater vehicles[J]. Journal of Navigation， 2014， 67(2): 227-247.

[7] Zhang Tao， Xu Xiao-su. The application of chaotic BP neural network in underwater terrain matching navigation [C]//21st Chinese Control and Decision Conference. 2009: 695-698.

[8] 諶劍， 張靜遠(yuǎn)， 查峰， 等. 一種改進(jìn)ICCP水下地形匹配算法[J]. 華中科技大學(xué)學(xué)報(bào): 自然科學(xué)版， 2012，40(10): 63-67.

Shen Jian， Zhang Jing-yuan， Zha Feng. Improved ICCP al gorithm for underwater terrain matching method[J]. Journal of Huazhong University of Science and Technology (Nature Science Edition)， 2012， 40(10): 63-67.

[9] Yuan Gan-nan， Tan Jia-lin， Liu Li-qiang， et al. Seabed terrain match algorithm based on hausdorff distance and particle swarm optimization[C]//Proceeding of the 2009 International Joint Conference on Computational Science and Optimization. 2009: 236-239.

[10] 孫楓， 王文晶， 高偉， 等. 用于無(wú)源重力導(dǎo)航的等值線(xiàn)匹配算法[J]. 儀器儀表學(xué)報(bào)， 2009， 30(4): 817-822.

Sun Feng， Wang Wen-jing， Gao Wei， et al. Contour matching algorithm for passive gravity navigation[J]. Chinese Journal of Scientific Instrument， 2009， 30(4): 817-822.

[11] 徐曉蘇， 吳劍飛， 徐勝保， 等. 基于仿射修正技術(shù)的水下地形ICCP匹配算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2014，22(3): 362-367.

Xu Xiao-su， Wu Jian-fei， Xu Sheng-bao， et al. ICCP algorithm for underwater terrain matching navigation based on affine correction[J]. Journal of Chinese Inertial Technology， 2014， 22(3): 362-367.

Seabed terrain-aided SINS location based on constrained particle swarm optimization method

ZHOU Ling1,2, CHENG Xiang-hong1
(1. School of Instrument Science & Engineering, Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. Department of Physics and Electronic Engineering, Yuncheng University, Yuncheng 044000, China)

In view of the problem of position error accumulation when an autonomous underwater vehicle (AUV) has sailed for a long time, a constrained particle swarm optimization (CPSO) method is presented for seabed terrain-aided SINS location. By improving the premature convergence of particle swarm optimization, the initialization of particle swarm is constrained in the depth contour neighborhood of each measurement. Three matching parameters, i.e. horizontal distance, horizontal turn angle and depth difference between each measurement, are calculated by the SINS and the depth measurement sequence. Meanwhile, the three parameters are calculated by the corresponding particle swarms. Thus the horizontal turn angle and the depth difference are converted into the equivalent relative horizontal distances, respectively. In the end, the sum of mean absolute differences of the relative horizontal distances corresponding to the same parameters is taken as the fitness function. The CPSO is applied to terrain matching for improving the AUV positioning accuracy. The simulation experiments of terrain-aided SINS in a certain sea chart show that the CPSO has advanced positioning accuracy of 20 m in comparison with the ICCP’s positioning accuracy of 200 m in a rough terrain, and when the positioning accuracy of the CPSO reaches 250 m, the ICCP fails to locate effectively in flat areas.

terrain matching; constrained particle swarm optimization; terrain-aided navigation; underwater terrain navigation; autonomous underwater vehicle

U666.1

A

1005-6734(2015)03-0369-04

10.13695/j.cnki.12-1222/o3.2015.03.016

2015-01-17；

2015-05-05

國(guó)家自然科學(xué)基金資助項(xiàng)目（61374215）

周玲（1981—），女，博士研究生，專(zhuān)業(yè)為儀器科學(xué)與技術(shù)。E-mail：s03031212@126.com

聯(lián) 系 人：程向紅（1963—），女，教授，博士生導(dǎo)師。E-mail：xhcheng@seu.edu.cn