[發(fā)明專利]一種用于多機器人協(xié)作的路徑規(guī)劃方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 202011129043.1
• 申請日: 2020-10-21
• 公開（公告）號: CN112415997B
• 公開（公告）日: 2023-07-28
• 發(fā)明（設計）人: 吳新開;霍向
• 申請（專利權(quán)）人: 北京洛必德科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京知匯林知識產(chǎn)權(quán)代理事務所(普通合伙) 11794
• 代理人: 董濤

1.一種用于多機器人協(xié)作的路徑規(guī)劃方法，其特征在于，包括：

S1、路徑規(guī)劃服務器對路徑規(guī)劃參數(shù)進行初始化，確定機器人的數(shù)量為n；

S2、路徑規(guī)劃服務器獲取各個機器人的初始位置、目標位置和運動路徑集合，機器人的初始運動路徑集合只包括初始位置一個路徑點；

S3、路徑規(guī)劃服務器根據(jù)各個機器人的初始位置和目標位置，建立機器人運行環(huán)境的全局二維柵格地圖，二維柵格地圖包括可通行柵格和障礙物柵格；

S4、機器人利用視覺傳感器、超聲波傳感器、紅外傳感器和激光雷達探測其當前位置在當前時刻的周圍環(huán)境，并把探測到的周圍環(huán)境信息以及機器人的當前位置信息發(fā)送給路徑規(guī)劃服務器；

S5、路徑規(guī)劃服務器根據(jù)機器人的周圍環(huán)境信息、目標位置信息、運動路徑集合以及機器人的當前位置信息為機器人鎖定決策區(qū)域；當運動路徑集合中最后的(n-1)個路徑點均為同一個位置且不是目標位置時，所述決策區(qū)域設定為以機器人在當前時刻的當前位置為中心，包括左上、上、右上、左、右、左下、下，右下8處柵格位置，當前位置不再為決策區(qū)域位置；當運動路徑集合中最后的(n-1)個路徑點不為同一個位置時，所述決策區(qū)域設定為以機器人在當前時刻的當前位置為中心，包括左上、上、右上、左、當前位置、右、左下、下，右下9處柵格位置；

S6、路徑規(guī)劃服務器根據(jù)機器人的當前位置、目標位置，以及障礙物的位置，為每個機器人對應的決策區(qū)域內(nèi)的每個柵格位置計算初始評價值；該步驟中所述的路徑規(guī)劃服務器根據(jù)機器人的當前位置、目標位置，以及障礙物的位置，為每個機器人對應的決策區(qū)域內(nèi)的每個柵格位置計算初始評價值，具體為：

式中，e(x_i)是x_i柵格位置的初始評價值，x_i∈X，X為機器人的決策位置集合，X＝{x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈}或者X＝{x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈,x₉}，其中，x₁到x₈分別對應機器人左上、上、右上、左、右、左下、下，右下8處柵格位置，x₉對應當前位置；是x_i柵格位置的第一評價參數(shù)，當x_i柵格位置是障礙物位置時，設置為一個正無窮大值∞⁺，當x_i柵格位置不是障礙物位置時，設置為1；c₁為第一距離評價參數(shù)，c₂為第二距離評價參數(shù)，c₁、c₂的在系統(tǒng)初始化后被導入系統(tǒng)；是機器人在當前位置運動到x_i柵格位置的距離，是x_i柵格位置到機器人目標位置的距離；

S7、路徑規(guī)劃服務器根據(jù)計算出的機器人對應的決策區(qū)域內(nèi)的柵格位置的初始評價值預測機器人下一時刻的初始預測位置；該步驟中所述的路徑規(guī)劃服務器根據(jù)計算出的機器人對應的決策區(qū)域內(nèi)的柵格位置的初始評價值預測機器人下一時刻的初始預測位置，具體為：

取初始評價值最小的柵格位置為機器人下一時刻的初始預測位置；

當存在有多個初始評價值最小的柵格位置時，選擇規(guī)則為：距離x_i柵格位置到機器人目標位置的距離越小優(yōu)先級越高，相同時，機器人在當前位置運動到x_i柵格位置的距離越小優(yōu)先級越高；和相同時，柵格位置距離最近距離障礙物的距離越大優(yōu)先級越高；根據(jù)優(yōu)先級確定多個初始評價值最小的柵格位置中的一個柵格位置，該位置為機器人下一時刻的初始預測位置；

S8、路徑規(guī)劃服務器據(jù)機器人的當前位置、目標位置、障礙物的位置，以及各個機器人下一時刻的初始預測位置，為每個機器人對應的決策區(qū)域內(nèi)的每個柵格位置計算優(yōu)化評價值；

S9、路徑規(guī)劃服務器根據(jù)計算出的機器人對應的決策區(qū)域內(nèi)的柵格位置的優(yōu)化評價值預測機器人下一時刻的優(yōu)化預測位置；

S10、路徑規(guī)劃服務器判斷機器人下一時刻的優(yōu)化預測位置是否為該機器人的目標位置；

S11、如果為是，則機器人運動到其下一時刻的優(yōu)化預測位置時即到達目標位置；

S12、如果為否，則機器人運動到其下一時刻的優(yōu)化預測位置后，更新其當前位置，將更新的當前位置存儲到運動路徑集合，然后執(zhí)行S4。