采用RSPM-PS算法的機(jī)械手末端避障路徑規(guī)劃

歐陽(yáng)云, 高振國(guó), 范麗玲, 王繼斌, 蔣坤良

(1. 華僑大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院, 福建 廈門(mén) 361021;2. 華僑大學(xué) 計(jì)算機(jī)視覺(jué)與機(jī)器學(xué)習(xí)重點(diǎn)實(shí)驗(yàn)室, 福建 廈門(mén) 361021)

隨著機(jī)器人技術(shù)的快速發(fā)展,工業(yè)機(jī)器人已經(jīng)廣泛地應(yīng)用于工業(yè)、農(nóng)業(yè)、娛樂(lè)、醫(yī)療等許多領(lǐng)域[1-5].工業(yè)機(jī)器人的機(jī)械手/機(jī)械臂在給定空間中按特定要求進(jìn)行運(yùn)動(dòng),機(jī)器手運(yùn)動(dòng)規(guī)劃一直是工業(yè)機(jī)器人控制領(lǐng)域的一項(xiàng)核心技術(shù).工業(yè)生產(chǎn)中最常用多關(guān)節(jié)機(jī)械手,其運(yùn)動(dòng)規(guī)劃主要包括機(jī)械手末端路徑規(guī)劃和關(guān)節(jié)空間軌跡規(guī)劃.機(jī)械手末端路徑規(guī)劃的目的是為機(jī)械手末端尋找一條從起始點(diǎn)至目標(biāo)點(diǎn)的可行路徑,關(guān)節(jié)空間軌跡規(guī)劃的目的則是針對(duì)末端路徑規(guī)劃的各路徑節(jié)點(diǎn),規(guī)劃?rùn)C(jī)械手各關(guān)節(jié)的運(yùn)動(dòng)角度或距離配置[6],以使機(jī)械手末端能處于特定的路徑節(jié)點(diǎn)目標(biāo)位置處.

目前,工業(yè)機(jī)器人機(jī)械手末端路徑規(guī)劃算法主要包括傳統(tǒng)路徑規(guī)劃算法、基于采樣的路徑規(guī)劃算法和智能仿生路徑規(guī)劃算法[7].傳統(tǒng)路徑規(guī)劃算法主要包括模擬退火算法[8]和人工勢(shì)場(chǎng)法[9],這兩種算法都易陷入局部最優(yōu)解,并且模擬退火算法缺乏對(duì)環(huán)境的普遍適應(yīng)性.基于采樣的路徑規(guī)劃算法主要包括快速搜索隨機(jī)樹(shù)( RRT)[10],在空間較大且障礙物較多的情況下,其隨機(jī)采樣的特性會(huì)導(dǎo)致算法效率極低,實(shí)時(shí)性不足,并且生成路徑長(zhǎng)度可能較長(zhǎng).Karaman等[11]對(duì)現(xiàn)有基于RRT改進(jìn)的算法(RRT*算法[12]、RRT*-Connect算法[13]、Informed RRT*算法[14-16]、Informed RRT*-Connect算法[17])進(jìn)行總結(jié).智能仿生路徑規(guī)劃算法主要包括蟻群算法域算法[18]和遺傳算法域算法[19]等,這兩種算法需要選擇和調(diào)整的參數(shù)較多,參數(shù)間的耦合性較高．在某些特殊情況下,上述機(jī)械手末端路徑規(guī)劃算法路徑規(guī)劃空間大、空間復(fù)雜性高、運(yùn)行速度慢、生成的路徑長(zhǎng),還經(jīng)常存在大量冗余路徑節(jié)點(diǎn),制約了關(guān)節(jié)空間軌跡規(guī)劃的有效執(zhí)行．因此,本文提出了一種采用分段點(diǎn)遷移遞歸-遞進(jìn)約簡(jiǎn)(recursive segmentation point migration-progressive simplification,RSPM-PS)算法的機(jī)械手末端路徑規(guī)劃方法.

1 RSPM-PS算法的總體流程框架

給定路徑規(guī)劃空間場(chǎng)景及障礙物布局,針對(duì)任意起始點(diǎn)s和目標(biāo)點(diǎn)d,路徑規(guī)劃的任務(wù)就是尋找一條不與任何障礙物碰撞的路徑path=[s,x1,x2,…,xn,d],其中,path代表從起始點(diǎn)s依次經(jīng)由分段點(diǎn)(路徑節(jié)點(diǎn))x1,x2,…,xn,最終到達(dá)目標(biāo)點(diǎn)d的分段線性路徑.各分段點(diǎn)之間(包括起始點(diǎn)s,目標(biāo)點(diǎn)d)的直連線段稱(chēng)為路徑段.

為輔助分段點(diǎn)遷移遞歸(RSPM)算法和遞進(jìn)約簡(jiǎn)(PS)算法的運(yùn)行,設(shè)計(jì)4種基礎(chǔ)模型:外接矩形包絡(luò)模型、碰撞檢測(cè)模型、碰撞類(lèi)型檢測(cè)模型和碰撞規(guī)避模型.外接矩形包絡(luò)模型將任意形狀空間障礙物用其外接矩形(或外接立方體)代替,以簡(jiǎn)化空間障礙物,從而提升碰撞檢測(cè)效率.碰撞檢測(cè)模型用于檢測(cè)任意兩點(diǎn)間的直連線與空間障礙物的碰撞情況.若與某障礙物有碰撞,則利用碰撞類(lèi)型檢測(cè)模型確定碰撞類(lèi)型.根據(jù)碰撞類(lèi)型和碰撞規(guī)避模型,增加直連線分段點(diǎn),確定分段點(diǎn)遷移方案,從而將分段點(diǎn)遷移出障礙物.用遷移后的分段路徑代替直連線,規(guī)避當(dāng)前障礙物.RSPM-PS算法的總體流程框架,如圖1所示.圖1中:obs1為障礙物1;obs2為障礙物2;n1～n4為分段點(diǎn).

圖1 RSPM-PS算法的總體流程框架圖

首先,采用RSPM算法,以起始點(diǎn)s至目標(biāo)點(diǎn)d的直連線為初始測(cè)試路徑,根據(jù)直連線與障礙物的碰撞情況,增加直連線的分段點(diǎn),并將分段點(diǎn)遷移出障礙物.用遷移后的分段路徑代替直連線,就可規(guī)避當(dāng)前障礙物.其次,遞歸檢測(cè)各路徑段與障礙物的碰撞情況,必要時(shí)增加直連線的分段點(diǎn),并將分段點(diǎn)遷移出障礙物,從而規(guī)避障礙物,最終得到不與障礙物碰撞的、由多段路徑構(gòu)成的可行規(guī)劃路徑(基礎(chǔ)路徑).最后,采用PS算法,以單向遞進(jìn)簡(jiǎn)化的方式去除基礎(chǔ)路徑的冗余路徑節(jié)點(diǎn),生成簡(jiǎn)化的最終路徑.

2 RSPM算法

基礎(chǔ)路徑規(guī)劃的總流程圖,如圖2所示.RSPM算法函數(shù)總體框架,如圖3所示.采用RSPM算法規(guī)劃基礎(chǔ)路徑,具體有如下4個(gè)步驟.

圖2 基礎(chǔ)路徑規(guī)劃的總流程圖

圖3 RSPM算法函數(shù)總體框架

1) 判斷兩點(diǎn)間直連線是否與障礙物發(fā)生碰撞,若不發(fā)生碰撞,則直連線是一條可行路徑,RSPM算法把該直連線作為路徑規(guī)劃結(jié)果返回;若發(fā)生碰撞,則直連線可能與多個(gè)障礙物發(fā)生碰撞.

2) 由于僅規(guī)避多個(gè)障礙物中離起點(diǎn)最近的障礙物,因此需確定該直連線與相應(yīng)障礙物的碰撞類(lèi)型、增加的分段點(diǎn)及遷移方案,以繞開(kāi)該障礙物,這樣基于直連線可以得到一個(gè)繞開(kāi)最近障礙物的多段路徑.

3) 針對(duì)新規(guī)劃的各路徑段,繼續(xù)遞歸檢測(cè)直連路徑.

4) 由于RSPM算法規(guī)避各障礙物,使路徑節(jié)點(diǎn)數(shù)量增大,路徑段數(shù)增多,最終得到不與任何障礙物碰撞的一條可行路徑,該路徑就是RSPM算法要返回的基礎(chǔ)路徑.

采用RSPM算法規(guī)劃的基礎(chǔ)路徑,如圖4所示.圖4中:粗實(shí)線表示最終的基礎(chǔ)路徑.由圖4可知以下3點(diǎn).

圖4 采用RSPM算法規(guī)劃的基礎(chǔ)路徑

1) 對(duì)起始點(diǎn)s至目標(biāo)點(diǎn)d進(jìn)行碰撞檢測(cè),發(fā)現(xiàn)起始點(diǎn)s至目標(biāo)點(diǎn)d的直連線首先會(huì)和障礙物obs1發(fā)生碰撞,首個(gè)碰撞段為該直連線與障礙物obs1的交集.

3) 對(duì)于第3個(gè)路徑段[n2,d],則按前述過(guò)程繼續(xù)處理,該路徑段的首個(gè)碰撞物是障礙物obs2,且碰撞段的兩個(gè)端點(diǎn)在障礙物的對(duì)立邊,首個(gè)碰撞段的中點(diǎn)位于obs2的上半部分.根據(jù)相應(yīng)的分段點(diǎn)遷移方案,在碰撞段中點(diǎn)處生成兩個(gè)分段點(diǎn)n3′和n4′,并且把它們分別向左上方向、右上方向遷移至obs2外,得到兩個(gè)遷移后的新分段點(diǎn)n3和n4.這樣,用路徑段[n2,n3,n4,d]替換路徑段[n2,d],就避開(kāi)了障礙物obs2.[n2,n3,n4,d]的各路徑段不再與障礙物碰撞,所以無(wú)需進(jìn)一步處理,算法結(jié)束.[s,n1,n2,n3,n4,d]就是最終的基礎(chǔ)路徑.

RSPM(起始點(diǎn),目標(biāo)點(diǎn))的偽代碼,如算法1所示.算法1返回的路徑加上目標(biāo)點(diǎn)即是基礎(chǔ)路徑.

算法1.RSPM(起始點(diǎn),目標(biāo)點(diǎn))的偽代碼.

條件:起始點(diǎn)、目標(biāo)點(diǎn)、障礙物坐標(biāo).

結(jié)果:起始點(diǎn)至目標(biāo)點(diǎn)的基礎(chǔ)路徑.

1) 對(duì)起始點(diǎn)至目標(biāo)點(diǎn)的直連線進(jìn)行碰撞檢測(cè);

2) IF(不發(fā)生碰撞)

3) 將起始點(diǎn)加入基礎(chǔ)路徑;

4) RETURN;

5) ENDIF

6) 根據(jù)碰撞檢測(cè)得到碰撞段端點(diǎn)的位置,并確定碰撞類(lèi)型檢測(cè);

7) 根據(jù)碰撞類(lèi)型,在碰撞段上生成相應(yīng)數(shù)量的分段點(diǎn);

8) 根據(jù)分段點(diǎn)在障礙物內(nèi)的位置,沿特定方向遷移出障礙物;

9) IF(分段點(diǎn)的數(shù)量為2)

10) RSPM(起始點(diǎn),第1個(gè)分段點(diǎn));

11) RSPM(第1個(gè)分段點(diǎn),第2個(gè)分段點(diǎn));

12) RSPM(第2個(gè)分段點(diǎn),目標(biāo)點(diǎn));

13) ELSE

14) RSPM(起始點(diǎn),分段點(diǎn));

15) RSPM(分段點(diǎn),目標(biāo)點(diǎn));

16) ENDIF

3 PS算法

若去除一條可行路徑的一個(gè)分段點(diǎn)后,得到的路徑仍然是一條不與障礙物碰撞的可行路徑,則這個(gè)分段點(diǎn)稱(chēng)為原始路徑上的冗余分段點(diǎn).由于RSPM算法規(guī)劃的基礎(chǔ)路徑可能存在冗余分段點(diǎn)(圖4),路徑[s,n1,n2,n3,n4,d]去除分段點(diǎn)n2后得到新路徑[s,n1,n3,n4,d]仍是一條可行路徑,所以n2是一個(gè)冗余分段點(diǎn).顯然,去除冗余分段點(diǎn)既可以降低路徑長(zhǎng)度,又可以降低路徑上分段點(diǎn)的數(shù)量,從而得到更優(yōu)的路徑.

采用PS算法檢測(cè)并去除基于RSPM算法規(guī)劃的基礎(chǔ)路徑的冗余分段點(diǎn),從而得到最終路徑,過(guò)程有如下3個(gè)步驟.

1) PS算法采用單向遞進(jìn)簡(jiǎn)化方式,按起始點(diǎn)s到終點(diǎn)d的順序,從起始點(diǎn)s后的第2個(gè)分段點(diǎn)開(kāi)始,依次檢查各分段點(diǎn)至起始點(diǎn)s的直連線和障礙物是否發(fā)生碰撞,被檢查的分段點(diǎn)稱(chēng)為當(dāng)前分段點(diǎn).

2) 順次檢查,直到找到某個(gè)分段點(diǎn)x,使得直連線[s,x]與障礙物碰撞.設(shè)分段點(diǎn)x的前一個(gè)分段點(diǎn)為分段點(diǎn)y,則起始點(diǎn)s到分段點(diǎn)y之間的分段點(diǎn)為冗余分段點(diǎn),刪除這些冗余分段點(diǎn).

3) 從分段點(diǎn)x的下一個(gè)分段點(diǎn)z開(kāi)始,繼續(xù)檢查從分段點(diǎn)y到當(dāng)前分段點(diǎn)的直連線是否與障礙物碰撞,直到到達(dá)目標(biāo)點(diǎn)d,得到最終路徑.

采用PS算法生成的最終路徑,如圖5所示.圖5中:細(xì)虛線表示基于RSPM算法規(guī)劃的基礎(chǔ)路徑[s,n1,n2,n3,n4,d],粗實(shí)線表示基于PS算法對(duì)該路徑進(jìn)行約簡(jiǎn)優(yōu)化規(guī)劃的最終路徑[s,n1,n3,n4,d].

圖5 采用PS算法生成的最終路徑

由圖5可知以下5點(diǎn).

1) 按起始點(diǎn)s到目標(biāo)點(diǎn)d的檢查方向,從起始點(diǎn)s后的第2個(gè)分段點(diǎn)n2開(kāi)始檢查,發(fā)現(xiàn)起始點(diǎn)s到分段點(diǎn)n2的直連線與障礙物發(fā)生碰撞,可知分段點(diǎn)n1不能刪除,起始點(diǎn)s到分段點(diǎn)n1之間沒(méi)有冗余分段點(diǎn).

2) 針對(duì)分段點(diǎn)n1,從分段點(diǎn)n2的下一個(gè)分段點(diǎn)n3開(kāi)始檢查,發(fā)現(xiàn)直連線[n1,n3]不與障礙物發(fā)生碰撞,而直連線[n1,n4]與障礙物發(fā)生碰撞,可知分段點(diǎn)知n3不能刪除,n1和n3之間的分段點(diǎn)n2是冗余分段點(diǎn),刪除分段點(diǎn)n2.

3) 針對(duì)分段點(diǎn)n3,從分段點(diǎn)n4的下一個(gè)目標(biāo)點(diǎn)d開(kāi)始檢查,發(fā)現(xiàn)直連線[n3,d]與障礙物碰撞,可知分段點(diǎn)n4不能刪除,n3和n4之間沒(méi)有冗余分段點(diǎn).

4) 針對(duì)分段點(diǎn)n4,繼續(xù)檢查剩余各分段點(diǎn).由于已檢查的分段點(diǎn)d是最后一個(gè)分段點(diǎn),并且n4和d之間沒(méi)有冗余分段點(diǎn),所以無(wú)需進(jìn)一步處理,算法結(jié)束.

5) 約簡(jiǎn)過(guò)程發(fā)現(xiàn)并刪除了1個(gè)冗余分段點(diǎn)n2,得到的最終路徑為[s,n1,n3,n4,d].

PS算法的偽代碼,如算法2所示.

算法2.PS算法的偽代碼.

條件:對(duì)于空間中任意的起始點(diǎn)和目標(biāo)點(diǎn)之間的可行路徑.

結(jié)果:無(wú)冗余分段點(diǎn)的最終路徑.

1) WHILE(TRUE)

2) 對(duì)起始點(diǎn)至目標(biāo)點(diǎn)的直連線進(jìn)行碰撞檢測(cè);

3) IF(不發(fā)生碰撞)

4) 將起始點(diǎn)和目標(biāo)點(diǎn)加入最終路徑;

5) BREAK;

6) ENDIF

7) 令p指向起始點(diǎn)后的第2個(gè)分段點(diǎn);

8) WHILE(TRUE)

9) 對(duì)起始點(diǎn)至分段點(diǎn)p指向節(jié)點(diǎn)的直連線進(jìn)行碰撞檢測(cè);

10) IF(發(fā)生碰撞)

11) 將起始點(diǎn)加入最終路徑;

12) 當(dāng)前分段點(diǎn)p指向的節(jié)點(diǎn)的前一個(gè)分段點(diǎn)為新的起始點(diǎn);

13) BREAK;

14) ELSE

15) 分段點(diǎn)p4指向路徑中當(dāng)前分段點(diǎn)p指向節(jié)點(diǎn)的后一個(gè)分段點(diǎn);

16) ENDIF

17) ENDWHILE

18) ENDWHILE

4 基礎(chǔ)模型

4.1 外接矩形包絡(luò)模型

全局避障路徑規(guī)劃算法需要根據(jù)已知空間及障礙物信息,建立二維或三維空間模型.空間信息主要包括起始點(diǎn)坐標(biāo)、目標(biāo)點(diǎn)坐標(biāo)和障礙物坐標(biāo).外接矩形包絡(luò)模型,如圖6所示.

圖6 外接矩形包絡(luò)模型

在二維場(chǎng)景中,外接矩形包絡(luò)模型方案會(huì)犧牲機(jī)械手的部分自由工作空間,但能顯著降低建模難度和計(jì)算量.

4.2 碰撞檢測(cè)模型

碰撞檢測(cè)是路徑規(guī)劃最重要的步驟之一,工業(yè)機(jī)器人機(jī)械手末端在空間中需要占據(jù)部分自由工作空間,因此,需要設(shè)定機(jī)械手的安全工作距離(R),即保證機(jī)械手末端中心點(diǎn)與障礙物不發(fā)生碰撞的最小距離.碰撞檢測(cè)模型,如圖7所示.圖7中:障礙物為已考慮安全工作距離的擴(kuò)展后的障礙物;距離分段點(diǎn)n1最近的兩個(gè)交點(diǎn)為首個(gè)碰撞段(粗實(shí)線段)的兩個(gè)端點(diǎn).由圖7可知:分段點(diǎn)n1至分段點(diǎn)n2的直連線會(huì)和障礙物的邊生成4個(gè)交點(diǎn),即分段點(diǎn)n1至分段點(diǎn)n2的直連線會(huì)和障礙物發(fā)生碰撞.

圖7 碰撞檢測(cè)模型

為判斷空間中任意兩點(diǎn)之間的直連線是否和某障礙物發(fā)生碰撞,采用碰撞檢測(cè)模型,該模型在障礙物外接矩形包絡(luò)模型的基礎(chǔ)上,向外擴(kuò)張安全工作距離.碰撞檢測(cè)模型以擴(kuò)張后的矩形作為障礙物的邊界,當(dāng)直連線跟擴(kuò)展后的矩形相交的,則認(rèn)為直連線與相應(yīng)障礙物發(fā)生碰撞.

在任意兩點(diǎn)之間直連線的碰撞檢測(cè)中,只需要判斷該直連線是否與障礙物碰撞,而不需要判斷障礙物的數(shù)量.后續(xù)規(guī)避處理總是針對(duì)與起始點(diǎn)距離最近的障礙物,而對(duì)于其他后發(fā)生碰撞的障礙物,則是在本次規(guī)避處理得到的新路徑段的基礎(chǔ)上,通過(guò)新路徑段的碰撞檢測(cè)和碰撞規(guī)避進(jìn)行解決.

4.3 碰撞類(lèi)型檢測(cè)模型

當(dāng)兩分段點(diǎn)之間的直連線和障礙物發(fā)生碰撞時(shí),需要進(jìn)行碰撞避障處理,但規(guī)避方案依賴(lài)于碰撞避障類(lèi)型.根據(jù)首個(gè)碰撞段的兩個(gè)端點(diǎn)相對(duì)于障礙物的位置,把碰撞類(lèi)型檢測(cè)模型分為兩類(lèi),如圖8所示.圖8中:模型1碰撞段的兩個(gè)端點(diǎn)在障礙物的相鄰邊;模型2碰撞段的兩個(gè)端點(diǎn)在障礙物的對(duì)立邊.

(a) 模型1 (b) 模型2

4.4 碰撞規(guī)避模型

碰撞類(lèi)型檢測(cè)模型1在首個(gè)碰撞段的中點(diǎn)處生成1個(gè)分段點(diǎn),將分段點(diǎn)向碰撞段第1個(gè)端點(diǎn)所在邊和碰撞段第2個(gè)端點(diǎn)所在邊的交點(diǎn)方向遷移至障礙物外,得到遷移后的新位置.碰撞類(lèi)型檢測(cè)模型1的碰撞規(guī)避模型1,如圖9(a)所示.由圖9(a)可知:碰撞段的第1個(gè)端點(diǎn)位于障礙物的左邊,第2個(gè)端點(diǎn)位于障礙物的上邊,因此,將分段點(diǎn)向左上方交點(diǎn)方向遷移至障礙物外,得到分段點(diǎn)遷移后的位置.

碰撞類(lèi)型檢測(cè)模型2在首個(gè)碰撞段的中點(diǎn)檢測(cè)障礙物中的相對(duì)位置,若該中點(diǎn)位于障礙物內(nèi)部的上方,則在碰撞段中點(diǎn)處生成兩個(gè)分段點(diǎn),將其中一個(gè)分段點(diǎn)向碰撞段第1個(gè)端點(diǎn)所在邊與障礙物所在邊的交點(diǎn)方向遷移至障礙物外,另一個(gè)分段點(diǎn)則向碰撞段第2個(gè)端點(diǎn)所在邊和障礙物所在邊的交點(diǎn)方向遷移至障礙物外,得到兩個(gè)分段點(diǎn)遷移后的新位置.其他情況做類(lèi)似處理.碰撞類(lèi)型檢測(cè)模型2的碰撞規(guī)避模型2,如圖9(b)所示.圖9(b)中:碰撞段的中點(diǎn)位于障礙物內(nèi)部的上方,并且碰撞段的第1個(gè)端點(diǎn)位于障礙物的左邊,第2個(gè)端點(diǎn)位于障礙物的右邊,因此,將兩個(gè)分段點(diǎn)依次分別向左上方交點(diǎn)方向、右上方交點(diǎn)方向遷移至障礙物外.

(a) 模型1 (b) 模型2

5 實(shí)驗(yàn)仿真與結(jié)果分析

通過(guò)仿真實(shí)驗(yàn)對(duì)比測(cè)試RSPM-PS算法的性能.采用MATLAB 2016a軟件,系統(tǒng)配置為64位Windows操作系統(tǒng),硬盤(pán)為10 GB以上的Intel(R) Core(TM) 2.50 GHz CPU,4 GB RAM.

4種代表性路徑規(guī)劃場(chǎng)景分別為單障礙物場(chǎng)景、狹窄通道場(chǎng)景、多障礙物二維場(chǎng)景和多障礙物三維場(chǎng)景.評(píng)價(jià)指標(biāo)為平均運(yùn)行時(shí)間和平均路徑長(zhǎng)度.4種代表性路徑規(guī)劃算法分別為RRT*[12]算法、RRT*-Connect[13]算法、Informed RRT*[14-16]算法、Informed RRT*-Connect[17]算法.對(duì)5種算法(RSPM算法與4種代表性路徑規(guī)劃算法)及進(jìn)行100次實(shí)驗(yàn),以各評(píng)價(jià)指標(biāo)值的平均值為最終指標(biāo)值,并計(jì)算其95%的置信區(qū)間.

實(shí)驗(yàn)仿真的參數(shù)設(shè)置,如表1所示.表1中:N為最大遷移點(diǎn)生成數(shù);Ls為生長(zhǎng)步長(zhǎng);r為到達(dá)目標(biāo)點(diǎn)區(qū)域半徑;M為最大樹(shù)節(jié)點(diǎn)生成數(shù).

表1 實(shí)驗(yàn)仿真的參數(shù)設(shè)置

5.1 單障礙物場(chǎng)景

基于文獻(xiàn)[17],在僅有1個(gè)障礙物的二維空間中,為了測(cè)試RSPM算法在不同起始點(diǎn)至目標(biāo)點(diǎn)距離的性能,設(shè)立單障礙物場(chǎng)景.單障礙物場(chǎng)景,如圖10所示.圖10中:5條路徑分別是5種算法在起始點(diǎn)至目標(biāo)點(diǎn)距離為100 mm時(shí)的一次路徑規(guī)劃結(jié)果;X為橫坐標(biāo)值;Y為縱坐標(biāo)值.

圖10 單障礙物場(chǎng)景

由圖11(a)可知: 5組不同起始點(diǎn)s到目標(biāo)點(diǎn)d距離下RSPM算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.001 5,0.002 8,0.002 7,0.002 7,0.002 6 s,而對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.080 8,0.286 5,0.333 8,0.272 2,0.319 2 s;RSPM算法規(guī)劃路徑的最小運(yùn)行時(shí)間為0.001 1 s,而對(duì)比算法規(guī)劃路徑的最小運(yùn)行時(shí)間為0.053 8 s.

(a) 平均運(yùn)行時(shí)間 (b) 平均路徑長(zhǎng)度

由圖11(b)可知:5組不同起始點(diǎn)s到目標(biāo)點(diǎn)d距離下RSPM算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為25.999 8,50.954 1,64.164 1,82.995 1,102.546 9 mm,而對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為28.626 5,60.881 1,74.936 7,92.302 6,108.129 6 mm.

5.2 狹窄通道場(chǎng)景

基于文獻(xiàn)[17],為了測(cè)試RSPM算法在不同寬度狹窄通道下的性能,設(shè)立狹窄通道場(chǎng)景進(jìn)行仿真實(shí)驗(yàn),5組狹窄通道寬度分別為2,3,4,5,6 mm.狹窄通道場(chǎng)景,如圖12所示.圖12中:5條路徑分別是5種算法在狹窄通道寬度為6 mm時(shí)的一次路徑規(guī)劃結(jié)果.狹窄通道場(chǎng)景下的實(shí)驗(yàn)結(jié)果,如圖13所示.圖13中:W為狹窄通道寬度.

圖12 狹窄通道場(chǎng)景

(a) 平均運(yùn)行時(shí)間 (b) 平均路徑長(zhǎng)度

由圖13(a)可知:5組不同狹窄通道寬度下RSPM算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.008 4,0.008 1,0.008 3,0.008 4,0.008 6 s,而對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為9.985 0,9.688 7,6.328 3,3.813 8,3.228 4 s;RSPM算法規(guī)劃路徑的最小運(yùn)行時(shí)間為0.006 7 s,而對(duì)比算法規(guī)劃路徑的最小運(yùn)行時(shí)間為1.001 6 s .

由圖13(b)可知:5組不同狹窄通道寬度下RSPM算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為220.609 0,217.734 2,214.863 4,211.997 1,209.135 4 mm,對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法生成路徑的平均路徑長(zhǎng)度分別為236.922 2,235.252 7,233.014 4,229.275 9,228.219 2 mm.

5.3 多障礙物二維場(chǎng)景

為了測(cè)試RSPM算法在不同起始點(diǎn)至目標(biāo)點(diǎn)距離下的性能,設(shè)立多障礙物二維場(chǎng)景進(jìn)行仿真實(shí)驗(yàn),5組起始點(diǎn)至目標(biāo)點(diǎn)坐標(biāo)距離分別是200,300,400,500,600 mm.多障礙物二維場(chǎng)景,如圖14所示.圖14中:5條路徑分別是5種算法在起始點(diǎn)至目標(biāo)點(diǎn)距離為600 mm時(shí)的一次路徑規(guī)劃結(jié)果.

圖14 多障礙物二維場(chǎng)景

多障礙物二維場(chǎng)景下的實(shí)驗(yàn)結(jié)果,如圖15所示.由圖15(a)可知:5組不同起始點(diǎn)s到目標(biāo)點(diǎn)d距離下RSPM算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.004 9,0.003 1,0.004 8,0.010 5,0.014 3 s,而對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.636 7,0.677 9,0.994 3,1.136 9,2.331 1 s;5組不同起始點(diǎn)s到目標(biāo)點(diǎn)d下RSPM算法規(guī)劃路徑的最小運(yùn)行時(shí)間僅為0.002 5 s,而4種對(duì)比算法規(guī)劃路徑的最小運(yùn)行時(shí)間為0.237 0 s.由圖15(b)可知:5組不同起始點(diǎn)s到目標(biāo)點(diǎn)d距離下RSPM算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為221.092 3,327.672 2,426.961 4,559.542 5,675.023 2 mm,而對(duì)比算法中表現(xiàn)最好的Informed RRT*-Connect算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為238.614 4,338.274 9,441.838 1,574.968 1,759.370 4 mm.

(a) 平均運(yùn)行時(shí)間 (b) 平均路徑長(zhǎng)度

5.4 多障礙物三維場(chǎng)景

在具有多個(gè)障礙物的三維場(chǎng)景中,為了測(cè)試RSPM算法在不同起始點(diǎn)至目標(biāo)點(diǎn)距離下的性能,設(shè)立多障礙物三維場(chǎng)景進(jìn)行仿真實(shí)驗(yàn),5組起始點(diǎn)和目標(biāo)點(diǎn)坐標(biāo)的距離分別是200,300,400,500,600 mm.多障礙物三維場(chǎng)景,如圖16所示.圖16中:5條路徑分別是5種算法在起始點(diǎn)至目標(biāo)點(diǎn)距離為600 mm時(shí)的一次路徑規(guī)劃結(jié)果,Z為Z軸坐標(biāo)值.

圖16 多障礙物三維場(chǎng)景

此外,可以根據(jù)現(xiàn)實(shí)情況設(shè)置場(chǎng)景邊界.在該實(shí)驗(yàn)中,假設(shè)Z軸0刻度為地面,即5種算法在Z軸0刻度及以下區(qū)域不規(guī)劃路徑.

多障礙物三維場(chǎng)景下實(shí)驗(yàn)結(jié)果,如圖17所示.

(a) 平均運(yùn)行時(shí)間 (b) 平均路徑長(zhǎng)度

由圖17(a)可知:5組不同起始點(diǎn)至目標(biāo)點(diǎn)距離下RSPM算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.008 5,0.011 7,0.008 0,0.015 7,0.019 5 s,而對(duì)比算法中表現(xiàn)最好的RRT*-Connect算法規(guī)劃路徑的平均運(yùn)行時(shí)間分別為0.673 9,0.917 0,0.856 7,1.095 4,1.183 7 s;5組不同起始點(diǎn)和目標(biāo)點(diǎn)距離下RSPM算法生成路徑的最小運(yùn)行時(shí)間僅為0.005 4 s,而4種對(duì)比算法規(guī)劃路徑的最小運(yùn)行時(shí)間為0.193 3 s.

由圖17(b)可知:5組不同起始點(diǎn)至目標(biāo)點(diǎn)距離下RSPM算法規(guī)劃路徑的平均路徑長(zhǎng)度分別為212.578 4,321.458 8,413.838 8,530.769 7,664.367 4 mm,而對(duì)比算法中表現(xiàn)最好的RRT*-Connect算法生成路徑的平均路徑長(zhǎng)度分別為257.719 0,365.224 9,467.727 1,586.573 4,692.050 4 mm.

5.5 路徑約簡(jiǎn)

為了測(cè)試PS算法約簡(jiǎn)路徑的性能,在多障礙物三維場(chǎng)景中,基于RSPM算法和4種對(duì)比算法在任意起始點(diǎn)s到目標(biāo)點(diǎn)d之間規(guī)劃一條路徑,再利用PS算法對(duì)生成的路徑進(jìn)行約簡(jiǎn).路徑約簡(jiǎn)實(shí)驗(yàn)結(jié)果,如圖18所示.圖18中:Np為分段點(diǎn)數(shù).

(a) 分段點(diǎn)數(shù) (b) 路徑長(zhǎng)度

由圖18可知:5種算法規(guī)劃路徑的分段點(diǎn)數(shù)比約簡(jiǎn)前分別減少了40%,57%,47%,55%,43%;路徑長(zhǎng)度分別減少了0.39%,3.74%,0.75%,2.07%,2.30%;5次路徑約簡(jiǎn)所用的時(shí)間分別為0.0045,0.010 1,0.004 0,0.007 6,0.005 5 s.

6 結(jié)論

1) 提出了一種采用分段點(diǎn)遷移遞歸(RSPM)和遞進(jìn)約簡(jiǎn)(PS)的機(jī)器人末端避障路徑規(guī)劃算法(RSPM-PS),其中,RSPM算法用于基礎(chǔ)路徑規(guī)劃,PS算法用于約簡(jiǎn)基礎(chǔ)路徑,生成最終路徑,是一種通用的路徑優(yōu)化處理方法.

2) 在4種仿真場(chǎng)景中,將RSPM算法和4種對(duì)比算法進(jìn)行對(duì)比實(shí)驗(yàn),RSPM算法在平均運(yùn)行時(shí)間和平均路徑長(zhǎng)度兩個(gè)性能指標(biāo)上均優(yōu)于4種對(duì)比算法,結(jié)果證明了RSPM算法的有效性和高效性.

3) 利用PS算法對(duì)5種算法生成的路徑進(jìn)行約簡(jiǎn),可以快速去除路徑上的冗余分段點(diǎn),進(jìn)一步縮短了平均路徑長(zhǎng)度,路徑約簡(jiǎn)結(jié)果證明了PS算法的有效性和高效性.

在未來(lái)的工作中,RSPM-PS算法將被應(yīng)用于工業(yè)機(jī)器人機(jī)械手末端避障路徑規(guī)劃,而路徑規(guī)劃結(jié)果將被應(yīng)用于工業(yè)機(jī)器人機(jī)械手在關(guān)節(jié)空間中的避障軌跡規(guī)劃.