[發明專利]一種USV的航跡閉環控制方法

有效

• 申請號: 201611076983.2
• 申請日: 2016-11-30
• 公開（公告）號: CN108121338B
• 公開（公告）日: 2019-12-03
• 發明（設計）人: 馮亮;劉陽;劉肖宇;湯哲;王子慶
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 21002 沈陽科苑專利商標代理有限公司
• 代理人: 李巨智<國際申請>=<國際公布>=<進入

1.一種USV的航跡閉環控制方法，其特征在于，包括以下步驟：

步驟1：設定目標航行路徑，且判斷位置傳感器和姿態傳感器是否處于安全工作狀態，如果是，則執行步驟2，否則報警；

步驟2：USV根據位置傳感器和姿態傳感器發送的實時位置信息和姿態信息計算偏離預定航線的距離，如果該距離小于設定距離，則延時等待后重新執行步驟2，否則執行步驟3；

步驟3：根據目標航行路徑、當前偏離預定航線的距離、位置和姿態信息，計算目標航向角ψ_r(k)；所述目標航向角ψ_r(k)的計算過程為：

期望航向ψ_r是垂足點(x’,y’)處的切線方向，則有：

ψ_r(k)＝arctan((y_i+1-y_i)/(x_i+1-x_i))

其中，用若干個直線段代替規劃的航線，(x_i,y_i),(x_i+1,y_i+1)為當前目標航行路徑航跡段i的兩個端點坐標，為目標航行路徑；

步驟4：根據目標航向角ψ_r(k)、當前航向角ψ(k)和航向角速度Δψ(k)，計算目標噴嘴角ω_r(k)；所述目標噴嘴角ω_r(k)的計算過程為：

其中，ω_r(k)為目標噴嘴角，ψ_r(k)為目標航向角，ψ(k)為當前航向角，Δψ(k)為航向角速度，K_P1為1.05，K_I1為0.011，K_D1為-4.1；

步驟5：根據目標噴嘴角ω_r(k)、當前噴嘴角ω(k)和噴嘴角速度Δω(k)，計算輸出動作量，所述輸出動作量的計算過程為：

其中，ACT為輸出動作量，ω_r(k)為目標噴嘴角，ω(k)為當前噴嘴角，Δω(k)為噴嘴角速度，K_P2為2.1，K_I2為0.03，K_D2為-2.5；

進而控制USV動作，返回步驟2。

2.根據權利要求1所述的USV的航跡閉環控制方法，其特征在于：所述偏離預定航線的距離的計算過程為：

ε²(k)＝(x(k)-x')²+(y(k)-y')²

其中，ε(k)為偏離預定航線的距離；x(k),y(k)為USV當前位置坐標；x’,y’為USV當前位置到目標航行路徑的垂線段交點坐標。