專(zhuān)利名稱(chēng)：集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明涉及自主式水下機(jī)器人(Autonomous Underwater Vehicle,AUV)導(dǎo)航技術(shù) 以及聲納微導(dǎo)航技術(shù)，屬于海洋工程領(lǐng)域。
背景技術(shù)：
自主式水下機(jī)器人是目前海洋工程領(lǐng)域技術(shù)發(fā)展的熱點(diǎn)，在水下環(huán)境監(jiān)測(cè)、近海 石油工程作業(yè)、水下搜索與測(cè)繪、以及水雷對(duì)抗和實(shí)時(shí)戰(zhàn)區(qū)警戒等軍事領(lǐng)域獲得越來(lái)越廣 泛的應(yīng)用。導(dǎo)航技術(shù)是實(shí)現(xiàn)AUV自主航行的關(guān)鍵，而AUV所具有的工作時(shí)間長(zhǎng)、環(huán)境復(fù)雜、 信息源少、隱蔽性要求高等特點(diǎn)，給穩(wěn)定、精確的導(dǎo)航實(shí)現(xiàn)帶來(lái)很大的挑戰(zhàn)。近年來(lái)國(guó)際上對(duì)于AUV導(dǎo)航技術(shù)的研究一直非常活躍。水下導(dǎo)航技術(shù)通常需要在 精度、操作距離和自動(dòng)化程度等之間進(jìn)行平衡，概括起來(lái)主要有以下幾種類(lèi)型航位推算與 慣性導(dǎo)航、聲學(xué)信標(biāo)導(dǎo)航、及地球物理導(dǎo)航等。充分兌現(xiàn)AUV移動(dòng)性帶來(lái)的好處，需要獲取精確的AUV位置和姿態(tài)信息。而在這 些位置信息不具備的條件下，可通過(guò)自身測(cè)量數(shù)據(jù)之間的關(guān)聯(lián)特性來(lái)修正載體運(yùn)動(dòng)引起的 誤差，如形成位移相位中心陣(Displaced Phase-CenterAntenna-DPCA)，利用相鄰發(fā)射重 合相位中心的相關(guān)特性進(jìn)行運(yùn)動(dòng)補(bǔ)償。其處理過(guò)程反過(guò)來(lái)即可用于獲取載體位置信息，基 于相鄰位置聲相干處理的微導(dǎo)航技術(shù)因此發(fā)展起來(lái)?，F(xiàn)有的水下機(jī)器人通常利用慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS) 采集的姿態(tài)及加速度信息或磁羅經(jīng)(Magnetic Compass, MC)采集姿態(tài)信息，以及多普勒記 程儀(Doppler Velocity Log, DVL)采集的速度信息進(jìn)行組合導(dǎo)航。但是由于上述姿態(tài)和 速度數(shù)據(jù)存在一定誤差，這些誤差會(huì)在組合導(dǎo)航的過(guò)程中隨時(shí)間積累，在AUV航行一定距 離之后，AUV位置誤差將積累到不可接受的程度。而研究結(jié)果表明，基于聲納處理的微導(dǎo)航 技術(shù)對(duì)于某些運(yùn)動(dòng)參數(shù)如橫向速度、前向速度可提供比常規(guī)慣性導(dǎo)航技術(shù)高一到兩個(gè)數(shù)量 級(jí)的測(cè)量精度，而對(duì)另一些參數(shù)如航偏角的估計(jì)又不如常規(guī)慣性導(dǎo)航設(shè)備精確。因此，將聲 納微導(dǎo)航獲取的高精度速度測(cè)量信息與現(xiàn)有的導(dǎo)航設(shè)備記錄的速度和姿態(tài)信息進(jìn)行融合， 可顯著提高AUV整體導(dǎo)航的精度。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，以 提高對(duì)裝載聲納的自主式水下機(jī)器人的導(dǎo)航精確度。為實(shí)現(xiàn)上述目的，本發(fā)明所采取的技術(shù)方案是該集成聲納微導(dǎo)航的自主式水下 機(jī)器人組合導(dǎo)航方法主要包括如下步驟(1)利用全球定位系統(tǒng)獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，利用傳感器 采集得到自主式水下機(jī)器人的速度信息和姿態(tài)信息，并利用聲納系統(tǒng)估計(jì)自主式水下機(jī)器 人的速度信息和/或姿態(tài)信息；(2)利用數(shù)據(jù)融合方法將傳感器采集到的所述速度信息和姿態(tài)信息以及聲納系統(tǒng)估計(jì)的所述速度信息和/或姿態(tài)信息進(jìn)行融合，并結(jié)合全球定位系統(tǒng)所獲取自主式水下機(jī) 器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì)位置信息和姿態(tài)信息。進(jìn)一步地，本發(fā)明傳感器采集到的所述速度信息為橫偏速度信息、前進(jìn)速度信息、 垂直速度信息中的任一種或任幾種。進(jìn)一步地，本發(fā)明傳感器采集到的所述姿態(tài)信息包括航偏角信息和航偏角速度信 肩、O進(jìn)一步地，本發(fā)明傳感器采集到的所述姿態(tài)信息還包括橫滾信息和/或縱傾信 肩、ο進(jìn)一步地，本發(fā)明聲納系統(tǒng)估計(jì)的所述速度信息包括橫偏速度信息。進(jìn)一步地，本發(fā)明聲納系統(tǒng)估計(jì)的所述速度信息還包括前進(jìn)速度信息和/或垂直 速度信息。進(jìn)一步地，本發(fā)明聲納系統(tǒng)估計(jì)的所述姿態(tài)信息包括航偏角信息。進(jìn)一步地，本發(fā)明聲納系統(tǒng)估計(jì)的所述姿態(tài)信息還包括橫滾信息和/或縱傾信 肩、ο進(jìn)一步地，本發(fā)明全球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息為 經(jīng)度信息和緯度信息。與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是通過(guò)數(shù)據(jù)融合方法，融合AUV載聲納估計(jì) 的速度和/或姿態(tài)信息，以及多種傳感器采集的速度和姿態(tài)信息，估計(jì)AUV絕對(duì)位置，提高 自主式水下機(jī)器人的導(dǎo)航精度。


圖1是本發(fā)明組合導(dǎo)航方法的工作示意圖。圖2是相位中心近似的幾何示意圖。圖3是等效相位中心陣示意圖。其中，1為發(fā)射陣元，2為接收陣，3為經(jīng)過(guò)相位中 心等效后的DPCA陣。圖4是利用DPCA估計(jì)AUV相鄰脈沖間橫向位移及航偏角示意圖。其中4為某次 脈沖發(fā)射時(shí)刻DPCA陣，5為相鄰次脈沖發(fā)射時(shí)刻DPCA陣，6表示相鄰脈沖間形成的重合相 位中心對(duì)。圖5 (a)是存在四對(duì)重合相位中心對(duì)時(shí)，利用DPCA微導(dǎo)航估計(jì)AUV航行時(shí)的橫偏 位移的仿真結(jié)果。其中星號(hào)線7表示利用DPCA估計(jì)出的AUV橫偏位移，實(shí)線8表示AUV真 實(shí)橫偏位移。圖5 (b)是存在四對(duì)重合相位中心對(duì)時(shí)，利用DPCA微導(dǎo)航估計(jì)AUV航行時(shí)的航偏 角的仿真結(jié)果。其中星號(hào)線9表示利用DPCA估計(jì)出的AUV航偏角，實(shí)線10表示AUV真實(shí) 航偏角。圖6是利用圖5中的橫偏位移和航偏角推算AUV軌跡仿真結(jié)果其中，a :DPCA估計(jì)值與實(shí)際值比較，星號(hào)線11表示利用DPCA估計(jì)出的AUV運(yùn)動(dòng) 軌跡，實(shí)線12表示AUV真實(shí)運(yùn)動(dòng)軌跡；b =AUV的估計(jì)軌跡相對(duì)于真實(shí)軌跡的誤差隨AUV前 距離的變化曲線(實(shí)線13)。圖7是本發(fā)明中AUV的一種載聲納系統(tǒng)重合等效相位中心示意圖(空心圓與實(shí)心圓分別表示相鄰脈沖的等效相位中心)。圖8是DPCA+IGC+partial DVL組合導(dǎo)航軌跡推算與DVL+IGC軌跡推算的比較，實(shí) 驗(yàn)數(shù)據(jù)處理結(jié)果。其中虛線14表示沒(méi)有結(jié)合微導(dǎo)航技術(shù)(DVL+IGC)的AUV絕對(duì)位置估計(jì) 結(jié)果，虛線15表示結(jié)合了微導(dǎo)航技術(shù)后(DPCA+IGC+partialDVL)AUV絕對(duì)位置估計(jì)結(jié)果，點(diǎn) 16表示AUV上浮時(shí)刻其上安裝的GPS記錄位置。圖9是DPCA+IGC+partial DVL組合導(dǎo)航軌跡推算與DVL+IGC軌跡推算的比較，實(shí) 驗(yàn)數(shù)據(jù)處理結(jié)果(不同批次的數(shù)據(jù))。其中虛線17表示沒(méi)有結(jié)合微導(dǎo)航技術(shù)(DVL+IGC)的 AUV絕對(duì)位置估計(jì)結(jié)果，虛線18表示結(jié)合了微導(dǎo)航技術(shù)后(DPCA+IGC+partial DVL) AUV絕 對(duì)位置估計(jì)結(jié)果，點(diǎn)19表示AUV上浮時(shí)刻其上安裝的GPS記錄的AUV絕對(duì)位置，星號(hào)線20 表示AUV下潛之前其上安裝的GPS記錄的AUV軌跡。
具體實(shí)施例方式本發(fā)明的組合導(dǎo)航方法的工作過(guò)程如圖1所示。導(dǎo)航數(shù)據(jù)來(lái)源于三種途徑，包括 (1)利用聲納接收數(shù)據(jù)，通過(guò)DPCA方法估計(jì)得到的AUV速度及姿態(tài)角信息；(2) INS記錄的 姿態(tài)信息，或MC記錄的姿態(tài)信息；(3)DVL記錄的AUV速度信息。利用數(shù)據(jù)融合方法對(duì)上述 三類(lèi)信息進(jìn)行融合，得到水下AUV組合導(dǎo)航運(yùn)動(dòng)軌跡，結(jié)合AUV下潛之前水面GPS接收機(jī)位 置信息，可最終估計(jì)AUV任一時(shí)刻的水下絕對(duì)位置。聲納微導(dǎo)航的工作原理如下如圖2所示，采用相位中心近似(Phase CenterApproximation, PCA)，將聲納基陣每個(gè)發(fā)射陣元T點(diǎn)和接收陣元R點(diǎn)聲路徑的時(shí)延 用位于它們幾何中心(相位中心)C點(diǎn)的雙程傳播近似，即由該聲納發(fā)射陣元發(fā)射經(jīng)水底反 射/散射后接收陣元接收信號(hào)的過(guò)程可以等價(jià)近似為發(fā)射陣和接收陣都位于它們相位中 心時(shí)的情況。在AUV航行過(guò)程中，其上搭載的側(cè)視聲納向水底周期性發(fā)射脈沖聲信號(hào)。每 個(gè)發(fā)射脈沖對(duì)應(yīng)構(gòu)成發(fā)射信號(hào)時(shí)刻AUV空間位置處的PCA相位中心陣。如圖3所示，設(shè)陣 元數(shù)為8，應(yīng)用PCA就可以將由一個(gè)發(fā)射陣元和8個(gè)接收陣元組成的均勻線陣，等價(jià)近似為 8個(gè)PCA組成的均勻線陣。通過(guò)設(shè)定AUV航行速度或調(diào)整聲納發(fā)射的發(fā)射周期，就可實(shí)現(xiàn)相 鄰發(fā)射脈沖周期內(nèi)相位中心陣的部分重疊。圖4中相鄰脈沖間存在4對(duì)重合相位中心對(duì)。 設(shè)相鄰脈沖間AUV橫向位移為Y，航偏角為θ，則在相鄰脈沖間AUV航偏角變化很小的條 件下，圖4中各重合相位中心的相對(duì)位移為Cli = γ + θ Xi, i = 1,2,3,4(1)其中PCA陣的陣元橫坐標(biāo)Xi為1-,(2) 、 2 )2式中Δ為接收陣陣元間距，N為陣元個(gè)數(shù)。由于DPCA陣元接收的信號(hào)相干性強(qiáng)， 因此可通過(guò)相關(guān)器估計(jì)相鄰脈沖間重合相位中心對(duì)接收信號(hào)的時(shí)延差^。根據(jù)幾何關(guān)系， 該時(shí)延差與該重合相位中心對(duì)的相對(duì)位移之間滿(mǎn)足Ti = —.(3)
C式(3)與式(1)相結(jié)合，當(dāng)相鄰脈沖間存在至少一對(duì)重合相位中心對(duì)時(shí)，即可估計(jì) AUV相鄰脈沖間的橫向位移γ ；當(dāng)相鄰脈沖間存在至少兩對(duì)重合相位中心對(duì)時(shí)，即可估計(jì)AUV相鄰脈沖間的橫向位移γ和航偏角θ。橫向位移Υ除以脈沖間隔即為AUV在此脈沖 間隔內(nèi)平均速度。當(dāng)相鄰脈沖間存在至少兩對(duì)重合相位中心對(duì)時(shí)，還可以利用水平方向波 束形成的方法將所有重合相位中心對(duì)變成一對(duì)虛擬的重合相位中心對(duì)，進(jìn)而獲得精度較單 對(duì)重合相位中心估計(jì)結(jié)果更高的AUV橫向速度。對(duì)重合相位中心經(jīng)插值處理，可以估計(jì)AUV 在航向上的前進(jìn)速度。如果采用多排水平線陣進(jìn)行垂直維更多相位中心的重疊，還可實(shí)現(xiàn) 對(duì)AUV垂直速度、縱傾角和橫滾角的估計(jì)。本發(fā)明集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法具體如下(I)AUV下潛之前，通過(guò)其上安裝的全球定位系統(tǒng)(GPS)獲取AUV的初始絕對(duì)位置。 全球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息一般為經(jīng)度信息和緯度信息。(2)在AUV航行過(guò)程中，AUV上安裝的傳感器采集AUV的速度和姿態(tài)信息。例如可 使用慣性導(dǎo)航系統(tǒng)或多普勒記程儀采集AUV的速度信息，使用磁羅經(jīng)采集AUV的姿態(tài)信息。(3)利用聲納系統(tǒng)估計(jì)自主式水下機(jī)器人的速度信息和/或姿態(tài)信息。如果聲納系統(tǒng)僅估計(jì)自主式水下機(jī)器人的速度信息，可采用數(shù)據(jù)融合方法融合將 該速度信息與傳感器所采集的AUV的速度和姿態(tài)信息，并結(jié)合全球定位系統(tǒng)所獲取自主式 水下機(jī)器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì)位置信息和姿態(tài)信息。如果聲納系統(tǒng)僅估計(jì)自主式水下機(jī)器人的姿態(tài)信息，可采用數(shù)據(jù)融合方法融合將 該姿態(tài)信息與傳感器所采集的AUV的速度和姿態(tài)信息，并結(jié)合全球定位系統(tǒng)所獲取自主式 水下機(jī)器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì)位置信息和姿態(tài)信息。如果聲納系統(tǒng)同時(shí)估計(jì)自主式水下機(jī)器人的速度信息和姿態(tài)信息，則采用數(shù)據(jù)融 合方法融合將該速度信息和姿態(tài)信息與傳感器所采集的AUV的速度和姿態(tài)信息，并結(jié)合全 球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì) 位置信息和姿態(tài)信息。此方案比聲納系統(tǒng)僅估計(jì)自主式水下機(jī)器人的姿態(tài)信息或速度信息 能夠得到更為精確的AUV的絕對(duì)位置信息和姿態(tài)信息。在本發(fā)明中，傳感器采集到的速度信息為橫偏速度信息、前進(jìn)速度信息、垂直速度 信息中的任一種或任幾種。橫偏速度信息、前進(jìn)速度信息、垂直速度信息分別是三維空間中 三個(gè)正交方向的速度信息。若傳感器采集到的姿態(tài)信息為AUV的航偏角信息和航偏角速度信息，則可用于進(jìn) 行AUV的二維空間的導(dǎo)航。若進(jìn)一步采集AUV的橫滾信息和/或縱傾信息，則可用于實(shí)現(xiàn) AUV的三維空間的導(dǎo)航，并有更佳的導(dǎo)航精度。若聲納系統(tǒng)估計(jì)到AUV的橫偏速度信息，即可用于進(jìn)行AUV的二維空間的導(dǎo)航。若 聲納系統(tǒng)估計(jì)的速度信息還包括前進(jìn)速度信息，則可提高AUV的導(dǎo)航精度。若聲納系統(tǒng)估 計(jì)的速度信息還包括垂直速度信息，則可用于實(shí)現(xiàn)AUV的三維空間的導(dǎo)航，并有更佳的導(dǎo) 航精度。若聲納系統(tǒng)估計(jì)的姿態(tài)信息為航偏角信息，即可用于進(jìn)行AUV的二維空間的導(dǎo) 航。若聲納系統(tǒng)估計(jì)的姿態(tài)信息還包括橫滾信息和/或縱傾信息，則可用于實(shí)現(xiàn)AUV的三 維空間的導(dǎo)航，并有更佳的導(dǎo)航精度。在本發(fā)明方法，一般所融合的速度信息和姿態(tài)信息越多，則AUV的導(dǎo)航精度越高。以下舉例說(shuō)明本發(fā)明所采用的數(shù)據(jù)融合方法。采用數(shù)據(jù)融合方法融合聲納系統(tǒng)和AUV載傳感器獲取的AUV速度和/或姿態(tài)信息，得到AUV在水下運(yùn)動(dòng)軌跡，結(jié)合AUV上安裝的GPS記錄的下潛時(shí)AUV初始絕對(duì)位置，得 到AUV在水下的絕對(duì)位置信息和姿態(tài)信息。由于AUV在水下通常定深航行，且深度維信息 由壓力傳感器獲取，而高度信息可通過(guò)DVL獲得，下文僅給出水平面內(nèi)兩維組合導(dǎo)航例子。
AUV的狀態(tài)向量如公式(4)所示 其中ρχ (η)和py (η)分別是在采樣點(diǎn)η時(shí)刻AUV在χ和y方向的水平位移。vf (η) 和V1 (η)分別是AUV在采樣點(diǎn)η時(shí)刻的真實(shí)前進(jìn)速度和橫向速度。θ (η)是AUV在η時(shí)刻 航偏角(與χ軸的夾角)；ω (η)是η時(shí)刻θ (η)變化的角速度。AUV的觀測(cè)向量如公式(5)所示 其中前進(jìn)速度Vf(n)可由多普勒計(jì)程儀或者聲納系統(tǒng)提供，橫向速度V1 (η)可以由 多普勒計(jì)程儀或者聲納系統(tǒng)提供。航偏角θ (η)可由慣性測(cè)量單元、聲納系統(tǒng)或磁羅經(jīng)提 供，航偏角速度ω (η)由慣性測(cè)量單元提供。
根據(jù)公式⑷和(5)，可以得到如公式(6)所示的狀態(tài)更新方程
x( + l) =
10 00
0 100
0 0 1Δ
0 0 01
0 0 00
0 0 00
cos(^( ))Aisin(爐(《))Δ
-sin(^(n))A<cos(p( ))A/ 0 0 0 0 1 0 0 1
6
x( ) +w
以及如公式(7)所示的觀測(cè)方程 "0 0 0 0 1 0" y( ) =
0 0 0 0 0 1 0 0 1 0 0 0
X(W)+ ’
/"S
7
(
0 0 0 1 0 0其中Δ t為速度采樣間隔，w和ν分別為狀態(tài)更新噪聲和觀測(cè)噪聲。在完成狀態(tài)向量、觀測(cè)向量、狀態(tài)更新方程和觀測(cè)方程定義之后，即可以利用卡爾 曼(Kalman)濾波器估計(jì)AUV的運(yùn)動(dòng)軌跡，結(jié)合AUV上安裝的GPS記錄的AUV下潛時(shí)刻絕對(duì) 位置，就可以得到AUV在水下的絕對(duì)位置。在該例子中只考慮水平面內(nèi)兩維組合導(dǎo)航，本發(fā)明可以實(shí)現(xiàn)AUV的三維導(dǎo)航。但 該例子中的狀態(tài)向量、觀測(cè)向量、狀態(tài)更新方程和觀測(cè)方程定義因視實(shí)際應(yīng)用而做相應(yīng)改變。本發(fā)明可利用目前組合導(dǎo)航中應(yīng)用最普遍的卡爾曼濾波器，一方面因?yàn)樗鼣?shù)值計(jì) 算上的優(yōu)越性，另一方面因?yàn)槠渚哂邢鄬?duì)簡(jiǎn)單和穩(wěn)健的特性。其他數(shù)據(jù)融合方法如例子濾 波器、遺傳算法等也可以作為本導(dǎo)航技術(shù)中的數(shù)據(jù)融合方法。實(shí)施例設(shè)AUV沿直線航行，速度為lm/s。AUV上裝有一排水平接收陣，陣元數(shù)為8，陣元間 距為0. lm。在航行過(guò)程中向海底發(fā)射聲信號(hào)并接收回波，信號(hào)(ping)間隔為0. ls，則相鄰 脈沖間有6個(gè)重合的等效相位中心。根據(jù)相鄰脈沖之間的相關(guān)性利用DPCA算法估計(jì)AUV 橫向速度結(jié)果如圖5所示，可以看出DPCA能夠準(zhǔn)確地估計(jì)出航行體的橫向速度。利用AUV的前進(jìn)速度和DPCA估計(jì)出的橫向速度進(jìn)行軌跡推算，結(jié)果如圖6所示，在400個(gè)脈沖時(shí)間 內(nèi)，AUV運(yùn)動(dòng)了 40米，而估計(jì)誤差在厘米級(jí)別，仿真環(huán)境較理想，下面給出實(shí)際實(shí)施例。
現(xiàn)場(chǎng)實(shí)驗(yàn)中，AUV載聲納系統(tǒng)為多線陣，由三排水聽(tīng)器組成，從上到下每排的水聽(tīng) 器數(shù)分別是6-4-6，間距為0. lm。根據(jù)DPCA導(dǎo)航理論，形成的等效相位中心間距為0. 05m。 在試驗(yàn)中AUV水下速度約為1. 5m/s，脈沖間隔為0. ls，則相鄰脈沖間可以有7個(gè)重合等效 相位中心，如圖7所示。利用上述7個(gè)重合等效相位中心可以估計(jì)相鄰脈沖間AUV的橫向 位移，除以脈沖間隔就可以得到橫向速度。利用DPCA導(dǎo)航估計(jì)得到的AUV橫向速度，結(jié)合 由智能羅經(jīng)儀(Intelligent Gyro Compass, IGC)測(cè)量得到的航偏角數(shù)據(jù)以及DVL的前進(jìn) 速度，可推算出AUV的運(yùn)動(dòng)軌跡。實(shí)驗(yàn)中，AUV水下航行速度為1. 5m/s，定深15m航行，航 行區(qū)域水深大于25m。圖8和圖9給出了利用本發(fā)明集成聲納微導(dǎo)航的自主式水下機(jī)器人 組合導(dǎo)航方法進(jìn)行AUV導(dǎo)航的兩批結(jié)果，并同沒(méi)有融合聲納微導(dǎo)航的AUV導(dǎo)航結(jié)果做比較。 其中虛線15、18表示橫偏速度由聲納系統(tǒng)提供，前進(jìn)速度由DVL提供，航偏角和航偏角速度 由IGC提供，數(shù)據(jù)融合方法為卡爾曼濾波時(shí)的AUV導(dǎo)航結(jié)果(DPCA+IGC+partial DVL)；而 虛線14、17表示前進(jìn)速度及橫向速度都由DVL提供，航偏角和航偏角速度由IGC提供，數(shù)據(jù) 融合方法為卡爾曼濾波時(shí)的AUV導(dǎo)航結(jié)果(DVL+IGC)。星號(hào)線20為GPS接收機(jī)記錄的位 置信息(AUV處于水面)。圖8中AUV從(50m，0m)附近下潛，圖9中AUV從(10m，250m)附 近下潛，兩圖中路徑中間部分處于水下，因此沒(méi)有GPS位置信息。從圖中明顯可以看出，采 用DPCA+IGC+partial DVL估計(jì)出的AUV軌跡比DVL+IGC估計(jì)出的AUV軌跡在AUV上浮時(shí) 刻的絕對(duì)位置更接近其上的GPS獲取的絕對(duì)位置(DVL長(zhǎng)時(shí)精度士0. 4% 士0. 2cm/s)。上 述實(shí)施例表明利用本發(fā)明集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法可顯著提高 組合導(dǎo)航系統(tǒng)精度。
權(quán)利要求
一種集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征是包括如下步驟(1)利用全球定位系統(tǒng)獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，利用傳感器采集得到自主式水下機(jī)器人的速度信息和姿態(tài)信息，并利用聲納系統(tǒng)估計(jì)自主式水下機(jī)器人的速度信息和/或姿態(tài)信息；(2)利用數(shù)據(jù)融合方法將傳感器采集到的所述速度信息和姿態(tài)信息以及聲納系統(tǒng)估計(jì)的所述速度信息和/或姿態(tài)信息進(jìn)行融合，并結(jié)合全球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì)位置信息和姿態(tài)信息。
2.根據(jù)權(quán)利要求1所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是傳感器采集到的所述速度信息為橫偏速度信息、前進(jìn)速度信息、垂直速度信息中的任一 種或任幾種。
3.根據(jù)權(quán)利要求1所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是傳感器采集到的所述姿態(tài)信息包括航偏角信息和航偏角速度信息。
4.根據(jù)權(quán)利要求3所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是傳感器采集到的所述姿態(tài)信息還包括橫滾信息和/或縱傾信息。
5.根據(jù)權(quán)利要求1所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是聲納系統(tǒng)估計(jì)的所述速度信息包括橫偏速度信息。
6.根據(jù)權(quán)利要求5所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是聲納系統(tǒng)估計(jì)的所述速度信息還包括前進(jìn)速度信息和/或垂直速度信息。
7.根據(jù)權(quán)利要求1所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是聲納系統(tǒng)估計(jì)的所述姿態(tài)信息包括航偏角信息。
8.根據(jù)權(quán)利要求7所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是聲納系統(tǒng)估計(jì)的所述姿態(tài)信息還包括橫滾信息和/或縱傾信息。
9.根據(jù)權(quán)利要求1所述的集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，其特征 是全球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息為經(jīng)度信息和緯度信息。
全文摘要
本發(fā)明公開(kāi)一種集成聲納微導(dǎo)航的自主式水下機(jī)器人組合導(dǎo)航方法，主要包括如下步驟(1)利用全球定位系統(tǒng)獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，利用傳感器采集得到自主式水下機(jī)器人的速度信息和姿態(tài)信息，并利用聲納系統(tǒng)估計(jì)自主式水下機(jī)器人的速度信息和/或姿態(tài)信息；(2)利用數(shù)據(jù)融合方法將傳感器采集到的所述速度信息和姿態(tài)信息以及聲納系統(tǒng)估計(jì)的速度信息和/或姿態(tài)信息進(jìn)行融合，并結(jié)合全球定位系統(tǒng)所獲取自主式水下機(jī)器人的初始絕對(duì)位置信息，得到自主式水下機(jī)器人的絕對(duì)位置信息和姿態(tài)信息。本發(fā)明的有益效果是通過(guò)融合AUV載聲納估計(jì)的速度和/或姿態(tài)信息，以及傳感器采集的速度和姿態(tài)信息，估計(jì)AUV絕對(duì)位置，提高AUV的導(dǎo)航精度。
文檔編號(hào)G01S15/88GK101900558SQ201010193618
公開(kāi)日2010年12月1日 申請(qǐng)日期2010年6月4日 優(yōu)先權(quán)日2010年6月4日
發(fā)明者孫鋒, 張博, 徐文, 李建龍 申請(qǐng)人:浙江大學(xué)