[發明專利]一種水產養殖作業船自動導航路徑跟蹤方法

有效

• 申請號: 201610095261.5
• 申請日: 2016-02-22
• 公開（公告）號: CN105629976B
• 公開（公告）日: 2018-06-01
• 發明（設計）人: 趙德安;羅吉;孫月平;洪劍青
• 申請（專利權）人: 江蘇大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種水產養殖作業船自動導航路徑跟蹤方法，其特征在于，包括以下步驟：

第一步：使用基于RTK模式的高精度GPS系統采集能反映目標作業區輪廓的各個角點的經緯度坐標，經軌跡規劃，得到作業船的目標航線和航速，并確定起始目標路徑；

第二步：通過基于RTK模式的高精度GPS系統以及慣導模塊采集得到作業船當前的位置、航向和航速的位姿和運動狀態數據，設定作業船的航行速度并判斷是否需要進行目標路徑切換；

第三步：采用實時插點的方法計算得到作業船的跟蹤目標點P：根據點到直線的垂直交點的計算公式，得到作業船的當前位置在當前目標路徑上的投影點；然后以上述得到的投影點為起點，沿著目標路徑前進方向前伸一定距離得到作業船的跟蹤目標點P，其中前伸的距離值由作業船的船體尺寸、航行速度因素確定；

第四步：將作業船的位姿和運動狀態數據與作業船的跟蹤目標點及目標路徑進行對比分析，得到作業船的航向偏差與航速偏差，分別通過PD、PI控制算法推算出作業船的航向控制量和航速控制量，并將兩者融合得到作業船的左右明輪控制量；

第五步：根據得到的控制量對作業船進行實時導航控制，實現路徑的跟蹤。

2.如權利要求1所述的水產養殖作業船自動導航路徑跟蹤方法，其特征在于：所述第一步中經軌跡規劃，得到作業船的目標航線和航速，并確定起始目標路徑，具體步驟如下：

步驟1.1，計算作業船與能反映目標作業區輪廓的各個角點{J0,J1,...,Jn}的距離{L0,L1,...,Ln}，并將距離最近的點作為目標航線的起點T0；

步驟1.2，以各個角點的經緯度坐標為基準，按照路徑最短原則推算出作業船的目標航線：

T0→T1→...→Tk-1→TK→TK+1→...→TN

對應的目標點集合為：

{T0,T1,...,Tk-1,TK,TK+1,...,TN}

其經緯度坐標為：

{(xT0,yT0),(xT1,yT1),...,(xTk-1,yTk-1),(xTk,yTk),(xTk+1,yTk+1),...,(xTn,yTn)}

其中x坐標表示經度，y坐標表示緯度，TN為作業船目標航線的終點；

步驟1.3，結合目標航線設定作業船的目標巡航速度VL和目標轉彎速度VS；

步驟1.4，確定路徑T0→T1為作業船起始的目標路徑。