本發(fā)明涉及人工智能，尤其涉及一種彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法。

背景技術(shù)：

1、隨著機(jī)器人技術(shù)的不斷成熟與豐富，機(jī)器人在工業(yè)生產(chǎn)、醫(yī)療服務(wù)、物流運(yùn)輸?shù)阮I(lǐng)域的應(yīng)用越來(lái)越廣泛，極大地提升了效率和便捷性。機(jī)器人系統(tǒng)通常由環(huán)境感知、決策、控制和通信模塊組成，其中決策模塊作為核心，負(fù)責(zé)智能決策和任務(wù)判斷。在當(dāng)今機(jī)器人技術(shù)領(lǐng)域中，路徑規(guī)劃研究占據(jù)了核心地位。路徑規(guī)劃是指在配置空間中尋找一條從初始位置到目標(biāo)位置的有效路徑，同時(shí)避開(kāi)各種障礙和滿足某些特定的優(yōu)化準(zhǔn)則。在實(shí)際場(chǎng)景的應(yīng)用中，機(jī)器人需要按照算法生成的軌跡，無(wú)碰撞的從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)，有時(shí)還需考慮多個(gè)目標(biāo)任務(wù)相互約束，并優(yōu)化多個(gè)過(guò)程參數(shù)，如縮短抵達(dá)時(shí)間、減少資源消耗、提高安全性及魯棒性等?？梢灶A(yù)見(jiàn)，軌跡規(guī)劃技術(shù)在未來(lái)應(yīng)用方面上將面臨更多，更嚴(yán)峻的挑戰(zhàn)，亟需創(chuàng)新算法的支持。

2、目前，常見(jiàn)的路徑規(guī)劃算法有算法、算法、遺傳算法、粒子群算法等。這些傳統(tǒng)方法已被廣泛應(yīng)用，但在面對(duì)不同問(wèn)題時(shí)仍會(huì)凸顯自身的局限性。算法需要大量采樣，且無(wú)用節(jié)點(diǎn)較多，效率不高。遺傳算法需要消耗較多算力進(jìn)行多次迭代才能收斂至最優(yōu)解。粒子群算法則容易在面對(duì)高維問(wèn)題時(shí)過(guò)早陷入局部最優(yōu)解。相較于上述傳統(tǒng)方法，快速隨機(jī)探索樹(shù)（rrt）算法因其低復(fù)雜度、適應(yīng)性強(qiáng)、無(wú)需對(duì)環(huán)境進(jìn)行預(yù)建模等優(yōu)點(diǎn)引起了廣泛的關(guān)注。然而rrt算法不含啟發(fā)式信息，探索空間隨機(jī)，且找到的路徑不是最優(yōu)解。為解決這一問(wèn)題，算法被提出，該算法不僅保持了概率完備性，還實(shí)現(xiàn)了漸進(jìn)最優(yōu)性。但這種持續(xù)的路徑優(yōu)化過(guò)程需要大量迭代，導(dǎo)致了算法在實(shí)現(xiàn)收斂到最優(yōu)解的速度相對(duì)較慢。則優(yōu)化了采樣方法，使用偏置點(diǎn)進(jìn)行智能采樣，但該方法更加依賴(lài)初始路徑的質(zhì)量。算法利用三角不等式原理，提供了一種更好的重選父節(jié)點(diǎn)及重布路線的方法，但由于父節(jié)點(diǎn)范圍變廣，搜索時(shí)間將被延長(zhǎng)。

3、rrt的一些改進(jìn)算法同樣被應(yīng)用在面對(duì)動(dòng)態(tài)環(huán)境中的運(yùn)動(dòng)規(guī)劃問(wèn)題，這些算法可分為兩類(lèi)：反應(yīng)式算法和主動(dòng)式算法。反應(yīng)式算法根據(jù)當(dāng)前條件在每個(gè)時(shí)刻只計(jì)算一個(gè)下一個(gè)動(dòng)作，當(dāng)環(huán)境變化時(shí)，需要進(jìn)行路徑重規(guī)劃，該類(lèi)算法可以應(yīng)對(duì)高度動(dòng)態(tài)和不可預(yù)測(cè)的環(huán)境，但也因此需要持續(xù)的高刷新率，其變體專(zhuān)注于提高重新規(guī)劃的快速性。主動(dòng)式算法則需要對(duì)運(yùn)動(dòng)的障礙物進(jìn)行軌跡預(yù)測(cè)，將可能碰撞的路徑刪去，進(jìn)而主動(dòng)避開(kāi)障礙物。riskrrt是一種基于時(shí)間的rrt算法，提出了一種指導(dǎo)規(guī)劃和尋路方法的概率碰撞風(fēng)險(xiǎn)函數(shù)，但并沒(méi)有考慮優(yōu)化路徑。

4、彈性帶（eb）方法可用于在動(dòng)態(tài)環(huán)境中優(yōu)化調(diào)整當(dāng)前軌跡。他以物理特性為啟發(fā)式指導(dǎo)，在優(yōu)化路徑的同時(shí)達(dá)到避免障礙物的效果。eb方法需要一個(gè)初始軌跡和配置空間的構(gòu)建。在動(dòng)態(tài)環(huán)境中，路徑規(guī)劃器需要盡快生成可行的軌跡。當(dāng)工作場(chǎng)景相當(dāng)大時(shí)，例如機(jī)場(chǎng)，構(gòu)建配置空間會(huì)消耗大量的計(jì)算資源，并且很難快速生成可行的初始軌跡。

5、雖然反應(yīng)式和主動(dòng)式算法在機(jī)器人運(yùn)動(dòng)規(guī)劃領(lǐng)域都有廣泛的應(yīng)用，但它們關(guān)注的重點(diǎn)不同。眾多方法一定程度的解決了一部分軌跡規(guī)劃的問(wèn)題，但仍面臨諸多挑戰(zhàn)。因此，如何能在動(dòng)態(tài)環(huán)境中高效生成初始軌跡、全程保持軌跡的安全性、盡可能的提升優(yōu)化軌跡的速度、盡可能的降低算力資源的消耗、保證軌跡的實(shí)時(shí)有效性，仍是機(jī)器人控制領(lǐng)域亟需解決的核心問(wèn)題。

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

1、根據(jù)上述提出的技術(shù)問(wèn)題，而提供一種彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法。本發(fā)明主要利用基于改進(jìn)快速的方法，使用可變采樣區(qū)域的目標(biāo)偏置采樣方法快速得到一條質(zhì)量較優(yōu)的初始可行路徑。通過(guò)將彈性帶方法（eb）與基于時(shí)間的rrt算法結(jié)合，對(duì)軌跡進(jìn)行動(dòng)態(tài)優(yōu)化，實(shí)現(xiàn)對(duì)機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃。

2、本發(fā)明采用的技術(shù)手段如下：

3、一種彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法，包括：

4、初始化機(jī)器人動(dòng)力學(xué)方程，并利用代價(jià)函數(shù)表示最優(yōu)運(yùn)動(dòng)規(guī)劃問(wèn)題；

5、構(gòu)建隨機(jī)樹(shù)數(shù)據(jù)集，結(jié)合可變采樣區(qū)域，執(zhí)行重布線操作，優(yōu)化隨機(jī)樹(shù)結(jié)構(gòu)；

6、利用目標(biāo)偏置方法調(diào)整限制采樣區(qū)域；

7、針對(duì)障礙物采用分層碰撞檢測(cè)方法，進(jìn)行粗略碰撞檢測(cè)和精細(xì)檢測(cè)排除無(wú)效路徑；

8、計(jì)算碰撞風(fēng)險(xiǎn)概率，將碰撞風(fēng)險(xiǎn)概率超出閾值的節(jié)點(diǎn)剔除；

9、應(yīng)用彈性帶方法優(yōu)化機(jī)器人的動(dòng)態(tài)軌跡，實(shí)現(xiàn)移動(dòng)機(jī)器人的動(dòng)態(tài)軌跡規(guī)劃。

10、進(jìn)一步地，所述初始化機(jī)器人動(dòng)力學(xué)方程，具體包括：

11、設(shè)狀態(tài)空間為，碰撞空間，自由空間，是的真子集；在動(dòng)態(tài)環(huán)境中，狀態(tài)空間是時(shí)變的，將碰撞空間記為，自由空間記為；令起始位置為和目標(biāo)位置為，將目標(biāo)區(qū)域定義為，表示目標(biāo)區(qū)域半徑，令機(jī)器人的控制空間為，，將機(jī)器人的動(dòng)力學(xué)表示如下：

12、

13、其中，表示機(jī)器人在時(shí)間戳t時(shí)的狀態(tài)，表示機(jī)器人的動(dòng)力學(xué)函數(shù)，通過(guò)狀態(tài)與控制推導(dǎo)狀態(tài)。

14、進(jìn)一步地，所述利用代價(jià)函數(shù)表示最優(yōu)運(yùn)動(dòng)規(guī)劃問(wèn)題，具體包括：

15、將所有可行軌跡集表示為，構(gòu)造代價(jià)函數(shù)如下：

16、

17、

18、其中，為常數(shù)，為機(jī)器人線速度；和為機(jī)器人前后的兩個(gè)狀態(tài)的位置信息，為時(shí)間戳?xí)r機(jī)器人的線速度；將最優(yōu)運(yùn)動(dòng)規(guī)劃問(wèn)題表示為：

19、

20、其中，表示代價(jià)最小的可行路徑。

21、進(jìn)一步地，所述構(gòu)建隨機(jī)樹(shù)數(shù)據(jù)集，結(jié)合可變采樣區(qū)域，執(zhí)行重布線操作，優(yōu)化隨機(jī)樹(shù)結(jié)構(gòu)，具體包括：

22、構(gòu)建隨機(jī)樹(shù)數(shù)據(jù)集，表示樹(shù)中的一組頂點(diǎn)，表示組內(nèi)各頂點(diǎn)的連接關(guān)系，確定起始點(diǎn)與目標(biāo)點(diǎn)位置，并構(gòu)建包含障礙物的環(huán)境地圖；在每次采樣過(guò)程迭代之前，優(yōu)先判斷是否已經(jīng)確定初始路徑；如未確定初始路徑，則使用所述可變采樣區(qū)域優(yōu)化采樣效果；

23、生成采樣點(diǎn)，遍歷現(xiàn)有隨機(jī)樹(shù)，尋找與距離最近的節(jié)點(diǎn)，從向方向按一個(gè)步長(zhǎng)的距離進(jìn)行延伸，形成新節(jié)點(diǎn)；如果與相連的路徑未被障礙物所阻擋，則該新節(jié)點(diǎn)通過(guò)自適應(yīng)調(diào)整的重連半徑重新選擇父節(jié)點(diǎn)，并執(zhí)行重布線操作，優(yōu)化隨機(jī)樹(shù)結(jié)構(gòu)；若與相連的路徑唄障礙物阻擋，則重新選取采樣策略，算法持續(xù)進(jìn)行，直至達(dá)到設(shè)定的最大迭代次數(shù)限制，任務(wù)結(jié)束。

24、進(jìn)一步地，所述利用目標(biāo)偏置方法調(diào)整限制采樣區(qū)域，具體包括：

25、針對(duì)復(fù)雜或特殊環(huán)境，利用目標(biāo)偏置方法調(diào)整限制采樣區(qū)域生成初始路徑：

26、

27、其中，為隨機(jī)采樣點(diǎn)，為(0,1)范圍內(nèi)生成的均勻分布隨機(jī)數(shù)，表示在自由空間中隨機(jī)選取采樣點(diǎn)；是設(shè)定的目標(biāo)偏差閾值，為目標(biāo)點(diǎn)，當(dāng)路徑未被障礙物阻擋時(shí)，在目標(biāo)點(diǎn)上采樣，朝向目標(biāo)點(diǎn)擴(kuò)展，直至遇到障礙物阻擋該路徑。

28、進(jìn)一步地，所述粗略碰撞檢測(cè)采用包圍體技術(shù)，對(duì)于每個(gè)多邊形障礙物，計(jì)算其中心位置及包圍圓半徑，得到障礙物中心到路徑段的最小距離，障礙物中心的坐標(biāo)為，路徑段的兩個(gè)端點(diǎn)和的坐標(biāo)分別為和，設(shè)向量為路徑段方向向量，為端點(diǎn)到的向量，有：

29、

30、

31、用參數(shù)表示在線段上的投影位置，通過(guò)內(nèi)積計(jì)算：

32、

33、

34、將限制在之內(nèi)，確保投影點(diǎn)在實(shí)際路徑上而非路徑延長(zhǎng)線上：

35、

36、根據(jù)參數(shù)，將投影點(diǎn)坐標(biāo)表示為：

37、

38、則最小距離為障礙物中心點(diǎn)到投影點(diǎn)距離，表示為：

39、

40、若最小距離大于包圍圓半徑則認(rèn)為無(wú)碰撞，粗略檢測(cè)完畢。

41、進(jìn)一步地，所述精細(xì)檢測(cè)使用線性插值方法在路徑上生成插值點(diǎn)，遍歷各點(diǎn)以檢測(cè)是否發(fā)生碰撞；

42、設(shè)路徑起始點(diǎn)和終止點(diǎn)分別為和，通過(guò)參數(shù)進(jìn)行線性插值，生成的任意點(diǎn)為：

43、

44、檢測(cè)過(guò)程中通過(guò)設(shè)置步長(zhǎng)生成插值點(diǎn)列，檢測(cè)障礙物在給定坐標(biāo)系下的上下左右邊界，即寬度和高度，將表示為：

45、

46、其中，為障礙物的上邊界位置參數(shù)，為障礙物的下邊界位置參數(shù)，為障礙物的左邊界位置參數(shù)，為障礙物的右邊界位置參數(shù)，為障礙物的橫向長(zhǎng)度參數(shù)，為障礙物的縱向長(zhǎng)度參數(shù)；和為障礙物中心坐標(biāo)。若插值點(diǎn)滿足條件：，認(rèn)為發(fā)生碰撞，算法終止并標(biāo)記該路徑無(wú)效。

47、進(jìn)一步地，所述計(jì)算碰撞風(fēng)險(xiǎn)概率，將碰撞風(fēng)險(xiǎn)概率超出閾值的節(jié)點(diǎn)剔除，具體包括：

48、將機(jī)器人的狀態(tài)表示為，是該節(jié)點(diǎn)的父節(jié)點(diǎn)和子節(jié)點(diǎn)，為樹(shù)深度，為時(shí)間戳，根節(jié)點(diǎn)的時(shí)間戳為，兩節(jié)點(diǎn)間的時(shí)間增量為；為控制輸入，為控制輸出，為時(shí)的概率碰撞風(fēng)險(xiǎn)，表示為：

49、

50、

51、其中，表示由于靜態(tài)障礙物的碰撞概率，表示由于時(shí)間戳處的移動(dòng)障礙物的碰撞概率，表示由于時(shí)間戳處的移動(dòng)障礙物的規(guī)劃軌跡的碰撞概率；在優(yōu)化過(guò)程中計(jì)算每個(gè)節(jié)點(diǎn)的碰撞概率，如果碰撞概率大于閾值，則不將相應(yīng)的節(jié)點(diǎn)添加到優(yōu)化軌跡中。

52、進(jìn)一步地，所述應(yīng)用彈性帶方法優(yōu)化機(jī)器人的動(dòng)態(tài)軌跡，具體包括：

53、將內(nèi)部收縮力定義如下：

54、

55、根據(jù)以下等式對(duì)節(jié)點(diǎn)位置進(jìn)行更新：

56、

57、其中，和為兩個(gè)正比例因子，彈性帶方法優(yōu)化過(guò)程將在以下條件被滿足時(shí)結(jié)束：

58、當(dāng)，為軌跡上的節(jié)點(diǎn)數(shù)，表明收縮力小于設(shè)定的力為最優(yōu)軌跡；當(dāng)機(jī)器人本身需要滿足的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束使優(yōu)化過(guò)程無(wú)法進(jìn)一步進(jìn)行時(shí)，優(yōu)化過(guò)程結(jié)束；當(dāng)新節(jié)點(diǎn)碰撞概率超過(guò)閾值，即，則將新節(jié)點(diǎn)無(wú)效化。

59、較現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：

60、本發(fā)明提供的彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法，提高了計(jì)算效率與實(shí)時(shí)性。本發(fā)明使用可變采樣區(qū)域的目標(biāo)偏置采樣方法，相較于全地圖隨機(jī)采樣提升了采樣效率，提高了生成初始軌跡的速度與質(zhì)量，在快速生成初始軌跡后，使用基于彈性帶的方法優(yōu)化每一個(gè)節(jié)點(diǎn)的位置，在此基礎(chǔ)上沒(méi)有改變最初生成的原始時(shí)基樹(shù)的結(jié)構(gòu)，沒(méi)有分支上的浪費(fèi)，且避免了額外的計(jì)算來(lái)修改時(shí)基樹(shù)上的節(jié)點(diǎn)信息。

61、本發(fā)明提供的彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法，確保了動(dòng)態(tài)環(huán)境中的安全性。本發(fā)明將概率碰撞風(fēng)險(xiǎn)函數(shù)作為軌跡的外部排斥力，使得軌跡上的節(jié)點(diǎn)可以避開(kāi)運(yùn)動(dòng)障礙可能會(huì)到達(dá)的地方，確保軌跡規(guī)劃的可行性和安全性。

62、本發(fā)明提供的彈性帶約束下的移動(dòng)機(jī)器人動(dòng)態(tài)軌跡規(guī)劃方法，提升了魯棒性與適應(yīng)性。通過(guò)基于時(shí)間的rrt算法，本發(fā)明在軌跡優(yōu)化過(guò)程中實(shí)現(xiàn)了對(duì)每個(gè)節(jié)點(diǎn)狀態(tài)及碰撞風(fēng)險(xiǎn)的檢測(cè)，顯著提升了系統(tǒng)的魯棒性和適應(yīng)性。機(jī)器人在復(fù)雜環(huán)境中能夠自適應(yīng)調(diào)整軌跡，滿足避障的需求。

63、基于上述理由本發(fā)明可在人工智能等領(lǐng)域廣泛推廣。