張 濤,張 馳,張佳宇
(1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,南京 210096;2.東南大學(xué) 儀器科學(xué)與工程學(xué)院,南京 210096)
慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)是目前自主水下航行器(Autonomous Underwater Vehicle,AUV)最常用的導(dǎo)航方式,但是由于系統(tǒng)本身特性的原因,導(dǎo)航誤差會(huì)隨時(shí)間逐漸累積[1],無法滿足AUV長航時(shí)的作業(yè)需求。為解決這一問題,可以利用地形匹配的結(jié)果對(duì)INS進(jìn)行校正。地形匹配是一種利用地球物理信息進(jìn)行位置定位的方法,將實(shí)時(shí)測(cè)量得到的水深數(shù)據(jù)與預(yù)先存儲(chǔ)的水下地形圖進(jìn)行匹配,進(jìn)而確定AUV在地形圖上的位置[2,3]。由于短時(shí)間內(nèi)水下地形不會(huì)發(fā)生變化,且無需接受其他外部傳感器信號(hào),地形匹配具有很高的可靠性和隱蔽性[4]。
目前,地形輪廓匹配算法(Terrain Contour Matching,TERCOM)和最近等值線迭代算法(Iterative closest contour point,ICCP)是最常用的兩種水下地形匹配方法[5]。TERCOM通過計(jì)算測(cè)量地形序列和背景地形的相關(guān)性對(duì)航跡進(jìn)行修正,實(shí)現(xiàn)簡單,但是該算法精度較低,對(duì)航向誤差要求較高,一般要小于2 °,否則會(huì)匹配失敗。ICCP在測(cè)深序列等值線附近對(duì)航跡進(jìn)行多次迭代變換,逐步靠近真實(shí)位置,在初始定位誤差較大時(shí),匹配耗時(shí)長,定位精度低甚至失敗[6]。文獻(xiàn)[7]在TERCOM算法的基礎(chǔ)上,引入不規(guī)則三角網(wǎng)模型替代傳統(tǒng)網(wǎng)格化水下地形圖,能夠有效降低誤匹配的概率,同時(shí)定位精度高于傳統(tǒng)的TERCOM算法。文獻(xiàn)[8]利用最小二乘對(duì)ICCP匹配結(jié)果進(jìn)行仿射修正,改善ICCP僅能做剛性變換的缺陷,提高匹配定位效果。文獻(xiàn)[9]提出一種聯(lián)合TERCOM和ICCP的匹配算法,利用TERCOM進(jìn)行粗匹配,ICCP進(jìn)行精匹配,克服ICCP在初始定位差較大時(shí)容易匹配失敗的問題。文獻(xiàn)[10]對(duì)ICCP的采樣方式進(jìn)行改進(jìn),結(jié)合滑動(dòng)窗搜索的方式縮小搜索范圍,提高了ICCP算法的實(shí)時(shí)性。以上方法都是基于單點(diǎn)測(cè)深系統(tǒng)進(jìn)行匹配,獲取的地形信息有限,在地形相似區(qū)域匹配精度較低。隨著海洋探測(cè)技術(shù)的發(fā)展,多波束測(cè)深系統(tǒng)在在水下地形探測(cè)方面發(fā)揮著重要作用,不同于傳統(tǒng)單波束測(cè)深方式,多波束測(cè)深系統(tǒng)一次測(cè)量能夠獲得多點(diǎn)地形數(shù)據(jù),探測(cè)范圍更廣,精度更高。
本文提出基于改進(jìn)遺傳算法的多波束水下地形匹配方法,在多波束測(cè)深系統(tǒng)采集的水深數(shù)據(jù)中選擇多條水深數(shù)據(jù),利用遺傳算法進(jìn)行地形匹配,引入正余弦算法對(duì)遺傳算法中的精英個(gè)體進(jìn)行優(yōu)化,加快遺傳算法的收斂速度和改善局部收斂問題,可以有效改善地形匹配算法在初始位置誤差較大時(shí),匹配定位誤差大、成功率低的問題。
水下地形匹配的基本思想是根據(jù)AUV實(shí)時(shí)測(cè)量的地形信息與提前存儲(chǔ)的水下地形圖進(jìn)行比對(duì),對(duì)INS指示的航跡進(jìn)行變換,從而確定AUV當(dāng)前位置。在進(jìn)入地形匹配適配區(qū)之前,由于慣性元件累積誤差的影響,INS指示的航跡與真實(shí)航跡之間存在初始平移位置誤差,航向誤差和速度誤差。實(shí)際中完成一次地形匹配所需時(shí)間較短,可認(rèn)為在匹配期間航向誤差和速度誤差為定值,因此慣導(dǎo)解算的速度v和航向h可以表示為:
遺傳算法(Ggenetic Algorithm,GA)是一種借鑒自然界生物進(jìn)化過程而構(gòu)造出的優(yōu)化搜索算法,根據(jù)“優(yōu)勝劣汰”原則,迭代更新種群,使得種群向著最優(yōu)的方向進(jìn)化[11]。遺傳算法從初始種群出發(fā),根據(jù)建立的適應(yīng)度函數(shù)對(duì)種群中個(gè)體的優(yōu)劣進(jìn)行評(píng)價(jià),通過選擇、交叉、變異等遺傳操作對(duì)種群中個(gè)體進(jìn)行變換以產(chǎn)生整體性能更優(yōu)的新種群,多次迭代后找到全局最優(yōu)解。為保證算法的收斂性,一般在種群更新過程中加入精英策略,對(duì)于每代個(gè)體中的最優(yōu)個(gè)體,不進(jìn)行更改,直接保留到下一代種群中,讓每代種群的最優(yōu)個(gè)體能夠順利進(jìn)入后續(xù)的迭代計(jì)算中。
以INS指示位置為搜索中心,在搜索空間內(nèi)生成多組仿射變換參數(shù)(tx,ty,α,θ),對(duì)INS指示航跡進(jìn)行變換,利用遺傳算法對(duì)變換參數(shù)進(jìn)行迭代計(jì)算,最后得到最優(yōu)變換參數(shù),對(duì)應(yīng)航跡即為最接近真實(shí)位置的匹配航跡。
正余弦算法(Sine Cosine Algorithm,SCA)是一種基于正弦和余弦數(shù)學(xué)模型的優(yōu)化算法,利用正弦余弦函數(shù)震蕩性特點(diǎn)來趨近最優(yōu)解[12]。其算法過程與傳統(tǒng)的基于種群的優(yōu)化算法類似,根據(jù)建立的適應(yīng)度函數(shù),對(duì)初始生成的候選解進(jìn)行多次循環(huán)迭代,最終得到最優(yōu)個(gè)體。正余弦優(yōu)化算法計(jì)算簡單,方便實(shí)現(xiàn),調(diào)節(jié)參數(shù)少,收斂速度快,因此非常適用于參數(shù)的快速尋優(yōu)。
SCA在待搜索區(qū)間內(nèi)隨機(jī)初始化生成候選解種群,設(shè)種群規(guī)模為N,包括N個(gè)個(gè)體(X1,X2...XN),根據(jù)適應(yīng)度函數(shù)計(jì)算適應(yīng)度值fSCA(Xi),記錄最優(yōu)個(gè)體位置,通過正余弦函數(shù)與隨機(jī)因子對(duì)候選解更新,生成下代種群,具體更新方程為:
式中,k為待優(yōu)化參數(shù)的第k維,t為當(dāng)前迭代次數(shù),Xt+1為下一代個(gè)體,Xt當(dāng)代個(gè)體,Pt為當(dāng)代最優(yōu)個(gè)體:三個(gè)參數(shù)是隨機(jī)因子,均服從均勻分布,r1為轉(zhuǎn)換參數(shù),控制SCA從全局開發(fā)轉(zhuǎn)向局部開發(fā),取值為:
其中,a為常數(shù),T為設(shè)定的最大迭代次數(shù)。
GA為保證算法的收斂性,通常加入精英策略,但是在精英個(gè)體是局部最優(yōu)的情況下,精英策略則加劇局部最優(yōu)個(gè)體對(duì)整體種群的影響,種群中其他個(gè)體向局部最優(yōu)靠攏,導(dǎo)致算法快速收斂于局部最優(yōu)。針對(duì)這個(gè)問題,引入SCA對(duì)每代種群的精英個(gè)體進(jìn)行優(yōu)化。
設(shè)GA搜索區(qū)域大小為(εx,εy,εα,εθ),分別對(duì)應(yīng)橫向平移、縱向平移、縮放量和旋轉(zhuǎn)角的最大變化量。SCA以每一代種群中的精英個(gè)體為搜索中心,設(shè)定搜索區(qū)間大小為(0.1εx,0.1εy,0.2εα,0.2εθ),在此區(qū)間內(nèi)生成候選集,利用更新方程對(duì)其進(jìn)行候選解更新,得到最優(yōu)解。SCA的最優(yōu)解替換GA原來的精英個(gè)體,并將SCA的候選解種群作為外部種群,與遺傳算法的種群合并,一起進(jìn)行后面的遺傳操作,使得種群向最優(yōu)方向進(jìn)行進(jìn)化。
傳統(tǒng)利用單波束系統(tǒng)進(jìn)行地形匹配的方法,每次測(cè)量只能獲得單點(diǎn)水深信息,進(jìn)行多次測(cè)量之后,得到多個(gè)位置點(diǎn)的水深信息與INS數(shù)據(jù),組成單條水深序列,利用水深序列與水下地形圖進(jìn)行比對(duì),確定載體在地圖上的位置。當(dāng)INS指示航跡與真實(shí)航跡初始定位誤差較大時(shí),由于需要搜索的范圍擴(kuò)大,非常容易出現(xiàn)地形相似的地方,利用單條水深序列進(jìn)行地形匹配的方法精度會(huì)大大下降甚至發(fā)生誤匹配。
不同于單波束系統(tǒng),多波束測(cè)深系統(tǒng)一次測(cè)量能夠得到一條與航向垂直的水深條帶,包含幾十甚至上百個(gè)水下地形深度信息,因此可以選擇測(cè)量數(shù)據(jù)中的多個(gè)測(cè)深數(shù)據(jù)點(diǎn)同時(shí)進(jìn)行地形匹配,提高算法在初始位置誤差較大或相似地形區(qū)域的匹配定位精度??紤]算法復(fù)雜度,在每次測(cè)深數(shù)據(jù)的中心位置和兩側(cè)間隔為dΔ的位置各選擇一個(gè)水深點(diǎn),組成三條水深序列。間隔dΔ根據(jù)地形標(biāo)準(zhǔn)差進(jìn)行自適應(yīng)計(jì)算得到:
其中,μ1和μ2為調(diào)節(jié)系數(shù),grid為地形圖網(wǎng)格間距,σ為M×N個(gè)網(wǎng)格的地形標(biāo)準(zhǔn)差,hw(i,j)為位置(i,j)處的測(cè)量水深,為該地區(qū)的平均水深。
適應(yīng)度函數(shù)用于反映匹配航跡和真實(shí)航跡之間的相似度,相似度越大,意味著匹配航跡越靠近真實(shí)航跡。GA中的適應(yīng)度函數(shù)由三條多波束測(cè)量水深與匹配航跡在水下地形圖上插值得到水深的差值的加權(quán)和構(gòu)成,根據(jù)多波束測(cè)深系統(tǒng)的測(cè)深特性,中間區(qū)域的測(cè)量數(shù)據(jù)誤差最小,越靠近兩側(cè)邊緣測(cè)量數(shù)據(jù)誤差越大,所以給三條水深序列分配不同的權(quán)重:
式中,1ω為中間序列權(quán)重,2ω和3ω分別為左右兩側(cè)序列權(quán)重,均為常值。
考慮到匹配過程中,INS的累計(jì)誤差逐漸增加,導(dǎo)致航跡后期的位置可信度逐漸降低,因此加入衰減系數(shù)λ,逐漸減小水深序列中數(shù)據(jù)點(diǎn)所占權(quán)重,適應(yīng)度函數(shù)具體形式為:
式中,i對(duì)應(yīng)三條水深序列,j對(duì)應(yīng)每條序列上的匹配點(diǎn),ijc為匹配航跡在水下地形圖上映射的水深,*ijc為實(shí)際測(cè)量水深,N為測(cè)量序列點(diǎn)數(shù)。匹配航跡水深與實(shí)際測(cè)量水深的差值越小,適應(yīng)度函數(shù)值越大,適應(yīng)度函數(shù)值取得最大值時(shí),此時(shí)匹配航跡最接近真實(shí)航跡。
與GA不同,SCA的適應(yīng)度函數(shù)值越小,表示待優(yōu)化參數(shù)越接近最佳值。在進(jìn)行SCA優(yōu)化時(shí),種群中的個(gè)體均在精英個(gè)體附近,較為接近真實(shí)航跡位置,此時(shí)對(duì)最大水深誤差更加敏感,SCA的適應(yīng)度函數(shù)為:
根據(jù)上述分析,基于SCA改進(jìn)GA的多波束水下地形匹配方法的具體步驟如下:
步驟一:在多波束測(cè)深系統(tǒng)的測(cè)深數(shù)據(jù)中選取三條水深序列,分別賦予不同權(quán)重。
步驟二:以INS指示航跡位置為搜索中心,設(shè)定搜索區(qū)間,在搜索區(qū)間內(nèi)隨機(jī)生成GA初始種群。
步驟三:對(duì)INS指示的初始航跡進(jìn)行仿射變換,在水下地形圖上進(jìn)行雙線性插值,得到變換后航跡對(duì)應(yīng)的水深,計(jì)算變換航跡的適應(yīng)度函數(shù)值。
步驟四:判斷迭代是否停止,滿足終止條件則輸出結(jié)果,否則,使用精英策略,保留精英個(gè)體。
步驟五:利用SCA對(duì)精英個(gè)體進(jìn)行優(yōu)化,生成外部種群與GA種群合并進(jìn)行遺傳操作。
步驟六:更新種群,跳轉(zhuǎn)至步驟三。
圖2 改進(jìn)GA的多波束地形匹配流程圖Fig.2 Flow chart of multi-beam terrain matching based on improved GA
為驗(yàn)證本文所提方法的有效性,對(duì)基于改進(jìn)遺傳算法的多波束地形匹配方法進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)基于我國某地區(qū)實(shí)際測(cè)量的水下地形數(shù)據(jù),地形圖的網(wǎng)格數(shù)為1484 1157×,網(wǎng)格間距為1 m,選擇其中兩塊不同地形特征區(qū)域設(shè)置仿真航跡,圖3和圖4為航跡對(duì)應(yīng)的水下投影點(diǎn)和地形圖,兩處地形的特征參數(shù)如表1所示。
圖4 航跡B及對(duì)應(yīng)地形圖Fig.4 Track B and corresponding topographic map
表1 地形特征參數(shù)Tab.1 Terrain feature parameters
圖3 航跡A及對(duì)應(yīng)地形圖Fig.3 Track A and corresponding topographic map
仿真中AUV移動(dòng)速度為3 m/s,每隔4 s進(jìn)行一次采樣,15個(gè)采樣點(diǎn)組成一個(gè)匹配序列,測(cè)量水深為真實(shí)水深增加標(biāo)準(zhǔn)差為0.3 m的白噪聲。航跡A的起始位置為(942,405)m,航向?yàn)楸逼?0°;航跡B的起始位置為(775,245)m,航向?yàn)檎龞|方向。慣性元件參數(shù)如表2所示。
表2 慣性器件誤差Tab.2 Inertial device error
根據(jù)上述仿真條件,仿真驗(yàn)證文獻(xiàn)[13]提出的多路徑并行ICCP算法和本文提出的基于改進(jìn)遺傳算法的多波束地形匹配方法,分別對(duì)航跡A和航跡B設(shè)置兩種不同大小的初始誤差,進(jìn)行匹配定位,其匹配結(jié)果如圖5-8所示。
圖5 航跡A初始誤差較小時(shí)匹配結(jié)果Fig.5 Matching results of small initial error of track A
圖6 航跡A初始誤差較大時(shí)匹配結(jié)果Fig.6 Matching results of large initial error of track A
圖7 航跡B初始誤差較小時(shí)匹配結(jié)果Fig.7 Matching results of small initial error of track B
圖8 航跡B初始誤差較大時(shí)匹配結(jié)果Fig.8 Matching results of large initial error of track B
圖中紅色實(shí)線為真實(shí)航跡位置,同時(shí)對(duì)應(yīng)于中間水深序列,紅色虛線為實(shí)測(cè)多波束水深數(shù)據(jù)中選取的兩側(cè)水深點(diǎn)對(duì)應(yīng)的位置;藍(lán)色實(shí)線代表慣導(dǎo)指示航跡,對(duì)應(yīng)于中間水深序列的指示位置,兩側(cè)的虛線為另外兩條水深序列指示位置;綠色實(shí)線和天藍(lán)色實(shí)線分別為多路徑并行ICCP匹配航跡和本文方法匹配航跡。從仿真結(jié)果圖可知,在初始誤差較小時(shí),多路徑并行ICCP與本文所提方法均能成功匹配,匹配航跡均在真實(shí)航跡附近;在初始位置誤差較大時(shí),由于慣性累積誤差的影響,導(dǎo)致航跡變形嚴(yán)重,并且搜索范圍增大,多路徑并行ICCP匹配效果較差,匹配航跡與真實(shí)航跡之間誤差較大,而本文所提方法匹配結(jié)果仍然在真實(shí)航跡附近,對(duì)初始誤差的大小不敏感。
仿真結(jié)果數(shù)據(jù)如表3所示,從表中可以看出,多路徑并行ICCP在初始位置誤差較大時(shí),匹配航跡的平均誤差在10 m左右,而本文所提方法在不同初始位置誤差的條件下,匹配航跡的平均誤差在2 m左右,航跡上的最大匹配誤差小于4 m,定位精度大大優(yōu)于多路徑并行ICCP算法,能夠更好實(shí)現(xiàn)不同初始位置誤差條件下的地形匹配。
表3 匹配結(jié)果對(duì)比Tab.3 Comparison of matching results
為進(jìn)一步驗(yàn)證改進(jìn)遺傳算法的效果,在航跡A(150,150)m初始位置誤差的條件下進(jìn)行航跡A四組匹配定位實(shí)驗(yàn):實(shí)驗(yàn)A使用本文所提算法進(jìn)行地形匹配;實(shí)驗(yàn)B在傳統(tǒng)遺傳算法的基礎(chǔ)上,使用本文提出的適應(yīng)度函數(shù);實(shí)驗(yàn)C在傳統(tǒng)遺傳算法上加入SCA進(jìn)行優(yōu)化;實(shí)驗(yàn)D使用傳統(tǒng)遺傳算法進(jìn)行匹配,使用三條水深序列的水深差值直接累加和作為適應(yīng)度函數(shù)。每組實(shí)驗(yàn)獨(dú)立進(jìn)行50次,分別計(jì)算每組實(shí)驗(yàn)的平均定位誤差、算法最終收斂時(shí)的平均迭代次數(shù)和平均運(yùn)行時(shí)間,實(shí)驗(yàn)結(jié)果如表4所示。
表4 遺傳算法對(duì)比實(shí)驗(yàn)Tab.4 Comparative experiments of GA
對(duì)比實(shí)驗(yàn)A和實(shí)驗(yàn)D結(jié)果可知,本文所提算法匹配結(jié)果相比于傳統(tǒng)遺傳算法定位精度提高42.4%,平均迭代次數(shù)降低35.27%,平均運(yùn)行時(shí)間減少45.25%。對(duì)比實(shí)驗(yàn)B和實(shí)驗(yàn)D可知,使用本文提出的適應(yīng)度函數(shù),遺傳算法的定位精度得到顯著提高,表明本文提出的適應(yīng)度函數(shù)能夠更好地衡量匹配航跡與真實(shí)航跡之間的相似度,有利于提高算法定位精度。對(duì)比實(shí)驗(yàn)C和實(shí)驗(yàn)D可知,加入SCA優(yōu)化后,遺傳算法收斂時(shí)的迭代次數(shù)和運(yùn)行時(shí)間明顯降低,算法的運(yùn)行效率得到提高。通過以上四組重復(fù)性實(shí)驗(yàn)的統(tǒng)計(jì)結(jié)果可以得出,在遺傳算法中加入SCA優(yōu)化和改進(jìn)適應(yīng)度函數(shù)能夠有效提高遺傳算法的匹配定位精度,加快收斂速度,遺傳算法的改進(jìn)效果得到驗(yàn)證。
為進(jìn)一步驗(yàn)證本文所提方法在實(shí)際應(yīng)用中的效果,在千島湖某水庫區(qū)域進(jìn)行實(shí)際船載實(shí)驗(yàn)。圖9為本次實(shí)驗(yàn)的實(shí)驗(yàn)船,船上搭載的主要設(shè)備有IMU、多波束測(cè)深系統(tǒng)和GPS-RTK接收機(jī)。其中,IMU輸出頻率為200 Hz,其中陀螺儀零偏小于0.02/h°,隨機(jī)游走小于,加速度計(jì)零偏小于100μg,隨機(jī)游走小于,實(shí)驗(yàn)區(qū)域地形圖的網(wǎng)格分辨率為1 m,多波束測(cè)深系統(tǒng)為中海達(dá)公司的iBeam 8140,具體參數(shù)如表5所示。
圖9 試驗(yàn)船及相關(guān)試驗(yàn)設(shè)備Fig.9 Experimental ship and related equipments
表5 多波束聲吶主要技術(shù)參數(shù)Tab.5 Main technical parameters of multibeam sonar
本次實(shí)驗(yàn)選取三條航跡,分別設(shè)置不同大小INS初始位置誤差,分別為(50,50)m、(150,150)m和(200,200)m。試驗(yàn)中船速約為1.5 m/s,每間隔8秒選擇一個(gè)匹配點(diǎn),15個(gè)匹配點(diǎn)組成一次航跡。為方便表示,隱去試驗(yàn)區(qū)域的經(jīng)緯度,使用直角坐標(biāo)系表示位置坐標(biāo),由IMU和GPS-RTK進(jìn)行松組合得到的位置作為真實(shí)參考航跡。
圖10-12分別為三種不同初始誤差條件下的匹配結(jié)果,表6為匹配結(jié)果數(shù)據(jù)對(duì)比,從匹配結(jié)果可以看出,本文所提方法在不同初始位置誤差下,匹配航跡的平均位置誤差在3 m以內(nèi),匹配航跡的最大誤差小于8 m,定位結(jié)果不受初始位置誤差的影響。而多路徑ICCP算法的精度要低于本文算法,并且在初始誤差增大時(shí),匹配誤差也逐漸增大,當(dāng)初始誤差達(dá)到200 m時(shí),匹配失敗。船載試驗(yàn)結(jié)果表明本文所提方法在不同大小的初始誤差條件下,均能達(dá)到較高的定位精度,方法的實(shí)際效果得到驗(yàn)證。
圖1 INS指示航跡與真實(shí)航跡示意圖Fig.1 INS indicated track and real track diagram
圖10 (50,50)m初始位置誤差匹配結(jié)果Fig.10 Matching results of (50,50)m initial position error
圖11 (150,150)m初始位置誤差匹配結(jié)果Fig.11 Matching results of (150,150)m initial position error
圖12 (200,200)m初始位置誤差匹配結(jié)果Fig.12 Matching results of (200,200)m initial position error
表6 船載實(shí)驗(yàn)匹配結(jié)果Tab.6 Matching results of shipboard experiments
本文針對(duì)水下地形匹配問題,提出基于SCA改進(jìn)遺傳算法的多波束地形匹配方法,根據(jù)地形特征自適應(yīng)選擇三條水深數(shù)據(jù)進(jìn)行匹配,保證算法在相似地形處的定位精度,降低誤匹配率;在適應(yīng)度函數(shù)中加入收斂因子,減小后期慣導(dǎo)累積誤差的影響;引入SCA對(duì)遺傳算法進(jìn)行優(yōu)化,加快算法收斂速度,降低陷入局部最優(yōu)的風(fēng)險(xiǎn)。仿真和船載實(shí)驗(yàn)結(jié)果表明,本文提出的基于改進(jìn)遺傳算法的多波束水下地形匹配方法不受初始誤差大小的影響,相比ICCP方法,定位精度更高。