基于改進(jìn)RRT-Connect算法的焊接機(jī)器人避障路徑規(guī)劃

孫靈碩

（武漢理工大學(xué) 物流工程學(xué)院，武漢430000）

隨著機(jī)器人技術(shù)的發(fā)展，焊接機(jī)器人逐漸替代了傳統(tǒng)的手工焊接[1-3]，在港口大型機(jī)械裝備的制造中發(fā)揮著越來(lái)越重要的作用[4]。其中，焊接機(jī)器人的路徑規(guī)劃問(wèn)題作為研究的重點(diǎn)，受到了廣泛關(guān)注[5-7]，其基本任務(wù)是快速找到一條從上一焊縫終點(diǎn)到下一焊縫起點(diǎn)的無(wú)碰撞路徑。目前可用于避障路徑規(guī)劃的算法很多，比如人工勢(shì)場(chǎng)法[8]、A* 算法[9-10]、Dijkstra 算法[11]等。其中，人工勢(shì)場(chǎng)法算法簡(jiǎn)單、實(shí)時(shí)性好、安全性高，但易于陷入局部最優(yōu)陷阱和出現(xiàn)路徑振蕩現(xiàn)象；A* 算法和Dijkstra 算法都是求解最優(yōu)路徑的經(jīng)典算法，但是需要對(duì)障礙環(huán)境進(jìn)行精確建模，在焊接機(jī)器人路徑規(guī)劃這樣的高維空間中計(jì)算量會(huì)劇增。RRT-Connect[12]算法是RRT 系列算法中的一種改進(jìn)型算法，通過(guò)隨機(jī)采樣的方式搜索路徑，避免了對(duì)環(huán)境的精確建模，在高維空間的路徑規(guī)劃問(wèn)題中具有良好的性能，適合解決焊接機(jī)器人的避障路徑規(guī)劃問(wèn)題。

盡管RRT-Connect算法相較于RRT 算法有了顯著的性能提升，但也存在一些不足之處。在焊接機(jī)器人采用RRT-Connect算法進(jìn)行路徑規(guī)劃時(shí)，當(dāng)工件存在較多凹形障礙區(qū)域時(shí)算法性能會(huì)出現(xiàn)明顯下降，具體表現(xiàn)為：路徑搜索次數(shù)增多、路徑質(zhì)量欠佳。本文針對(duì)RRT-Connect算法在焊接機(jī)器人路徑規(guī)劃中存在的問(wèn)題，提出了改進(jìn)的RRT-Connect算法，改進(jìn)算法的主要?jiǎng)?chuàng)新點(diǎn)為：

（1）檢測(cè)并標(biāo)記出位于凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)及其附近區(qū)域。

（2）根據(jù)標(biāo)出的區(qū)域縮減自由狀態(tài)空間，修剪隨機(jī)樹(shù)上處于凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)。

通過(guò)上述做法，不斷增強(qiáng)隨機(jī)樹(shù)規(guī)避凹形障礙區(qū)域的能力，避免隨機(jī)樹(shù)陷入凹形障礙區(qū)域，將隨機(jī)樹(shù)導(dǎo)向更有價(jià)值的區(qū)域。最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了改進(jìn)算法的優(yōu)勢(shì)。

1 問(wèn)題分析

1.1 RRT-Connect算法

RRT-Connect算法通過(guò)不斷擴(kuò)展快速探索隨機(jī)樹(shù)T（V，E）來(lái)探索狀態(tài)空間X，直到得到一條可行的路徑。狀態(tài)空間X 指的是路徑規(guī)劃的工作空間。有障礙物的狀態(tài)空間記為Xobs，其余的記為自由狀態(tài)空間Xfree，Xfree=X-Xobs。T（V，E）是一種樹(shù)形數(shù)據(jù)結(jié)構(gòu)，V 代表隨機(jī)樹(shù)上的樹(shù)節(jié)點(diǎn)，E 表示樹(shù)節(jié)點(diǎn)之間所形成的邊。RRT-Connect算法的流程如下：

（1）首先算法在起點(diǎn)qinit和目標(biāo)點(diǎn)qgoal各初始化一棵快速探索隨機(jī)樹(shù)，分別記為T(mén)a和Tb。

（2）算法進(jìn)入搜索過(guò)程，算法在自由狀態(tài)空間Xfree中均勻隨機(jī)地選取一個(gè)采樣點(diǎn)，記為qrand。

（3）Ta執(zhí)行Extend 過(guò)程，Extend 過(guò)程如圖1所示。

圖1 Extend 過(guò)程示意圖Fig.1 Diagram of Extend process

Extend 過(guò)程首先遍歷Ta上的樹(shù)節(jié)點(diǎn)，找到距離qrand最近的樹(shù)節(jié)點(diǎn)qnear，而后向qrand搜索一固定步長(zhǎng)，產(chǎn)生新樹(shù)節(jié)點(diǎn)qnew并進(jìn)行碰撞檢測(cè)。

（4）隨后Tb執(zhí)行Connect 過(guò)程，過(guò)程如圖2所示。

圖2 Connect 過(guò)程示意圖Fig.2 Diagram of Connect process

Connect 過(guò)程本質(zhì)上是迭代執(zhí)行Extend 過(guò)程。首先遍歷Tb上的樹(shù)節(jié)點(diǎn)，找到距離qnew最近的樹(shù)節(jié)點(diǎn)qnear，隨后向著qnew盡可能多地前進(jìn)多個(gè)步長(zhǎng)，產(chǎn)生多個(gè)新的樹(shù)節(jié)點(diǎn)，直到發(fā)生碰撞或與qnew發(fā)生連接。

（5）若最新的樹(shù)節(jié)點(diǎn)Sn沒(méi)有與qnew發(fā)生連接，則交換兩棵隨機(jī)樹(shù)Ta和Tb，進(jìn)入下一次搜索過(guò)程。

RRT-Connect算法引入了啟發(fā)性的貪婪策略，顯著提高了算法的收斂速度，但有時(shí)也會(huì)將隨機(jī)樹(shù)導(dǎo)入低價(jià)值區(qū)域，造成算法性能下降。

1.2 焊接機(jī)器人路徑規(guī)劃

焊接機(jī)器人在進(jìn)行避障路徑規(guī)劃時(shí)，若待焊接工件上有較多凹形障礙區(qū)域，RRT-Connect算法的搜索次數(shù)增多、算法速度變慢、路徑質(zhì)量偏差。凹形障礙區(qū)域在幾何上直觀表現(xiàn)為一個(gè)由工件圍成的“口袋型”的區(qū)域。工件及其形成的凹形障礙區(qū)域如圖3所示。

圖3 工件形成的凹形障礙區(qū)域Fig.3 Concave area formed by obstacle

由于在算法的路徑搜索過(guò)程中，兩棵隨機(jī)樹(shù)交替執(zhí)行Connect 過(guò)程，則在開(kāi)始的幾次搜索過(guò)程中，兩棵隨機(jī)樹(shù)都極易陷入凹形障礙區(qū)域中。陷入凹形障礙區(qū)域的隨機(jī)樹(shù)如圖4所示，以隨機(jī)樹(shù)Tb上的最新樹(shù)節(jié)點(diǎn)S 為例，圖中灰色區(qū)域均為樹(shù)節(jié)點(diǎn)S 的Voronoi圖區(qū)域。而隨機(jī)樹(shù)Ta接下來(lái)通過(guò)Extend 過(guò)程產(chǎn)生的新的樹(shù)節(jié)點(diǎn)一定在此區(qū)域內(nèi)，因此當(dāng)隨機(jī)樹(shù)Tb接著執(zhí)行Connect 過(guò)程必然會(huì)選擇樹(shù)節(jié)點(diǎn)S 作為最近樹(shù)節(jié)點(diǎn)。但是由于樹(shù)節(jié)點(diǎn)S 距離障礙物太近，幾乎不可能向著隨機(jī)樹(shù)Ta的方向產(chǎn)生新的樹(shù)節(jié)點(diǎn)，只會(huì)白白的消耗搜索次數(shù)，算法性能此時(shí)出現(xiàn)退化。

圖4 陷入凹形障礙區(qū)域的隨機(jī)樹(shù)Fig.4 Random trees trapped by concave area

2 改進(jìn)的RRT-Connect算法

根據(jù)前文所述，本文提出了一種改進(jìn)的RRTConnect 算法，在Extend 過(guò)程和Connect 過(guò)程中加入檢測(cè)模塊，用來(lái)檢測(cè)每次路徑搜索中產(chǎn)生的新樹(shù)節(jié)點(diǎn)是否位于凹形障礙區(qū)域。若檢測(cè)到該樹(shù)節(jié)點(diǎn)位于凹形障礙區(qū)域，則將此樹(shù)節(jié)點(diǎn)所在位置及其附近區(qū)域標(biāo)記為凹形障礙區(qū)域Xcon，并將此凹形障礙區(qū)域從自由狀態(tài)空間Xfree中刪去，同時(shí)補(bǔ)充到障礙狀態(tài)空間Xobs中去。最后將隨機(jī)樹(shù)上位于凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)刪去。接下來(lái)對(duì)凹形障礙區(qū)域的定義以及改進(jìn)算法的主要工作進(jìn)行詳細(xì)介紹。

2.1 凹形障礙區(qū)域

在幾何學(xué)中，一個(gè)幾何圖形可分為凸或凹的。如圖5所示，圖5（a）為凸多邊形；圖5（b）是凹多邊形。凸幾何圖形是指內(nèi)部為凸集的幾何圖形，凹體的定義也同理。下面分別給出凸幾何圖形和凹幾何圖形的定義：

定義1任意2 個(gè)屬于圖形上的點(diǎn)所連成的線段全部位于幾何圖形的內(nèi)部或邊界的圖形稱為凸幾何圖形。

定義2存在2 個(gè)屬于圖形上的點(diǎn)所連成的線段不完全位于幾何圖形的內(nèi)部或邊界的圖形稱為凹幾何圖形。

類似的，也可以推出凹體的定義。凸幾何圖形和凹幾何圖形的等價(jià)條件如圖5（c）和圖5（d）所示?；谏鲜龆x可以進(jìn)一步推導(dǎo)出凹形障礙區(qū)域的定義：

定義3所有2 個(gè)屬于圖形上的點(diǎn)所連成的線段中不位于幾何圖形的內(nèi)部或邊界部分的全體集合稱為該圖形所形成的凹形障礙區(qū)域。

凹形障礙區(qū)域的定義將為接下來(lái)的算法改進(jìn)提供理論基礎(chǔ)。

圖5 凸多邊形、凹多邊形及其等價(jià)條件Fig.5 Convex polygons，concave polygons and their judgment

2.2 凹形障礙區(qū)域檢測(cè)與標(biāo)記

為避免隨機(jī)樹(shù)陷入凹形障礙區(qū)域，首先考慮將自由狀態(tài)空間中的凹形障礙區(qū)域檢測(cè)并標(biāo)記出來(lái)。直接對(duì)整個(gè)自由狀態(tài)空間Xfree檢測(cè)凹形障礙區(qū)域這一做法計(jì)算量過(guò)大，而考察Xfree內(nèi)的某一點(diǎn)是否位于凹形障礙區(qū)域則較為簡(jiǎn)單。所以改進(jìn)的RRTConnect 算法通過(guò)檢測(cè)路徑搜索過(guò)程中產(chǎn)生的新樹(shù)節(jié)點(diǎn)是否位于凹形障礙區(qū)域的方式來(lái)標(biāo)記凹形障礙區(qū)域。以二維平面為例，根據(jù)前文所述凹形障礙區(qū)域的定義，那么可以推導(dǎo)出圖形外某一點(diǎn)是否位于該圖形所形成的凹形障礙區(qū)域的等價(jià)條件為：

定理1存在過(guò)此點(diǎn)所在位置的一條直線，以此點(diǎn)為分界點(diǎn)，若該直線在此分界點(diǎn)的兩端均與圖形存在交點(diǎn)，則說(shuō)明此點(diǎn)位于凹形障礙區(qū)域。

凹形障礙區(qū)域檢測(cè)的具體做法如圖6所示，過(guò)新產(chǎn)生的樹(shù)節(jié)點(diǎn)S2隨機(jī)地做若干條直線，判斷其中是否存在一條直線L1在此樹(shù)節(jié)點(diǎn)兩端均與障礙物存在交點(diǎn)。若存在上述直線L1，則說(shuō)明此樹(shù)節(jié)點(diǎn)所在位置位于凹形障礙區(qū)域。

圖6 凹形障礙區(qū)域檢測(cè)示意圖Fig.6 Detection of concave area

若樹(shù)節(jié)點(diǎn)被檢測(cè)出位于凹形障礙區(qū)域，則標(biāo)記此樹(shù)節(jié)點(diǎn)所在位置及其附近區(qū)域?yàn)橐炎R(shí)別的凹形障礙區(qū)域Xcon，如圖7所示。

圖7 凹形障礙區(qū)域標(biāo)記示意圖Fig.7 Mark of concave area

2.3 縮減自由狀態(tài)空間與修剪隨機(jī)樹(shù)

在得出已識(shí)別的凹形障礙區(qū)域Xcon后，將此區(qū)域納入到障礙狀態(tài)空間Xobs中去，與之相對(duì)的自由狀態(tài)空間Xfree則減去此區(qū)域。隨機(jī)樹(shù)在接下來(lái)的路徑搜索過(guò)程中就會(huì)避免在此次標(biāo)記出的凹形障礙區(qū)域內(nèi)產(chǎn)生采樣點(diǎn)以及新的樹(shù)節(jié)點(diǎn)。在后續(xù)的搜索過(guò)程中，算法會(huì)不斷積累檢測(cè)和標(biāo)記出凹形障礙區(qū)域，使得隨機(jī)樹(shù)規(guī)避凹形障礙區(qū)域的能力不斷增強(qiáng)，最終達(dá)到規(guī)避凹形障礙區(qū)域的目的。

根據(jù)前文所述，改進(jìn)的算法在Extend 過(guò)程和Connect 過(guò)程中加入了凹形區(qū)域識(shí)別模塊。這里以執(zhí)行完Connect 過(guò)程后的隨機(jī)樹(shù)為例，介紹隨機(jī)樹(shù)修剪的具體過(guò)程。由于Connect 過(guò)程在一次搜索過(guò)程中可以產(chǎn)生多個(gè)新的樹(shù)節(jié)點(diǎn)，為了盡可能多的識(shí)別出凹形障礙區(qū)域，在隨機(jī)樹(shù)通過(guò)Connect 過(guò)程產(chǎn)生并添加了多個(gè)新的樹(shù)節(jié)點(diǎn)之后，凹形障礙區(qū)域檢測(cè)模塊再對(duì)新的樹(shù)節(jié)點(diǎn)進(jìn)行檢測(cè)。隨機(jī)樹(shù)修剪過(guò)程如圖8所示，算法按照最新添加到隨機(jī)樹(shù)上的樹(shù)節(jié)點(diǎn)最先檢測(cè)的順序，直到檢測(cè)出某一新的樹(shù)節(jié)點(diǎn)不位于凹形障礙區(qū)域或者遍歷完所有新的樹(shù)節(jié)點(diǎn)時(shí)結(jié)束，隨后對(duì)隨機(jī)樹(shù)進(jìn)行修剪，將隨機(jī)樹(shù)上位于凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)刪去。這使得算法在后續(xù)的路徑搜索過(guò)程中可以不受陷入凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)產(chǎn)生的負(fù)面影響，從而提高算法的收斂速度。

2.4 算法實(shí)現(xiàn)

根據(jù)上述兩點(diǎn)算法改進(jìn)內(nèi)容，改進(jìn)的RRT-Connect算法流程如圖9所示。

圖8 隨機(jī)樹(shù)修剪過(guò)程示意圖Fig.8 Trimming process of random tree

圖9 改進(jìn)的RRT-Connect算法流程Fig.9 Improved RRT-Connect algorithm

改進(jìn)的RRT-Connect算法首先在起點(diǎn)處qinit和目標(biāo)點(diǎn)處qgoal各初始化一棵快速探索隨機(jī)樹(shù)Ta與Tb；然后進(jìn)入搜索過(guò)程，在自由狀態(tài)空間Xfree內(nèi)選取采樣點(diǎn)qrand，在2 棵隨機(jī)樹(shù)執(zhí)行完各自的路徑搜索過(guò)程并產(chǎn)生新的樹(shù)節(jié)點(diǎn)后，檢測(cè)新產(chǎn)生的樹(shù)節(jié)點(diǎn)是否位于凹形障礙區(qū)域，將位于凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)所在位置及其附近區(qū)域標(biāo)記為已識(shí)別的凹形障礙區(qū)域Xcon；然后更新自由狀態(tài)空間Xfree和障礙狀態(tài)空間Xobs。其中自由狀態(tài)空間Xfree需要減去已識(shí)別的凹形障礙區(qū)域Xcon部分，障礙狀態(tài)空間Xobs則加上已識(shí)別的凹形障礙區(qū)域Xcon部分；最后修剪掉隨機(jī)樹(shù)上位于已識(shí)別的凹形障礙區(qū)域Xcon的樹(shù)節(jié)點(diǎn)。若兩棵隨機(jī)樹(shù)未發(fā)生連接，則交換2 棵隨機(jī)樹(shù)的身份進(jìn)行下一次搜索，直到返回一條可行路徑或者搜索次數(shù)耗盡時(shí)結(jié)束。

3 實(shí)驗(yàn)與分析

為了對(duì)改進(jìn)的RRT-Connect算法進(jìn)行性能測(cè)試，本仿真實(shí)驗(yàn)采用港口機(jī)械裝備中常見(jiàn)的H 型鋼截面作為待焊接工件進(jìn)行避障。H 型鋼如圖10所示，由1 塊腹板拼接在2 塊翼板的翼緣中間組成，常用作港口起重運(yùn)輸機(jī)械中的承載結(jié)構(gòu)。

圖10 H 型鋼工件Fig.10 The H-beam

本仿真實(shí)驗(yàn)利用圖11所示的地圖進(jìn)行仿真實(shí)驗(yàn)，地圖中的空白區(qū)域代表自由狀態(tài)空間，黑色區(qū)域代表待焊接H 型鋼工件的截面所形成的障礙物狀態(tài)空間，圖中灰色正方形和三角形所在位置分別代表路徑規(guī)劃的起點(diǎn)和目標(biāo)點(diǎn)。

圖11 仿真實(shí)驗(yàn)地圖Fig.11 Environment map of simulation

在仿真實(shí)驗(yàn)中，分別以RRT 算法和RRT-Connect算法作為改進(jìn)的RRT-Connect算法的比較對(duì)象，通過(guò)比較這3 種算法在搜索次數(shù)、路徑長(zhǎng)度以及路徑平滑度等指標(biāo)來(lái)證明改進(jìn)的RRT-Connect算法性能的優(yōu)越性。圖12 為改進(jìn)的RRT-Connect算法的仿真結(jié)果，如圖12（a）所示，圖中灰色的折線段代表改進(jìn)的RRT-Connect算法在起點(diǎn)和目標(biāo)點(diǎn)處擴(kuò)展出的隨機(jī)樹(shù)，黑色的折線段代表規(guī)劃出的路徑；圖12（b）中的灰色區(qū)域代表算法在路徑搜索過(guò)程中標(biāo)記出的凹形障礙區(qū)域，從仿真結(jié)果可以看出，生成的路徑成功地避開(kāi)了標(biāo)記出的凹形障礙區(qū)域。

圖12 改進(jìn)的RRT-Connect算法仿真實(shí)驗(yàn)結(jié)果Fig.12 Simulation results of the improved RRT-Connect algorithm

RRT-Connect算法和RRT 算法的仿真結(jié)果分別如圖13 和圖14所示。圖中黑色的折線段代表算法規(guī)劃出的路徑，灰色的折線段表示算法生成的隨機(jī)樹(shù)。由于算法搜索次數(shù)越多，隨機(jī)樹(shù)探索的空間越大，通過(guò)與圖12（a）的對(duì)比可以看出，相比于其它兩種算法，改進(jìn)的RRT-Connect算法在搜索次數(shù)上具有明顯的優(yōu)勢(shì)，而且所生成的路徑長(zhǎng)度更短。

圖13 RRT-Connect算法仿真實(shí)驗(yàn)結(jié)果Fig.13 Simulation results of the RRT-Connect algorithm

圖14 RRT 算法仿真實(shí)驗(yàn)結(jié)果Fig.14 Simulation results of RRT algorithm

在仿真實(shí)驗(yàn)中，分別對(duì)上述3 種算法進(jìn)行100 次仿真實(shí)驗(yàn)，分別從算法搜索次數(shù)、路徑長(zhǎng)度以及路徑平滑度3 個(gè)指標(biāo)對(duì)這3 種算法進(jìn)行分析對(duì)比。3 種算法在100 次重復(fù)實(shí)驗(yàn)中的搜索次數(shù)、路徑長(zhǎng)度與路徑平滑度的平均值如表1所示，從數(shù)據(jù)可以看出，改進(jìn)的RRT-Connect算法在搜索次數(shù)上具有突出的優(yōu)勢(shì)，在路徑長(zhǎng)度以及路徑平滑度上也取得了一定的改進(jìn)成果。相比于RRT-Connect算法，改進(jìn)的算法在算法搜索次數(shù)上降低了47.30%，路徑長(zhǎng)度降低了9.96%，路徑平滑度也有明顯改善。其中搜索次數(shù)大幅降低的原因歸功于改進(jìn)的算法有效地避免了隨機(jī)樹(shù)陷入凹形障礙區(qū)域，將隨機(jī)樹(shù)的搜索方向?qū)騼r(jià)值更高的區(qū)域，加快了路徑搜索的速度。此外，由于隨機(jī)樹(shù)成功地規(guī)避了凹形障礙區(qū)域，減少了路徑的彎折，所以改進(jìn)的算法生成的路徑在長(zhǎng)度上和路徑平滑度上也具有一定的優(yōu)勢(shì)。

表1 仿真實(shí)驗(yàn)結(jié)果比較Tab.1 Comparison of simulation experiment result s

4 結(jié)語(yǔ)

本文針對(duì)RRT-Connect算法在凹形障礙區(qū)域內(nèi)出現(xiàn)性能下降的問(wèn)題，提出了一種改進(jìn)的RRT-Connect算法。通過(guò)在路徑搜索過(guò)程中不斷檢測(cè)和標(biāo)記出凹形障礙區(qū)域，不斷更新自由狀態(tài)空間與障礙狀態(tài)空間，并對(duì)隨機(jī)樹(shù)上陷入凹形障礙區(qū)域的樹(shù)節(jié)點(diǎn)進(jìn)行修剪，有效地避免了隨機(jī)樹(shù)在路徑搜索過(guò)程中陷入凹形障礙區(qū)域所造成的算法性能下降的問(wèn)題。最后通過(guò)仿真實(shí)驗(yàn)，驗(yàn)證了改進(jìn)后的算法相對(duì)于RRT-Connect算法在搜索次數(shù)上具有突出優(yōu)勢(shì)，在路徑長(zhǎng)度及路徑平滑度上取得了一定的改進(jìn)成果。

本文所述改進(jìn)的RRT-Connect算法目前主要用于只有單個(gè)待焊接工件的環(huán)境，下一步將提高算法的泛化能力，考慮針對(duì)多個(gè)工件所形成的復(fù)雜環(huán)境進(jìn)行進(jìn)一步的改進(jìn)，以便更好地服務(wù)于工程實(shí)際問(wèn)題。