本發(fā)明涉及機(jī)械臂路徑規(guī)劃，具體而言，涉及一種基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃方法及系統(tǒng)。

背景技術(shù)：

1、機(jī)械臂具有高精度和高效率作業(yè)優(yōu)勢(shì)，廣泛應(yīng)用于裝備加工制造、大型設(shè)施維護(hù)、水果自動(dòng)采摘等生產(chǎn)活動(dòng)中。機(jī)械臂路徑規(guī)劃是指在機(jī)械臂的初始構(gòu)型和目標(biāo)構(gòu)型之間規(guī)劃生成一條由一系列離散節(jié)點(diǎn)(每個(gè)節(jié)點(diǎn)對(duì)應(yīng)一組機(jī)械臂關(guān)節(jié)構(gòu)型)組成的無碰撞路徑，使得機(jī)械臂沿著該路徑就可以無碰撞地從初始狀態(tài)運(yùn)動(dòng)到目標(biāo)狀態(tài)以完成作業(yè)任務(wù)。機(jī)械臂的路徑規(guī)劃通常在高維關(guān)節(jié)空間中進(jìn)行，便于處理關(guān)節(jié)角狀態(tài)約束和避免機(jī)械臂逆運(yùn)動(dòng)學(xué)解的存在性和多解性問題。然而，由于笛卡爾空間與高維關(guān)節(jié)空間之間的多維耦合和強(qiáng)非線性映射關(guān)系，導(dǎo)致無法在高維關(guān)節(jié)空間中顯式描述機(jī)械臂運(yùn)動(dòng)的無障礙空間，進(jìn)而使得dijkstra和a*等依賴于顯式柵格地圖的規(guī)劃算法難以高效適配機(jī)械臂系統(tǒng)。此外，隨著機(jī)械臂關(guān)節(jié)空間維數(shù)和任務(wù)復(fù)雜度的增加，路徑規(guī)劃的計(jì)算復(fù)雜度和時(shí)間成本呈指數(shù)增長(zhǎng)，這使得機(jī)械臂的快速路徑規(guī)劃仍然是一個(gè)具有挑戰(zhàn)性的問題。

2、為了解決機(jī)械臂高維關(guān)節(jié)空間路徑規(guī)劃問題，國(guó)內(nèi)外研究人員提出了基于隨機(jī)采樣的快速探索隨機(jī)樹(rapidly-exploring?random?trees，簡(jiǎn)稱rrt)算法，通過在機(jī)械臂關(guān)節(jié)空間中隨機(jī)采樣擴(kuò)展新節(jié)點(diǎn)和增量式生長(zhǎng)隨機(jī)樹來規(guī)劃?rùn)C(jī)械臂從初始構(gòu)型運(yùn)動(dòng)到目標(biāo)構(gòu)型的無碰撞路徑，避免了對(duì)機(jī)械臂無障礙關(guān)節(jié)空間區(qū)域的顯式表達(dá)，因而對(duì)高維機(jī)械臂的運(yùn)動(dòng)規(guī)劃問題有很好的適用性。然而，rrt算法由于關(guān)節(jié)空間采樣探索過程的強(qiáng)隨機(jī)性和盲目性，使得路徑節(jié)點(diǎn)擴(kuò)展效率較低、隨機(jī)樹生長(zhǎng)速度較慢，進(jìn)而導(dǎo)致機(jī)械臂路徑規(guī)劃效率低、耗時(shí)較長(zhǎng)。

3、近年來，針對(duì)制造業(yè)自動(dòng)化對(duì)機(jī)械臂快速路徑規(guī)劃的實(shí)際需求，國(guó)內(nèi)外學(xué)者相繼提出了rrt-connect、gbi-rrt等從rrt算法改進(jìn)而來的變體算法(用rrts表示)并驗(yàn)證了其在機(jī)械臂路徑規(guī)劃任務(wù)中的有效性。然而，rrts算法為了加快隨機(jī)樹向目標(biāo)節(jié)點(diǎn)的生長(zhǎng)，將采樣點(diǎn)概率性地設(shè)置為與目標(biāo)節(jié)點(diǎn)重合。通過最小化由歐氏距離決定的代價(jià)，從隨機(jī)樹中已有的節(jié)點(diǎn)中選擇距離目標(biāo)節(jié)點(diǎn)最近的節(jié)點(diǎn)，然后生成一個(gè)新的節(jié)點(diǎn)并進(jìn)行碰撞檢測(cè)。擴(kuò)展過程將迭代執(zhí)行，直到獲得新的無碰撞節(jié)點(diǎn)。然而，事實(shí)證明，朝向目標(biāo)節(jié)點(diǎn)擴(kuò)展新節(jié)點(diǎn)失敗的最鄰近節(jié)點(diǎn)，其再次朝向目標(biāo)節(jié)點(diǎn)擴(kuò)展也會(huì)因發(fā)生碰撞而失敗。因此，隨機(jī)樹將重復(fù)向目標(biāo)節(jié)點(diǎn)生成無效的新節(jié)點(diǎn)，導(dǎo)致額外時(shí)間消耗。這嚴(yán)重限制了機(jī)械臂在對(duì)時(shí)間消耗極端敏感的作業(yè)任務(wù)中的實(shí)際應(yīng)用。此外，在現(xiàn)有的研究中，如果新節(jié)點(diǎn)遇到障礙物，從最近鄰近節(jié)點(diǎn)向采樣點(diǎn)擴(kuò)展的新節(jié)點(diǎn)將被丟棄。然后，在采樣空間中向新的采樣點(diǎn)重新生成一個(gè)新節(jié)點(diǎn)并進(jìn)行碰撞檢測(cè)。上述過程將迭代執(zhí)行，直到成功獲取新的無碰撞節(jié)點(diǎn)或達(dá)到迭代極限。然而，由于rrts算法的隨機(jī)性，新的采樣點(diǎn)可能位于采樣空間的任何位置，從障礙物附近的節(jié)點(diǎn)向新的采樣點(diǎn)擴(kuò)展的新節(jié)點(diǎn)仍然有很高的碰撞概率，造成低效避障探索和過多時(shí)間消耗，限制了rrts算法在多障礙物環(huán)境中的應(yīng)用。

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

1、本發(fā)明要解決的技術(shù)問題是：

2、為了解決現(xiàn)有機(jī)械臂路徑規(guī)劃算法因概率性目標(biāo)偏執(zhí)擴(kuò)展方式造成的隨機(jī)樹多次重復(fù)擴(kuò)展失敗和路徑規(guī)劃時(shí)間成本高的問題。

3、本發(fā)明為解決上述技術(shù)問題所采用的技術(shù)方案：

4、本發(fā)明提供了一種基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃方法，包括以下步驟：

5、s100、建立地面慣性坐標(biāo)系、機(jī)械臂基座坐標(biāo)系和機(jī)械臂各關(guān)節(jié)坐標(biāo)系，根據(jù)機(jī)械臂作業(yè)任務(wù)需求，確定機(jī)械臂初始構(gòu)型θstart和目標(biāo)構(gòu)型θgoal；構(gòu)建環(huán)境障礙物的碰撞包圍盒模型，并確定所有障礙物的碰撞包圍盒模型的三維尺寸及位姿坐標(biāo)；

6、s200、在機(jī)械臂關(guān)節(jié)空間中同時(shí)創(chuàng)建兩棵相互吸引的隨機(jī)樹etree1和隨機(jī)樹etree2，隨機(jī)樹etree1以機(jī)械臂的初始構(gòu)型θstart為起始節(jié)點(diǎn)，以目標(biāo)構(gòu)型θgoal為目標(biāo)節(jié)點(diǎn)；隨機(jī)樹etree2以機(jī)械臂的目標(biāo)構(gòu)型θgoal為起始節(jié)點(diǎn)，以初始構(gòu)型θstart為目標(biāo)節(jié)點(diǎn)；隨機(jī)樹etree1和隨機(jī)樹etree2交替擴(kuò)展新節(jié)點(diǎn)；

7、s300、給定目標(biāo)偏向基準(zhǔn)概率βgoal∈[0,1]，按均勻概率產(chǎn)生隨機(jī)數(shù)βrand＝rand(1,1)，若隨機(jī)數(shù)βrand小于基準(zhǔn)概率βgoal，則在關(guān)節(jié)空間中隨機(jī)生成θrand作為采樣點(diǎn)θsample，并基于最小化加權(quán)歐幾里德距離從隨機(jī)樹etree1中找到與θsample最鄰近的節(jié)點(diǎn)θnearest；若隨機(jī)數(shù)βrand大于或等于基準(zhǔn)概率βgoal，則選擇目標(biāo)節(jié)點(diǎn)θgoal作為采樣點(diǎn)θsample，并基于最小化加權(quán)歐幾里德距離成本，從候選池eunexplored的現(xiàn)有節(jié)點(diǎn)中選擇最接近采樣點(diǎn)θsample的最鄰近節(jié)點(diǎn)θnearest；

8、s400、在步驟s300獲取采樣點(diǎn)θsample的最鄰近節(jié)點(diǎn)θnearest后，從θnearest出發(fā)沿θnearest和θsample的連線方向θdir按照預(yù)定步長(zhǎng)θstep擴(kuò)展生成新節(jié)點(diǎn)θnew，判斷θnearest與θnew的連線是否與障礙物發(fā)生碰撞，若不發(fā)生碰撞則把新節(jié)點(diǎn)θnew添加到隨機(jī)樹etree1中；若發(fā)生碰撞則基于原擴(kuò)展方向θdir轉(zhuǎn)向得到新擴(kuò)展方向θver_unit，然后讓最鄰近節(jié)點(diǎn)θnearest沿新擴(kuò)展方向θver_unit擴(kuò)展得到新節(jié)點(diǎn)θnew，并再次判斷θnearest與θnew的連線是否與障礙物發(fā)生碰撞，若再次發(fā)生碰撞則舍棄θnew并跳轉(zhuǎn)執(zhí)行步驟s300，若不發(fā)生碰撞則把成功生成的新節(jié)點(diǎn)θnew添加到隨機(jī)樹etree1中；

9、s500、當(dāng)隨機(jī)樹etree1成功擴(kuò)展得到新節(jié)點(diǎn)θnew后，將新節(jié)點(diǎn)θnew與隨機(jī)樹etree2上的所有節(jié)點(diǎn)逐一配對(duì)，按照距離由近及遠(yuǎn)進(jìn)行捷徑連接，并在連線上按照給定步長(zhǎng)θstep_collision進(jìn)行插值點(diǎn)碰撞檢測(cè)，若未檢測(cè)到捷徑連接線上所有插值點(diǎn)都無碰撞的配對(duì)節(jié)點(diǎn)則交換兩棵隨機(jī)樹并跳轉(zhuǎn)執(zhí)行步驟s300；若找到無碰撞的配對(duì)節(jié)點(diǎn)，則表明兩棵隨機(jī)樹etree1和etree2已成功連接，然后按照生長(zhǎng)步長(zhǎng)θstep在配對(duì)節(jié)點(diǎn)間補(bǔ)入新節(jié)點(diǎn)θadd，并添加到隨機(jī)樹etree1中；

10、s600、在etree1和etree2中分別從連接處的θnew和θother出發(fā)逐節(jié)點(diǎn)回溯到各自的起始節(jié)點(diǎn)，得到一條由一系列離散節(jié)點(diǎn)有序連接而成的路徑πpath，由此獲得機(jī)械臂從初始構(gòu)型θstart運(yùn)動(dòng)到目標(biāo)構(gòu)型θgoal的無碰撞路徑。

11、進(jìn)一步地，在步驟s300中，當(dāng)隨機(jī)數(shù)βrand小于基準(zhǔn)概率βgoal時(shí)，采樣點(diǎn)θsample的最鄰近節(jié)點(diǎn)θnearest為，

12、θsampleθrand，βrand＜βgoal??(1)

13、

14、其中，ωi表示機(jī)械臂第i個(gè)關(guān)節(jié)角的權(quán)值，θk是隨機(jī)樹etree1中的第k個(gè)節(jié)點(diǎn)。

15、進(jìn)一步地，在步驟s300中，當(dāng)隨機(jī)數(shù)βrand大于或等于基準(zhǔn)概率βgoal時(shí)，采樣點(diǎn)θsample的最鄰近節(jié)點(diǎn)θnearest為，

16、θsampleθgoal，βrand≥βgoal??(3)

17、

18、eunexplored＝exclude(eunexplored，θnearest)??(5)

19、其中，eunexplored＝exclude(eunexplored，θnearest)表示把候選池eunexplored中被選為目標(biāo)節(jié)點(diǎn)θgoal最鄰近節(jié)點(diǎn)的θnearest移出候選池eunexplored。

20、進(jìn)一步地，在步驟s400中，從θnearest出發(fā)沿著θnearest和θsample的連線方向θdir按照預(yù)定步長(zhǎng)θstep擴(kuò)展生成新節(jié)點(diǎn)θnew，即，

21、θsn＝θsample-θnearest??(6)

22、

23、θnew＝θnearest+θstepθdir??(8)

24、其中，θsn表示節(jié)點(diǎn)擴(kuò)展方向，θdir表示沿著節(jié)點(diǎn)擴(kuò)展方向的單位向量。

25、進(jìn)一步地，在步驟s400中，新擴(kuò)展方向θver_unit的計(jì)算方法為，

26、θver＝rand(1,n)?(9)

27、

28、

29、

30、其中，θdir是節(jié)點(diǎn)的原擴(kuò)展方向，θver是轉(zhuǎn)向后的新擴(kuò)展方向，θver_unit是新擴(kuò)展方向的單位向量，imax是θdir的所有元素絕對(duì)值中的最大值對(duì)應(yīng)的列序號(hào)，βflag是用于表征兩個(gè)節(jié)點(diǎn)之間的捷徑連接線是否存在碰撞的標(biāo)志量。

31、進(jìn)一步地，在步驟s500中，插值點(diǎn)碰撞檢測(cè)為，

32、

33、

34、其中，函數(shù)用于對(duì)etree2中的元素按照規(guī)則f重新升序排列。

35、進(jìn)一步地，在步驟s500中，當(dāng)找到無碰撞配對(duì)節(jié)點(diǎn)后，將新節(jié)點(diǎn)θadd添加到隨機(jī)樹etree1中，

36、θadd＝θnew+k(θnear-θnew)/nadd,?k＝1,2,…,nadd-1?(15)

37、

38、一種基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃系統(tǒng)，該系統(tǒng)具有與上述步驟對(duì)應(yīng)的程序模塊，運(yùn)行時(shí)執(zhí)行上述的基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃方法中的步驟。

39、一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)存儲(chǔ)有計(jì)算機(jī)程序，所述計(jì)算機(jī)程序配置為由處理器調(diào)用時(shí)實(shí)現(xiàn)基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃方法的步驟。

40、相較于現(xiàn)有技術(shù)，本發(fā)明的有益效果是：

41、①針對(duì)現(xiàn)有路徑規(guī)劃方法時(shí)間成本高、擴(kuò)展質(zhì)量差的問題，本發(fā)明提出了基于動(dòng)態(tài)候選池引導(dǎo)的節(jié)點(diǎn)選取策略來引導(dǎo)最鄰近節(jié)點(diǎn)的最佳選擇，通過候選池賦予節(jié)點(diǎn)記憶性，避免了同一個(gè)節(jié)點(diǎn)被多次選中為目標(biāo)節(jié)點(diǎn)的最鄰近節(jié)點(diǎn)，解決了現(xiàn)有算法概率性目標(biāo)偏執(zhí)擴(kuò)展方式造成的隨機(jī)樹多次重復(fù)擴(kuò)展失敗問題，使規(guī)劃進(jìn)程及時(shí)跳出局部最優(yōu)，實(shí)現(xiàn)了隨機(jī)樹朝向目標(biāo)節(jié)點(diǎn)方向快速擴(kuò)展生成無碰撞節(jié)點(diǎn)，進(jìn)而大大降低路徑規(guī)劃的時(shí)間成本。

42、②針對(duì)現(xiàn)有路徑算法在障礙物附近采樣擴(kuò)展新節(jié)點(diǎn)的碰撞概率高，難以快速生成可行新節(jié)點(diǎn)的問題，提出了基于碰撞節(jié)點(diǎn)啟發(fā)的避障區(qū)域探索策略來調(diào)節(jié)障礙物附近新節(jié)點(diǎn)的擴(kuò)展方向，使隨機(jī)樹能夠以較小的盲目性快速擴(kuò)展無碰撞新節(jié)點(diǎn)以繞過障礙物區(qū)域，避免隨機(jī)樹在障礙物附近執(zhí)行過多的重復(fù)探索和無效擴(kuò)展，從而提高節(jié)點(diǎn)質(zhì)量并縮短路徑規(guī)劃時(shí)長(zhǎng)。

43、綜上，本發(fā)明以機(jī)械臂為應(yīng)用對(duì)象，在傳統(tǒng)路徑規(guī)劃算法的基礎(chǔ)上進(jìn)行改進(jìn)優(yōu)化，構(gòu)建了基于動(dòng)態(tài)候選池引導(dǎo)和碰撞節(jié)點(diǎn)啟發(fā)的機(jī)械臂快速路徑規(guī)劃方法，在保證機(jī)械臂避障效果的同時(shí)，可以更快響應(yīng)機(jī)械臂任務(wù)需求，在短時(shí)間內(nèi)規(guī)劃出無碰撞路徑，進(jìn)而在相同時(shí)間內(nèi)執(zhí)行更多作業(yè)任務(wù)，能夠更好地滿足機(jī)械臂快速、低時(shí)耗要求，具有較高的工程應(yīng)用價(jià)值。通過與現(xiàn)有路徑規(guī)劃算法的對(duì)比實(shí)驗(yàn)也可證明，本發(fā)明在時(shí)間成本、擴(kuò)展質(zhì)量、內(nèi)存成本和探索質(zhì)量上均有所改善，尤其在實(shí)現(xiàn)成本上，均值時(shí)間僅為現(xiàn)有算法的約1/7，證明了本算法可有效提高路徑規(guī)劃速度。