[發明專利]一種水上無人船路徑規劃方法

在審

• 申請號: 202111507580.X
• 申請日: 2021-12-10
• 公開（公告）號: CN114077256A
• 公開（公告）日: 2022-02-22
• 發明（設計）人: 王攀攀
• 申請（專利權）人: 威海海洋職業學院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 青島潤集專利代理事務所(普通合伙) 37327
• 代理人: 趙以芳

1.一種水上無人船路徑規劃方法，其特征在于，包括以下步驟：

(1)構建水面無人船運動的數學模型；水面無人船的運動模型包括地面坐標系η＝[x,y,ψ]^T和運動坐標系v＝[u,v,r]^T，對這兩個坐標系建立對應的關系，通過坐標變換，得到對于水面無人船在水平面上的兩個坐標系之間的關系如式(1)、(2)所示：

η＝R(ψ)ν (1)

其中，x，y為水面無人船的位置坐標；u為縱蕩速度，v為橫蕩速度，r為艏搖角速度；R(ψ)為大地坐標系到運動坐標系的旋轉矩陣；ψ為艏搖角度，范圍4∈(-π,m)，水面無人船的運動模型如式(3)所示：

M(ν)ν+C(ν)ν+D(ν)ν＝τ (3)

式(3)中各項的表達式如下所示：

τ＝[τ_u 0 τ_r]^T (4)

C₂₃＝-C₃₂＝m₁₁u (8)

其中τ為控制力，R為艏搖旋轉矩陣，M為慣性系數矩陣，C為科氏力和向心力矩陣，D為阻尼系數矩陣；

(2)分析水面無人船在路徑規劃中的優化目標

根據建立的水面無人船運動的數學模型，得到無人船的動力學和運動學模型，描述船舶運動規律，使用改進LOS法進行路徑跟蹤的控制模型如式(10)-(13)所示：

其中，x_c,y_c，ψ_c是跟蹤誤差，x_sf,y_sf,ψ_sf表示目標點位姿信息，s＝ν_r,ν_t和ν_r是對目標路徑的垂向和切向速度分解，K(s)為參考點曲率，有：

ψ″_ff＝ψ″_B+χ” (16)

式中ψ_B是船艏向；

水面無人船在航行中對x_c,y_c，ψ_c三個跟蹤誤差進行控制，對航速進行控制,使航速趨近于期望航速u，因此水面無人船路徑跟蹤控制的目標是使誤差P_c最小，如式(20)所示；

P_c＝(x_c,y_c,ψ_c,ν_c)^T→0 (20)

為使規劃的掃海路徑便于跟蹤，當水面無人船保持航向穩定，進行直線航行時，艏向角的角速度為0，即式(20)將簡化為式(21)：

P_c＝(x_c,y_c,ν_c)^T→0 (21)

當水面無人船保持勻速直線航行時，航速V不變，將進一步簡化為式(22)

P_c＝(x_c,y_c)^T→0 (22)；

(3)水面無人船局部區域自主避障策略研究

在經典斥力勢場函數的基礎上進行改進，改進后的斥力勢場函數在隨著機器人接近目標點的同時，斥力會減小，如式(23)所示：

其中，p-p_goal是一個矢量，表示機器人和目標點之間的歐幾里德距離，方向是在機器人和目標點的連線上，由機器人指向目標點；該式與經典勢函數中的斥力勢場函數比較，增加了p-p_goal乘子，以此來保證機器人在到達目標點時的勢場值是全局最小；

(4)水面無人船全局路徑規劃最優控制研究

基于模擬退火的混合粒子群算法進行水面無人船全局路徑規劃最優控制，基于模擬退火的混合粒子群算法是在基本粒子群算法中加入模擬退火算法的操作，粒子群算法中加入的因素主要包括初始溫度、退溫方式和以一定概率接受較差解的能力；

(5)仿真驗證試驗；