本發(fā)明涉及機(jī)器人路徑規(guī)劃方案設(shè)計(jì)，具體涉及一種基于蟻群算法和動(dòng)態(tài)窗口的路徑規(guī)劃方法及系統(tǒng)。

背景技術(shù)：

1、生產(chǎn)線環(huán)境之中，環(huán)境復(fù)雜且多變，時(shí)常搬運(yùn)生產(chǎn)工具、生產(chǎn)原料或者生產(chǎn)原材料會(huì)帶來(lái)的靜態(tài)障礙物位置的變化，會(huì)使agv搬運(yùn)機(jī)器人會(huì)面對(duì)動(dòng)態(tài)障礙物的避障問題。因此，對(duì)于多變的環(huán)境需要設(shè)計(jì)出合理的環(huán)境建模更新策略以及對(duì)應(yīng)復(fù)雜的動(dòng)態(tài)障礙物需要設(shè)計(jì)出靈活的避障策略。因此，如何在無(wú)碰撞且滿足客戶需求的情況下，合理地進(jìn)行地圖更新、路徑規(guī)劃、動(dòng)態(tài)避障都是尚未解決的難題。且現(xiàn)有技術(shù)中，缺乏根據(jù)生產(chǎn)線的生產(chǎn)作業(yè)任務(wù)、實(shí)時(shí)的對(duì)agv的搬運(yùn)路徑進(jìn)行進(jìn)一步調(diào)優(yōu)的方案，而往往生產(chǎn)線上會(huì)臨時(shí)堆放了很多靜態(tài)障礙物，不同的生產(chǎn)作業(yè)任務(wù)往往堆放的靜態(tài)障礙物是不同的，機(jī)器人由于無(wú)法提前預(yù)知，會(huì)導(dǎo)致原有的路徑規(guī)劃失效，機(jī)器人無(wú)法通過，需要重新規(guī)劃路徑，可能會(huì)導(dǎo)致機(jī)器人繞行很遠(yuǎn)的路，會(huì)導(dǎo)致浪費(fèi)大量的時(shí)間。

2、因此，現(xiàn)有技術(shù)還有待進(jìn)一步發(fā)展。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的在于克服上述技術(shù)不足，提供一種基于蟻群算法和動(dòng)態(tài)窗口的路徑規(guī)劃方法及系統(tǒng)，以解決現(xiàn)有技術(shù)存在的問題。

2、為達(dá)到上述技術(shù)目的，根據(jù)本發(fā)明的第一方面，本發(fā)明提供了一種基于蟻群算法和動(dòng)態(tài)窗口的路徑規(guī)劃方法，包括：

3、s100、構(gòu)建生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，采用柵格建模的策略將預(yù)設(shè)環(huán)境地圖分成若干網(wǎng)格區(qū)域，采用蟻群算法進(jìn)行全局路徑規(guī)劃，采用拓展樹隨機(jī)優(yōu)化方法對(duì)蟻群算法進(jìn)行優(yōu)化，進(jìn)而實(shí)現(xiàn)agv搬運(yùn)機(jī)器人的全局靜態(tài)路徑規(guī)劃，得到第一移動(dòng)路徑；利用生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，獲取第一移動(dòng)路徑附近的各生產(chǎn)設(shè)備的編號(hào)，進(jìn)而在mes系統(tǒng)中獲取agv搬運(yùn)機(jī)器人在沿第一路徑移動(dòng)過程中會(huì)依次經(jīng)過的各生產(chǎn)設(shè)備當(dāng)前的生產(chǎn)任務(wù)，形成生產(chǎn)任務(wù)列表，在agv搬運(yùn)機(jī)器人按照預(yù)設(shè)搬運(yùn)路線移動(dòng)的過程中，利用設(shè)置于agv搬運(yùn)機(jī)器人上的深度相機(jī)采集前方的深度圖像數(shù)據(jù)，對(duì)所采集數(shù)據(jù)進(jìn)行處理，利用處理后的深度相機(jī)數(shù)據(jù)按照預(yù)設(shè)判定規(guī)則進(jìn)行可通行窗口的判斷，基于判定結(jié)果制定行進(jìn)規(guī)則，在agv搬運(yùn)機(jī)器人按照第一移動(dòng)路徑和制定的行進(jìn)規(guī)則移動(dòng)，完成搬運(yùn)任務(wù)后，獲取agv搬運(yùn)機(jī)器人的第一移動(dòng)路徑、實(shí)際移動(dòng)路徑和生產(chǎn)任務(wù)列表，記為路徑耦合數(shù)據(jù)集合，獲取預(yù)設(shè)數(shù)量的路徑耦合數(shù)據(jù)集合，將所采集的路徑耦合數(shù)據(jù)集合按照第一預(yù)設(shè)比例分為訓(xùn)練數(shù)據(jù)集和驗(yàn)證數(shù)據(jù)集；

4、s200、利用所采集的訓(xùn)練數(shù)據(jù)集訓(xùn)練路徑自動(dòng)調(diào)優(yōu)模型，利用驗(yàn)證數(shù)據(jù)集對(duì)路徑自動(dòng)調(diào)優(yōu)模型進(jìn)行模型優(yōu)化，優(yōu)化模型參數(shù)，完成路徑自動(dòng)調(diào)優(yōu)模型訓(xùn)練；

5、s300、當(dāng)模型訓(xùn)練完成后，獲取下次搬運(yùn)任務(wù)的agv搬運(yùn)機(jī)器人的第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表，將第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表輸入到路徑自動(dòng)調(diào)優(yōu)模型中，將模型輸出的路徑作為最優(yōu)路徑。

6、具體的，所述構(gòu)建生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，包括：

7、構(gòu)建生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，控制agv搬運(yùn)機(jī)器人進(jìn)入生產(chǎn)線后，利用設(shè)置于agv搬運(yùn)機(jī)器人上的深度相機(jī)采集agv搬運(yùn)機(jī)器人的周圍圖像數(shù)據(jù)，利用agv搬運(yùn)機(jī)器人上設(shè)置的深度相機(jī)以及定位裝置采集的周圍圖像數(shù)據(jù)和位置數(shù)據(jù)，更新生產(chǎn)線的預(yù)設(shè)環(huán)境地圖。

8、具體的，所述對(duì)所采集數(shù)據(jù)進(jìn)行處理包括：

9、首先得到深度圖像數(shù)據(jù)中的深度數(shù)組的集合{ad1,ad2,…,adn}以及對(duì)應(yīng)的角度和距離{d1,d2,…,dn}，其中，激光雷達(dá)的125°對(duì)應(yīng)深度相機(jī)視角的0°，將圖像中距離在d1±dσ范圍內(nèi)的點(diǎn)進(jìn)行統(tǒng)計(jì)，將坐標(biāo)存放在數(shù)組ad1中，統(tǒng)計(jì)得到數(shù)組ad1中的橫坐標(biāo)的最大值、最小值和縱坐標(biāo)的最大值、最小值，根據(jù)ad1中的橫坐標(biāo)的最大值、最小值和縱坐標(biāo)的最大值、最小值建立目標(biāo)搜索框m1，將圖像中距離在d2±dσ范圍內(nèi)的點(diǎn)進(jìn)行統(tǒng)計(jì)，將坐標(biāo)存放在數(shù)組ad2中,…,依此類推，便將深度圖像劃分為一系列目標(biāo)搜索框mn。

10、具體的，所述利用處理后的深度相機(jī)數(shù)據(jù)按照預(yù)設(shè)判定規(guī)則進(jìn)行可通行窗口的判斷，基于判定結(jié)果制定行進(jìn)規(guī)則，包括：

11、分別計(jì)算相鄰目標(biāo)搜索框的距離差值，并判斷最大的距離差值是否大于或等于第一預(yù)設(shè)閾值，若是，判斷產(chǎn)生最大距離差值的兩個(gè)目標(biāo)搜索框中距離較大的目標(biāo)搜索框的寬度是否大于或等于第二預(yù)設(shè)閾值同時(shí)判斷該目標(biāo)搜索框的高度是否大于或等于第三預(yù)設(shè)閾值，若是，目標(biāo)搜索框所指示的區(qū)域?yàn)榭赏ㄐ写翱冢瑒t計(jì)算該目標(biāo)搜索框的中心與agv搬運(yùn)機(jī)器人行駛方向的偏角，并根據(jù)距離和偏角計(jì)算得到agv搬運(yùn)機(jī)器人需要向左或向右移動(dòng)的距離，agv搬運(yùn)機(jī)器人向左或向右移動(dòng)該距離，使agv搬運(yùn)機(jī)器人的前進(jìn)方向與目標(biāo)搜索框的中心位于同一垂直面后，agv搬運(yùn)機(jī)器人沿垂直面與地平線的交線進(jìn)入可通行窗口。

12、具體的，所述利用所采集的訓(xùn)練數(shù)據(jù)集訓(xùn)練路徑自動(dòng)調(diào)優(yōu)模型，利用驗(yàn)證數(shù)據(jù)集對(duì)路徑自動(dòng)調(diào)優(yōu)模型進(jìn)行模型優(yōu)化，優(yōu)化模型參數(shù)，完成路徑自動(dòng)調(diào)優(yōu)模型訓(xùn)練，包括：

13、將訓(xùn)練數(shù)據(jù)集分批次輸入預(yù)設(shè)網(wǎng)絡(luò)層中進(jìn)行訓(xùn)練，所述預(yù)設(shè)網(wǎng)絡(luò)層包括transformer網(wǎng)絡(luò)層，所述transformer網(wǎng)絡(luò)層則用于根據(jù)當(dāng)前次路徑耦合數(shù)據(jù)集合前向傳播預(yù)測(cè)下一次的路徑耦合數(shù)據(jù)集合，進(jìn)而獲取預(yù)測(cè)的損失值；計(jì)算預(yù)設(shè)網(wǎng)絡(luò)層的損失值并輸入優(yōu)化器進(jìn)行優(yōu)化，確定路徑自動(dòng)調(diào)優(yōu)模型的參數(shù)梯度下降最快的方向；所述路徑自動(dòng)調(diào)優(yōu)模型根據(jù)損失值和模型的參數(shù)梯度進(jìn)行反向傳播，優(yōu)化路徑自動(dòng)調(diào)優(yōu)模型的參數(shù)。

14、具體的，所述利用所采集的訓(xùn)練數(shù)據(jù)集訓(xùn)練路徑自動(dòng)調(diào)優(yōu)模型，利用驗(yàn)證數(shù)據(jù)集對(duì)路徑自動(dòng)調(diào)優(yōu)模型進(jìn)行模型優(yōu)化，優(yōu)化模型參數(shù)，完成路徑自動(dòng)調(diào)優(yōu)模型訓(xùn)練，還包括：

15、每次訓(xùn)練后將驗(yàn)證數(shù)據(jù)集分批次輸入前一次訓(xùn)練的預(yù)設(shè)網(wǎng)絡(luò)層中進(jìn)行模型參數(shù)驗(yàn)證，進(jìn)行循環(huán)訓(xùn)練，總的訓(xùn)練輪次設(shè)置為第一預(yù)設(shè)輪次；記錄所述預(yù)設(shè)網(wǎng)絡(luò)層的損失值，判斷損失值是否滿足第一預(yù)設(shè)條件，并根據(jù)判斷結(jié)果判斷是否結(jié)束訓(xùn)練并輸出模型參數(shù)。

16、具體的，所述判斷損失值是否滿足第一預(yù)設(shè)條件，并根據(jù)判斷結(jié)果判斷是否結(jié)束訓(xùn)練并輸出模型參數(shù)，包括：

17、若損失值滿足第一預(yù)設(shè)條件，則結(jié)束訓(xùn)練并輸出當(dāng)前路徑自動(dòng)調(diào)優(yōu)模型的參數(shù)；若損失值不滿足第一預(yù)設(shè)條件，則繼續(xù)進(jìn)行訓(xùn)練。

18、具體的，所述第一預(yù)設(shè)條件包括：

19、在進(jìn)行了第二預(yù)設(shè)輪次的訓(xùn)練后，下一輪次訓(xùn)練所得到的損失值均大于或者等于在進(jìn)行第二預(yù)設(shè)輪次的訓(xùn)練過程中出現(xiàn)過的損失值。

20、具體的，所述采用蟻群算法進(jìn)行全局路徑規(guī)劃，采用拓展樹隨機(jī)優(yōu)化方法對(duì)蟻群算法進(jìn)行優(yōu)化，包括：

21、假設(shè)螞蟻的總數(shù)量為m，節(jié)點(diǎn)之間的信息素用矩陣pheromone表示，最優(yōu)路徑長(zhǎng)度為bestlength，最優(yōu)路徑為besttour，每只螞蟻都有各自的內(nèi)存，來(lái)存儲(chǔ)該螞蟻已經(jīng)訪問過的節(jié)點(diǎn)，進(jìn)而避免該螞蟻對(duì)此節(jié)點(diǎn)進(jìn)行第二次搜索；另外建立一個(gè)允許訪問的節(jié)點(diǎn)表來(lái)表示該螞蟻訪問的節(jié)點(diǎn)，使用一個(gè)矩陣來(lái)存儲(chǔ)該螞蟻在一次迭代中經(jīng)過的路徑所釋放出的信息素信息；假定算法總共運(yùn)行maxgen次，當(dāng)前迭代次數(shù)為t；

22、其步驟如下：

23、步驟a、參數(shù)初始化設(shè)置；

24、螞蟻的數(shù)量為k，信息素啟發(fā)因子為α，期望啟發(fā)因子為β，信息素?fù)]發(fā)因子ρ，信息素的強(qiáng)度為q；

25、步驟b、循環(huán)次數(shù)t＝t+1；

26、步驟c、螞蟻的禁忌表索引號(hào)k＝1；

27、步驟d、螞蟻數(shù)目k＝k+1；

28、步驟e、螞蟻個(gè)體根據(jù)狀態(tài)轉(zhuǎn)移概率公式計(jì)算的概率選擇元素并前進(jìn)，起始點(diǎn)的概率分布根據(jù)如下公式進(jìn)行計(jì)算：

29、

30、其中tij是節(jié)點(diǎn)i和節(jié)點(diǎn)j之間的信息素?fù)]發(fā)量，ηij是啟發(fā)式函數(shù)；

31、步驟f、修改禁忌表指針，即選擇好之后將螞蟻移動(dòng)到新的節(jié)點(diǎn)，并把該節(jié)點(diǎn)移動(dòng)到該螞蟻個(gè)體的禁忌表中；

32、步驟g、更新信息素，信息素更新公式如下：

33、

34、其中δtij表示所有螞蟻在路徑上釋放的信息素的總和；

35、步驟h、如果達(dá)到最大迭代次數(shù)maxgen，算法終止，輸出第一路徑；否則，轉(zhuǎn)至步驟b。

36、根據(jù)本發(fā)明的第二方面，提供一種基于蟻群算法和動(dòng)態(tài)窗口的路徑規(guī)劃系統(tǒng)，包括：

37、獲取模塊，用于利用生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，獲取第一移動(dòng)路徑附近的各生產(chǎn)設(shè)備的編號(hào)，進(jìn)而在mes系統(tǒng)中獲取agv搬運(yùn)機(jī)器人在沿第一路徑移動(dòng)過程中會(huì)依次經(jīng)過的各生產(chǎn)設(shè)備當(dāng)前的生產(chǎn)任務(wù)，形成生產(chǎn)任務(wù)列表；用于當(dāng)模型訓(xùn)練完成后，獲取下次搬運(yùn)任務(wù)的agv搬運(yùn)機(jī)器人的第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表；

38、控制模塊，用于構(gòu)建生產(chǎn)線的預(yù)設(shè)環(huán)境地圖，采用柵格建模的策略將預(yù)設(shè)環(huán)境地圖分成若干網(wǎng)格區(qū)域，采用蟻群算法進(jìn)行全局路徑規(guī)劃，采用拓展樹隨機(jī)優(yōu)化方法對(duì)蟻群算法進(jìn)行優(yōu)化，進(jìn)而實(shí)現(xiàn)agv搬運(yùn)機(jī)器人的全局靜態(tài)路徑規(guī)劃，得到第一移動(dòng)路徑；用于在agv搬運(yùn)機(jī)器人按照預(yù)設(shè)搬運(yùn)路線移動(dòng)的過程中，利用設(shè)置于agv搬運(yùn)機(jī)器人上的深度相機(jī)采集前方的深度圖像數(shù)據(jù)，對(duì)所采集數(shù)據(jù)進(jìn)行處理，利用處理后的深度相機(jī)數(shù)據(jù)按照預(yù)設(shè)判定規(guī)則進(jìn)行可通行窗口的判斷，基于判定結(jié)果制定行進(jìn)規(guī)則，在agv搬運(yùn)機(jī)器人按照第一移動(dòng)路徑和制定的行進(jìn)規(guī)則移動(dòng)，完成搬運(yùn)任務(wù)后，獲取agv搬運(yùn)機(jī)器人的第一移動(dòng)路徑、實(shí)際移動(dòng)路徑和生產(chǎn)任務(wù)列表，記為路徑耦合數(shù)據(jù)集合，獲取預(yù)設(shè)數(shù)量的路徑耦合數(shù)據(jù)集合，將所采集的路徑耦合數(shù)據(jù)集合按照第一預(yù)設(shè)比例分為訓(xùn)練數(shù)據(jù)集和驗(yàn)證數(shù)據(jù)集；用于利用所采集的訓(xùn)練數(shù)據(jù)集訓(xùn)練路徑自動(dòng)調(diào)優(yōu)模型，利用驗(yàn)證數(shù)據(jù)集對(duì)路徑自動(dòng)調(diào)優(yōu)模型進(jìn)行模型優(yōu)化，優(yōu)化模型參數(shù)，完成路徑自動(dòng)調(diào)優(yōu)模型訓(xùn)練；用于當(dāng)模型訓(xùn)練完成后，用于將下次搬運(yùn)任務(wù)對(duì)應(yīng)的第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表輸入到路徑自動(dòng)調(diào)優(yōu)模型中，將模型輸出的路徑作為最優(yōu)路徑。

39、有益效果：

40、本發(fā)明首先基于蟻群算法進(jìn)行全局路徑規(guī)劃，采用拓展樹隨機(jī)優(yōu)化方法對(duì)蟻群算法進(jìn)行優(yōu)化，得到第一移動(dòng)路徑，在機(jī)器人沿第一移動(dòng)路徑移動(dòng)的過程中，利用設(shè)置于agv搬運(yùn)機(jī)器人上的深度相機(jī)采集前方的深度圖像數(shù)據(jù)，對(duì)所采集數(shù)據(jù)進(jìn)行處理，利用處理后的深度相機(jī)數(shù)據(jù)按照預(yù)設(shè)判定規(guī)則進(jìn)行可通行窗口的判斷，基于判定結(jié)果制定行進(jìn)規(guī)則，在agv搬運(yùn)機(jī)器人按照第一移動(dòng)路徑和制定的行進(jìn)規(guī)則移動(dòng)，完成搬運(yùn)任務(wù)后，獲取agv搬運(yùn)機(jī)器人的第一移動(dòng)路徑、實(shí)際移動(dòng)路徑和生產(chǎn)任務(wù)列表，記為路徑耦合數(shù)據(jù)集合，進(jìn)而訓(xùn)練路徑自動(dòng)調(diào)優(yōu)模型，當(dāng)模型訓(xùn)練完成后，獲取下次搬運(yùn)任務(wù)的agv搬運(yùn)機(jī)器人的第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表，將第一路徑、第一路徑對(duì)應(yīng)的生產(chǎn)任務(wù)列表輸入到路徑自動(dòng)調(diào)優(yōu)模型中，將模型輸出的路徑作為最優(yōu)路徑。實(shí)現(xiàn)了在無(wú)碰撞且滿足客戶需求的情況下，合理地進(jìn)行地圖更新、路徑規(guī)劃、動(dòng)態(tài)避障，且實(shí)現(xiàn)了根據(jù)生產(chǎn)線的生產(chǎn)作業(yè)任務(wù)、實(shí)時(shí)的對(duì)agv的搬運(yùn)路徑進(jìn)行進(jìn)一步調(diào)優(yōu)，解決了不同的生產(chǎn)作業(yè)任務(wù)往往堆放的靜態(tài)障礙物是不同的、機(jī)器人由于無(wú)法提前預(yù)知、會(huì)導(dǎo)致原有的路徑規(guī)劃失效、機(jī)器人無(wú)法通過、需要重新規(guī)劃路徑、可能會(huì)導(dǎo)致機(jī)器人繞行很遠(yuǎn)的路、會(huì)導(dǎo)致浪費(fèi)大量的時(shí)間的問題，進(jìn)一步提高了路徑規(guī)劃的可靠性和agv的搬運(yùn)效率。