李 凱，吳文啟，張 鷺

（國(guó)防科學(xué)技術(shù)大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，長(zhǎng)沙410073）

0 引言

AUV水下自主導(dǎo)航是目前世界范圍內(nèi)的研究熱點(diǎn)［1］，INS/DVL（慣性/Doppler 測(cè)速儀）是 AUV常用的水下組合導(dǎo)航方式。當(dāng)AUV上浮到水面時(shí)，還可以通過(guò)衛(wèi)星導(dǎo)航校正導(dǎo)航誤差。DVL是AUV組合導(dǎo)航中的重要組成部分，能夠?yàn)锳UV提供較為精確的載體相對(duì)水底或水層的速度信息［2-3］。但是，DVL在實(shí)際工作中會(huì)因?yàn)閺?fù)雜的水底環(huán)境和AUV機(jī)動(dòng)（上浮、下潛、轉(zhuǎn)向）等各種原因出現(xiàn)較大的測(cè)速偏差，即DVL粗差。一些數(shù)值較大的DVL粗差，可以直接根據(jù)AUV運(yùn)動(dòng)學(xué)約束或慣性導(dǎo)航信息進(jìn)行檢測(cè)和剔除。但是當(dāng)采用較低成本的慣性導(dǎo)航系統(tǒng)（如航向姿態(tài)參考系統(tǒng)）時(shí)，慣性導(dǎo)航提供的速度信息本身精度不高，誤差增長(zhǎng)快，一些符合AUV運(yùn)動(dòng)學(xué)約束、與正常速度值接近、持續(xù)時(shí)間長(zhǎng)的DVL粗差往往難以檢測(cè)和剔除。

側(cè)掃聲納能夠掃描水底的地貌特征，是AUV的主要水下探測(cè)設(shè)備。目前，關(guān)于側(cè)掃聲納在水下組合導(dǎo)航中的研究主要集中在圖像匹配定位范圍內(nèi)，匹配定位精度在幾十米之內(nèi)［4］。但是，對(duì)于側(cè)掃聲納圖像的成像原理與載體速度之間聯(lián)系的研究還沒(méi)有深入開(kāi)展。

本文提出一種基于側(cè)掃聲納圖像的AUV側(cè)向速度估計(jì)方法。該方法基于SIFT特征匹配算法從側(cè)掃聲納圖像中的相鄰數(shù)據(jù)幀找到相關(guān)特征點(diǎn)，并根據(jù)相關(guān)特征點(diǎn)的像素差值換算出實(shí)際的載體側(cè)向速度。將求得的側(cè)向速度應(yīng)用于檢測(cè)并剔除DVL速度信息中的粗差數(shù)據(jù)，利用高精度的INS/GPS組合導(dǎo)航數(shù)據(jù)對(duì)結(jié)果的準(zhǔn)確性進(jìn)行了驗(yàn)證。

1 水下DVL測(cè)速數(shù)據(jù)存在的粗差問(wèn)題

在對(duì)實(shí)際AUV水下實(shí)驗(yàn)的導(dǎo)航數(shù)據(jù)進(jìn)行分析時(shí)，會(huì)發(fā)現(xiàn)DVL的測(cè)速信息相較于INS/GPS組合導(dǎo)航的速度信息存在一些比較明顯的粗差，圖1所示曲線數(shù)據(jù)來(lái)源于實(shí)際湖測(cè)試驗(yàn)。AUV在水面航行，誤差曲線是以INS/GPS組合導(dǎo)航得到的AUV前向、側(cè)向速度值為基準(zhǔn)，計(jì)算DVL各個(gè)方向速度的誤差。那些偏差較大的粗差數(shù)據(jù)，根據(jù)AUV運(yùn)動(dòng)學(xué)約束或慣性導(dǎo)航信息便可判定。如圖1（a）所示的DVL前向、側(cè)向速度誤差曲線中，出現(xiàn)了一處達(dá)到1m/s的呈突跳變化的誤差值。而如圖1（b）所示的DVL前向、側(cè)向速度誤差曲線中，可以看到誤差變化相對(duì)平緩，幅值相對(duì)小，往往較難檢測(cè)和剔除。

對(duì)比圖1（b）曲線可以發(fā)現(xiàn)，DVL前向速度誤差在側(cè)向速度誤差較大（大于0.2m/s）時(shí)，其前向速度誤差值也明顯大于其他時(shí)刻。也就是說(shuō)，DVL側(cè)向速度的異常值對(duì)應(yīng)著的前向速度也會(huì)出現(xiàn)異常。依據(jù)DVL速度之間的相關(guān)性，只需找到一個(gè)方向速度的異常值即可。

為了解決水下組合導(dǎo)航中DVL速度粗差的檢測(cè)剔除問(wèn)題，本文創(chuàng)新性引入了基于側(cè)掃聲納圖像的AUV側(cè)向速度估計(jì)方法，該方法估計(jì)出的側(cè)向速度可以有效檢測(cè)和剔除DVL側(cè)向速度粗差。

2 由側(cè)掃聲納圖像數(shù)據(jù)求取側(cè)向速度

2.1 側(cè)掃聲納圖像特點(diǎn)

圖2為由側(cè)掃聲納數(shù)據(jù)繪制而成的側(cè)掃聲納圖像，從圖像中可以清晰地觀測(cè)到水底線、水面線以及水底地貌的特征信息。側(cè)掃聲納通過(guò)聲波周期性地向水底掃描，并依照回波的強(qiáng)弱和時(shí)間來(lái)設(shè)定像素點(diǎn)的明暗。每個(gè)周期內(nèi)的回波構(gòu)成一幀圖像，這樣將每一幀數(shù)據(jù)拼接在一起就能構(gòu)成完整的側(cè)掃聲納圖像［5］。

在側(cè)掃聲納圖像的縱向信息中，由于聲納實(shí)時(shí)繪制的圖像只是數(shù)據(jù)幀簡(jiǎn)單的堆疊，圖像反映的物體輪廓形狀和真實(shí)情況相差較大，縱向還存在著聲線斜距變形和比例尺不等變形等畸變現(xiàn)象［6］。因此，側(cè)掃聲納圖像在縱向上難以提供精確的位移和速度信息。

側(cè)掃聲納圖像的橫向數(shù)據(jù)屬于同一數(shù)據(jù)幀，根據(jù)側(cè)掃聲納圖像成像原理，可計(jì)算出每一幀圖像相對(duì)上一幀圖像對(duì)應(yīng)特征點(diǎn)像素的橫向平移量，結(jié)合特征像素平移量與AUV實(shí)際橫向位移的比例尺度關(guān)系以及聲納側(cè)掃周期可推算出AUV側(cè)向速度。利用這一特點(diǎn)，便可以在AUV的側(cè)向速度與側(cè)掃聲納圖像的橫向信息之間建立聯(lián)系。

提取側(cè)向速度的關(guān)鍵，在于如何將圖像中的像素點(diǎn)距離信息換算為實(shí)際的水平距離信息。側(cè)掃聲納的掃描原理如圖3所示（只畫(huà)出半邊示意圖，右邊與左邊原理一致），換能器在一定周期內(nèi)按照固定的夾角發(fā)射一窄束聲波脈沖，根據(jù)反射時(shí)間確定AUV中心到入射點(diǎn)之間的斜距。像素點(diǎn)間的斜距是等差分布的，每條斜距都比前一條斜距多出Range/N，即：

其中，dn為第n個(gè)像素點(diǎn)到AUV中心的斜距，Range為最大斜距，N為總像素?cái)?shù)。

這樣，求取任意一個(gè)像素到AUV中心點(diǎn)的水平距離的關(guān)鍵在于求取當(dāng)前數(shù)據(jù)幀AUV距離水底的深度h。如圖2所示，水底線特征十分明顯，利用SIFT特征匹配算法可以比較容易地提取出水底線，從而得到其在每一數(shù)據(jù)幀之中的位置。在一個(gè)數(shù)據(jù)幀中，設(shè)水底線對(duì)應(yīng)的像素位置在n0處，此時(shí)AUV距離水底的高度h為：

假設(shè)水底環(huán)境比較平緩，高度起伏不大，近似認(rèn)為水底是一個(gè)平面。這樣根據(jù)圖3所示，像素點(diǎn)n對(duì)應(yīng)的水底特征點(diǎn)到AUV正下方的中心點(diǎn)的橫向水平距離s滿足：

根據(jù)式（1）～式（3），可以得到：

這樣，特征像素點(diǎn)n到水底線像素點(diǎn)n0的像素位置差異與實(shí)際水下特征點(diǎn)到AUV正下方的中心點(diǎn)的橫向水平距離s之間便建立了聯(lián)系。當(dāng)水底起伏較大時(shí)，雖然會(huì)影響s的準(zhǔn)確性，但對(duì)s變化的測(cè)量影響并不大，不會(huì)造成大的AUV速度估計(jì)誤差。

2.2 基于SIFT特征匹配算法的特征點(diǎn)提取

提取側(cè)向速度的關(guān)鍵是找到相鄰兩幀數(shù)據(jù)之間的相關(guān)點(diǎn)，相關(guān)點(diǎn)不一定是實(shí)際中的同一個(gè)點(diǎn)，可以是同一特征物體的邊緣線上的相鄰點(diǎn)。由于AUV不斷地前進(jìn)，這樣相鄰幀之間有可能掃描不到相同的點(diǎn)。但是，相鄰的數(shù)據(jù)幀會(huì)掃描到相同的特征標(biāo)志物邊緣線。由于對(duì)聲波的反射強(qiáng)度與周?chē)h(huán)境有差異，這些特征標(biāo)志物在聲納圖像中與周?chē)h(huán)境之間存在明顯的邊緣線，邊緣線兩側(cè)像素的亮度呈現(xiàn)顯著的差異。由此，可以判斷每一幀數(shù)據(jù)屬于邊緣線的特征點(diǎn)。

SIFT特征是圖像的局部特征，其對(duì)旋轉(zhuǎn)、尺度縮放、亮度變化保持不變性，對(duì)視角變化、仿射變換、噪聲也保持一定程度的穩(wěn)定性。相對(duì)于其他算法，它的實(shí)時(shí)性和獨(dú)特性都較好。所以，選取SIFT特征匹配算法作為查找側(cè)掃聲納圖像特征點(diǎn)的工具。

SIFT算法可以在不同的尺度空間上查找特征點(diǎn)，該算法通過(guò)計(jì)算一幅圖中的特征點(diǎn)及其有關(guān)位置和方向得到特征并進(jìn)行圖像特征點(diǎn)匹配。SIFT算法可以檢測(cè)到側(cè)掃聲納圖像中的線特征，其中包括水底線和水底標(biāo)志物的邊緣線。利用SIFT算法提取出的水底線將用來(lái)求取AUV距離水底高度h，而標(biāo)志物的邊緣線將被用來(lái)匹配相關(guān)特征點(diǎn)。

在實(shí)時(shí)運(yùn)行時(shí)，可以將當(dāng)前掃描得到的數(shù)據(jù)幀和之前的50個(gè)數(shù)據(jù)幀構(gòu)成固定寬度的滑動(dòng)窗口來(lái)利用SIFT特征匹配算法進(jìn)行特征點(diǎn)提取。

（1）SIFT 算法核心

一個(gè)二維圖像的尺度空間L（x，y，σ）定義為一個(gè)變化尺度的Gaussian函數(shù)，G（x，y，σ）與原圖像素信息I（x，y）的卷積。

其中，G（x，y，σ）是尺度可變Gaussian函數(shù)，（x，y）是尺度空間坐標(biāo)。σ大小決定圖像的平滑程度，大尺度對(duì)應(yīng)圖像的概貌特征，小尺度對(duì)應(yīng)圖像的細(xì)節(jié)特征。為了兼顧圖像概貌和細(xì)節(jié)特征，σ的值一般取為0.6。為了有效地在尺度空間檢測(cè)到穩(wěn)定的關(guān)鍵點(diǎn)，提出了Gaussian差分尺度空間D（x，y，σ），它由不同尺度的Gaussian差分與原圖像卷積生成。

其中，k為常值系數(shù)，一般取值為 21/3或21/4。

為了尋找尺度空間的極值點(diǎn)，每一個(gè)采樣點(diǎn)要和它所有的相鄰點(diǎn)比較，即中間的檢測(cè)點(diǎn)要和它同尺度的8個(gè)相鄰點(diǎn)和上下相鄰尺度對(duì)應(yīng)的9×2個(gè)點(diǎn)共26個(gè)點(diǎn)比較，看其是否比它的圖像域和尺度域的相鄰點(diǎn)大或者小，以確保在尺度空間和二維圖像空間都檢測(cè)到極值點(diǎn)。檢測(cè)到的極值點(diǎn)就被認(rèn)為是圖像在該尺度下的一個(gè)特征點(diǎn)［7］。

（2）除去干擾特征點(diǎn)的方法

這一步本質(zhì)上要去掉那些不屬于線特征，其位置既不在水底線也不在標(biāo)志物的邊緣線上的干擾特征點(diǎn)。這些干擾特征點(diǎn)為局部曲率非常不對(duì)稱(chēng)的像素，其空間尺度函數(shù)Taylor展開(kāi)式如下：

其中，X＝（x，y，σ）T，對(duì)式（8）求導(dǎo)并令其為0，得到精確的位置。

在已經(jīng)檢測(cè)到的特征點(diǎn)中，要去掉低對(duì)比度的特征點(diǎn)和不穩(wěn)定的邊緣響應(yīng)點(diǎn)。去除低對(duì)比度的點(diǎn)：把式（9）代入式（8），即在極值點(diǎn)處D（X）取值，得：

經(jīng)過(guò)反復(fù)試驗(yàn)，若D）≥0.03，該特征點(diǎn)就保留下來(lái)，否則丟棄。

從圖4可以看出，檢測(cè)到的特征點(diǎn)基本上分布在較為明顯的分界線上。

（3）水底線提取方法

從圖2可以看出，兩條水底線的中間部分為沒(méi)有聲波反射的黑色區(qū)域。像素的亮度值很小，接近于0。其中只有一些水里的浮游物體或水流波紋會(huì)產(chǎn)生一些回波，使得黑色區(qū)域里出現(xiàn)一些亮度較高的特征點(diǎn)。

去干擾特征點(diǎn)的方法可以有效地檢測(cè)掉這些零散的點(diǎn)特征向量，這樣兩條水底線特征向量之間就不存在多余的干擾特征點(diǎn)。對(duì)于每一數(shù)據(jù)幀只需提取距離中心線最近的兩個(gè)特征點(diǎn)，就能夠提取出兩條對(duì)稱(chēng)的水底線。

如圖5所示，圖像下半部分提取的線條與上面實(shí)際水底線基本重合。提取水底線采取距離中心線最近的特征點(diǎn)的方法，這樣少許誤差點(diǎn)集中在水底線之間，而水底線兩側(cè)的區(qū)域沒(méi)有誤差點(diǎn)存在。

（4）相關(guān)特征點(diǎn)匹配方法

SIFT算法為每個(gè)特征點(diǎn)計(jì)算一個(gè)方向，利用關(guān)鍵點(diǎn)鄰域像素的梯度方向分布特性為每個(gè)關(guān)鍵點(diǎn)指定方向參數(shù)，相互關(guān)聯(lián)的特征點(diǎn)應(yīng)具有相同的方向參數(shù)。

m（x，y）與θ（x，y）為（x，y）處梯度的模值和方向公式。相鄰數(shù)據(jù)幀的相關(guān)特征點(diǎn)具有橫坐標(biāo)相差很小，模值與方向信息基本一致的特點(diǎn)。這樣，依據(jù)以上準(zhǔn)則便可在每對(duì)相鄰數(shù)據(jù)幀之間找到多組相關(guān)特征點(diǎn)。

2.3 載體側(cè)向速度提取算法

如果一對(duì)相鄰數(shù)據(jù)幀匹配到的相關(guān)特征點(diǎn)的像素值分別為n1與n2，其對(duì)應(yīng)的水底線分別為n01與n02，則表觀的載體側(cè)向速度Vy為：

其中，Rpetition為每幀數(shù)據(jù)的掃描周期，由側(cè)掃聲納數(shù)據(jù)文件直接給出。所求得的表觀地載體側(cè)向速度Vy包括載體實(shí)際側(cè)向速度和特征標(biāo)志物自身寬度變化速率：

每相鄰兩幀數(shù)據(jù)之間都存在多個(gè)匹配的相關(guān)特征點(diǎn)。其中，無(wú)論是大小還是方向都是不隨標(biāo)志物的變化而變化的，而是隨著特征點(diǎn)不同而不斷變化的。因?yàn)檩d體在一個(gè)幾十毫秒的周期內(nèi)所掃描到的特征標(biāo)志物的寬度變化是非常小的，所以一般來(lái)說(shuō)對(duì)于曲率較小的標(biāo)志物?。 為了提高載體側(cè)向速度在表觀的載體速度V中所占比重，將所求得的Vy求平均值：

其中，Num為每相鄰數(shù)據(jù)幀中相關(guān)特征點(diǎn)的個(gè)數(shù)。為了避免一些曲率較大的特征標(biāo)志物造成的影響，將每一組表觀的載體速度中剔除一個(gè)最大值和一個(gè)最小值。對(duì)于所有表觀的載體速度，其中的正負(fù)不一，且其絕對(duì)值都比較小。求平均值之后的會(huì)因?yàn)檎?fù)值的抵消作用而趨近于0，每一組的相關(guān)特征點(diǎn)的數(shù)量越多，的平均值就越趨近于0。這樣，只要樣本數(shù)量足夠多就能夠保證≈。

如圖6所示，該框圖為側(cè)掃聲納圖像提取載體側(cè)向速度并應(yīng)用與檢測(cè)DVL速度粗差的詳細(xì)方法流程圖。

2.4 誤差分析

設(shè)匹配到的相關(guān)特征點(diǎn)n1、n2的像素誤差為δn，這樣提取的側(cè)向速度的誤差?為：

其中，Range一般為125m，Rpetition為170ms，N＝1000，n0/n1的數(shù)值選取范圍為1/3～3/4。 這樣，求得的表觀載體側(cè)向速度的誤差 ?（n1，n2）≈K（δn1－δn2）。 其中，K≈1（m/s）/像素。 SIFT 特征匹配算法屬于內(nèi)插值亞像素級(jí)圖像配準(zhǔn)算法范疇，文獻(xiàn)[8]和文獻(xiàn)[9]提到該類(lèi)型算法配準(zhǔn)精度可以達(dá)到0.13個(gè)像素，所以速度精度不低于0.2m/s。

3 試驗(yàn)驗(yàn)證

3.1 驗(yàn)證方法

用于試驗(yàn)驗(yàn)證的數(shù)據(jù)來(lái)源于湖北某水域的AUV水下測(cè)試數(shù)據(jù)，水域水深約為100m。該試驗(yàn)采用的是872型側(cè)掃聲納，所生成的聲納圖像清晰，具有很高的參考價(jià)值。由于DVL的數(shù)據(jù)異常普遍發(fā)生在速度較快、航向角變化較大的時(shí)刻，因此單獨(dú)提取出側(cè)向速度較大的一段數(shù)據(jù)進(jìn)行比對(duì)，這樣得出的結(jié)果更具有說(shuō)服力［10-12］。

3.2 數(shù)據(jù)驗(yàn)證

如圖7所示，3條速度曲線均取自相同時(shí)刻，此時(shí)載體側(cè)向速度在逐漸衰減。由圖7（a）可以看到，側(cè)掃聲納圖像提取出的載體側(cè)向速度曲線存在較為明顯的噪聲，幅度大致在±0.1m/s之間。但是本文的研究目標(biāo)在于檢測(cè)和剔除DVL的數(shù)據(jù)粗差，這種幅值不大的噪聲并沒(méi)有影響。在圖7（b）對(duì)應(yīng)的DVL側(cè)向速度中，可以發(fā)現(xiàn)其中有幾處數(shù)值相較于圖7（a）和圖 7（c）存在較為明顯的偏差，下面通過(guò)3個(gè)側(cè)向速度之間的誤差曲線進(jìn)行驗(yàn)證。

如圖8（a）所示，側(cè)掃聲納提取出的載體側(cè)向速度誤差在0.2m/s以內(nèi)，符合理論誤差的推算；圖8（b）所示的曲線顯示側(cè)掃聲納圖像提取出的側(cè)向速度與DVL側(cè)向速度的差值曲線存在幾處數(shù)值較大的點(diǎn)。將這些差值大于0.2m/s的DVL數(shù)據(jù)作為需要檢測(cè)的粗差，檢測(cè)粗差后的DVL數(shù)據(jù)誤差曲線如圖9和圖10所示。從圖像中可以看，檢測(cè)粗差后的DVL數(shù)據(jù)誤差與原始誤差基本保持一致，但是明顯檢測(cè)出了大于0.2m/s的誤差。也就是說(shuō)采用這種DVL粗差檢測(cè)的方法，能夠有效地將DVL側(cè)向速度數(shù)據(jù)中大于0.2m/s的粗差檢測(cè)出來(lái)。

從圖9、圖10可以看到，剔除與側(cè)向速度相對(duì)應(yīng)的粗差數(shù)據(jù)后的DVL前向速度誤差有了明顯的減小，雖然沒(méi)有達(dá)到確定的指標(biāo)，但是較大偏差都明顯減少。這說(shuō)明，該檢測(cè)方法對(duì)于DVL前向速度誤差改善有明顯效果。

驗(yàn)證了3個(gè)側(cè)掃聲納數(shù)據(jù)文件共15km水底數(shù)據(jù)，總體效果良好，比較穩(wěn)定。DVL側(cè)向速度數(shù)據(jù)誤差能夠檢測(cè)出0.2m/s粗差的水平，DVL前向速度偏差能得到明顯的改善。

4 結(jié)論

本文提出的基于側(cè)掃聲納圖像估計(jì)得到的載體側(cè)向速度的算法，經(jīng)過(guò)實(shí)際實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證具有較高的精度。它可以有效地檢測(cè)和剔除Doppler測(cè)速儀的粗差，從而改善組合導(dǎo)航性能。但也存在一些需要進(jìn)一步解決的問(wèn)題，如基于側(cè)掃聲納圖像估計(jì)提取出的側(cè)向速度信息仍存在較為明顯的噪聲等，今后將繼續(xù)在減小速度噪聲方面開(kāi)展研究。