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

背景技術(shù)：

1、

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

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

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

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

1、

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

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

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

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

9、步驟5，對(duì)于終點(diǎn)樹t2，在采樣空間中隨機(jī)并行采樣多個(gè)節(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)碰撞檢測(cè)和動(dòng)態(tài)選取步長(zhǎng)擴(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)，得到初始路徑，對(duì)初始路徑進(jìn)行全局路徑重規(guī)劃和局部路徑重規(guī)劃，濾除非關(guān)鍵節(jié)點(diǎn)，提升初始路徑質(zhì)量；

11、步驟7，對(duì)于步驟6所得的優(yōu)化路徑，在路徑相鄰節(jié)點(diǎn)間構(gòu)建控制點(diǎn)，通過設(shè)置閾值選擇構(gòu)建控制點(diǎn)方式，利用三次b樣條曲線對(duì)構(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ù)，對(duì)地圖進(jìn)行二值化處理；

14、步驟1.2：設(shè)置機(jī)器人移動(dòng)的起始點(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ò)展步長(zhǎng)。

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

17、步驟2.1：在狀態(tài)空間中同時(shí)并行采樣多個(gè)節(jié)點(diǎn)，并行采樣點(diǎn)的個(gè)數(shù)由地圖復(fù)雜度決定，經(jīng)過大量的實(shí)驗(yàn)研究分析，發(fā)現(xiàn)簡(jiǎn)單環(huán)境中并行采樣三個(gè)節(jié)點(diǎn)，復(fù)雜環(huán)境中并行采樣五個(gè)節(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，相鄰三個(gè)節(jié)點(diǎn)xparent、xnear和xrand間的角度tn，確定篩選后剩余采樣點(diǎn)到起始點(diǎn)和目標(biāo)點(diǎn)的歐式距離a(x)和n(x)，建立起點(diǎn)樹t1評(píng)價(jià)函數(shù)f1(x)；

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

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

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

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

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

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)空間中同時(shí)并行采樣多個(gè)節(jié)點(diǎn)，并行采樣點(diǎn)的個(gè)數(shù)由地圖復(fù)雜度決定，簡(jiǎn)單環(huán)境中并行采樣三個(gè)節(jié)點(diǎn)，復(fù)雜環(huán)境中并行采樣五個(gè)節(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，相鄰三個(gè)節(jié)點(diǎn)xparent、xnear和xrand的角度tn，確定篩選后剩余采樣點(diǎn)到xt1new和目標(biāo)點(diǎn)的歐式距離s(x)和b(x)，建立終點(diǎn)樹t2評(píng)價(jià)函數(shù)f2(x)，分別計(jì)算篩選后剩余采樣點(diǎn)的評(píng)價(jià)函數(shù)值，從中選取出評(píng)價(jià)函數(shù)最小的采樣點(diǎn)，根據(jù)步驟3動(dòng)態(tài)選取擴(kuò)展步長(zhǎng)，根據(jù)步驟4進(jìn)行中間點(diǎn)碰撞檢測(cè)，判斷是否在障礙物上，若在障礙物上，則返回步驟二，若不在障礙物上，將新節(jié)點(diǎn)添加到隨機(jī)樹t2中。

29、具體的，步驟6中所述對(duì)初始路徑進(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ā)生碰撞，刪除兩個(gè)節(jié)點(diǎn)之間所有的路徑節(jié)點(diǎn)；如果發(fā)生碰撞，將該節(jié)點(diǎn)的子節(jié)點(diǎn)作為新的線段起始端點(diǎn)，重復(fù)執(zhí)行上述操作，回溯至起點(diǎn)結(jié)束；

32、步驟6.3：對(duì)新生成的路徑段進(jìn)行插值處理，插值距離為最小步長(zhǎng)，得到節(jié)點(diǎn)較密集的路徑用于局部路徑重規(guī)劃。以起點(diǎn)為線段的起始端點(diǎn)，依次遍歷連接后續(xù)節(jié)點(diǎn)，檢查該線段是否與障礙物發(fā)生碰撞。不發(fā)生碰撞則將兩個(gè)節(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樣條曲線對(duì)構(gòu)建控制點(diǎn)的路徑進(jìn)行擬合的過程如下：

34、步驟7.1：在相鄰節(jié)點(diǎn)間構(gòu)建控制點(diǎn)，設(shè)置閾值選擇構(gòu)建控制點(diǎn)方式，判斷相鄰兩個(gè)節(jié)點(diǎn)間路徑段長(zhǎng)度是否小于閾值，如果小于閾值，在路徑段中點(diǎn)構(gòu)建控制點(diǎn)，否則以兩個(gè)節(jié)點(diǎn)為起始點(diǎn)分別取正常步長(zhǎng)長(zhǎng)度構(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算法概率完備性和時(shí)間復(fù)雜度分析如下所示：

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

38、本發(fā)明算法和rrt算法區(qū)別主要體現(xiàn)在多點(diǎn)并行加權(quán)啟發(fā)式采樣、動(dòng)態(tài)步長(zhǎng)擴(kuò)展和雙向引導(dǎo)機(jī)制，對(duì)于多點(diǎn)并行加權(quán)啟發(fā)采樣策略，parallelsampling函數(shù)隨機(jī)采樣多個(gè)節(jié)點(diǎn)進(jìn)行評(píng)價(jià)函數(shù)比較，時(shí)間復(fù)雜度與randomsampling函數(shù)和comparisondistance函數(shù)相同，評(píng)價(jià)函數(shù)只執(zhí)行一些簡(jiǎn)單的計(jì)算，時(shí)間成本可以忽略不計(jì)，對(duì)于動(dòng)態(tài)步長(zhǎng)擴(kuò)展策略，rapidextend函數(shù)的時(shí)間成本主要取決于checkpath函數(shù)，其時(shí)間復(fù)雜度與checkconstraint函數(shù)成正比，采樣次數(shù)趨于無窮大時(shí)，比例系數(shù)是障礙物區(qū)域的面積與無障礙物區(qū)域的面積的比例，該比例是已知常數(shù)，checkpath函數(shù)在每次隨機(jī)采樣中至多執(zhí)行兩次，取決于狀態(tài)空間和安全距離，因此，rapidextend函數(shù)的時(shí)間復(fù)雜度與checkconstraint函數(shù)的時(shí)間復(fù)雜度相同，對(duì)于雙向引導(dǎo)機(jī)制，起點(diǎn)樹結(jié)構(gòu)與rrt單樹結(jié)構(gòu)相同，終點(diǎn)樹中函數(shù)nearest時(shí)間復(fù)雜度與函數(shù)comparisondistance時(shí)間復(fù)雜度相同，函數(shù)guidesampling時(shí)間復(fù)雜度與函數(shù)parallelsampling時(shí)間復(fù)雜度相同，rrt算法的時(shí)間復(fù)雜度取決于randomsampling函數(shù)、comparisondistance函數(shù)和checkconstraint函數(shù)，本發(fā)明算法在rrt算法基礎(chǔ)上增加和修改了一些函數(shù)，該函數(shù)時(shí)間復(fù)雜度與comparisondistance函數(shù)和checkconstraint函數(shù)相同，因此，本發(fā)明算法時(shí)間復(fù)雜度與rrt算法時(shí)間復(fù)雜度相同，而rrt算法時(shí)間復(fù)雜度與bi-rrt算法相同，所以本發(fā)明算法時(shí)間復(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)，建立評(píng)價(jià)函數(shù)對(duì)并行采樣點(diǎn)進(jìn)行篩選擇優(yōu)后擴(kuò)展，同時(shí)兼顧了路徑長(zhǎng)度和路徑平滑度，構(gòu)建雙樹引導(dǎo)機(jī)制，提高雙向隨機(jī)樹融合速度，防止局部空間無效節(jié)點(diǎn)生成，根據(jù)探索到的節(jié)點(diǎn)周圍環(huán)境信息采取不同的自適應(yīng)擴(kuò)展步長(zhǎng)，規(guī)避過度探索和碰撞區(qū)域擴(kuò)展帶來的負(fù)面影響，同時(shí)避免在不同環(huán)境下算法穩(wěn)定性不足的情況，提出路徑重布線優(yōu)化方法，降低初始路徑長(zhǎng)度和節(jié)點(diǎn)數(shù)，經(jīng)過b樣條平滑優(yōu)化后的最終路徑滿足移動(dòng)機(jī)器人的動(dòng)力學(xué)特性。