[發(fā)明專利]路徑規(guī)劃方法、路徑規(guī)劃系統(tǒng)、機器人及存儲介質(zhì)

有效

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

本發(fā)明提供了一種路徑規(guī)劃方法、路徑規(guī)劃系統(tǒng)、機器人及存儲介質(zhì)，包括如下步驟：設(shè)定最小膨脹距離d_min，最大膨脹距離d_max；接受需要規(guī)劃的原始柵格地圖，根據(jù)d_min和d_max生成新的地圖類型distmap；在distmap里輸入起點和終點，將起點列為當前點P’；判斷當前點P’的臨近網(wǎng)格是否可通行，并根據(jù)新啟發(fā)式函數(shù)O(x)計算臨近網(wǎng)格的cost值；將P’附近可通行的點加入openlist隊列，并根據(jù)每個點的cost值建立最小堆；判斷P’是否為終點，否則重復上述步驟，是則結(jié)束搜索。本發(fā)明降低柵格地圖的搜索空間，解決大地圖下規(guī)劃速度過慢的問題，并在保證通行性的前提下自適應選擇離障礙物遠的、更容易通行和安全的路徑。

技術(shù)領(lǐng)域

本發(fā)明涉及機器人算法的技術(shù)領(lǐng)域，具體地，涉及一種路徑規(guī)劃方法、路徑規(guī)劃系統(tǒng)、機器人及存儲介質(zhì)。

背景技術(shù)

目前常用的基于柵格地圖的服務機器人路徑規(guī)劃方法，如dijstra、A*、存在規(guī)劃速度過慢(隨著地圖面積變大，規(guī)劃速度呈指數(shù)級下降，見圖1)的問題、規(guī)劃出的路徑雖然最短但非最優(yōu)(距離障礙物過近存在安全隱患，見圖2)的問題。此外，目前基于costmap的路徑規(guī)劃，其靜態(tài)的膨脹范圍存在路徑最短和路徑離障礙物距離太近的矛盾(見圖3)。

公開號為CN110361009B的專利文獻公開了一種路徑規(guī)劃方法、路徑規(guī)劃系統(tǒng)以及移動機器人，通過獲取待測環(huán)境中的障礙物位置信息，根據(jù)障礙物位置信息建立勢場地圖，提取所述勢場地圖中的第一關(guān)鍵點并對第一關(guān)鍵點進行篩選以獲得第二關(guān)鍵點，由第二關(guān)鍵點獲得拓撲地圖，獲取從機器人當前位置到目標點位置的最優(yōu)路徑。但是該專利文獻仍然存在大地圖下規(guī)劃速度過慢的缺陷。

發(fā)明內(nèi)容

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種路徑規(guī)劃方法、路徑規(guī)劃系統(tǒng)、機器人及存儲介質(zhì)。

根據(jù)本發(fā)明提供的一種路徑規(guī)劃方法，包括如下步驟：

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

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

步驟3：在distmap里輸入起點和終點，將起點列為當前點P’；

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

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

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

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

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

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

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

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

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