亚洲成年人黄色一级片,日本香港三级亚洲三级,黄色成人小视频,国产青草视频,国产一区二区久久精品,91在线免费公开视频,成年轻人网站色直接看

一種基于并行采樣點(diǎn)優(yōu)化RRT算法的機(jī)器人路徑規(guī)劃方法

文檔序號:40391067發(fā)布日期:2024-12-20 12:14閱讀:8來源:國知局
一種基于并行采樣點(diǎn)優(yōu)化RRT算法的機(jī)器人路徑規(guī)劃方法

本發(fā)明涉及機(jī)器人自主導(dǎo)航,具體涉及一種基于并行采樣點(diǎn)優(yōu)化rrt算法的機(jī)器人路徑規(guī)劃方法。


背景技術(shù):

1、

2、近年來人工智能技術(shù)的進(jìn)步促使越來越多自主移動機(jī)器人應(yīng)用于各個領(lǐng)域,如運(yùn)輸、制造和醫(yī)療等行業(yè),路徑規(guī)劃是移動機(jī)器人的關(guān)鍵技術(shù)和主要挑戰(zhàn),路徑規(guī)劃問題是在有障礙物的空間中,根據(jù)一定評價(jià)標(biāo)準(zhǔn)(規(guī)劃時間和路徑長度等),規(guī)劃出從起點(diǎn)到目標(biāo)點(diǎn)、滿足機(jī)器人運(yùn)動約束的全局無碰撞路徑。

3、根據(jù)不同的理論,機(jī)器人路徑規(guī)劃算法主要分為三類:(1)基于圖的規(guī)劃算法,如dijkstra和a*算法,可以找到最優(yōu)路徑,但在復(fù)雜或者分辨率較高的環(huán)境中計(jì)算量呈指數(shù)級增長;(2)基于人工勢場的規(guī)劃算法,通過最小化勢能函數(shù)來尋找路徑,這種方法可能會陷入局部最小值,導(dǎo)致路徑規(guī)劃失??;(3)基于采樣的規(guī)劃算法,如快速探索隨機(jī)樹(rapidly-exploring?random?tree,rrt)算法,通過生成隨機(jī)采樣點(diǎn)探索未知空間,將無障礙空間中采樣的一組點(diǎn)連接起來,構(gòu)建可行路徑。

4、rrt算法避免對空間進(jìn)行幾何建模,搜索速度快,具有概率完備性,廣泛應(yīng)用于移動機(jī)器人、無人機(jī)和機(jī)械臂等路徑規(guī)劃問題,但采樣的隨機(jī)性和盲目性會造成大量無效采樣節(jié)點(diǎn),導(dǎo)致迭代次數(shù)增加,搜索得到的路徑存在大量冗余節(jié)點(diǎn),路徑曲折不連續(xù),規(guī)劃路徑不具備漸近最優(yōu)性,針對不同規(guī)模的地圖環(huán)境,算法規(guī)劃效率和路徑質(zhì)量仍有很大的提升空間。


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

1、

2、本發(fā)明的目的是針對現(xiàn)有技術(shù)的不足,提供一種基于并行采樣點(diǎn)優(yōu)化rrt算法的機(jī)器人路徑規(guī)劃方法,利用多點(diǎn)并行加權(quán)啟發(fā)式采樣策略減少無效采樣節(jié)點(diǎn),并在采樣過程中結(jié)合評價(jià)函數(shù)降低隨機(jī)采樣的盲目性,提高采樣質(zhì)量,采用動態(tài)步長擴(kuò)展策略和雙樹引導(dǎo)機(jī)制,使隨機(jī)樹擴(kuò)展具有靈活性和方向性,加快算法收斂,在算法初始路徑生成后,對路徑進(jìn)行重布線優(yōu)化,降低路徑長度和曲率,提升路徑質(zhì)量。

3、為實(shí)現(xiàn)上述目的,本發(fā)明提供了如下技術(shù)方案:

4、一種基于并行采樣點(diǎn)優(yōu)化rrt算法的機(jī)器人路徑規(guī)劃方法,包括以下步驟:

5、步驟1,定義xfree和xobs分別為自由空間和障礙物空間,xinit為機(jī)器人在狀態(tài)空間中運(yùn)動的起始節(jié)點(diǎn),xgoal為目標(biāo)節(jié)點(diǎn),創(chuàng)建起點(diǎn)樹t1和終點(diǎn)樹t2,設(shè)置算法參數(shù);

6、步驟2,對于起點(diǎn)樹t1,在狀態(tài)空間中隨機(jī)并行采樣多個節(jié)點(diǎn),并行采樣點(diǎn)的個數(shù)由地圖復(fù)雜度決定,對并行采樣點(diǎn)進(jìn)行篩選,舍棄落入障礙物內(nèi)的采樣點(diǎn),歸一化處理距離和角度兩個度量,建立含有距離約束和角度約束的評價(jià)函數(shù),利用評價(jià)函數(shù)對剩余采樣點(diǎn)選取最優(yōu)節(jié)點(diǎn)進(jìn)行擴(kuò)展,確定最優(yōu)節(jié)點(diǎn)的坐標(biāo)和新節(jié)點(diǎn)的擴(kuò)展方向;

7、步驟3,檢測最近節(jié)點(diǎn)周圍障礙物分布信息,動態(tài)選取擴(kuò)展步長,以最近節(jié)點(diǎn)為起始端點(diǎn),向最優(yōu)節(jié)點(diǎn)延伸步長,得到起點(diǎn)樹t1中新的擴(kuò)展節(jié)點(diǎn);

8、步驟4,連接新的擴(kuò)展節(jié)點(diǎn)與最近節(jié)點(diǎn)進(jìn)行中間點(diǎn)碰撞檢測,離散化路徑段,得到所有離散路徑點(diǎn)后,判斷其是否在障礙物上,若在障礙物上,則返回步驟二,若不在障礙物上,將新節(jié)點(diǎn)添加到隨機(jī)樹t1中;

9、步驟5,對于終點(diǎn)樹t2,在采樣空間中隨機(jī)并行采樣多個節(jié)點(diǎn),起點(diǎn)樹t1中新生成的擴(kuò)展節(jié)點(diǎn)作為終點(diǎn)樹t2的局部目標(biāo)點(diǎn),為終點(diǎn)樹t2選取最優(yōu)節(jié)點(diǎn)提供啟發(fā)式,確定最優(yōu)節(jié)點(diǎn)的坐標(biāo),通過步驟3和步驟4進(jìn)行中間點(diǎn)碰撞檢測和動態(tài)選取步長擴(kuò)展。

10、步驟6,重復(fù)執(zhí)行步驟2至步驟5,直至起點(diǎn)樹t1和終點(diǎn)樹t2新生成的節(jié)點(diǎn)小于連接閾值,連接兩點(diǎn),分別以兩樹新生成的節(jié)點(diǎn)為起始端點(diǎn)回溯至起始點(diǎn)和目標(biāo)點(diǎn),得到初始路徑,對初始路徑進(jìn)行全局路徑重規(guī)劃和局部路徑重規(guī)劃,濾除非關(guān)鍵節(jié)點(diǎn),提升初始路徑質(zhì)量;

11、步驟7,對于步驟6所得的優(yōu)化路徑,在路徑相鄰節(jié)點(diǎn)間構(gòu)建控制點(diǎn),通過設(shè)置閾值選擇構(gòu)建控制點(diǎn)方式,利用三次b樣條曲線對構(gòu)建控制點(diǎn)的路徑進(jìn)行擬合優(yōu)化,生成滿足機(jī)器人最大曲率約束且擬合度高的平滑路徑。

12、具體的,步驟一中所述定義自由空間、障礙物空間、起始點(diǎn)和目標(biāo)點(diǎn),創(chuàng)建兩棵隨機(jī)樹,過程如下:

13、步驟1.1:導(dǎo)入地圖數(shù)據(jù),對地圖進(jìn)行二值化處理;

14、步驟1.2:設(shè)置機(jī)器人移動的起始點(diǎn)坐標(biāo)xinit和終點(diǎn)坐標(biāo)xgoal,分別以起始點(diǎn)和目標(biāo)點(diǎn)為根節(jié)點(diǎn)構(gòu)建兩棵隨機(jī)樹,定義兩棵隨機(jī)樹的連接閾值。

15、進(jìn)一步地,步驟一中所述算法參數(shù)值包括距離因子、角度因子、安全距離和隨機(jī)樹擴(kuò)展步長。

16、具體的,步驟2中所述起點(diǎn)樹t1多點(diǎn)并行加權(quán)啟發(fā)式采樣和選取最優(yōu)節(jié)點(diǎn)進(jìn)行擴(kuò)展的過程如下:

17、步驟2.1:在狀態(tài)空間中同時并行采樣多個節(jié)點(diǎn),并行采樣點(diǎn)的個數(shù)由地圖復(fù)雜度決定,經(jīng)過大量的實(shí)驗(yàn)研究分析,發(fā)現(xiàn)簡單環(huán)境中并行采樣三個節(jié)點(diǎn),復(fù)雜環(huán)境中并行采樣五個節(jié)點(diǎn)規(guī)劃效果最佳,判斷并行采樣節(jié)點(diǎn)是否在障礙物范圍內(nèi),舍棄落入障礙物內(nèi)的采樣點(diǎn),找出隨機(jī)樹分支中距離篩選后剩余采樣點(diǎn)最近的節(jié)點(diǎn)xnear;

18、步驟2.2:確定xnear到目標(biāo)點(diǎn)的歐氏距離dn,起始點(diǎn)到目標(biāo)點(diǎn)的歐氏距離dn,相鄰三個節(jié)點(diǎn)xparent、xnear和xrand間的角度tn,確定篩選后剩余采樣點(diǎn)到起始點(diǎn)和目標(biāo)點(diǎn)的歐式距離a(x)和n(x),建立起點(diǎn)樹t1評價(jià)函數(shù)f1(x);

19、步驟2.3:分別計(jì)算篩選后剩余采樣點(diǎn)的評價(jià)函數(shù)值,從中選取出評價(jià)函數(shù)最小的采樣點(diǎn),確定最優(yōu)節(jié)點(diǎn)的坐標(biāo)以及新節(jié)點(diǎn)xnew的擴(kuò)展方向。

20、具體的,步驟3中所述檢測最近節(jié)點(diǎn)周圍障礙物分布信息,動態(tài)選取擴(kuò)展步長過程如下:

21、步驟3.1:繪制一個以最近節(jié)點(diǎn)xnearest為圓心,安全距離rsafe_dis1和rsafe_dis2為半徑的圓,提取圓上各區(qū)域的點(diǎn);

22、步驟3.2:檢測圓域上點(diǎn)集是否在狀態(tài)空間外和障礙物上,動態(tài)選取擴(kuò)展步長lstep1、lstep2和lstep3,以最近節(jié)點(diǎn)為起始端點(diǎn),向最優(yōu)節(jié)點(diǎn)延伸步長,得到起點(diǎn)樹t1中新的擴(kuò)展節(jié)點(diǎn);

23、具體的,步驟4中所述連接最優(yōu)節(jié)點(diǎn)與最近節(jié)點(diǎn)進(jìn)行中間點(diǎn)碰撞檢測過程如下:

24、步驟4.1:連接新的擴(kuò)展節(jié)點(diǎn)與最近節(jié)點(diǎn),以地圖最小分辨率得到所有離散路徑點(diǎn);

25、步驟4.2:得到所有離散路徑點(diǎn)后,判斷其是否在障礙物上,若在障礙物上,則返回步驟二重新在空間中并行采樣,反之,則將新的擴(kuò)展節(jié)點(diǎn)添加到隨機(jī)樹t1中。

26、具體的,步驟5中所述起點(diǎn)樹t1中新生成的擴(kuò)展節(jié)點(diǎn)作為終點(diǎn)樹t2的局部目標(biāo)點(diǎn),為終點(diǎn)樹t2選取最優(yōu)節(jié)點(diǎn)提供啟發(fā)式的過程如下:

27、步驟5.1:在狀態(tài)空間中同時并行采樣多個節(jié)點(diǎn),并行采樣點(diǎn)的個數(shù)由地圖復(fù)雜度決定,簡單環(huán)境中并行采樣三個節(jié)點(diǎn),復(fù)雜環(huán)境中并行采樣五個節(jié)點(diǎn),判斷并行采樣節(jié)點(diǎn)是否在障礙物范圍內(nèi),舍棄落入障礙物內(nèi)的采樣點(diǎn),找出隨機(jī)樹分支中距離篩選后剩余采樣點(diǎn)最近的節(jié)點(diǎn)xnear;

28、步驟5.2:確定起點(diǎn)樹t1新生成的節(jié)點(diǎn)xt1new到目標(biāo)點(diǎn)的歐氏距離ln,xnear到xt1new的歐氏距離ln,相鄰三個節(jié)點(diǎn)xparent、xnear和xrand的角度tn,確定篩選后剩余采樣點(diǎn)到xt1new和目標(biāo)點(diǎn)的歐式距離s(x)和b(x),建立終點(diǎn)樹t2評價(jià)函數(shù)f2(x),分別計(jì)算篩選后剩余采樣點(diǎn)的評價(jià)函數(shù)值,從中選取出評價(jià)函數(shù)最小的采樣點(diǎn),根據(jù)步驟3動態(tài)選取擴(kuò)展步長,根據(jù)步驟4進(jìn)行中間點(diǎn)碰撞檢測,判斷是否在障礙物上,若在障礙物上,則返回步驟二,若不在障礙物上,將新節(jié)點(diǎn)添加到隨機(jī)樹t2中。

29、具體的,步驟6中所述對初始路徑進(jìn)行全局路徑重規(guī)劃和局部路徑重規(guī)劃的過程如下:

30、步驟6.1:重復(fù)執(zhí)行步驟2至步驟5,直至起點(diǎn)樹t1和終點(diǎn)樹t2新生成的節(jié)點(diǎn)重合或小于設(shè)置的連接閾值,連接兩點(diǎn),分別以兩樹新生成的節(jié)點(diǎn)為起始端點(diǎn)回溯至起始點(diǎn)和目標(biāo)點(diǎn),得到初始路徑,

31、步驟6.2:以終點(diǎn)為線段的起始端點(diǎn)回溯初始路徑,依次連接后續(xù)路徑節(jié)點(diǎn),檢查連接的路徑段是否與障礙物發(fā)生碰撞。如果沒有發(fā)生碰撞,刪除兩個節(jié)點(diǎn)之間所有的路徑節(jié)點(diǎn);如果發(fā)生碰撞,將該節(jié)點(diǎn)的子節(jié)點(diǎn)作為新的線段起始端點(diǎn),重復(fù)執(zhí)行上述操作,回溯至起點(diǎn)結(jié)束;

32、步驟6.3:對新生成的路徑段進(jìn)行插值處理,插值距離為最小步長,得到節(jié)點(diǎn)較密集的路徑用于局部路徑重規(guī)劃。以起點(diǎn)為線段的起始端點(diǎn),依次遍歷連接后續(xù)節(jié)點(diǎn),檢查該線段是否與障礙物發(fā)生碰撞。不發(fā)生碰撞則將兩個節(jié)點(diǎn)之間的所有路徑節(jié)點(diǎn)刪除,反之則以該節(jié)點(diǎn)的父節(jié)點(diǎn)為線段的起始端點(diǎn)再次執(zhí)行上述操作,直至遍歷到目標(biāo)點(diǎn)。

33、具體的,步驟7中所述在相鄰節(jié)點(diǎn)間構(gòu)建控制點(diǎn),通過設(shè)置閾值選擇構(gòu)建控制點(diǎn)方式,利用b樣條曲線對構(gòu)建控制點(diǎn)的路徑進(jìn)行擬合的過程如下:

34、步驟7.1:在相鄰節(jié)點(diǎn)間構(gòu)建控制點(diǎn),設(shè)置閾值選擇構(gòu)建控制點(diǎn)方式,判斷相鄰兩個節(jié)點(diǎn)間路徑段長度是否小于閾值,如果小于閾值,在路徑段中點(diǎn)構(gòu)建控制點(diǎn),否則以兩個節(jié)點(diǎn)為起始點(diǎn)分別取正常步長長度構(gòu)建控制點(diǎn);

35、步驟7.2:將構(gòu)建的控制點(diǎn)和優(yōu)化后的路徑節(jié)點(diǎn)作為輸入,利用b樣條調(diào)和函數(shù)和曲線公式得到b樣條曲線的離散化軌跡點(diǎn),連接得到樣條曲線優(yōu)化后的平滑路徑。

36、本發(fā)明基于并行采樣點(diǎn)優(yōu)化rrt算法概率完備性和時間復(fù)雜度分析如下所示:

37、對于本發(fā)明算法,多點(diǎn)并行加權(quán)啟發(fā)式采樣策略是在隨機(jī)采樣基礎(chǔ)上增加并行采樣點(diǎn),引導(dǎo)隨機(jī)樹向目標(biāo)點(diǎn)生長,減少隨機(jī)樹無效采樣,因此,不會對算法收斂產(chǎn)生負(fù)面影響,至少可以找到與rrt算法相同的路徑解,雙向引導(dǎo)機(jī)制可以加快雙樹融合的速度,不會影響概率完備性,rrt和bi-rrt算法的概率完備性已經(jīng)被證明,因此算法具有概率完備性。

38、本發(fā)明算法和rrt算法區(qū)別主要體現(xiàn)在多點(diǎn)并行加權(quán)啟發(fā)式采樣、動態(tài)步長擴(kuò)展和雙向引導(dǎo)機(jī)制,對于多點(diǎn)并行加權(quán)啟發(fā)采樣策略,parallelsampling函數(shù)隨機(jī)采樣多個節(jié)點(diǎn)進(jìn)行評價(jià)函數(shù)比較,時間復(fù)雜度與randomsampling函數(shù)和comparisondistance函數(shù)相同,評價(jià)函數(shù)只執(zhí)行一些簡單的計(jì)算,時間成本可以忽略不計(jì),對于動態(tài)步長擴(kuò)展策略,rapidextend函數(shù)的時間成本主要取決于checkpath函數(shù),其時間復(fù)雜度與checkconstraint函數(shù)成正比,采樣次數(shù)趨于無窮大時,比例系數(shù)是障礙物區(qū)域的面積與無障礙物區(qū)域的面積的比例,該比例是已知常數(shù),checkpath函數(shù)在每次隨機(jī)采樣中至多執(zhí)行兩次,取決于狀態(tài)空間和安全距離,因此,rapidextend函數(shù)的時間復(fù)雜度與checkconstraint函數(shù)的時間復(fù)雜度相同,對于雙向引導(dǎo)機(jī)制,起點(diǎn)樹結(jié)構(gòu)與rrt單樹結(jié)構(gòu)相同,終點(diǎn)樹中函數(shù)nearest時間復(fù)雜度與函數(shù)comparisondistance時間復(fù)雜度相同,函數(shù)guidesampling時間復(fù)雜度與函數(shù)parallelsampling時間復(fù)雜度相同,rrt算法的時間復(fù)雜度取決于randomsampling函數(shù)、comparisondistance函數(shù)和checkconstraint函數(shù),本發(fā)明算法在rrt算法基礎(chǔ)上增加和修改了一些函數(shù),該函數(shù)時間復(fù)雜度與comparisondistance函數(shù)和checkconstraint函數(shù)相同,因此,本發(fā)明算法時間復(fù)雜度與rrt算法時間復(fù)雜度相同,而rrt算法時間復(fù)雜度與bi-rrt算法相同,所以本發(fā)明算法時間復(fù)雜度與bi-rrt算法相同。

39、與現(xiàn)有技術(shù)相比,本發(fā)明的有益效果是:

40、本發(fā)明提供一種基于并行采樣點(diǎn)優(yōu)化rrt算法的機(jī)器人路徑規(guī)劃方法,增設(shè)并行采樣點(diǎn)進(jìn)行多點(diǎn)尋優(yōu)擴(kuò)展,為引導(dǎo)隨機(jī)樹展開和減少路徑拐點(diǎn),建立評價(jià)函數(shù)對并行采樣點(diǎn)進(jìn)行篩選擇優(yōu)后擴(kuò)展,同時兼顧了路徑長度和路徑平滑度,構(gòu)建雙樹引導(dǎo)機(jī)制,提高雙向隨機(jī)樹融合速度,防止局部空間無效節(jié)點(diǎn)生成,根據(jù)探索到的節(jié)點(diǎn)周圍環(huán)境信息采取不同的自適應(yīng)擴(kuò)展步長,規(guī)避過度探索和碰撞區(qū)域擴(kuò)展帶來的負(fù)面影響,同時避免在不同環(huán)境下算法穩(wěn)定性不足的情況,提出路徑重布線優(yōu)化方法,降低初始路徑長度和節(jié)點(diǎn)數(shù),經(jīng)過b樣條平滑優(yōu)化后的最終路徑滿足移動機(jī)器人的動力學(xué)特性。

當(dāng)前第1頁1 2 
網(wǎng)友詢問留言 已有0條留言
  • 還沒有人留言評論。精彩留言會獲得點(diǎn)贊!
1