專利名稱：機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法
技術(shù)領(lǐng)域：
本發(fā)明屬于一種圖像數(shù)據(jù)處理方法，尤其涉及一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償 方法。
背景技術(shù)：
自主式水下機(jī)器人(Autonomous Underwater Vehicles,簡(jiǎn)稱AUV)要實(shí)現(xiàn)真正的自主 導(dǎo)航，很大一部分取決于它們根據(jù)所經(jīng)過路線建立精確地圖以及自身定位的能力。這是近年 來AUV研究領(lǐng)域的焦點(diǎn)問題，基于此人們提出了很多優(yōu)秀的導(dǎo)航和定位方法，例如知名的"同 時(shí)定位與地圖構(gòu)建方法"簡(jiǎn)稱SLAM方法。簡(jiǎn)單的說，導(dǎo)航的成功一般要包括以下四部分感 知，機(jī)器人必須能解釋它的傳感器信息，提取有意義的數(shù)據(jù)；定位，機(jī)器人必須確定它在環(huán) 境中的位置；認(rèn)知，機(jī)器人必須決定如何行動(dòng)以達(dá)到目標(biāo)；運(yùn)動(dòng)控制，機(jī)器人必須調(diào)節(jié)它的 運(yùn)動(dòng)姿態(tài)，以實(shí)現(xiàn)期望的運(yùn)動(dòng)軌跡。不論采用哪種導(dǎo)航方法，獲取AUV所在環(huán)境的背景信息 都是不可或缺的一步，它是實(shí)現(xiàn)機(jī)器人精確定位以及運(yùn)動(dòng)控制的前提。常用的采集環(huán)境背景 信息的傳感器有激光掃描儀、照相機(jī)和聲吶等。但是在水下環(huán)境中，使用激光是不可行的； 使用照相機(jī)也存在著很多困難，其中一項(xiàng)就是受照明條件的限制。聲吶用于水下系統(tǒng)中己經(jīng) 有很長(zhǎng)一段時(shí)間了，尤其是用于AUV上，目前最常用的是低成本的機(jī)械掃描式成像聲吶。但 是由于聲吶角度的不確定性，其提供的距離信息要比激光掃描等設(shè)備提供的信息更難以解釋； 而且，自主導(dǎo)航與定位方法需要機(jī)器人周圍的環(huán)境特征作為輸入，經(jīng)處理后更新機(jī)器人的位 姿， 一般以聲吶掃描一周獲得的點(diǎn)特征為一個(gè)處理周期，聲吶每發(fā)射一次聲波獲得一次回波 數(shù)據(jù)，當(dāng)聲波遇到障礙物時(shí)反映在回波數(shù)據(jù)中為高幅度值，經(jīng)過點(diǎn)特征提取算法提取出需要 的點(diǎn)特征位置，聲吶掃描一周后就獲得了一幅機(jī)器人周圍環(huán)境的特征地圖，機(jī)器人的位置如 果靜止不動(dòng)，按照上述思路可畫出其周圍環(huán)境的2D圖像，但實(shí)際上機(jī)器人是時(shí)刻運(yùn)動(dòng)的，機(jī) 械掃描式成像聲吶的慢速掃描又需要一定的時(shí)間，機(jī)器人的位置并非固定在同一位置上，再 用同樣的方法畫出一周的聲吶圖像就會(huì)發(fā)生失真，用帶有失真的聲吶圖像供導(dǎo)航方法使用， 會(huì)導(dǎo)致機(jī)器人的定位誤差積累越來越大，從而減弱了 AUV導(dǎo)航與定位的能力。目前，關(guān)于如 何補(bǔ)償機(jī)械掃描聲吶的運(yùn)動(dòng)導(dǎo)致的圖像失真的方法非常少見。

發(fā)明內(nèi)容
本發(fā)明的目的在于為實(shí)現(xiàn)自主導(dǎo)航與定位的水下機(jī)器人提供不失真的和實(shí)時(shí)的環(huán)境目標(biāo) 特征，提出了一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法。本發(fā)明是采用以下的技術(shù)方案實(shí)現(xiàn)的 一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法， 其中該方法包括以下步驟-
(1) 利用機(jī)械掃描式成像聲吶獲取數(shù)據(jù)成像聲吶控制系統(tǒng)每發(fā)射一次波束，就會(huì)返回一 Ping回波數(shù)據(jù)，計(jì)算各項(xiàng)有關(guān)障礙物目標(biāo)的參數(shù)，聲吶串口返回的數(shù)據(jù)包括聲吶此時(shí)的各種 狀態(tài)參數(shù)和該波束采用的回波強(qiáng)度值，并建立容量為360°掃描范圍的數(shù)據(jù)緩沖區(qū)來保存聲 吶串口送來的每Ping數(shù)據(jù)；
(2) 計(jì)算機(jī)器人坐標(biāo)及插值通過測(cè)量傳感器設(shè)備采集的數(shù)據(jù)計(jì)算出機(jī)器人的位移，并以測(cè) 量傳感器與聲吶工作頻率的關(guān)系對(duì)測(cè)量傳感器采集的數(shù)據(jù)做插值，當(dāng)聲吶每個(gè)Ping到來時(shí)取 出當(dāng)前時(shí)刻測(cè)量傳感器計(jì)算出的機(jī)器人位移值；
(3) 失真補(bǔ)償以每個(gè)Ping為單位，加入當(dāng)前時(shí)刻由測(cè)量傳感器設(shè)備計(jì)算出的機(jī)器人位移 值和旋轉(zhuǎn)角度來校正障礙物目標(biāo)的位置；
(4) 點(diǎn)特征提取以聲吶掃描一周為單位取得一幅失真補(bǔ)償后的目標(biāo)特征圖像，經(jīng)過特征提 取之后的點(diǎn)特征圖像送入數(shù)據(jù)關(guān)聯(lián)部分和核心的導(dǎo)航算法處理部分進(jìn)行處理；
(5) 機(jī)器人位姿估計(jì)系統(tǒng)處理并反饋導(dǎo)航與定位算法的核心部分對(duì)機(jī)器人的位置進(jìn)行估計(jì) 和更新，其輸出的機(jī)器人位姿反饋到失真校正算法之前，作為一次輸入對(duì)機(jī)器人的位置進(jìn)行 修正，形成一個(gè)閉合循環(huán)過程。
本發(fā)明中，所述的點(diǎn)特征提取方法包括以下步驟
(1) 對(duì)每個(gè)Ping的聲吶數(shù)據(jù)做闕值分割，剔除表示非障礙物和噪聲的Bins;
(2) 在經(jīng)過閾值分割的Bins中尋找強(qiáng)度最大的Bin,以它為中心做稀疏化，凡是在它周圍 一定距離之內(nèi)的點(diǎn)都被去掉，只保留該點(diǎn)特征和未被去掉的點(diǎn)；
(3) 每個(gè)Ping的稀疏化如上進(jìn)行，當(dāng)掃描完一周之后，對(duì)一周之內(nèi)剩余的點(diǎn)再做稀疏化， 簇?fù)碓谝粋€(gè)圓周范圍之內(nèi)的點(diǎn)簇只保留強(qiáng)度最大的點(diǎn)，以此類推，形成最精煉和最具代表性 的本周的點(diǎn)特征圖像。
所述的測(cè)量傳感器可以采用多普勒速度計(jì)(Do卯ler Velocity Log，簡(jiǎn)稱DVL)、數(shù)字羅 盤、陀螺儀等。
本發(fā)明以機(jī)械掃描式成像聲吶為主傳感器負(fù)責(zé)采集機(jī)器人周圍環(huán)境的特征信息和創(chuàng)建環(huán) 境的聲學(xué)圖像；以dvl、數(shù)字羅盤、陀螺儀等為主要的測(cè)量傳感器負(fù)責(zé)獲取機(jī)器人運(yùn)動(dòng)的相 關(guān)數(shù)據(jù)和輔助定位信息；以實(shí)時(shí)修正點(diǎn)特征位置為主要方法，能夠很好地校正由機(jī)器人運(yùn)動(dòng) 導(dǎo)致的聲學(xué)圖像的失真，為機(jī)器人導(dǎo)航定位提供更為精確的目標(biāo)特征。
本發(fā)明的有益效果是本發(fā)明實(shí)現(xiàn)了一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法，用于以自主導(dǎo)航與定位為目標(biāo)的水下自主機(jī)器人上，保證機(jī)器人能精確地感知它在環(huán)境中的位
置，從而完成對(duì)其自身運(yùn)動(dòng)姿態(tài)的控制，以實(shí)現(xiàn)期望的運(yùn)動(dòng)軌跡。


圖1為機(jī)器人導(dǎo)航系統(tǒng)的工作流程圖； 圖2為機(jī)械掃描式成像聲吶失真補(bǔ)償算法的流程圖； 圖3為聲吶掃描一個(gè)Ping的示意圖； 圖4為聲吶串口返回的一個(gè)Ping數(shù)據(jù)格式； 圖5為失真補(bǔ)償方法原理圖。
具體實(shí)施例方式
本發(fā)明中，所述的自主導(dǎo)航方法以SL細(xì)為例，其執(zhí)行過程大體可以分為三個(gè)階段預(yù)測(cè)， 觀測(cè)和更新。首先將機(jī)器人姿態(tài)和點(diǎn)特征存儲(chǔ)在一個(gè)獨(dú)立的狀態(tài)向量中，然后通過先預(yù)測(cè)再 觀測(cè)的迭代遞推過程來估計(jì)系統(tǒng)狀態(tài)，實(shí)現(xiàn)對(duì)機(jī)器人的定位和特征地圖的創(chuàng)建。預(yù)測(cè)階段實(shí) 際上執(zhí)行的是通常意義下的航位推算方法，造成機(jī)器人狀態(tài)估計(jì)誤差較大；觀測(cè)階段獲得特 征值，如果第一次觀測(cè)到該特征，將其添加到狀態(tài)向量中；如果該特征已存儲(chǔ)在狀態(tài)向量中， 更新AUV和地圖中己有特征的值。本發(fā)明所采用的運(yùn)動(dòng)失真補(bǔ)償方法是在獲取到一個(gè)Ping的 回波數(shù)據(jù)后，根據(jù)此時(shí)DVL、數(shù)字羅盤、陀螺儀等所測(cè)的機(jī)器人運(yùn)動(dòng)狀態(tài)數(shù)據(jù)推算出機(jī)器人 的位移對(duì)各個(gè)障礙物點(diǎn)的坐標(biāo)進(jìn)行修正，再進(jìn)行點(diǎn)特征提取，當(dāng)一周掃描完成所有點(diǎn)特征的 位置都得到修正。這時(shí)畫出的一周點(diǎn)特征圖像就是不失真的聲吶圖像，可供給導(dǎo)航算法作為 輸入使用。圖1體現(xiàn)了本失真補(bǔ)償方法在整個(gè)導(dǎo)航系統(tǒng)中的位置。
該運(yùn)動(dòng)失真補(bǔ)償方法主要包括五個(gè)步驟，下面結(jié)合圖2對(duì)該方法做進(jìn)一步說明
第一步，利用機(jī)械掃描式成像聲吶獲取數(shù)據(jù)本發(fā)明中，機(jī)械掃描式成像聲吶1的發(fā)射基陣 以步進(jìn)方式旋轉(zhuǎn)，聲吶控制系統(tǒng)每發(fā)出一個(gè)旋轉(zhuǎn)指令，聲吶頭的波束指向開始順時(shí)針或逆時(shí) 針轉(zhuǎn)動(dòng)一個(gè)步進(jìn)角度，以一定的垂直開角和水平開角向探測(cè)區(qū)域發(fā)射一次聲脈沖，并在新的 方位上暫停片刻，以接收回波數(shù)據(jù)。由于暫停時(shí)間相對(duì)于聲吶慢速掃描一周所需時(shí)間的比例 非常小，因此可以忽略暫停時(shí)間對(duì)聲吶掃描帶來的影響。當(dāng)聲波遇到障礙物時(shí)反映在回波數(shù) 據(jù)中為高幅度值，如圖3所示，聲波遇到障礙物2時(shí)，對(duì)應(yīng)回波位置為高幅度值。接收到回 波后，聲吶頭再旋轉(zhuǎn)同樣的角度，繼續(xù)如上的過程。聲吶串口返回的數(shù)據(jù)中包括聲吶此時(shí)的 各種狀態(tài)參數(shù)和該波束采樣的回波強(qiáng)度值，這些強(qiáng)度值離散成一套灰度值Bins。如圖4所示， 前45個(gè)字節(jié)為與聲吶設(shè)置和掃描目標(biāo)相關(guān)的各種狀態(tài)參數(shù)，除去作為結(jié)尾標(biāo)志的最后一個(gè)字 節(jié)的后面所有字節(jié)就是這一 Ping回波數(shù)據(jù)中的灰度值Bins,其大小表示是否掃描到了障礙物。雖然聲吶可以檢測(cè)三維物體的存在，卻不能在平面上確定它們的位置，因此聲吶產(chǎn)生的 是水下環(huán)境的二維平面圖像。導(dǎo)航定位方法一般以獲得一周的聲吶圖像作為一次完整的輸入 項(xiàng)，因此，建立了容量為360。掃描范圍的數(shù)據(jù)緩沖區(qū)來保存聲吶串口送來的每Ping數(shù)據(jù)， 等待接下來的失真補(bǔ)償時(shí)使用。
第二步，利用測(cè)量傳感器設(shè)備計(jì)算機(jī)器人坐標(biāo)及插值A(chǔ)UV上常用的測(cè)量傳感器設(shè)備有加 速度計(jì)、數(shù)字羅盤和陀螺儀等；較先進(jìn)的AUV采用DVL，它可以直接測(cè)出AUV相對(duì)于海底的3D 速度而無需積分，本實(shí)施例中采用DVL作為測(cè)量傳感器。DVL以一定的采集頻率fd測(cè)量一次機(jī) 器人的運(yùn)動(dòng)狀態(tài)，包括當(dāng)前時(shí)間、機(jī)器人在三維坐標(biāo)系中X、 Y、 Z方向上相對(duì)水流的三個(gè)速度 和相對(duì)海底的三個(gè)速度，在三維坐標(biāo)系中X、 Y、 Z方向的三個(gè)旋轉(zhuǎn)角度，水溫等等，并根據(jù)每 次測(cè)量得到的速度值乘以時(shí)間得到機(jī)器人的位移。由于DVL的采集頻率fd和聲吶的工作頻率f, 不一樣，后者的頻率高，因此為了保證每個(gè)Ping都能夠使用由DVL測(cè)量并計(jì)算出的機(jī)器人位移， 需要根據(jù)兩者頻率的關(guān)系對(duì)DVL數(shù)據(jù)做插值即每獲得一次DVL數(shù)據(jù)，按照^/fd的關(guān)系由速度 乘時(shí)間補(bǔ)上相應(yīng)個(gè)數(shù)的機(jī)器人位移值。這樣當(dāng)聲吶每個(gè)Ping到來時(shí)就可以取一次當(dāng)前時(shí)刻的 DVL算出的機(jī)器人位移值。
由于通過聲吶獲取的是平面環(huán)境信息，所以機(jī)器人的旋轉(zhuǎn)角度只需要Z方向的地磁偏角。 同樣的，地磁偏角可根據(jù)兩次數(shù)據(jù)之間的差值做線性平均，取得機(jī)器人旋轉(zhuǎn)角度的插值數(shù)據(jù)， 從而也可以保證聲吶每Ping數(shù)據(jù)都有對(duì)應(yīng)的機(jī)器人旋轉(zhuǎn)角度數(shù)據(jù)，供下一步失真補(bǔ)償使用。
第三步，失真補(bǔ)償在某一時(shí)刻從數(shù)據(jù)緩沖區(qū)中獲得一個(gè)Ping，從如圖4所示的聲吶數(shù)據(jù) 包中取出各個(gè)有用的參數(shù)進(jìn)行計(jì)算，其中最重要的是當(dāng)前Ping的角度。接著加入當(dāng)前時(shí)刻由 DVL數(shù)據(jù)計(jì)算出的機(jī)器人位移值和旋轉(zhuǎn)角度來校正障礙物目標(biāo)的位置，原理如圖5所示，圖中 上標(biāo)B表示全局坐標(biāo)系，S表示機(jī)器人即聲吶自身的坐標(biāo)系，下標(biāo)R表示機(jī)器人的坐標(biāo)，0表示 機(jī)器人周圍環(huán)境中目標(biāo)特征的坐標(biāo)，0f為機(jī)器人坐標(biāo)系中聲吶掃描的目標(biāo)物的角度，由聲吶 設(shè)備提供，《,為全局坐標(biāo)系與機(jī)器人坐標(biāo)系的夾角，該值由DVL等設(shè)備提供的地磁偏角獲得， 失真補(bǔ)償過程如下所述。
在聲吶掃描每一周的起始時(shí)刻建立全局坐標(biāo)系，在全局坐標(biāo)系下機(jī)器人的初始位置為(O， 0)，聲吶掃描一個(gè)Ping的時(shí)間后機(jī)器人運(yùn)動(dòng)到了位置04,W)，該坐標(biāo)由DVL數(shù)據(jù)計(jì)算出的機(jī) 器人位移值提供；機(jī)器人相比初始時(shí)刻的旋轉(zhuǎn)角度為^s，該角度由DVL數(shù)據(jù)計(jì)算出的機(jī)器人 旋轉(zhuǎn)角度提供。此時(shí)在機(jī)器人自身的坐標(biāo)系中，距離機(jī)器人即聲吶p,'的位置(《,《)上測(cè)到 障礙物目標(biāo)。沒有加入失真補(bǔ)償?shù)姆椒óa(chǎn)生一周的聲吶圖像，認(rèn)為在這一周之內(nèi)機(jī)器人一直 固定在(0， 0)上而且沒有角度的轉(zhuǎn)動(dòng)，再利用此坐標(biāo)和距離產(chǎn)生的圖像。但是實(shí)際上機(jī)器人的位置和角度都產(chǎn)生了變化，顯^利用此坐標(biāo)和距離畫圖肯定會(huì)失真。有鑒于此，要將機(jī) 器人的位移和角度變動(dòng)考慮進(jìn)去，將機(jī)器人坐標(biāo)系中的障礙物坐標(biāo)轉(zhuǎn)化為全局坐標(biāo)系中的坐 標(biāo)，轉(zhuǎn)化公式如下所述
其中i為每個(gè)Bin在這一 Ping中的位置。以此類推，聲吶每掃描一個(gè)Ping，將掃描到的障 礙物的坐標(biāo)根據(jù)上述公式算出它的失真補(bǔ)償后的位置。掃描完一周之后，畫出這一周的聲吶 圖像，就是不失真的圖像。
第四步，點(diǎn)特征提取加入校正之后的聲吶圖像含有的障礙物信息十分龐大，而導(dǎo)航定 位需要最精確和最有代表性的特征，因此必.須要經(jīng)過特征提取這一步， 一般的提取特征包括 點(diǎn)特征、線特征、橢圓特征等。以自主導(dǎo)航為目標(biāo)的AUV在深海中作業(yè)時(shí)最常見的障礙物目標(biāo) 是點(diǎn)特征，所以本發(fā)明使用點(diǎn)特征提取方法，該方法主要采取以下三個(gè)步驟-
(1) 對(duì)每個(gè)Ping的聲吶數(shù)據(jù)做閾值分割，剔除表示非障礙物和噪聲的Bins;
(2) 在經(jīng)過閾值分割的Bins中尋找強(qiáng)度最大的Bin，以它為中心做稀疏化，凡是在它周圍一 定距離之內(nèi)的點(diǎn)都被去掉，只保留該點(diǎn)特征和未被去掉的點(diǎn)；
(3) 每個(gè)Ping的稀疏化如上進(jìn)行，當(dāng)掃描完一周之后，對(duì)一周之內(nèi)剩余的點(diǎn)再做稀疏化，簇 擁在一個(gè)圓周范圍之內(nèi)的點(diǎn)簇只保留強(qiáng)度最大的點(diǎn)，以此類推，形成最精煉和最具代表性的 本周的點(diǎn)特征圖像， '
經(jīng)過特征提取之后的點(diǎn)特征圖像就可以送入數(shù)據(jù)關(guān)聯(lián)部分和核心的導(dǎo)航算法部分進(jìn)行處 理了。
第五步，機(jī)器人位姿估計(jì)系統(tǒng)處理并反饋導(dǎo)航定位算法的核心部分就是對(duì)機(jī)器人的位 姿進(jìn)行估計(jì)和更新，因此其輸出的機(jī)器人位姿可以反饋到失真校正算法之前作為一次輸入對(duì) 機(jī)器人的位置進(jìn)行修正，構(gòu)成一個(gè)循環(huán)。
權(quán)利要求
1、一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法，其特征在于該方法包括以下步驟(1)利用機(jī)械掃描式成像聲吶獲取數(shù)據(jù)成像聲吶控制系統(tǒng)每發(fā)射一次波束，就會(huì)返回一Ping回波數(shù)據(jù)，計(jì)算各項(xiàng)有關(guān)障礙物目標(biāo)的參數(shù)，聲吶串口返回的數(shù)據(jù)包括聲吶此時(shí)的各種狀態(tài)參數(shù)和該波束采用的回波強(qiáng)度值，并建立容量為360°掃描范圍的數(shù)據(jù)緩沖區(qū)來保存聲吶串口送來的每Ping數(shù)據(jù)；(2)計(jì)算機(jī)器人坐標(biāo)及插值通過測(cè)量傳感器設(shè)備采集的數(shù)據(jù)計(jì)算出機(jī)器人的位移，并以測(cè)量傳感器與聲吶工作頻率的關(guān)系對(duì)測(cè)量傳感器采集的數(shù)據(jù)做插值，當(dāng)聲吶每個(gè)Ping到來時(shí)取出當(dāng)前時(shí)刻測(cè)量傳感器計(jì)算出的機(jī)器人位移值；(3)失真補(bǔ)償以每個(gè)Ping為單位，加入當(dāng)前時(shí)刻由測(cè)量傳感器設(shè)備計(jì)算出的機(jī)器人位移值和旋轉(zhuǎn)角度來校正障礙物目標(biāo)的位置；(4)點(diǎn)特征提取以聲吶掃描一周為單位取得一幅失真補(bǔ)償后的目標(biāo)特征圖像，經(jīng)過特征提取之后的點(diǎn)特征圖像送入數(shù)據(jù)關(guān)聯(lián)部分和核心的導(dǎo)航算法處理部分進(jìn)行處理；(5)機(jī)器人位姿估計(jì)系統(tǒng)處理并反饋導(dǎo)航與定位算法的核心部分對(duì)機(jī)器人的位置進(jìn)行估計(jì)和更新，其輸出的機(jī)器人位姿反饋到失真校正算法之前，作為一次輸入對(duì)機(jī)器人的位置進(jìn)行修正，形成一個(gè)閉合循環(huán)過程。
2、 根據(jù)權(quán)利要求l所述的機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法，其特征在于所述 的測(cè)量傳感器為多普勒速度計(jì)、數(shù)字羅盤、陀螺儀。
3、 根據(jù)權(quán)利要求l所述的機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法，其特征在于所述的 點(diǎn)特征提取方法包括以下步驟(1) 對(duì)每個(gè)Ping的聲吶數(shù)據(jù)做闕值分割，剔除表示非障礙物和噪聲的Bins;(2) 在經(jīng)過閾值分割的Bins中尋找強(qiáng)度最大的Bin,以它為中心做稀疏化，凡是在它周圍 一定距離之內(nèi)的點(diǎn)都被去掉，只保留該點(diǎn)特征和未被去掉的點(diǎn)；(3) 每個(gè)Ping的稀疏化如上進(jìn)行，當(dāng)掃描完一周之后，對(duì)一周之內(nèi)剩余的點(diǎn)再做稀疏化， 簇?fù)碓谝粋€(gè)圓周范圍之內(nèi)的點(diǎn)簇只保留強(qiáng)度最大的點(diǎn)，以此類推，形成最精煉和最具代表性 的本周的點(diǎn)特征圖像。
全文摘要
本發(fā)明屬于一種圖像數(shù)據(jù)處理方法，尤其涉及一種機(jī)械掃描式成像聲吶的運(yùn)動(dòng)失真補(bǔ)償方法。其目的在于為實(shí)現(xiàn)自主導(dǎo)航與定位的水下機(jī)器人提供不失真的和實(shí)時(shí)的環(huán)境目標(biāo)特征。該方法包括以下步驟成像聲吶獲取數(shù)據(jù)、計(jì)算機(jī)器人坐標(biāo)及插值、失真補(bǔ)償、點(diǎn)特征提取、機(jī)器人位姿估計(jì)系統(tǒng)處理并反饋，其用于以自主導(dǎo)航與定位為目標(biāo)的水下自主機(jī)器人上，保證機(jī)器人能精確地感知它在環(huán)境中的位置，從而完成對(duì)其自身運(yùn)動(dòng)姿態(tài)的控制，以實(shí)現(xiàn)期望的運(yùn)動(dòng)軌跡。
文檔編號(hào)G01S7/539GK101672916SQ20091001893
公開日2010年3月17日 申請(qǐng)日期2009年9月17日 優(yōu)先權(quán)日2009年9月17日
發(fā)明者波 何, 沛 宋, 舒 田, 帥 趙, 陳樹娟 申請(qǐng)人:中國(guó)海洋大學(xué)