程建華,丁惠倩,常 樂,葛靖宇

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

0 引言

自主水下航行器(autonomous underwater vehicle,AUV)在不頻繁上浮的情況下進行長航時水下航行時,必須有精確可靠的水下導(dǎo)航和定位。作為AUV的基本導(dǎo)航系統(tǒng),捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdowninertial na-vigation system,SINS)的一個主要缺點是存在時間累積誤差,這就需要依靠其他導(dǎo)航系統(tǒng)來頻繁地修改和校正誤差。目前,常見的水下輔助慣性導(dǎo)航系統(tǒng)的導(dǎo)航方式主要包括聲學(xué)導(dǎo)航[1]、地球物理信息導(dǎo)航[2]和航位推算導(dǎo)航[3]等,其中,地球物理信息導(dǎo)航又可分為地磁輔助導(dǎo)航[4]、重力輔助導(dǎo)航[5]以及地形輔助導(dǎo)航[6-7]。水下地形輔助導(dǎo)航(underwater terrain aided navigation,UTAN)是一種利用先進的水下數(shù)字地圖和測得的水下地形高程來估計AUV位置的技術(shù),可以實現(xiàn)完全自主式導(dǎo)航定位,從而得到高精度的水下位置信息,用以修正捷聯(lián)慣性導(dǎo)航系統(tǒng)的誤差。隨著多波束測深系統(tǒng)的發(fā)展和逐步應(yīng)用,水下地形測量的精度越來越高,范圍也越來越廣,其工作時發(fā)射的多個波束呈現(xiàn)扇面開角,相對于單波束測深系統(tǒng)只能發(fā)射一個波束來說,所測量得到的地形信息大大增加,AUV航行時所測量的地形信息整體呈現(xiàn)為曲面,而非單波束系統(tǒng)呈現(xiàn)的曲線,這使得AUV可以更有效率地得到精度更高的水下數(shù)字地圖和實時掃測地形分布。多波束測深系統(tǒng)的這些優(yōu)勢為水下地形輔助匹配導(dǎo)航創(chuàng)造了良好的條件[8]。

國內(nèi)外學(xué)者針對基于相關(guān)性匹配的經(jīng)典地形匹配算法有不同程度的改進。趙龍等[9]提出了結(jié)合地形輪廓匹配(terrain contour matching,TERCOM)和粒子濾波(particle filter,PF)的地形輔助慣導(dǎo)算法,初始位置信息在采集模式下由TERCOM獲取,連續(xù)跟蹤在跟蹤模式下由PF算法實現(xiàn),不同模式之間的切換由模式控制邏輯控制,在初始誤差較大的情況下能夠快速、準(zhǔn)確的定位;李培娟等[10]提出了結(jié)合迭代最近等值線(iterated closest contour point,ICCP)算法和卡爾曼濾波器的地形導(dǎo)航系統(tǒng),以ICCP匹配位置與SINS的差值作為卡爾曼濾波器的測量,利用濾波結(jié)果反饋校正SINS輸出,得到最優(yōu)估計,可以滿足AUV長時間保持精確導(dǎo)航的要求,大大降低了位置誤差;王漢兵等[11]分析了多波束測深數(shù)據(jù)特征,選擇條帶中心和邊緣兩側(cè)數(shù)據(jù)點作為三條并行路徑,基于中心光束基準(zhǔn)面多波束測深數(shù)據(jù)原理,設(shè)置路徑權(quán)重,實現(xiàn)了多路徑并行的ICCP算法,可以在具有更多地形特征的區(qū)域?qū)崿F(xiàn)更高的定位精度;趙建虎等[12]指出,TERCOM可靠性與準(zhǔn)確性受到單一匹配序列的影響,因此采用兩個匹配序列來增強算法,通過TERCOM得到新的匹配區(qū)域后,使用基于鏈碼的多等值線地形匹配(multi-conto-urs terrain matching,MCTM)算法精確匹配,兩種算法克服了彼此的缺點,獲得了更高的水下導(dǎo)航精度和可靠性。以上文獻對經(jīng)典的地形匹配方法進行了不同程度的改進與結(jié)合,但是改進后的方法依然是面向線對線的匹配,沒有利用到多波束測深系統(tǒng)掃測得到地形面的特點。

ICCP算法借助等值線數(shù)字地圖來定位匹配,目前,已應(yīng)用在地球物理信息導(dǎo)航領(lǐng)域,即地磁輔助導(dǎo)航、重力輔助導(dǎo)航以及地形輔助導(dǎo)航中。傳統(tǒng)的地球物理信息導(dǎo)航所使用的測量物理信息儀器分別為地磁傳感器、重力儀和單波束測深系統(tǒng),理論上能測量的僅僅為AUV當(dāng)前垂直位置的物理信息。因此,當(dāng)AUV航行時,只能獲得一條線信息。但是,目前地形匹配導(dǎo)航中開始廣泛使用多波束測深系統(tǒng),該系統(tǒng)在航行時可以獲得地形面信息,而ICCP算法通常用來處理線信息,這就要求引入其他的算法來處理信息,從而完成地形匹配。迭代最近點(iterative closest point,ICP)[13]算法是ICCP算法的基礎(chǔ),目前常用于點云配準(zhǔn)領(lǐng)域,該方法可以應(yīng)用于多波束測深數(shù)據(jù)與水下數(shù)字地圖的匹配問題?；谝陨戏治?本文提出了一種TERCOM與ICP算法聯(lián)合的水下地形輔助導(dǎo)航算法,利用多波束測深系統(tǒng)掃測的地形面,解決UTAN中TERCOM算法精度不高、ICP算法在初始誤差較大情況下迭代次數(shù)過多的問題。

1 地形匹配算法分析

目前,地形匹配常用的算法大致分為兩種:相關(guān)性匹配方法和基于濾波的方法。相關(guān)性匹配方法主要有TERCOM算法,該方法在進行匹配前需要采集一定的數(shù)據(jù)點才能匹配,是一種批相關(guān)處理的方法。桑迪亞慣性地形輔助導(dǎo)航(Sandia inertial terrain-aided navigation,SITAN)[14]算法不同于利用相關(guān)分析法的TERCOM算法,它是一種遞推的擴展卡爾曼濾波系統(tǒng),利用數(shù)字地圖和傳感器實時測得的數(shù)據(jù),通過卡爾曼濾波器對慣導(dǎo)系統(tǒng)實時不間斷地修正。本文將著重介紹TERCOM算法和ICP算法。

1.1 TERCOM算法原理

TERCOM算法從數(shù)字地圖中選取一組和SINS指示航跡平行的序列,將真實高程序列與選取的高程序列按照某種算法度量進行匹配相關(guān)分析,依據(jù)相關(guān)分析結(jié)果就可以得到AUV的真實位置偏差,從而修正SINS的參數(shù),減小累積誤差。

目前有3個算法常用于相關(guān)分析,分別是交叉相關(guān)(correlation,COR)算法、平均絕對差(mean absolute differences,MAD)算法和平均標(biāo)準(zhǔn)差(mean square differences,MSD)算法。這3個算法的定義如式(1)、式(2)、式(3)

(1)

(2)

(3)

其中,hr代表真實高程序列;hm代表選取的高程序列。

在上面的相關(guān)分析指標(biāo)中,MAD和MSD是距離決策算法,取最小值時獲得最佳匹配位置,而COR是一種形狀決策算法,取最大值時獲得最佳匹配位置[1]。這兩種算法的定位結(jié)果相互獨立,同時滿足距離決策和形狀決策算法的定位結(jié)果可以視為真實位置。如果沒有測量誤差,這兩種類型的算法得到相同的定位結(jié)果。經(jīng)過實驗與分析發(fā)現(xiàn),MSD算法的準(zhǔn)確性略高于COR和MAD算法,因此在研究中多采用MSD算法[6]。由于TERCOM算法的相關(guān)性分析是基于網(wǎng)格來計算的,所以始終存在著半個網(wǎng)格間距的量化誤差,即TERCOM算法精度最高只能達到網(wǎng)格長度的一半。

1.2 ICP算法原理

ICP算法一直被認為是點云數(shù)據(jù)配準(zhǔn)中最經(jīng)典的算法,并且在相關(guān)領(lǐng)域的應(yīng)用已經(jīng)十分成熟。ICP 算法是一種基于最小二乘法的點云精確配準(zhǔn)方法,通過最小化相應(yīng)點對之間的距離,不斷地迭代更新以獲得配準(zhǔn)參數(shù)的估計值。

已知原點云P={p1,p2,…,pn}和目標(biāo)點云Q={q1,q2,…,qn},通過不斷迭代,將P點云中的pi與Q點云中的qi距離最短的點作為對應(yīng)點求解變換矩陣,當(dāng)誤差損失函數(shù)達到設(shè)定閾值或者達到迭代次數(shù)時停止迭代,獲得最佳旋轉(zhuǎn)矩陣R和平移矩陣t,達到最佳配準(zhǔn)效果。

一般來說,在沒有誤差的情況下點云數(shù)據(jù)滿足式(4)

qi=Rpi+t

(4)

(5)

(6)

設(shè)R*、t*為最優(yōu)解,可以將目標(biāo)函數(shù)優(yōu)化問題分為兩部分

(7)

t*=μq-Rμp

(8)

R*=UVT

(9)

t*=μq-R*μp

(10)

將變換矩陣應(yīng)用到點云P中的所有點上,使所有點都進行坐標(biāo)變換,如式(11)所示完成點云P的坐標(biāo)更新

Pnew=R*P+t*

(11)

ICP算法最大的優(yōu)點是結(jié)果比較穩(wěn)定,但缺點也很明顯,例如計算效率太低、兩個數(shù)據(jù)集的重疊比高及容易落入局部最優(yōu)解。

2 TERCOM-ICP聯(lián)合匹配算法

通過上述算法分析可知,TERCOM算法簡單、計算量小,但是在有偏轉(zhuǎn)的情況下誤差較大,精度只有網(wǎng)格長度的一半;ICP算法能夠有效地處理旋轉(zhuǎn)偏差,但是容易落入局部最優(yōu)解,計算量大?；谝陨显?本文提出了一種聯(lián)合匹配算法,充分利用多波束測深設(shè)備測得的水下地形面,先通過TERCOM算法粗略匹配,將匹配后的大致位置作為指示輸入ICP算法中進行精確匹配。

當(dāng)AUV經(jīng)過可匹配地形區(qū)時,利用多波束測深系統(tǒng)測得AUV距離水下地面的深度,用壓力傳感器獲得所處深度,進而得到當(dāng)前AUV位置下方真實的地形高程。之后根據(jù)SINS輸出的位置從數(shù)字地圖中確定TERCOM算法的匹配搜索區(qū)域,該區(qū)域是一個矩形區(qū)域,以SINS指示的最終位置作為中心,6σ作為邊的長度,σ表示SINS導(dǎo)航誤差的標(biāo)準(zhǔn)差。使用該方法確定的搜索范圍包含AUV真實航行的最終位置,然后從SINS導(dǎo)航航跡中提取若干個地形高程剖面圖,這些位置應(yīng)該與真實地形高程剖面圖平行。將真實高程序列與選取的高程序列按照MSD算法進行匹配相關(guān)分析,得到MSD值最小的高程序列所指示的位置即為TERCOM算法匹配位置,也就是ICP算法的指示位置。

在進行ICP算法地形匹配時,將通過多波束測深系統(tǒng)以及指示位置計算出的地形坐標(biāo)數(shù)據(jù)視為ICP算法中的原點云,從水下地形數(shù)據(jù)中提取相匹配的格網(wǎng)數(shù)據(jù)作為目標(biāo)點云。水下地形格網(wǎng)數(shù)據(jù)通過K-D樹(K-Dimensional tree)存儲[15]。

K-D樹[16]是一種基于二叉搜索的數(shù)據(jù)結(jié)構(gòu),解決在k維空間為數(shù)據(jù)集建立索引的問題。它可以有效地存儲、管理和搜索數(shù)據(jù),并用于提高ICP算法的運行效率。K-D樹將待匹配區(qū)域地形坐標(biāo)在x、y、z這3個維度投影中分成若干個半平面,使得每個點都位于自己的空間中,這種將空間劃分為子空間的方法是使用空間中值計算來實現(xiàn)的。整個模型基于一個根節(jié)點,在樹的下方層層遞進地分為若干個葉節(jié)點,然后使用最臨近搜索找出在樹中與輸入點最接近的點。將要搜索的節(jié)點與劃分維度的空間中值進行比較,如果搜索節(jié)點對應(yīng)值小于或等于劃分維度的中位數(shù),則進入左側(cè)子分支,相反則進入右側(cè)子分支。循環(huán)到二叉樹的葉節(jié)點,并沿著路徑找出在要查詢的點的同一子空間內(nèi)的近點;如果在搜索路徑節(jié)點的其他子空間節(jié)點上有另一個較近的點,則轉(zhuǎn)到子空間的節(jié)點以搜索距離中的近點。重復(fù)上述過程直到搜索路徑無效,結(jié)束搜索。

通過K-D樹搜索到地形坐標(biāo)數(shù)據(jù)的最臨近點數(shù)據(jù),再經(jīng)過ICP算法得到兩組數(shù)據(jù)的旋轉(zhuǎn)矩陣和平移矩陣,變換后得到修正后的地形坐標(biāo)數(shù)據(jù)。重復(fù)上述過程直至達到迭代次數(shù),最后得到的修正地形坐標(biāo)數(shù)據(jù)即為最終匹配結(jié)果。整個算法的流程如圖1所示。

圖1 TERCOM-ICP聯(lián)合算法流程圖Fig.1 Flow chart of TERCOM-ICP joint algorithm

3 仿真實驗分析

本文采用Python進行仿真實驗分析,實驗區(qū)域三維地形圖和等高線圖分別如圖2和圖3所示,X、Y表示經(jīng)度、緯度方向格網(wǎng)編號,格網(wǎng)長度為10 m。假設(shè)AUV以4 m/s的速度行駛,速度誤差為0.05 m/s,陀螺儀常值漂移誤差為0.001(°)/h,每隔20 s進行一次采樣,經(jīng)過10次采樣進行一次匹配,共進行10次匹配。每完成一次匹配后,給予一個均值為0、標(biāo)準(zhǔn)差為5°的隨機角度,從而使AUV的航向發(fā)生變化。本文采用TERCOM-ICP聯(lián)合匹配方法,在圖示區(qū)域內(nèi)選擇一條較平緩的軌跡作為實驗一和一條較崎嶇的軌跡作為實驗二進行仿真實驗。

圖2 實驗區(qū)域三維地形圖Fig.2 3D topographic map of the experimental area

圖3 實驗區(qū)域等高線圖Fig.3 Contour map of the experimental area

實驗一匹配結(jié)果如圖4所示。圖5所示分別為第2個匹配點和第9個匹配點的結(jié)果。AUV真實軌跡的起始點為(400 m,8 000 m),初始誤差為(400 m,200 m),初始航向角為110°。

圖4 實驗一匹配結(jié)果Fig.4 Matching results of experiment one

圖5 實驗一局部匹配結(jié)果Fig.5 Local matching results of experiment one

圖6所示為TERCOM算法和TERCOM-ICP聯(lián)合算法的誤差曲線圖。

圖6 實驗一匹配誤差Fig.6 Matching errors of experiment one

表1中,D表示匹配位置與真實位置之間的直線距離誤差。從圖示結(jié)果以及表格中的數(shù)據(jù)來看,TERCOM-ICP算法能夠有效減小單純使用TERCOM算法的匹配誤差,平均匹配誤差能達到20 m以內(nèi)。從圖表可以看出,TERCOM算法的匹配結(jié)果在一定程度上影響著后續(xù)ICP算法的結(jié)果,一方面可能是因為該匹配點處地形變化不明顯或存在其他相似地形;另一方面是因為TERCOM算法的匹配結(jié)果作為指示輸入到ICP算法中,從而對最終匹配結(jié)果產(chǎn)生一定的影響。

表1 實驗一匹配誤差參數(shù)對比Tab.1 Comparison of matching error parameters in experiment one

實驗二匹配結(jié)果如圖7所示。圖8所示分別為第2個匹配點和第9個匹配點的結(jié)果。AUV真實軌跡的起始點為(1 000 m,2 000 m),初始誤差為(-200 m,400 m),初始航向角為45°。

圖7 實驗二匹配結(jié)果Fig.7 Matching results of experiment two

圖8 實驗二局部匹配結(jié)果Fig.8 Local matching results of experiment two

圖9所示為TERCOM算法和TERCOM-ICP聯(lián)合算法的誤差曲線圖。

圖9 實驗二匹配誤差Fig.9 Matching errors of experiment two

從圖示結(jié)果以及表2的數(shù)據(jù)來看,TERCOM-ICP算法能夠有效減小使用TERCOM算法的匹配誤差,平均匹配誤差能達到20 m以內(nèi)。在傳統(tǒng)的ICCP算法中,通常AUV的航跡所經(jīng)過的區(qū)域較為崎嶇時,即在地形變化較明顯的區(qū)域,匹配結(jié)果會明顯好于AUV的航跡經(jīng)過的區(qū)域較為平緩時,也就是地形變化不明顯的區(qū)域[17],但是本文提出的TERCOM-ICP算法在兩處區(qū)域內(nèi)匹配結(jié)果精度并沒有明顯差別。產(chǎn)生以上情況的原因是ICCP算法是一種線匹配方法,當(dāng)AUV在平緩區(qū)域內(nèi)移動時,所能獲取的水下地形信息量遠不如地形變化明顯的區(qū)域,因而易產(chǎn)生匹配誤差較大的情況;而ICP算法是一種面匹配方法,在同樣平緩的區(qū)域內(nèi),ICP算法通過多波束測深系統(tǒng)所能獲得的水下地形信息量遠大于ICCP算法,從而在匹配時能有效減小產(chǎn)生較大誤差的可能。

表2 實驗二匹配誤差參數(shù)對比Tab.2 Comparison of matching error parameters in experiment two

表3數(shù)據(jù)表示實驗一和實驗二分別使用ICP算法和TERCOM-ICP聯(lián)合算法進行同樣路徑的地形匹配所用時間。根據(jù)表中數(shù)據(jù)分析可知,TERCOM算法計算簡單,在地形匹配時先進行粗匹配,之后再使用ICP算法精匹配,所用時間要小于單純使用ICP算法的匹配時間。由此可驗證本文提出的TERCOM-ICP算法能夠避免ICP算法效率較低的缺陷,減少整個匹配過程的時間。

表3 實驗所用時間比較Tab.3 Comparison of time spent in experiments

4 結(jié)論

鑒于點云數(shù)據(jù)配準(zhǔn)和地形匹配算法等有關(guān)概念的相關(guān)性,將ICP算法引入到地形匹配算法中,與傳統(tǒng)的TERCOM地形匹配算法相結(jié)合,提出了一種TERCOM-ICP聯(lián)合算法,經(jīng)仿真實驗可得到以下結(jié)論:

1)通過仿真實驗驗證了TERCOM-ICP算法在地形匹配領(lǐng)域的可行性,與TERCOM算法相比,能夠提高匹配導(dǎo)航的整體精度,與ICP算法比較,能夠減少匹配時間。

2)引入ICP算法能夠有效利用多波束測深系統(tǒng)掃測得到的地形面優(yōu)勢,增加所能獲得的水下地形數(shù)據(jù)信息量,降低產(chǎn)生較大匹配誤差的可能。

3)本文僅驗證了TERCOM-ICP算法的可行性,未對AUV航行過程中可能遇到的各種誤差的影響進行分析。此外,TERCOM算法和ICP算法都需要累積一定的路程后才可進行匹配,所以導(dǎo)航初期屬于純慣性導(dǎo)航階段,在對實時性要求較高的情況下,該方法還有待改進提升。