[發明專利]一種無人艇的實時避碰和目標跟蹤方法

有效

• 申請號: 201910555673.6
• 申請日: 2019-06-25
• 公開（公告）號: CN110262492B
• 公開（公告）日: 2022-04-08
• 發明（設計）人: 胡常青;文龍貽彬;江淮;李清洲;徐宇新;王學鋒;趙榮利
• 申請（專利權）人: 北京航天控制儀器研究所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 張歡

一種無人艇實時避碰和目標跟蹤方法，針對無人艇的運動特性和無人艇路徑規劃過程中需要遵守國際海上規則公約且會受到風浪流干擾等特點以及無人艇目標跟蹤過程對速度平滑性的要求，構建了海事規則和轉向避碰模型、自適應跟蹤模型、無人艇運動學模型以及無人艇能耗模型。之后，為無人艇設定了航行和目標跟蹤兩種工作模式。針對兩種工作模式設定相應中終點條件，并在算法中加入多種約束條件，再利用最小二乘和G2O圖優化算法，計算得到最優路徑。本發明可以應用于無人艇的路徑規劃領域，可以實時得到一條考慮了航行時間、碰撞風險、國際海上規則公約、能耗、無人艇的運動學限制、速度/加速度限制等約束條件的最優平滑軌跡。

技術領域

本發明涉及無人艇路徑規劃領域，具體是一種無人艇的實時避碰和目標跟蹤方法

背景技術

隨著無人駕駛系統和人工智能技術的快速發展，繼無人機和無人車后，無人艇(Unmanned Surface Vehicle,USV)在軍用和民用領域中越來越受歡迎，在海上搜救、海事取證、環境監測和敵方偵察等各種海洋應用中發揮著重要作用。

路徑規劃技術是水面無人艇領域中的核心技術之一，是從有人走向無人的關鍵一步，是無人艇智能化的重點發展方向，在一定程度上標志著水面無人艇自主航行能力的高低。因此研究無人艇的路徑規劃技術，將有助于無人艇的進一步發展。水面無人艇的路徑規劃是指在有障礙物的工作環境中，如何尋找一條從給定起點到終點適當的運動路徑，使水面無人艇在運動過程中能安全、無碰地繞過所有障礙物。而實時避碰和目標跟蹤更是其中的難點，是需要重點研究的內容。實時避碰和目標跟蹤是以全局路徑或者被跟蹤目標信息為指導，通過傳感器信息確定無人艇的實時位置，及得到局部范圍內障礙物的分布情況；尋找一種滿足一定評價標準和約束的運動方案，可以調整航向和航速，高度智能化、自適應避開海上各類障礙物。

目前大多數的路徑規劃方法，如勢場法、柵格法、A*算法、遺傳算法等，都是通過規劃坐標點的方式，來得到最優路徑，在規劃過程中沒有考慮時間序列信息以及無人艇的運動約束，這樣會導致規劃出來的路徑過于理想化，存在路徑不可達情況，即控制器無法跟隨此路徑。

而TEB算法可以實時規劃出無人艇的軌跡，通過綜合考慮無人艇的運動模型和運動約束，并對無人艇執行任務時的速度與加速度進行規劃，以滿足光滑性和速度可控性等要求。

發明內容

本發明所要解決的技術問題是：克服現有技術的不足，本發明提供了一種無人艇的實時避碰和目標跟蹤方法，針對無人艇航行和目標跟蹤場景，能夠實時規劃出一條包含路徑坐標點序列、時間序列以及到達任一路徑點的速度、角速度、方向等信息的軌跡，確保路徑能夠被控制器執行，并且規劃出來的路徑符合國際海上避碰規則公約，而在目標跟蹤過程中，無人艇能夠以平滑速度進行跟蹤，從而降低控制器的控制難度。

本發明所采用的技術方案是：一種無人艇的實時避碰和目標跟蹤方法，包括如下步驟：

步驟S10，根據無人艇航行軌跡的起點和終點位置，根據時間間隔和狀態序列個數，初始化帶有時間序列的狀態B:＝(Q,τ)，

其中：狀態集合Q＝{q_i}_i＝1...n,n∈N，狀態集合τ＝{ΔT_j}_j＝1...n-1,n∈N，q_i＝(p_i,β_i)^T＝(x_i,y_i,β_i)^T，p_i＝(x_i,y_i)為無人艇當前在地圖中的位置坐標，β_i為無人艇當前轉向角度，并定義東向為0°，逆時針為正，ΔT_j為時間分辨率；

步驟S20，根據COLREGS，構建海事規則模型和無人艇的轉向避碰模型；