基于聲吶輔助自主導(dǎo)航的auv導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及基于聲吶輔助自主導(dǎo)航的AUV導(dǎo)航方法，屬于海底探測技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 目前AUV執(zhí)行測繪任務(wù)時，理想探測軌跡與實際探測軌跡往往存在差異。理論上 按照預(yù)先設(shè)定軌跡可以提供對目標(biāo)海域的100%有效覆蓋，但其中隱含的假設(shè)是航行中任 何一點都滿足有界誤差性能，在實際的調(diào)查任務(wù)中，由于對目標(biāo)區(qū)域中真實海底環(huán)境的特 征分布是一無所知的，倘若移動觀測設(shè)備沒有精準(zhǔn)的定位，就沒有高質(zhì)量高準(zhǔn)確度的海底 地形地貌圖。因此如何提高移動觀測設(shè)備的定位準(zhǔn)確性成為移動探測領(lǐng)域所面臨的技術(shù)焦 點和瓶頸問題。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明的目的在于克服現(xiàn)有AUV導(dǎo)航模塊存在的上述缺陷，提出了一種基于聲吶 輔助自主導(dǎo)航的AUV導(dǎo)航方法，彌補了現(xiàn)有測繪系統(tǒng)理想探測軌跡與實際值存在差異的問 題，本發(fā)明利用iSAM算法（incremental Smoothing and Mapping，遞增平滑與地圖構(gòu)建算 法）運行常數(shù)項步數(shù)的策略，將目標(biāo)區(qū)域進行分塊，使處于局部測繪區(qū)域的AUV對選擇繼續(xù) 探索未知區(qū)域還是回訪之前的海底環(huán)境做出智能化的決策，兼顧海底導(dǎo)航性能和目標(biāo)區(qū)域 覆蓋率兩種競爭訴求，為解決海底運動觀測時定位誤差累計這一關(guān)鍵技術(shù)問題提供了可行 性方案。
[0004] 本發(fā)明所述的基于聲吶輔助自主導(dǎo)航的AUV導(dǎo)航方法，包括如下步驟：
[0005] 步驟一、AUV潛入海底，側(cè)掃聲吶工作，對海底圖像特征進行采集；
[0006] 步驟二、將高維海底聲吶探測圖像數(shù)據(jù)進行降維處理；
[0007] 采用詞袋BoW(Bag of Words)局部特征表達(dá)模型來描繪海底探測圖像，以滿足對 海量觀測信息的快速的反應(yīng)能力；
[0008] 步驟三、分別計算兩側(cè)聲吶圖像的特征顯著率：
[0009] 采用歸一化圖像熵來表示聲吶圖像的特征顯著率，
[0011] 其中I ω I表示聲吶觀測圖像中的局部特征總數(shù)，l〇g2| ω I為特征均勻分布時獲得 的最大熵，P(Wl)為局部特征的先驗概率密度函數(shù)，圖像熵
表 示聲吶探測圖像中特征的多樣性；將上述特征顯著率的計算結(jié)果與自定義閾值進行比較， 產(chǎn)生具有顯著特性的聲吶觀測節(jié)點；
[0012] 步驟四、當(dāng)航行器航行100步時，計算其當(dāng)前航行的不確定度P，并與不確定閾值 P。進行比較：
[0014] 其中I ΣΜΡ|為自航式測繪系統(tǒng)探索時位姿的一步預(yù)測協(xié)方差的模，I sallcJ表示 其位姿允許協(xié)方差的模；如果不確定度大于等于不確定閾值，則導(dǎo)航?jīng)Q策AUV重訪某顯著 性位姿點；如果不確定度小于此不確定閾值，則導(dǎo)航?jīng)Q策AUV繼續(xù)探索。
[0015] 步驟五、AUV根據(jù)步驟四的決策進行探索或回訪：
[0016] 若AUV導(dǎo)航?jīng)Q策判決為回訪，則根據(jù)最終得到回訪點和之前得到的回訪路徑進行 回訪。然后原路返回。將之前探索的區(qū)域標(biāo)記為已探索過的區(qū)域，然后再繼續(xù)進行未知區(qū) 域的探索；若AUV導(dǎo)航?jīng)Q策判決為繼續(xù)探索，則AUV繼續(xù)連續(xù)探索100步后，同時也將之前 探索過的所有區(qū)域，標(biāo)記為已探索過的區(qū)域，然后轉(zhuǎn)向步驟四。
[0017] 進一步地，步驟二中，采用詞袋BoW局部特征表達(dá)模型來描繪海底探測圖像的過 程分為三部分：
[0018] 第一部分，利用SURF (Speeded Up Robust Features,快速魯棒性特征）算法從探 測聲吶圖像中提取海底詞匯向量，這些向量代表的是聲吶圖像中局部不變的特征點；
[0019] 第二部分，將提取到的特征映射到字典樹；
[0020] 第三部分，利用海底聲吶觀測圖像提取的局部特征生成典型樣本，將典型樣本對 應(yīng)不同詞匯；通過此種方法將高維海量海底探測數(shù)據(jù)映射到低維。
[0021] 進一步地，步驟四中顯著性位姿點的選取將由100步運行的不確定度的總和和 100步內(nèi)區(qū)域覆蓋率決定；AUV航行100步，再進行與閾值之間的比對，而不選擇每一步都進 行比較，是因為對于大量的圖像信息的處理不僅需要耗費時間，而且隨著時間的推移，計算 總的位姿不確定度的復(fù)雜度越來越高，因此選擇只對當(dāng)前100步進行一次導(dǎo)航?jīng)Q策，使計 算復(fù)雜度可控。
[0022] 此回訪顯著性位姿點的選取步驟如下：
[0023] a，對步驟三得到的顯著性聲吶觀測節(jié)點進行聚類，得到候選回訪點。
[0024] b，對候選回訪點進行路徑規(guī)劃。
[0025] c，計算到每個候選回訪點的總不確定度禮&
[0026] 候選回訪點的總的不確定度由三部分組成，包括里程計信息矩陣、聲吶信息矩陣 以及iSAM信息矩陣。
[0027] d，計算子區(qū)域覆蓋率。
[0028] 子區(qū)域覆蓋率，即水下航行器在1〇〇步之內(nèi)對已探索的區(qū)域和探索目標(biāo)區(qū) 域的比值。其中子探索目標(biāo)區(qū)域Asub_t # &為當(dāng)前100步探索的區(qū)域和未探索區(qū)域的總和， 也就等于總的目標(biāo)區(qū)域減去已探索過的區(qū)域，則子區(qū)域覆蓋率的表達(dá)式為：
[0031]當(dāng)AUV進行探索時，尤的值為0 ;當(dāng)水下航行器進行回訪時，.的值 為l(pk)與D的乘積。其中，l(Pk)是由回訪第kth個節(jié)點所增加的期望路徑的長度，D是聲 納的視圖寬度值，尤是通過回訪行為所產(chǎn)生的預(yù)計的冗余覆蓋區(qū)域。
[0032] e，確定回訪點k。
[0033] 懲罰因子的由兩部分組成，一部分是水下航行器運動的不確定度參數(shù)Ρ|，另一部 分則是子區(qū)域覆蓋率°懲罰項的表達(dá)式為：
[0035] 其中α是權(quán)重。
[0036] 懲罰項取相反數(shù)即可得到綜合回饋值。即：
[0037] Rk=-Ck
[0038] 最后，重訪問的第P個節(jié)點被定義為最小的綜合回饋值，即：
[0039] k*= arg max Rk= arg min Ck
[0040] 其中k e {〇, 1，2... Nwp}，當(dāng)k = 0時對應(yīng)回訪行為。
[0041] 另外，進行下一次探索之前，將當(dāng)前位置設(shè)置為局部起始點，忽略已探索過的區(qū) 域。
[0042] 本發(fā)明的有益效果是：本發(fā)明提出的基于聲吶輔助自主導(dǎo)航的導(dǎo)航方法，其采用 iSAM算法預(yù)期測繪位置精度較同配置卡爾曼濾波方法可提高30%以上。本發(fā)明克服了傳 統(tǒng)導(dǎo)航模塊單一的期望通過提高導(dǎo)航性能來獲得高準(zhǔn)確度的海底地形地貌測繪信息的缺 陷，提高導(dǎo)航性能，具有定位的高度一致性，并兼顧了目標(biāo)覆蓋率，使100%最大航程范圍內(nèi) 累計誤差可控。本發(fā)明可為進行高質(zhì)量高準(zhǔn)確度的地形地貌探測奠定堅固的技術(shù)基礎(chǔ)。
【附圖說明】
[0043] 圖1是本發(fā)明流程示意圖。
[0044] 圖2是探索/回訪行為選擇原理示意圖。
【具體實施方式】
[0045] 下面結(jié)合附圖對本發(fā)明作進一步說明。
[0046] 如圖1-2所示，本發(fā)明所述的基于聲吶輔助自主導(dǎo)航的AUV導(dǎo)航方法，包括如下步 驟：
[0047] 步驟一、AUV潛入海底，側(cè)掃聲吶工作，對海底圖像特征進行采集；
[0048] 步驟二、將高維海底聲吶探測圖像數(shù)據(jù)進行降維處理：
[0049] 采用詞袋BoW局部特征表達(dá)模型來描繪海底探測圖像，以滿足對海量觀測信息的 快速的反應(yīng)能力；
[0050] 步驟三、分別計算兩側(cè)聲吶圖像的特征顯著率：
[0051] 采用歸一化圖像熵來表示聲吶圖像的特征顯著率，
[0053] 其中I ω I表示聲吶觀測圖像中的局部特征總數(shù)，l〇g2| ω I為特征均勻分布時獲得 的最大熵，P(Wl)為局部特征的先驗概率密度函數(shù)，圖像熵
表 示聲吶探測圖像中特征的多樣性；將上述特征顯著率的計算結(jié)果與自定義閾值進行比較， 產(chǎn)生具有顯著特性的聲吶觀測節(jié)點；
[0054] 步驟四、當(dāng)航行器航行100步時，計算其當(dāng)前航行的不確定度P，并與不確定閾值 P。進行比較：
[0056] 其中| Σ_|為自航式測繪系統(tǒng)探索時位姿的一步預(yù)測協(xié)方差的模，| XallcJ表示 其位姿允許協(xié)方差的模；如果不確定度大于等于不確定閾值，則導(dǎo)航?jīng)Q策AUV重訪某顯著 性位姿點；如果不確定度小于此不確定閾值，則導(dǎo)航?jīng)Q策AUV繼續(xù)探索。
[0057] a，對步驟三得到的顯著性聲吶觀測節(jié)點進行聚類，得到候選回訪點；
[0058] b，對候選回訪點進行路徑規(guī)劃；
[0059] c，計算到每個候選回訪點的總不確定度4 ;
[0060] 候選回訪點的總的不確定度由三部分組成，包括里程計信息矩陣、聲吶信息矩陣 以及iSAM信息矩陣：
[0061] ①里程計信息矩陣
[0063] 其中為里程計信息矩陣，為回訪路徑上的兩個虛擬節(jié)點叫與η 1+1之間 的雅克比矩陣，Ρ為回訪虛擬節(jié)點個數(shù)，R為里程計的測量噪聲協(xié)方差。
[0064] ②聲吶信息矩陣
[0066] 其中As_為聲吶信息矩陣，為虛擬節(jié)點i與其候選匹配節(jié)點m之間的雅 克比矩陣，P為回訪虛擬節(jié)點個數(shù)，ζ 1是i的聲吶測量的候選節(jié)點的索引集合，P ^3成功鏈 接的概率，R為聲吶的測量噪聲協(xié)方差。
[0067] ③ISAM信息矩陣
[0068] AlsaB= (ATA) '= (RTR) 1 (5)
[0069] 其中A為測量雅克比矩陣，R為對A進行QR分解得到的上三角矩陣。其中A與R 的計算方法如下：
[0070] I. SLAM的概率模型
[0071 ] 根據(jù)信度網(wǎng)模型來表述SLAM問題，則所有變量和測量的聯(lián)合概率密度模型如下：
[0073] 其中機器人位姿X = {xj，路標(biāo)L = {1』，控制輸入U = {uj，特征測量Z = {zk}， P(x〇)表示初始狀態(tài)，P(Xi |xi " 表示運云力概率模型，尸(? 表示測量概率模型。
[0074] 運動方程為：Xi= f ; (Xi i〇+Wi (7)
[0075] 其中K是服從均值為0,協(xié)方差為Λ i的正態(tài)分布過程噪聲。其運動概率模型為：
[0077] 測量方程為：
[0078] 其中vk是服從均值為0,協(xié)方差為Γ k的正態(tài)分布觀測噪聲。其測量概率模型為：
[0080] II.把SLAM問題變?yōu)樽钚《朔▎栴}
[0081] 根據(jù)有效的測量，為了得到未知環(huán)境的最優(yōu)估計，將SLAM問題轉(zhuǎn)換為等價的基于 最大后驗（MAP)估計的最小二乘問題。全程軌跡X