[發明專利]路徑規劃方法、路徑規劃系統、機器人及存儲介質

有效

• 申請號: 202111365624.X
• 申請日: 2021-11-18
• 公開（公告）號: CN113792109B
• 公開（公告）日: 2022-02-11
• 發明（設計）人: 劉宇星;楊洪杰;張晨博;楊俊;郭震
• 申請（專利權）人: 上海景吾智能科技有限公司
• 主分類號: G06F16/29
• 分類號: G06F16/29;G06Q10/04;G01C21/20
• 代理公司: 上海段和段律師事務所 31334
• 代理人: 李佳俊;郭國中

1.一種路徑規劃方法，其特征在于，包括如下步驟：

步驟1：設定最小膨脹距離d_min，最大膨脹距離d_max；

步驟2：接受需要規劃的原始柵格地圖，根據d_min和d_max生成新的地圖類型distmap；

步驟3：在distmap里輸入起點S（xs, ys）和終點F(xf, yf)，將起點列為當前點P’(xp,yp)；

步驟4：判斷當前點P’的臨近網格是否可通行，并根據新啟發式函數O(x)計算臨近網格的cost值；

步驟5：將P’附近可通行的點加入openlist隊列，并根據每個點的cost值建立最小堆；

步驟6：判斷P’是否為終點，否則重復步驟3~6，是則結束搜索；

所述步驟2包括如下步驟：

步驟2.1：將原始柵格地圖中的所有被占據網格加入待處理隊列occ_list，未被占據的網格的dist值初始化為d_max，其中，dist值為未被占據網格到被占據網格的最小距離，d_max為最大膨脹距離；

步驟2.2：將occ_list中每個被占據的網格記為網格點p_i，將周圍未被占據的網格加入dist_expand_list，并計算每個未被占據的網格離p_i的dist值；

步驟2.3：如果未被占據的網格距p_i的dist值大于 d_max，則停止將周圍未被占據的網格加入dist_expand_list，將當前處理的未被占據的網格從dist_expand_list中出列；否則將點p_i附近的未被占據的網格繼續加入dist_expand_list，如果dist_expand_list中的網格的dist值有改變，則取最小值；

步驟2.4：如果dist_expand_list隊列不為空，重復步驟2.3，否則重復步驟2.2；

步驟2.5：如果occ_list隊列不為空，重復步驟2.2，否則結束，生成新的地圖類型distmap；

所述步驟4包括如下步驟：

步驟4.1：遍歷當前網格的鄰近網格，判斷當前網格的dist值，dist值通過步驟2生成distmap的算法得到，若dist值 d_min，則不加入openlist隊列，否則加入openlist隊列；

步驟4.2：根據新提出的啟發式搜索函數O(x) = H(x) + G(x) + D(x)來計算網格的O(x)值；

所述步驟4.2中，H（x）表示當前網格P’（xp, yp）點到起點S(xs, ys)的歐式距離，H(x)= SQRT((xs - xp)^2 + (ys - yp)^2)；

所述步驟4.2中，G(x)表示當前網格P’（xp, yp）點到終點F(xf, yf)的歐式距離，G(x)= SQRT((xf - xp)^2 + (yf - yp)^2)；

所述步驟4.2中，D(x)表示當前網格在distmap下的權重值，D(x) = -(p_dist * safe_scale)；

其中p_dist為distmap生成時得到的當前網格的dist值；safe_scale為根據實際場景配置的參數，safe_scale表示安全系數，該系數越大說明整體的O(x)cost值受dist的影響越大，越傾向于走離障礙物遠的網格。

2.根據權利要求1所述的路徑規劃方法，其特征在于，所述步驟6中，判斷當前點P’(xp,yp) 和終點F(xf, yf)，若xp == xf yp == yf即認為P’為終點，否則將P’周圍網格加入openlist繼續循環處理。

3.一種機器人，其特征在于，所述機器人包括處理器和存儲器，所述存儲器儲存有若干指令，所述處理器通過執行所述若干指令實現權利要求1或2任意一項所述的路徑規劃方法。

4.一種計算機可讀存儲介質，其上存儲有計算機程序，其特征在于，所述計算機程序被處理器執行時實現權利要求1或2中任一項所述的路徑規劃方法。