本發(fā)明涉及一種定位與導(dǎo)航，尤其涉及一種基于地形特征匹配的水下地形匹配導(dǎo)航方法，屬于水下定位與導(dǎo)航技術(shù)。

背景技術(shù)：

水下機(jī)器人由于其成本低廉、可拓展性強(qiáng)、可以完成各種信息采集與利用等任務(wù)深受各國家研究機(jī)構(gòu)重視，由于在水下環(huán)境中，高頻信號(hào)衰減快，所以GPS等常規(guī)定位方式并不適用，由于定位與導(dǎo)航是水下機(jī)器人能否安全返航的重要技術(shù)因素，所以找到可行的定位與導(dǎo)航技術(shù)成為了水下機(jī)器人領(lǐng)域的重點(diǎn)技術(shù)。

現(xiàn)階段水下機(jī)器人導(dǎo)航技術(shù)主要有慣性導(dǎo)航、水聲學(xué)導(dǎo)航、航位推算導(dǎo)航以及地形匹配導(dǎo)航。慣性導(dǎo)航與航位推算導(dǎo)航由于精度有限以及需要定期上浮利用GPS修正等因素受到限制，不利于水下機(jī)器人長期水下作業(yè)，而水聲學(xué)導(dǎo)航需要聲學(xué)基陣，實(shí)際操作過程需要輔助才能完成。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了提供一種基于地形特征匹配的水下地形匹配導(dǎo)航方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：第一步：處理已知的先驗(yàn)地圖，計(jì)算先驗(yàn)地圖中的每一點(diǎn)的高度的梯度通過對(duì)高度的梯度grad(x,y)的大小進(jìn)行篩選，選出梯度的模長||grad(x,y)||＞閾值Α的點(diǎn)作為先驗(yàn)地圖的特征點(diǎn)；

第二步：將第一步得到的所有先驗(yàn)地圖的特征點(diǎn)的梯度的模長||grad(x,y)||做霍夫變換，識(shí)別出每一個(gè)特征的擬合線段，并記錄這些線段的起始點(diǎn)坐標(biāo)位置(x_s,y_s),(x_e,y_e)以及每條線段對(duì)應(yīng)的高度作為實(shí)際特征信息存入實(shí)際特征信息庫；

第三步：啟動(dòng)多波束聲納，采集水下機(jī)器人所在位置及周邊的實(shí)時(shí)水深數(shù)據(jù)，記錄得到的每一個(gè)數(shù)據(jù)點(diǎn)的坐標(biāo)位置(x^*，y^*)以及對(duì)應(yīng)數(shù)據(jù)點(diǎn)的深度h^*；

第四步：計(jì)算第三步中的每一個(gè)數(shù)據(jù)點(diǎn)水深的梯度通過對(duì)水深的梯度grad(x^*,y^*)的大小進(jìn)行篩選，選出梯度的模長||grad(x^*,y^*)||＞Α的點(diǎn)作為多波束聲納數(shù)據(jù)的特征點(diǎn)；

第五步：將第四步得到的多波束聲納數(shù)據(jù)的特征點(diǎn)的梯度的模長||grad(x,y)||做霍夫變換，并識(shí)別出每一個(gè)特征的擬合線段，記錄這些線段的起始點(diǎn)坐標(biāo)位置(x_s,y_s),(x_e,y_e)以及每條線段對(duì)應(yīng)的高度信息、與機(jī)器人的相對(duì)位置信息，作為樣本特征信息；

第六步：逐條讀取樣本特征信息，判斷實(shí)際特征信息庫中是否存在高度相匹配的特征：若存在則進(jìn)行下一步，若不存在，則讀取下一條特征，重復(fù)此步驟；

第七步：判斷經(jīng)第六步得到的特征中是否存在長度相一致的特征，若存在則進(jìn)行下一步，若不存在，則讀取下一條特征，重復(fù)第六步；

第八步：讀取第七步得到的相匹配的特征的端點(diǎn)坐標(biāo)位置(x^*_s,y^*_s),(x^*_e,y^*_e)，找到能將樣本特征端點(diǎn)坐標(biāo)(x_s,y_s),(x_e,y_e)轉(zhuǎn)換為匹配特征端點(diǎn)(x^*_s,y^*_s),(x^*_e,y^*_e)的坐標(biāo)變換；

第九步：根據(jù)第八步中的坐標(biāo)變換以及第五步中與機(jī)器人的相對(duì)位置信息得到機(jī)器人此時(shí)的位置(x₁,y₁)，(x₂,y₂)…(x_n,y_n)；

第十步：根據(jù)第九步得到的位置，得到最終確定的精確坐標(biāo)位置

第十一步：將(x_match,y_match)反饋給主控計(jì)算機(jī)，完成地形匹配定位。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：本發(fā)明使用梯度進(jìn)行判斷，可克服漲落潮差等因素影響，由于只對(duì)篩選出的特征進(jìn)行匹配，很大程度上減小了匹配過程的計(jì)算量。

本發(fā)明通過提取探測(cè)到地形的特征(實(shí)際為較為陡峭的點(diǎn))信息來與已知地形進(jìn)行快速匹配，在不依賴于GPS、初始位置等信息的情況下進(jìn)行相對(duì)快速的地形匹配定位與導(dǎo)航任務(wù)。

附圖說明

圖1為本發(fā)明的流程圖；

圖2為本發(fā)明提取特征點(diǎn)的流程圖；

圖3為本發(fā)明匹配操作的流程圖。

具體實(shí)施方式

下面結(jié)合附圖與具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述。

本發(fā)明涉及的是一種基于地形梯度特征匹配的水下地形匹配方法，包括已知的地形的處理來提取特征點(diǎn)和通過處理探測(cè)到的地形得到特征點(diǎn)并與上一步得到的已知特征點(diǎn)進(jìn)行匹配來確定自身位置。

結(jié)合圖1說明整體的實(shí)現(xiàn)方式，本發(fā)明處理先驗(yàn)地圖數(shù)據(jù)，提取特征點(diǎn)，將特征點(diǎn)的梯度進(jìn)行霍夫變換，得到特征的長度、端點(diǎn)位置，將這些信息存儲(chǔ)為實(shí)際特征點(diǎn)信息庫，用作后續(xù)匹配。將多波束聲納傳回的數(shù)據(jù)進(jìn)行處理，也應(yīng)用算法提取出特征，將這些特征信息進(jìn)行整合，得到樣本特征信息。將樣本特征信息與之前生成的實(shí)際特征點(diǎn)信息庫進(jìn)行匹配，得到對(duì)應(yīng)的位置信息，進(jìn)行處理得到此時(shí)機(jī)器人的精確位置坐標(biāo)。

結(jié)合圖2說明提取特征的過程，首先將先驗(yàn)地圖高度數(shù)據(jù)/探測(cè)到的高度數(shù)據(jù)進(jìn)行處理，由下式計(jì)算出高度的梯度grad(x,y)/grad(x^*,y^*)。

在此過程后判斷高度的梯度大小是否大于閾值A(chǔ)即判斷||grad(x,y)||＞Α，若||grad(x,y)||≤Α則認(rèn)為此點(diǎn)不是特征點(diǎn)，若||grad(x,y)||＞Α，則認(rèn)為此點(diǎn)是特征點(diǎn)，記錄這個(gè)點(diǎn)的梯度等信息。

判斷是否數(shù)據(jù)讀取完畢，若讀取完畢則進(jìn)行下一步，否則讀取下一點(diǎn)數(shù)據(jù)重復(fù)上述步驟。

將所有的特征點(diǎn)的梯度的模長||grad(x,y)||進(jìn)行霍夫變換，得到各個(gè)特征所對(duì)應(yīng)的線段，記錄線段的端點(diǎn)(x_s,y_s),(x_e,y_e)，以及每一條線段所對(duì)應(yīng)位置的高度，得到實(shí)際特征信息存入實(shí)際特征點(diǎn)信息庫/樣本特征點(diǎn)信息。

結(jié)合圖3說明匹配操作的實(shí)現(xiàn)方式，逐條讀取樣本特征信息，判斷實(shí)際特征信息中是否存在高度相匹配的特征，若不存在則讀取下一條數(shù)據(jù)重新判斷；若存在則判斷這些特征中是否存在長度相一致的特征，若相同則認(rèn)為找到相匹配的特征點(diǎn)，讀取端點(diǎn)坐標(biāo)(x^*_s,y^*_s),(x^*_e,y^*_e)進(jìn)行下一步操作，若不存在，則讀取下一條數(shù)據(jù)重新重復(fù)上述步驟。找到能將樣本特征端點(diǎn)坐標(biāo)(x_s,y_s),(x_e,y_e)轉(zhuǎn)換為匹配特征端點(diǎn)(x^*_s,y^*_s),(x^*_e,y^*_e)的旋轉(zhuǎn)變換，據(jù)此得到機(jī)器人此時(shí)的位置(x_match,y_match)。

本發(fā)明的具體流程如下：

1、處理先驗(yàn)地圖，計(jì)算每一點(diǎn)的高度的梯度通過對(duì)高度的梯度grad(x,y)的大小進(jìn)行篩選，選出梯度的模長||grad(x,y)||＞閾值Α的點(diǎn)作為特征點(diǎn)。

2、將這些特征點(diǎn)的梯度的模長||grad(x,y)||做霍夫變換，識(shí)別出每一個(gè)特征的擬合線段，也即得到各個(gè)特征所對(duì)應(yīng)的線段，記錄這些線段的起始點(diǎn)坐標(biāo)(x_s,y_s),(x_e,y_e)，以及每條線段對(duì)應(yīng)的高度信息，作為實(shí)際特征信息存入實(shí)際特征點(diǎn)信息庫。

3、啟動(dòng)多波束聲納，采集水下機(jī)器人所在位置及周邊的實(shí)時(shí)水深數(shù)據(jù)，記錄得到的每一個(gè)數(shù)據(jù)點(diǎn)的位置(x^*，y^*)，以及對(duì)應(yīng)點(diǎn)的深度h^*。

4、計(jì)算每一個(gè)測(cè)量點(diǎn)水深的梯度通過對(duì)水深的梯度grad(x^*,y^*)的大小進(jìn)行篩選，選出梯度的模長||grad(x^*,y^*)||＞Α的點(diǎn)作為特征點(diǎn)。

5、將這些特征點(diǎn)的梯度的模長||grad(x,y)||做霍夫變換，識(shí)別出每一個(gè)特征的擬合線段，記錄這些線段的起始點(diǎn)坐標(biāo)(x_s,y_s),(x_e,y_e)，以及每條線段對(duì)應(yīng)的高度信息、與機(jī)器人的相對(duì)位置信息，作為樣本特征信息。

6、通過對(duì)比特征的大小及對(duì)應(yīng)高度，在實(shí)際特征點(diǎn)信息庫中篩選出與之相匹配的特征，找到兩者的空間位置關(guān)系，也即逐條讀取樣本特征信息，判斷實(shí)際特征信息中是否存在高度相匹配的特征，若存在則進(jìn)行下一步，若不存在，則讀取下一條特征，重復(fù)此步驟。

7、判斷這些特征中是否存在長度相一致的特征，若存在則進(jìn)行下一步，若不存在，則讀取下一條特征，重復(fù)步驟(6)。

8、此時(shí)認(rèn)為找到相匹配的特征，讀取端點(diǎn)坐標(biāo)(x^*_s,y^*_s),(x^*_e,y^*_e)，找到能將樣本特征端點(diǎn)坐標(biāo)(x_s,y_s),(x_e,y_e)轉(zhuǎn)換為匹配特征端點(diǎn)(x^*_s,y^*_s)，(x^*_e,y^*_e)的旋轉(zhuǎn)變換。

9、根據(jù)步驟(8)中的坐標(biāo)變換，以及步驟(5)中特征與機(jī)器人的相對(duì)位置信息，也即根據(jù)樣本特征與實(shí)際特征的空間關(guān)系以及樣本特征與機(jī)器人的相對(duì)位置信息得到機(jī)器人此時(shí)的位置(x₁,y₁)，(x₂,y₂)…(x_n,y_n)。

10、處理這些定位位置，得到最終確定的精確定位點(diǎn)

11、將(x_match,y_match)反饋給主控計(jì)算機(jī)，完成地形匹配定位。

具體的說本發(fā)明的提取給定區(qū)域的特征包括：

(1)計(jì)算每一點(diǎn)的高度的梯度

(2)判斷每一點(diǎn)高度的梯度的模長||grad(x,y)||是否大于給定的值閾值A(chǔ)；

(3)若梯度的模長小于等于閾值A(chǔ)，即||grad(x,y)||≤Α，則此點(diǎn)不是特征點(diǎn)；

(4)若梯度的模長大于閾值A(chǔ)，即||grad(x,y)||＞Α，則此點(diǎn)視為特征點(diǎn)；

(5)將所有的特征點(diǎn)的梯度的模長||grad(x,y)||進(jìn)行霍夫變換，得到各個(gè)特征所對(duì)應(yīng)的線段；

(6)記錄線段的端點(diǎn)(x_s,y_s),(x_e,y_e)，以及每一條線段所對(duì)應(yīng)位置的高度，作為實(shí)際特征信息存入實(shí)際特征點(diǎn)信息庫/樣本特征點(diǎn)信息用來匹配。