[發明專利]一種水下路徑規劃方法和系統

有效

• 申請號: 202110285235.X
• 申請日: 2021-03-17
• 公開（公告）號: CN113074730B
• 公開（公告）日: 2023-05-05
• 發明（設計）人: 李渝舟;金山
• 申請（專利權）人: 華中科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 華中科技大學專利中心 42201
• 代理人: 夏倩

本發明公開了一種水下路徑規劃方法和系統，屬于路徑規劃技術領域。水下環境中水流的影響較大，在水下的障礙物大多不是靜止的，而是隨著水流運動的同時水下潛航器的運動也會受到水流的影響，本發明充分考慮這些因素，結合水下潛航器的運動學特性、水流的影響、水下潛航器的運行速度以及運行時間等，設計了針對水下環境的代價函數；在進行軌跡優化時，采用BSpline算法結合水流的速度和軌跡的平滑度，能夠生成一條安全、動態可行的軌跡，更適用于運行于水下環境的路徑規劃；并且本發明采用視覺SLAM來進行水下建圖，相比聲納更加低廉且更容易獲得，成本更低。

技術領域

本發明屬于路徑規劃技術領域，更具體地，涉及一種水下路徑規劃方法和系統。

背景技術

21世紀以來，隨著陸地資源短缺、人口膨脹、環境惡化等問題日益嚴重，各個國家紛紛把目光投向了海洋，加快對海洋的開發與利用。水下潛航器作為開發海洋，探索海洋的重要工具，非常適合用來進行做水下搜索、水下識別和水下打撈作業。路徑規劃技術作為水下潛航器的核心部分，路徑規劃水平的高低標志著水下潛航器智能化水平的高低。因此研究水下潛航器的路徑規劃算法對于水下潛航器的發展是至關重要的。

目前水下路徑規劃算法發展十分迅猛，常用于水下的路徑規劃算法主要有A*算法、RRT算法和Hybrid?A*算法等。A*算法是采用歐式幾何距離作為代價函數，沒有考慮水下潛航器的運動學特性，生成的軌跡往往不符合實際的運動情況。RRT算法在進行路徑規劃時，沒有考慮到水下水流對水下潛航器運動的影響，在真實水下環境中會出現較大的偏差。Hybrid?A*算法處理的是靜態障礙物，它沒有考慮到由于水流的影響，水下的障礙物并不是靜止不動的，而是會隨著水流一起運動的。因此，設需要設計一種適用于水下環境的路徑規劃算法。

發明內容

針對現有技術的以上缺陷或改進需求，本發明提供了一種水下路徑規劃方法和系統，其目的在于，針對水下運行環境對現有路徑規劃和優化算法進行改進，為水下潛航器生成安全、動態可行的軌跡。

為實現上述目的，本發明提供了一種水下路徑規劃方法，包括：

S1.對水下潛航器進行定位并獲得周圍環境的點云圖；

S2.利用定位設備的位姿信息和點云圖構建稠密圖；

S3.結合水下潛航器的運動學特性、水流對水下潛航器消耗能量的影響、水下潛航器的運行速度和水下潛航器的運行時間，利用Hybrid?A*算法對水下潛航器進行路徑規劃；

S4.結合障礙物相對水下潛航器的方向和水流方向，利用Bspline算法對規劃路徑進行優化，得到最終的水下潛航器運行路徑；其中，障礙物相對水下潛航器的方向根據稠密圖得到。

進一步地，步驟S1具體為，利用視覺傳感器對水下潛航器進行定位并獲得周圍環境的點云圖，以降低成本。

進一步地，步驟S2具體為，利用視覺傳感器的位姿信息和點云圖構建ESDF圖，以便對障礙物進行距離和梯度信息的查詢，減少計算量。

進一步地，步驟S3具體包括：

01.預測水下潛航器下一個時刻到達節點：

利用代價函數f_total＝λ₁J(T)+λ₂W_c+λ₃T+λ₄F_v選取水下潛航器設定鄰域內代價值最小的節點，作為水下潛航器下一個時刻到達節點；