本發(fā)明屬于水下導航與路徑規(guī)劃，具體涉及一種海底地形輔助導航重定位及路徑重規(guī)劃方法、程序、設(shè)備及存儲介質(zhì)。

背景技術(shù)：

1、在海洋探索中，智能水下機器人(autonomous?underwater?vehicle，auv)能夠承受巨大的海洋壓力，同時具有自主作業(yè)能力，隨著auv續(xù)航能力的提升，能夠完成長達數(shù)月的水下自主航行，搭載多種海洋環(huán)境感知設(shè)備的auv能夠高效且準確的完成海洋環(huán)境探索。目前的auv導航方法中，地形輔助導航方法能夠提供有界的導航誤差，使得auv具備長期水下導航能力，是目前auv水下導航方法的一個主要研究方向。

2、但是auv在進行地形輔助導航中，可能存在連續(xù)的誤匹配，導致auv丟失自身位置，難以實現(xiàn)有效的水下作業(yè)；同時auv還存在“綁架”情況，導致其丟失對自身位置進行估計的能力。目前對于海底地形輔助導航連續(xù)匹配失效或“綁架”的問題研究較少，目前主要的研究集中在如何提高導航結(jié)果的精度或者如何在小范圍初始誤差下進行導航誤差收斂，但是針對實際地形輔助導航中可能存在的無法預知導航誤差范圍的問題，難以實現(xiàn)搜索范圍未知情況下的在線重定位以及重新規(guī)劃新的路徑。

3、公開日為2023年12月5日、公開號為cn113532438b、發(fā)明名稱為“一種大初始定位誤差下的改進iccp地形匹配方法”的專利申請，該專利利用改進的iccp來實現(xiàn)大初始定位誤差下的導航結(jié)果，但該方法仍需要有待匹配的地形區(qū)域，無法實現(xiàn)在自身位置區(qū)域完全未知情況下的在線重定位，導致auv在存在“綁架”問題或者自身位置狀態(tài)未知情況下無法實現(xiàn)導航誤差的快速修正。

4、公開日為2022年10月28日、公開號為cn110763234b、發(fā)明名稱為“一種水下機器人海底地形匹配導航路徑規(guī)劃方法”的專利申請，該專利將地形起伏度考慮為水下機器人狀態(tài)轉(zhuǎn)移概率的影響因子，保證了地形匹配算法在規(guī)劃得到的整條路徑上均能得到較高的匹配精度。但該方法并未將導航失效或者auv存在“綁架”情況下的路徑在線重規(guī)劃考慮到路徑規(guī)劃中，無法實現(xiàn)匹配失效下的在線路徑重規(guī)劃。

技術(shù)實現(xiàn)思路

1、本發(fā)明的目的在于解決auv在進行地形輔助導航時由于存在連續(xù)誤匹配或者存在“綁架”情況的問題，提供一種海底地形輔助導航重定位及路徑重規(guī)劃方法、程序、設(shè)備及存儲介質(zhì)。本發(fā)明首先通過在整個先驗海圖區(qū)域提取水深值對應(yīng)的等深線點，之后通過粒子距離等深線的歐式距離進行粒子權(quán)重更新，通過漸進搜索使得auv完成在線重定位；完成重定位后確定自身的位置，根據(jù)auv距離目標點的距離進行路徑重規(guī)劃，尋找到一條到達目標點的合適路徑。

2、一種海底地形輔助導航重定位方法，包括以下步驟：

3、步驟1：初始化粒子群中各粒子的位置向量；所述位置向量為二維平面坐標向量；

4、步驟2：根據(jù)auv搭載的單波束聲吶和深度計，獲取當前時刻auv所在位置的水深測量值，在先驗海圖中提取與水深測量值對應(yīng)的等深線點，獲取各等深線點的位置向量；

5、步驟3：對于每個粒子，計算粒子距離各等深線點的最短距離，基于最短距離更新粒子的權(quán)重；

6、步驟4：根據(jù)各粒子的權(quán)重及每個粒子的位置向量，估計當前時刻auv的位置向量，并根據(jù)上一時刻auv的位置向量，估計auv的航向及航行距離

7、步驟5：根據(jù)auv搭載的慣性導航系統(tǒng)，獲取auv從上一時刻運動到當前時刻的運動距離d1(t)；若連續(xù)n個時刻，則結(jié)束重定位過程；否則，根據(jù)auv的航向與航行距離估計值，更新粒子群中各粒子的位置向量，auv運動到下一時刻后返回步驟2。

8、進一步地，所述步驟1中根據(jù)開始執(zhí)行重定位時慣性導航系統(tǒng)提供的auv位置向量，進行粒子群中各粒子的位置向量初始化；為保證能夠快速的完成重定位，設(shè)置較大的粒子初始化分布方差，保證不同的粒子具有不同的運動軌跡。

9、進一步地，所述步驟3中基于最短距離更新粒子的權(quán)重的方法具體為：

10、

11、其中，wi表示第i個粒子的權(quán)重；σd為與先驗海圖中兩條等深線之間最短距離的均值相關(guān)的值；dimin表示第i個粒子距離各等深線點的最短距離。

12、進一步地，所述步驟4中根據(jù)各粒子的權(quán)重及每個粒子的位置向量，估計當前時刻auv的位置向量，具體為：

13、

14、其中，表示auv的位置向量估計值；p表示粒子群中粒子的總數(shù)；wi表示第i個粒子的權(quán)重；xi表示第i個粒子的位置向量。

15、一種基于海底地形輔助導航重定位方法的路徑重規(guī)劃方法，包括以下步驟：

16、步驟一：根據(jù)auv搭載的慣性導航系統(tǒng)，獲取auv從上一時刻運動到當前時刻的運動距離d1(t)；根據(jù)auv搭載的地形輔助導航系統(tǒng)，獲取auv從上一時刻運動到當前時刻的運動距離d2(t)；若連續(xù)n個時刻，|d1(t)-d2(t)|大于設(shè)定的閾值δ，則執(zhí)行重定位；否則，執(zhí)行步驟二；

17、步驟二：在預建立的環(huán)境模型中執(zhí)行auv當前時刻位置至目標點的最優(yōu)路徑規(guī)劃；

18、步驟三：auv根據(jù)路徑規(guī)劃運動到下一時刻，若未達到目標點，則返回步驟一。

19、進一步地，所述步驟二中預建立的環(huán)境模型是根據(jù)先驗海圖計算得到每個區(qū)域的地形起伏度，并考慮每個網(wǎng)格點距離目標點的距離，設(shè)定越靠近目標點的地形起伏度影響越大，越遠離目標點的地形起伏度影響越小，來完成環(huán)境模型的建立。

20、進一步地，所述步驟二中在預建立的環(huán)境模型中，采用rrt*進行在線路徑規(guī)劃，具體包括以下步驟：

21、步驟2.1：根據(jù)路徑規(guī)劃的起點zinit初始化隨機樹t，隨機樹t以zgoal為目標在搜索空間內(nèi)進行擴展，通過隨機采樣生成節(jié)點zrand，確定隨機樹中距離新生成的節(jié)點最近的節(jié)點znear，并從節(jié)點znear向zrand延長一定步長p后的得到新節(jié)點znew，對新生成的節(jié)點進行避障檢測，無障礙物后將節(jié)點加入到隨機樹中；

22、步驟2.2：計算隨機樹中距離znew最近的節(jié)點為zneighbor，計算從zinit到zneighbor的花費，以及從zneighbor到znew的歐式距離，并將zneighbor設(shè)為節(jié)點znew的父節(jié)點zparent；

23、步驟2.3：計算從zinit到附近相連節(jié)點znear的花費以及znear到znew的歐式距離，同時考慮節(jié)點的獎勵函數(shù)，尋找到成本最小的節(jié)點znear作為znew的父節(jié)點zparent；根據(jù)尋找到的隨機樹父節(jié)點進行重新布線，計算從zinit到節(jié)點znew的花費以及znear到znew的歐式距離，同時考慮節(jié)點的獎勵函數(shù)，通過節(jié)點重新布線保證所有節(jié)點到初始點花費最?。?/p>

24、步驟2.4：隨機樹繼續(xù)擴展，直到生成的znew節(jié)點距離目標zgoal的距離小于步長閾值時，將znew和zgoal連通；

25、步驟2.5：若未達到最大的迭代次數(shù)，則返回步驟2.2；否則，輸出最優(yōu)的從zinit到zgoal的路徑。

26、一種計算機裝置/設(shè)備/系統(tǒng)，包括存儲器、處理器及存儲在存儲器上的計算機程序，所述處理器執(zhí)行所述計算機程序以實現(xiàn)上述海底地形輔助導航重定位及路徑重規(guī)劃方法的步驟。

27、一種計算機可讀存儲介質(zhì)，其上存儲有計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)上述海底地形輔助導航重定位及路徑重規(guī)劃方法的步驟。

28、一種計算機程序產(chǎn)品，包括計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)上述海底地形輔助導航重定位及路徑重規(guī)劃方法的步驟。

29、本發(fā)明的有益效果在于：

30、本發(fā)明將慣性導航系統(tǒng)短期提供的航行距離較為準確的特性引入到地形輔助導航方法導航結(jié)果有效性評估中，判斷是否需要重定位；在重定位中通過在全局提取水深數(shù)據(jù)對應(yīng)的等深線點，并利用等深線點配合隨機生成的粒子，通過逐步迭代逐漸靠進真實位置，并利用慣性導航系統(tǒng)提供的航行距離完成重定位結(jié)果的評估；最后將與目標點的距離和地形坡度帶入到環(huán)境建模中，提高重定位后路徑規(guī)劃結(jié)果到達目標點的定位精度，提高導航結(jié)果的有效性。本發(fā)明充分考慮在連續(xù)誤匹配或“綁架”問題下進行auv重定位存在的問題，并提出相應(yīng)的解決方法，在完成重定位的基礎(chǔ)上，進行局部路徑重規(guī)劃，保證到達目標點時導航結(jié)果的有效性。