[發明專利]動態環境下基于AIS數據的多目標船舶航路規劃方法

在審

• 申請號: 202210337025.5
• 申請日: 2022-04-01
• 公開（公告）號: CN114675652A
• 公開（公告）日: 2022-06-28
• 發明（設計）人: 朱坤;連雪海;潘佳梁;李杼;駱曹飛;富玲峰;王旭;張群峰;王森峰;田明達
• 申請（專利權）人: 中電科（寧波）海洋電子研究院有限公司;中國人民解放軍92941部隊第140所
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京天華專利代理有限責任公司 32218
• 代理人: 肖麗

1.一種動態環境下基于AIS數據的多目標船舶航路規劃方法，具體步驟如下：

步驟1、獲取目標船AIS數據

拖輪通過其自帶的AIS系統與附近所需引導的目標船進行通信，獲取目標船的AIS數據并統計目標船的數量N(N≥2)，AIS數據包含目標船的當前位置、航向以及航速；AIS數據如下：N艘目標船的當前位置為經緯度坐標，記為G1(lon₁，lat₁)，G2(lon₂，lat₂)，……,GN(lon_N，lat_N)；N艘目標船的航向為真航向，記為α₁，α₂，……,α_N；N艘目標船的航速記為V₁，V₂，……,V_N；

步驟2、建立坐標系

以拖輪的初始位置為原點O，以平行于真子午線且過原點O的直線為Y軸，以垂直于真子午線且過原點O的直線為x軸，建立直角坐標系，在直角坐標系XOY內取兩點A1和A2，點A1和A2的坐標信息為已知，即點A1和A2的直角坐標為A1(x₁,y₁)，A2(x₂，y₂)；點A1和A2的經緯度坐標為A1′(lon1′,lat1′)，A2′(lon2′，lan2′)；

步驟3、坐標轉換

以點A1為起點，A2和Gi(1≤i≤N)為終點，形成兩條向量A12和A1i，向量A12的直角坐標為(dx₁，dy₁)，經緯度坐標為(dlon₁，dlat₁)；向量A1i的直角坐標為(dx_i，dy_i)，經緯度坐標為(dlon_i，dlat_i)；其中，dx₁＝x₂-x₁，dy₁＝y₂-y₁；dlon₁＝lon2′-lon1′，dlat₁＝lat2′-lat1′；dx_i＝x_i-x₁，dy_i＝y_i-y₁；dlon_i＝loni′-lon1′，dlat_i＝lati′-lat1′；x_i和y_i為第i艘目標船在直角坐標系XOY內的坐標；

向量A12和A1i在直角坐標系和經緯度坐標系下的模記為k1，k2，k3，k4；則

根據兩向量在不同坐標系中的長度比相同以及兩向量在不同坐標系中的夾角不變的原則，可得向量A12和A1i的直角坐標和經緯度坐標滿足以下關系：

根據上式，可求得第i艘目標船在直角坐標系XOY內的坐標x_i和y_i；

步驟4、目標船所處海域劃分

根據所有目標船的初始直角坐標Gi′(x_i，y_i),以拖輪的初始位置坐標為原點，形成一個包含所有目標船只的圓，其半徑為R1，并把半徑R1四等分再確定3個圓，其半徑分別為R2，R3，R4(R4＜R3＜R2＜R1)，把所有目標船所處海域劃分成5塊區域，分別為Ⅰ區、Ⅱ區、Ⅲ區、Ⅳ區以及Ⅴ區；

步驟5、各海域船只分類

計算所有目標船距離拖輪初始位置的距離l_i：

比較l_i與R1，R2，R3，R4之間的關系，對目標船進行分類：

若l_i＜R4，則該目標船屬于Ⅰ區；

若R4＜l_i＜R3，則該目標船屬于Ⅱ區；

若R3＜l_i＜R2，則該目標船屬于Ⅲ區；

若R2＜l_i＜R1，則該目標船屬于Ⅳ區；

并統計各區域內的船只數量為N_j(j＝Ⅰ～Ⅳ)，滿足并對分區完成的目標船進行重新編號為G_jm(0＜m＜N_j)；

步驟6、拖輪航路初步規劃

j(j＝Ⅰ～Ⅳ)區針對多目標船的拖輪的航路規劃如下：

6.1、以拖輪所在位置O_n(x_n，y_n)(0≤n≤N-1)(點O_n坐標為已知)為起點，平行于y軸建立軸y’，并在軸y’上一定距離z處取點A_n(x_n，y′_n)(y′_n＝y_n+z)；

6.2、以點O_n為起點，A_n和G_jm為終點，形成兩條向量OA_n和OG_jm，向量OA_n的直角坐標為(0，z)；向量OG_jm的直角坐標為(x_jm-x_n，y_jm-y_n)；x_jm和y_jm為目標船G_jm在直角坐標系XOY內的坐標；

6.3、計算向量OG_jm的模S_jm，公式如下：

6.4、計算兩條向量OA_n和OG_jm的夾角θ_jm，公式如下：

6.5、設拖輪沿β_jm方向，以速度V行駛，行駛時間為t，在B點與目標船G_jm相遇；其中β_jm是拖輪行駛方向與向量OG_jm之間的夾角；速度V為已知，且滿足V≥{V₁，V₂，......，V_N}_max；

6.6、計算拖輪和目標船G_jm的行駛距離H_jm和F_jm，公式如下：

H_jm＝V*t_jm

F_jm＝V_jm*t_jm

6.7、對目標船G_jm的行駛航向α_jm進行判斷：若0°＜α_jm＜180°，執行步驟6.8；若180°＜α_jm＜360°，執行步驟6.9；若α_jm＝0°或360°，執行步驟6.10；若α_jm＝180°，執行步驟6.11；

6.8、角度β_jm滿足以下方程：

其中γ_jm＝α_jm-(θ_jm+β_jm)，0°＜β_jm＜α_jm-θ_jm；

化簡得：

在0°＜β_jm＜α_jm-θ_jm條件下，求得t_jm的最小值，即可獲得最優路徑；

6.9、角度β_jm滿足以下方程：

其中，γ_jm＝180°-δ_jm-β_jm，δ_jm＝180°-ε_jm-θ_jm，其中ε_jm＝360°-α_jm，故γ_jm＝360°+θ_jm-α_jm-β_jm，0°＜β_jm＜360°-α_jm+θ_jm；

化簡得：

在0°＜β_jm＜360°-α_jm+θ_jm條件下，求得t_jm的最小值，即可獲得最優路徑；

6.10、角度β_jm滿足以下方程：

其中，γ_jm＝180°-δ_jm-β_jm，δ_jm＝180°-θ_jm，故γ_jm＝θ_jm-β_jm，0°＜β_jm＜θ_jm；

化簡得：

在0°＜β_jm＜θ_jm條件下，求得t_jm的最小值，即可獲得最優路徑；

6.11、角度β_jm滿足以下方程：

其中，γ_jm＝180°-δ_jm-β_jm，δ_jm＝θ_jm，故γ_jm＝180°-θ_jm-β_jm，0°＜β_jm＜180°-θ_jm；

化簡得：

在0°＜β_jm＜180°-θ_jm條件下，求得t_jm的最小值，即可獲得最優路徑；

6.12、對目標船的數量進行減一處理：N←N-1；

步驟7、計算剩余N艘目標船的當前位置

在確定拖輪從當前位置運動至目標船G_jm的最短路徑所需時間t_jm后，剩余N艘目標船G_jn(0＜n＜N)從初始位置G_jn(x_jn，y_jn)(位置G_jn為已知)運行至當前位置G′_jn(x′_jn，y′_jn)，當前位置坐標計算如下：

7.1、對目標船G_jn的航速V_jn進行分解，得到：

其中，V_jnx、V_jny分別為航速V_jn沿水平和豎直方向的分量；

7.2、t_jm時刻后，目標船G_jn的行駛距離如下：

其中，S_jnx、S_jny分別為目標船G_jn沿水平和豎直方向在t_jm內的運動距離；

7.3、對剩余N艘目標船G_jn的行駛航向α_jn進行判斷：若0°≤α_jn＜90°，執行步驟7.4；若90°≤α_jn＜180°，執行步驟7.5；若180°≤α_jn＜270°，執行步驟7.6；若270°≤α_jn≤360°，執行步驟7.7；

7.4、角度τ_jn滿足τ_jn＝α_jn，則目標船G_jn當前位置坐標G′_jn(x′_jn，y′_jn)，其中

7.5、角度τ_jn滿足τ_jn＝180°-α_jn，則目標船G_jn當前位置坐標G′_jn(x′_jn，y′_jn)，其中

7.6、角度τ_jn滿足τ_jn＝α_jn-180°，則目標船G_jn當前位置坐標G′_jn(x′_jn，y′_jn)，其中

7.7、角度τ_jn滿足τ_jn＝360°-α_jn，則目標船G_jn當前位置坐標G′_jn(x′_jn，y′_jn)，其中

步驟8、目標船所處海域判斷更新

計算剩余N艘目標船G_jn距離拖輪初始位置的距離l_i：

比較l′_i與R1，R2，R3，R4之間的關系，對原先各區域目標船的分類進行更新迭代：

若l′_i＜Rj，則該目標船屬于Ⅰ區；

若R4＜l′_i＜R3，則該目標船屬于Ⅱ區；

若R3＜l′_i＜R2，則該目標船屬于Ⅲ區；

若R2＜l′_i＜R1，則該目標船屬于Ⅳ區；

若l′_i＞R4，則該目標船屬于Ⅴ區；

更新各區域內現有船只的數量為N_s(s＝Ⅰ～Ⅴ)，滿足

步驟9、路徑規劃判斷

對剩余目標船數量N進行判斷：若N≠0，則對下一艘目標船G_jm+1進行路徑規劃，轉至步驟6；若N＝0，則執行步驟10；

步驟10、智能避障規劃：對目標船的初步路徑進行避障處理。