基于改進RRT-Connect算法的焊接機器人避障路徑規劃

孫靈碩

（武漢理工大學 物流工程學院，武漢430000）

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

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

（1）檢測并標記出位于凹形障礙區域的樹節點及其附近區域。

（2）根據標出的區域縮減自由狀態空間，修剪隨機樹上處于凹形障礙區域的樹節點。

通過上述做法，不斷增強隨機樹規避凹形障礙區域的能力，避免隨機樹陷入凹形障礙區域，將隨機樹導向更有價值的區域。最后通過仿真實驗驗證了改進算法的優勢。

1 問題分析

1.1 RRT-Connect算法

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

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

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

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

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

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

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

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

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

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

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

1.2 焊接機器人路徑規劃

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

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

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

圖4 陷入凹形障礙區域的隨機樹Fig.4 Random trees trapped by concave area

2 改進的RRT-Connect算法

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

2.1 凹形障礙區域

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

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

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

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

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

凹形障礙區域的定義將為接下來的算法改進提供理論基礎。

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

2.2 凹形障礙區域檢測與標記

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

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

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

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

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

圖7 凹形障礙區域標記示意圖Fig.7 Mark of concave area

2.3 縮減自由狀態空間與修剪隨機樹

在得出已識別的凹形障礙區域Xcon后，將此區域納入到障礙狀態空間Xobs中去，與之相對的自由狀態空間Xfree則減去此區域。隨機樹在接下來的路徑搜索過程中就會避免在此次標記出的凹形障礙區域內產生采樣點以及新的樹節點。在后續的搜索過程中，算法會不斷積累檢測和標記出凹形障礙區域，使得隨機樹規避凹形障礙區域的能力不斷增強，最終達到規避凹形障礙區域的目的。

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

2.4 算法實現

根據上述兩點算法改進內容，改進的RRT-Connect算法流程如圖9所示。

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

圖9 改進的RRT-Connect算法流程Fig.9 Improved RRT-Connect algorithm

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

3 實驗與分析

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

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

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

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

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

圖12 改進的RRT-Connect算法仿真實驗結果Fig.12 Simulation results of the improved RRT-Connect algorithm

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

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

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

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

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

4 結語

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

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