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

周 玲，程向紅

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

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

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

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

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

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

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

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

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

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

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

則粒子在t+1時刻的位置通過下式更新獲得：

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

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

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

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

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

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

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

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

④ 對每個粒子，將它的適應(yīng)值和全局粒子所經(jīng)歷過的最優(yōu)適應(yīng)值作比較，如果較小，則將其對應(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)初始位置誤差較大時，東向位置誤差為800 m，北向位置誤差為10 m，如圖3所示，其他條件的設(shè)置同圖2。仿真結(jié)果顯示：CPSO時長8.69 s，航跡從A到B；ICCP運(yùn)行時長28.62 s，航跡從C到D。與圖2相比較，ICCP算法受初始位置誤差的影響較大，定位速度和精度均變差，而CPSO基本不受初始位置誤差的影響，定位效率和效果均明顯優(yōu)于ICCP算法。

在地形平坦區(qū)航行時，如圖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平臺下進(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，加速度計零偏為0.1 mg，初始航向誤差為0.3°。此外，通過設(shè)置初始位置誤差以仿真AUV在進(jìn)入地形匹配區(qū)域時已經(jīng)累積的位置誤差。

仿真實(shí)驗(yàn)中，測量兩種不同地形的水深序列。CPSO參數(shù)設(shè)置為：待匹配航跡上各水深值的粒子個數(shù)為50，學(xué)習(xí)因子c1=2.1，c2=2.0，慣性權(quán)重ω在0.9～0.4范圍線性遞減，最大迭代次數(shù)設(shè)為20；水深測量間隔為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)初始位置誤差較小時，如圖2所示。仿真的真實(shí)航跡為起始水平位置(555 m, 20 m)，北偏東40°直航，東向位置誤差為100 m，北向位置誤差為10 m。地形熵為0.54，為地形起伏較明顯區(qū)。仿真結(jié)果顯示ICCP運(yùn)行時長7.7 s ，CPSO時長8.9 s。航，地形熵為0.77。慣性導(dǎo)航系統(tǒng)初始東向位置誤差500 m，北向位置誤差10 m。仿真結(jié)果顯示：CPSO時長13.98 s，航跡從A到B；ICCP運(yùn)行時長18.8 s，航跡從C到D。與地形變化明顯區(qū)的圖2和圖3比較，兩種方法在地形平坦區(qū)的定位精度均下降，需要更長時間和更多水深測量進(jìn)行定位，但兩種方法相比較，ICCP更易陷入局部最優(yōu)。

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

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

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

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

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

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

4 結(jié) 論

基于CPSO的海底地形輔助慣性導(dǎo)航定位方法，通過在水深測量值的等值域內(nèi)進(jìn)行粒子群初始化，極大的提高了CPSO的搜索效率，改善了粒子“早熟”問題。結(jié)合地形特征和慣導(dǎo)短時間導(dǎo)航精度高的特點(diǎn)，輔助慣導(dǎo)系統(tǒng)準(zhǔn)確定位。通過某海圖內(nèi)海底地形匹配仿真實(shí)驗(yàn)表明，在AUV經(jīng)過長時間航行累積了較大位置誤差后，進(jìn)入海圖覆蓋區(qū)域，CPSO地形匹配結(jié)果明顯優(yōu)于ICCP算法，具有效率高、魯棒性強(qiáng)的特點(diǎn)，地形匹配的結(jié)果可作為AUV長航時后慣性導(dǎo)航系統(tǒng)的“位置初始對準(zhǔn)”。此外，再結(jié)合非線性濾波算法可實(shí)現(xiàn)地形輔助慣性導(dǎo)航的實(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í)時ICCP算法重力匹配仿真[J]. 中國慣性技術(shù)學(xué)報， 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]. 中國慣性技術(shù)學(xué)報， 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é)報: 自然科學(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] 孫楓， 王文晶， 高偉， 等. 用于無源重力導(dǎo)航的等值線匹配算法[J]. 儀器儀表學(xué)報， 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]. 中國慣性技術(shù)學(xué)報， 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

國家自然科學(xué)基金資助項目（61374215）

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

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