基于TERCOM-ICP聯合算法的水下地形匹配方法研究

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

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

0 引言

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

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

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

1 地形匹配算法分析

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

1.1 TERCOM算法原理

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

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

(1)

(2)

(3)

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

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

1.2 ICP算法原理

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

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

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

qi=Rpi+t

(4)

(5)

(6)

設R*、t*為最優解,可以將目標函數優化問題分為兩部分

(7)

t*=μq-Rμp

(8)

R*=UVT

(9)

t*=μq-R*μp

(10)

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

Pnew=R*P+t*

(11)

ICP算法最大的優點是結果比較穩定,但缺點也很明顯,例如計算效率太低、兩個數據集的重疊比高及容易落入局部最優解。

2 TERCOM-ICP聯合匹配算法

通過上述算法分析可知,TERCOM算法簡單、計算量小,但是在有偏轉的情況下誤差較大,精度只有網格長度的一半;ICP算法能夠有效地處理旋轉偏差,但是容易落入局部最優解,計算量大。基于以上原因,本文提出了一種聯合匹配算法,充分利用多波束測深設備測得的水下地形面,先通過TERCOM算法粗略匹配,將匹配后的大致位置作為指示輸入ICP算法中進行精確匹配。

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

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

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

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

圖1 TERCOM-ICP聯合算法流程圖Fig.1 Flow chart of TERCOM-ICP joint algorithm

3 仿真實驗分析

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

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

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

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

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

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

圖6所示為TERCOM算法和TERCOM-ICP聯合算法的誤差曲線圖。

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

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

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

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

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

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

圖9所示為TERCOM算法和TERCOM-ICP聯合算法的誤差曲線圖。

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

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

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

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

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

4 結論

鑒于點云數據配準和地形匹配算法等有關概念的相關性,將ICP算法引入到地形匹配算法中,與傳統的TERCOM地形匹配算法相結合,提出了一種TERCOM-ICP聯合算法,經仿真實驗可得到以下結論:

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

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

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