[發(fā)明專利]一種基于艏向及航速的智能船避碰路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202011337999.0
• 申請日: 2020-11-25
• 公開（公告）號: CN112363512B
• 公開（公告）日: 2023-08-01
• 發(fā)明（設(shè)計）人: 張隆輝;劉正鋒;魏納新;匡曉峰;劉長德
• 申請（專利權(quán)）人: 中國船舶科學(xué)研究中心
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 無錫華源專利商標(biāo)事務(wù)所(普通合伙) 32228
• 代理人: 聶啟新

1.一種基于艏向及航速的智能船避碰路徑規(guī)劃方法，其特征在于，所述方法包括：

將當(dāng)前航行海域地理坐標(biāo)按預(yù)設(shè)的柵格分辨率進(jìn)行柵格網(wǎng)絡(luò)劃分，得到柵格網(wǎng)絡(luò)索引為：

其中，表示柵格網(wǎng)絡(luò)索引，符號表示取整，為航行海域地理坐標(biāo)，為柵格分辨率，表示從所有地理坐標(biāo)與柵格分辨率的取整比值中取最小值；

根據(jù)航海圖信息，設(shè)柵格網(wǎng)絡(luò)內(nèi)可航行的柵格值為0，不可航行的柵格值為1；

獲取智能船舶當(dāng)前時刻的航向角及航速；

利用船載雷達(dá)探測船舶周邊障礙物并在所述柵格網(wǎng)絡(luò)上進(jìn)行標(biāo)記，設(shè)礙航物所在柵格值為1；

根據(jù)所述當(dāng)前時刻的航向角及航速，判斷智能船舶周圍至少八個柵格范圍內(nèi)下一時刻可航行的柵格，并設(shè)可航行的柵格值為0，不可航行的柵格值為1；

獲取所有所述柵格網(wǎng)絡(luò)中值為0的柵格，并將其存儲在矩陣元素中；

應(yīng)用全局路徑規(guī)劃方法求解所述矩陣元素間最短連接路徑；

將最短連接路徑上的矩陣元素轉(zhuǎn)換為所述航行海域地理坐標(biāo)，獲得智能船舶可航行的避碰路徑。

其中，所述根據(jù)所述當(dāng)前時刻的航向角及航速，判斷智能船舶周圍至少八個柵格范圍內(nèi)下一時刻可航行的柵格，包括：

設(shè)當(dāng)前智能船舶所在的柵格網(wǎng)絡(luò)索引為，當(dāng)前智能船舶的航向角為、航速為，則當(dāng)所述航速時，對于不同的所述航向角，有如下情況：

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)所述航速時，對于不同的所述航向角，有如下情況：

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

當(dāng)時，智能船舶可航行的柵格為、；

其中，所述應(yīng)用全局路徑規(guī)劃方法求解所述矩陣元素間最短連接路徑，包括：

建立待搜索矩陣元素集合和已搜索矩陣元素集合；

將當(dāng)前智能船舶位置作為起點(diǎn)加入所述待搜索矩陣元素集合，計算所述待搜索矩陣元素集合中各個元素對應(yīng)的搜索優(yōu)先級，并將所述搜索優(yōu)先級按照從小到大排序；所述搜索優(yōu)先級的計算公式為：

其中，為搜索優(yōu)先級，為元素到起點(diǎn)的距離，為當(dāng)前元素到終點(diǎn)的直線距離，且，其中為當(dāng)前元素對應(yīng)的柵格索引，為終點(diǎn)對應(yīng)的柵格索引；為父元素到起點(diǎn)的直線距離，為父元素對應(yīng)的柵格索引；

若所述待搜索矩陣元素集合不為空，則從所述待搜索矩陣元素集合中選取搜索優(yōu)先級值最小的元素；

若所述元素不是終點(diǎn)，則將所述元素從所述待搜索矩陣元素集合中刪除，并加入所述已搜索矩陣元素集合，同時執(zhí)行遍歷所述元素對應(yīng)柵格的周圍八個柵格元素的步驟；若所述元素是終點(diǎn)，則執(zhí)行從終點(diǎn)開始依次連接父元素直到所述起點(diǎn)的步驟；

遍歷所述元素對應(yīng)柵格的周圍八個柵格元素，如果當(dāng)前選中的鄰近柵格對應(yīng)元素在所述已搜索矩陣元素集合中，則搜索下一個鄰近柵格；如果當(dāng)前選中的鄰近柵格對應(yīng)元素為不可航行柵格，則搜索下一個鄰近柵格；如果當(dāng)前選中的鄰近柵格對應(yīng)元素不在所述待搜索矩陣元素集合中，則將所述元素定義為元素的父元素，并將所述元素放入所述待搜索矩陣元素集合中，當(dāng)遍歷完所有鄰近柵格元素后，重新執(zhí)行所述計算所述待搜索矩陣元素集合中各個元素對應(yīng)的搜索優(yōu)先級的步驟；

從終點(diǎn)開始依次連接所述父元素直到所述起點(diǎn)，得到所述矩陣元素間最短連接路徑。

2.根據(jù)權(quán)利要求1所述的基于艏向及航速的智能船避碰路徑規(guī)劃方法，其特征在于，所述得到所述矩陣元素間最短連接路徑的步驟后，所述方法還包括：

采用船舶運(yùn)動模型對所述智能船舶可航行的避碰路徑進(jìn)行平滑，所述船舶運(yùn)動模型為：

其中，為船舶的質(zhì)量，為船舶的速度，為船舶的加速度，為船舶的阻尼，為船舶受到的舵、槳控制力。

3.根據(jù)權(quán)利要求1所述的基于艏向及航速的智能船避碰路徑規(guī)劃方法，其特征在于，所述將最短連接路徑上的矩陣元素轉(zhuǎn)換為所述航行海域地理坐標(biāo)，包括：

轉(zhuǎn)換采用的計算公式為：

。