[發明專利]一種基于時間最優與能量最優的無人艇自主跟蹤方法

在審

• 申請號: 202210624460.6
• 申請日: 2022-06-02
• 公開（公告）號: CN114995425A
• 公開（公告）日: 2022-09-02
• 發明（設計）人: 胡常青;文龍貽彬;王心玥;謝家文;楊長坤;王瀟
• 申請（專利權）人: 航天時代（青島）海洋裝備科技發展有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 范曉毅

本發明公開了一種基于時間最優與能量最優的無人艇自主跟蹤方法，首先建立穩定跟蹤模型，使無人艇進入繞后跟蹤階段或速度跟蹤階段，繞后跟蹤階段包括：建立無人艇運動模型，根據無人艇當前時刻狀態集合確定無人艇下一時刻狀態集合，進而得到預期跟蹤軌跡集合，從無人艇控制能量最優、時間最優、以及防止路徑抖動三個方面分別建立代價函數，結合啟發函數搜索后得到最優跟蹤軌跡；速度跟蹤階段包括：一步求解進行速度規劃，并對規劃結果進行可行性判斷，可行時完成規劃，不可行時，通過采樣求解的方法重新規劃。本發明能夠實時規劃出包含時間、速度、位置、方向信息的軌跡，確保路徑安全平滑，平穩可靠，并能盡快追蹤上目標進入穩定跟蹤狀態。

技術領域

本發明屬于無人艇路徑規劃領域，具體涉及一種基于時間最優與能量最優的無人艇的目標跟蹤方法。

背景技術

隨著無人駕駛系統和人工智能技術的快速發展，繼無人機和無人車后，無人艇(Unmanned Surface Vehicle,USV)在軍用和民用領域中越來越受歡迎，在海上搜救、海事取證、環境監測和敵方偵察等各種海洋應用中發揮著重要作用，而若要在上述應用場景中真正發揮出無人艇的價值，自主跟蹤就是無人艇需要具備的一個基礎能力。

路徑規劃技術一直是水面無人艇領域中的核心技術之一，是從有人走向無人的關鍵一步，是無人艇智能化的重點發展方向。目前路徑規劃領域有多個研究方向，如基于圖搜索的方法、基于采樣的方法、基于數值優化的方法以及智能算法等。但多數路徑規劃主要側重于避碰，而忽視了如何在避碰的同時實現對目標的自主跟蹤，導致規劃過程中沒有綜合考慮時間序列信息以及速度信息，從而降低跟蹤過程中的魯棒性。

發明內容

本發明的目的在于克服上述缺陷，提供一種基于時間最優與能量最優的無人艇自主跟蹤方法，首先建立穩定跟蹤模型，并根據穩定跟蹤模型使無人艇進入繞后跟蹤階段或速度跟蹤階段，繞后跟蹤階段的執行步驟包括：建立無人艇運動模型，根據無人艇當前時刻狀態集合確定無人艇下一時刻狀態集合，進而得到無人艇當前時刻的預期跟蹤軌跡集合，從無人艇控制能量最優、時間最優、以及防止路徑抖動三個方面分別建立代價函數，結合啟發函數搜索后得到最優跟蹤軌跡；速度跟蹤階段的執行步驟包括：一步求解進行速度規劃，并對規劃結果進行可行性判斷，當結果可行時完成規劃，當結果不可行時，通過采樣求解的方法重新規劃。本發明針對無人艇自主跟蹤場景，能夠實時規劃出一條包含時間、速度、位置、方向信息的軌跡，確保路徑安全平滑，平穩可靠，并能盡快追蹤上目標進入穩定跟蹤狀態。

為實現上述發明目的，本發明提供如下技術方案：

一種基于時間最優與能量最優的無人艇自主跟蹤方法，包括如下步驟：

建立穩定跟蹤模型，并根據穩定跟蹤模型確定無人艇處于繞后跟蹤階段或速度跟蹤階段；

當無人艇處于繞后跟蹤階段時，執行步驟S1.1～S1.6：

S1.1建立無人艇運動模型，所述無人艇運動模型用于根據無人艇當前時刻狀態和下一時刻運動參數確定無人艇下一時刻狀態；所述無人艇的狀態包括無人艇的位置、速度及航向角，所述運動參數包括加速度、步長時間和轉向角；

S1.2在當前時刻，對無人艇下一時刻的運動參數進行采樣，得到采樣運動參數集合；根據無人艇當前時刻狀態、采樣運動參數集合以及無人艇運動模型得到無人艇當前時刻的預期跟蹤軌跡集合；

S1.3舍棄預期跟蹤路徑集合中與障礙物產生碰撞的軌跡；

S1.4根據無人艇當前時刻的預期跟蹤軌跡集合建立控制能量代價函數、時間代價函數和平滑代價函數，并根據控制能量代價函數、時間代價函數和平滑代價函數得到無人艇由起點位置到當前位置的代價函數；

S1.5根據Dubin模型建立啟發函數；

S1.6根據無人艇由起點位置到當前位置的代價函數以及啟發函數得到估值函數；并根據估值函數搜索得到最優跟蹤軌跡；