[發(fā)明專利]基于D*Lite優(yōu)化算法的自主駕駛船舶航路規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010125992.6
• 申請日: 2020-02-27
• 公開（公告）號: CN111220160B
• 公開（公告）日: 2023-07-04
• 發(fā)明（設計）人: 王曉原;夏媛媛;姜雨函;董曉斐;李瑩瑩
• 申請（專利權(quán)）人: 智慧航海（青島）科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06Q10/04
• 代理公司: 北京易捷勝知識產(chǎn)權(quán)代理有限公司 11613
• 代理人: 齊勝杰

1.一種基于D*Lite優(yōu)化算法的自主駕駛船舶航路規(guī)劃方法，其特征在于，包括以下步驟：

步驟S1、航行過程中自主駕駛船舶按照預設時間間隔獲取環(huán)境信息，根據(jù)當前獲取的環(huán)境信息和上一次獲取的環(huán)境信息，若當前目標地圖區(qū)域內(nèi)的環(huán)境信息發(fā)生改變，判斷自主駕駛船舶的目標路徑點是否發(fā)生改變；

步驟S2、若目標路徑點發(fā)生改變，更新目標路徑點，將當前路徑點作為起航點，根據(jù)更新后的目標路徑點確定終止點，并根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑；

所述目標地圖區(qū)域為影響當前路徑點和目標路徑點間航路規(guī)劃的地圖區(qū)域；

在所述步驟S1之前，還包括：

步驟S01、航行起始時，獲取自主駕駛船舶的初始路徑點和目標路徑點，并將初始路徑點作為起航點，根據(jù)目標路徑點確定終止點；

步驟S02、根據(jù)初始目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶的初始航行路徑，使得自主駕駛船舶從初始路徑點起沿初始航行路徑航行；

根據(jù)初始目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶的初始航行路徑，包括：

根據(jù)初始目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，基于預設的自主駕駛船舶安全邊界，采用D*Lite算法規(guī)劃自主駕駛船舶的初始航行路徑；

在所述步驟S2之后，還包括：

步驟S3、判斷自主駕駛船舶繼續(xù)航行的路徑是否達到預設最大步數(shù)，若未達到，自主駕駛船舶從當前時刻至下一次獲取環(huán)境信息的時刻依據(jù)當前規(guī)劃的航行路徑航行；若達到，依據(jù)搜索半徑擴大規(guī)則擴大D*Lite算法的搜索半徑，重新根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑，重復步驟S3；

所述步驟S3中，在自主駕駛船舶繼續(xù)航行的路徑未達到預設最大步數(shù)之后，自主駕駛船舶從當前時刻至下一次獲取環(huán)境信息的時刻依據(jù)當前規(guī)劃的航行路徑航行之前，包括對自主駕駛船舶繼續(xù)航行的路徑進行平滑處理：

curr＝curr+alpha×(succ-curr)+beta×(pred-2×curr+succ)

其中，curr為當前路徑點；alpha為位置變化系數(shù)，設為0～1；succ為當前路徑點八鄰域中的后繼路徑點；beta為船舶轉(zhuǎn)向系數(shù)，設為0～1；pred為當前路徑點八鄰域中的前繼路徑點；

所述根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑，包括：

從終止點開始，根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息，通過增量式搜索至起航點；

增量式搜索過程中，根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，計算擴展節(jié)點周圍八鄰域內(nèi)每一個節(jié)點的節(jié)點信息，并剔除障礙物節(jié)點及其相鄰節(jié)點的節(jié)點信息；所述節(jié)點信息包括V_min(curr,goal)值、D(curr,start)值和K_curr值；

選取V_min(curr,goal)值最小的節(jié)點作為下一個擴展節(jié)點。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述根據(jù)更新后的目標路徑點確定終止點，包括：

判斷更新后的目標路徑點是否為障礙物路徑點，若否，將更新后的目標路徑點作為終止點；若是，根據(jù)當前目標地圖區(qū)域的環(huán)境信息和更新后的目標路徑點，選取距離更新后的目標路徑點最近的港口靠離泊位置作為終止點。

3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑，包括：根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，基于預設的自主駕駛船舶安全邊界，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑。

4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟S2還包括：若目標路徑點未發(fā)生改變，將當前路徑點作為起航點，并根據(jù)當前目標地圖區(qū)域內(nèi)的環(huán)境信息、起航點和終止點，采用D*Lite算法規(guī)劃自主駕駛船舶繼續(xù)航行的路徑。

5.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述根據(jù)目標路徑點確定終止點，包括：

判斷目標路徑點是否為障礙物路徑點，若否，將目標路徑點作為終止點；若是，根據(jù)當前目標地圖區(qū)域的環(huán)境信息和目標路徑點，選取距離目標路徑點最近的港口靠離泊位置作為終止點。