[發明專利]一種柵格地圖的路徑規劃方法

在審

• 申請號: 202011548679.X
• 申請日: 2020-12-24
• 公開（公告）號: CN112683275A
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 陳雙;鄭亮;孫龍龍;陳昕
• 申請（專利權）人: 哈爾濱工業大學蕪湖機器人產業技術研究院
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/32;G01C21/34;G05D1/02
• 代理公司: 蕪湖安匯知識產權代理有限公司 34107
• 代理人: 鐘雪

1.一種柵格地圖的路徑規劃方法，其特征在于，所述方法具體包括如下步驟：

S1、構建柵格地圖，對柵格地圖中的障礙物進行膨脹處理；

S2、構建拓撲地圖，在狹道的出口及入口處設有拓撲點，兩個拓撲點均位于狹道的安全區域；

S3、基于用Floyd算法獲取起始拓撲點至終止拓撲點的拓撲路徑點集，刪除拓撲路徑點集中的起始拓撲點及終止拓撲點，將剩余的拓撲路徑點加入路徑點集中；

S4、在路徑點集的首尾處分別加入任務起點及任務終點，基于A-stars算法依次規劃路徑點集中相鄰路徑點間的路徑，生成全局路徑點集。

2.如權利要求1所述柵格地圖的路徑規劃方法，其特征在于，路徑點集中相鄰路徑點間的路徑規劃方法具體包括如下步驟：

S41、生成一個包含開始節點n0的搜索圖G，把n0放在OPEN表中，生成空表CLOSED表；

S42、選擇OPEN表上的第一個節點，將其從OPEN表中移入CLOSE表，稱該節點為n；

S43、檢測節點n是否為目標節點，若檢測結果為是，則在搜索圖G中基于n到n0的指針找到相鄰路徑點間的路徑，若檢測結果為否，則轉向步驟S44；

S44、擴展節點n，生成其后繼節點集M，節點n的祖先不可在節點集M中，且節點集M中的節點均為可行走的節點；

S45、針對后繼節點集M中每個不在搜索圖G中的節點，建立一個指向n的指針，針對節點集M中每個已在OPEN表或CLOSE表中的節點m，若到目前為止到達節點m的估價函數f最小值通過節點n，則將節點m的指針修改為指向節點n；

S46、基于估價函數f從小至大的順序重新排列OPEN表中的節點，并返回步驟S44。

3.如權利要求1所述柵格地圖的路徑規劃方法，其特征在于，估價函數f(n)表示如下：

f(n)＝g(n)+h(n)

g(n)為任務起點到節點n的最短路徑值，h(n)為節點n到任務終點的最短路徑；

h(n)＝(|N(x)-E(x)|+|N(y)-E(y)|)*a+cross*b；

cross＝|point2(x)*point1(y)-point2(y)*point1(x))|

point1＝S-E；

point2＝N-E；

其中，S表示任務起點在柵格地圖中的坐標(S(x)，S(y))；E表示任務終點在柵格地圖中的坐標(E(x)，E(y))；N表示節點n在柵格地圖中的坐標(N(x)，N(y))；point1(x)、point1(y)分別表示柵格地圖下任務起點與任務終點之間的橫坐標差值和縱坐標差值；point2(x)、point2(y)分別表示柵格地圖下當前節點n與任務終點之間的橫坐標差值和縱坐標差值；a為水平移動一個柵格的代價，b為對角線的代價值。