[發明專利]一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法

在審

• 申請號: 202210049835.0
• 申請日: 2022-01-17
• 公開（公告）號: CN114527747A
• 公開（公告）日: 2022-05-24
• 發明（設計）人: 聶勇;張敏捷;呂小文;孫向偉;李貞輝;唐建中;陳正
• 申請（專利權）人: 浙江大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 林超

1.一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法，其特征在于：

步驟100，預設無人船航行的初始路徑；

步驟200，控制無人船按照初始路徑航行，獲取無人船的實時運動狀態；

步驟300，根據無人船的實時運動狀態，判斷無人船真實的航行路徑是否偏離初始路徑，若航行路徑偏離初始路徑，則獲取初始路徑上的目標點；

步驟400，根據目標點獲取無人船的偏航距離和偏航角；

步驟500，根據無人船的實時運動狀態、偏航距離和偏航角，校正無人船的實時運動狀態。

2.根據權利要求1所述的一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法，其特征在于：

所述的步驟100中，預設的無人船航行的初始路徑為一系列路徑點構成的曲線路徑，以初始路徑中第一個路徑點作為坐標原點，緯線為x軸，經線為y軸，初始路徑的路徑點表示為：

P(i)＝[px(i),py(i)]

其中，P(i)為初始路徑中第i個路徑點，px(i)為第i個路徑點的x軸坐標，py(i)為第i個路徑點的y軸坐標。

3.根據權利要求2所述的一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法，其特征在于：

所述的步驟200中，無人船的實時運動狀態包括當前時刻t無人船的航向角、航行速度v(t)和當前位置[x(t)，y(t)]，其中，x(t)為無人船在當前時刻t的x軸坐標，y(t)為無人船在當前時刻t的y軸坐標。

4.根據權利要求2所述的一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法，其特征在于：

所述的步驟300中，判斷無人船真實的航行路徑是否偏離初始路徑：

若無人船真實的航行路徑不偏離初始路徑，則繼續控制無人船按照初始路徑航行；

若無人船真實的航行路徑偏離初始路徑，則進行以下判斷：

若無人船真實的航行路徑是第一次偏離初始路徑，則遍歷初始路徑中所有的路徑點，向當前無人船的前方取距離無人船的當前位置最近的一個路徑點作為目標點；

若無人船真實的航行路徑不是第一次偏離初始路徑，則從無人船上一次偏離初始路徑獲取的目標點P(m)開始，遍歷無人船前方的初始路徑中間的所有路徑點，獲取距離無人船的當前位置最近的一個路徑點作為當前的目標點，其中，P(m)為初始路徑中第m個路徑點，v(t)為當前無人船的航行速度，Δt為由控制無人船航行時發出的控制信號的控制頻率的單位時間，s為初始路徑的路徑點間的平均距離。

5.根據權利要求3所述的一種基于自適應預瞄點和投影定位的無人船軌跡跟蹤方法，其特征在于：

所述的步驟400中，將目標點在初始路徑上的切線作為投影線，獲取無人船的當前位置至目標點之間的直線在投影線上的投影長度作為偏航距離D，獲取無人船的當前位置在真實的航行路徑上的切線與投影線之間的夾角作為偏航角α。

6.根據權利要求1所述的一種基于自適應預瞄點和投影位的無人船軌跡跟蹤方法，其特征在于：

所述的步驟500中，預設投影閾值λ和航向角閾值

航向角的調整：

根據當前無人船的偏航距離D與偏航角α，進行以下判斷，調整當前無人船的航向角：

若D≤λ或時，則不對當前無人船的航向角進行調整；

若Dλ且時，則對當前無人船的航向角進行調整，使得當前無人船的航向朝向目標點；

航行速度的調整：

根據當前無人船的偏航距離D與偏航角α，同時進行以下判斷，調整當前無人船的航行速度：

若D≤λ且時，或者D≤λ且時，則在當前無人船前方的初始路徑中，從目標點開始，以相同距離d₁取兩個初始路徑上的點作為預瞄點，d₁＝v(t)*Δt，v(t)為當前無人船的航行速度，Δt為由控制無人船航行時發出的控制信號的控制頻率的單位時間；并進行以下判斷：

若無人船從當前位置沿直線航行到該兩個預瞄點時的航向角均小于航向角閾值則控制當前無人船進行加速；

若無人船從當前位置沿直線航行到該兩個預瞄點時的航向角均不小于航向角閾值或僅有其中一個航向角不小于航向角閾值則保持當前無人船的航行速度v(t)；

若Dλ時，則在當前無人船前方的初始路徑中，從目標點開始，按照以下公式以距離d_j取第j個預瞄點：

其中，κ_j為第j個預瞄點在初始路徑中的曲率，j＝1、2、3；

共取三個預瞄點P1、P2和P3，計算預瞄點距離d，d＝d₁+d₂+d₃；記當前目標點為P0，計算無人船的航向角分別與向量P0P1、P0P2和P0P3的夾角α1、α2和α3，再按照以下公式計算當前無人船的航向轉角為：

其中，T₀為預設跟隨時間常數；

根據航向轉角進行以下判斷：

若時，則控制當前無人船進行減速；

若時，則保持當前無人船的航行速度v(t)。