孫靈碩

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

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

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

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

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

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

1 問題分析

1.1 RRT-Connect算法

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

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

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

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

圖1 Extend 過程示意圖Fig.1 Diagram of Extend process

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

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

圖2 Connect 過程示意圖Fig.2 Diagram of Connect process

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

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

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

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

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

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

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

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

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

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

2.1 凹形障礙區(qū)域

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

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

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

類似的，也可以推出凹體的定義。凸幾何圖形和凹幾何圖形的等價條件如圖5（c）和圖5（d）所示。基于上述定義可以進(jìn)一步推導(dǎo)出凹形障礙區(qū)域的定義：

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

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

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

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

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

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

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

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

若樹節(jié)點被檢測出位于凹形障礙區(qū)域，則標(biāo)記此樹節(jié)點所在位置及其附近區(qū)域為已識別的凹形障礙區(qū)域Xcon，如圖7所示。

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

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

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

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

2.4 算法實現(xiàn)

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

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

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

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

3 實驗與分析

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

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

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

圖11 仿真實驗地圖Fig.11 Environment map of simulation

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

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

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

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

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

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

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

4 結(jié)語

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

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