基于逆向重布線改進RRT-Connect算法的機械臂路徑規(guī)劃

摘 要： 為解決RRT-Connect算法在復(fù)雜非結(jié)構(gòu)環(huán)境下規(guī)劃出的路徑成本較長、有效節(jié)點數(shù)過多、曲折不平滑等問題，提出了一種基于逆向重布線的改進RRT-Connect算法（Improved-RRT-Connect）.首先，在算法的起始位置加入碰撞檢測函數(shù)，當起點和終點之間不存在障礙物時，能夠快速生成一條路徑；其次，通過對生成的路徑采取逆向重布線的措施，對生成的路徑中的節(jié)點進行重新篩選布線，達到縮短生成路徑長度，減少有效節(jié)點數(shù)量的目的；最后，通過結(jié)合B樣條曲線對生成的路徑進行擬合處理，得到一條平滑連續(xù)的路徑.通過Improved-RRT-Connect算法與RRT、RRT*-Smart、RRT-Connect等算法的對比實驗表明，在規(guī)劃時間上，Improved-RRT-Connect算法比RRT、RRT*-Smart算法減少了52.14%、98.53%，在路徑成本上比RRT、RRT-Connect算法減少了19.39%、17.15%，闡明了提出的Improved-RRT-Connect算法的優(yōu)越性.

關(guān)鍵詞： RRT-Connect算法；機械臂；碰撞檢測；逆向重布線；B樣條曲線

中圖分類號：TP241.3"" 文獻標志碼：A"""" 文章編號：1673-4807（2024）02-047-06

Improved RRT-Connect algorithm based on reverserewiring for manipulator path planning

Abstract：In order to solve the problems of long path cost， excessive number of effective nodes and uneven twists and turns in complex unstructured environment， an improved RRT-Cconnect algorithm based on reverse rewiring is proposed. Firstly， a collision detection function is added to the starting position of the algorithm， which can quickly generate a path when there is no obstacle between the starting point and the ending point. Secondly， through the measures of reverse rewiring of the generated path， the nodes in the generated path are re-screened and routed， so as to shorten the length of the generated path and reduce the number of effective nodes. Finally， a smooth and continuous path is obtained by fitting the generated path with B-spline curve. The experimental results show that the planning time of the improved RRT-Connect algorithm is 52.14% and 98.53% less than that of RRT and RRT*-smart algorithms， respectively， and the path cost is reduced by 19.39% and 17.15%. The advantages of the improved-RRT-Connect algorithm proposed in this paper are illustrated.

Key words：RRT-Connect algorithm， manipulator， collision detection， reverse rewiring， B-spline curve

采摘機器人通常由機械臂和導(dǎo)航車組成，采摘主體為機械臂.采摘環(huán)境中存在諸多未知的靜態(tài)和動態(tài)的不規(guī)則障礙物，所以如何給機械臂規(guī)劃一條合適的運動軌跡，使其在該條路徑上能夠迅速無碰撞的采摘果實是機械臂采摘果實研究中的重中之重.

為了解決機械臂在多維空間中尋找最優(yōu)路徑解的問題，提出了基于搜索的路徑規(guī)劃和基于智能算法的路徑規(guī)劃，其中典型的算法有A*算法、人工勢場法、蟻群算法、遺傳算法等[1-2].A*算法是一種搜索式的路徑規(guī)劃算法，添加了啟發(fā)式函數(shù)，減少了路徑搜索的面積，文獻[3-4]提出了改進的A*算法，解決了傳統(tǒng)A*算法產(chǎn)生的鋸齒路徑和交叉路徑中存在的節(jié)點多、距離遠、轉(zhuǎn)彎角度大的問題.文獻[5-6]分別提出基于改進的預(yù)規(guī)劃人工勢場法和基于動態(tài)窗法（dynamic window approach，DWA）的改進有源濾波器方法，有效解決了機器人在傳統(tǒng)人工勢場法下容易陷入局部極小值的問題.文獻[7]提出一種改進的自適應(yīng)蟻群算法，有效的解決了原始蟻群算法尋找路徑耗費時間長，路徑解非最優(yōu)的不足；文獻[8]通過調(diào)整蟻群算法的合適參數(shù)以及添加與信息素相關(guān)的正弦函數(shù)克服了蟻群算法收斂時間長，容易陷入局部最優(yōu)解的缺點.文獻[9]提出了一種提高累積檢測概率（cumulative detection probability，CDP）的改進遺傳算法，該方法有效的提高了遺傳算法的收斂速度和穩(wěn)定性；文獻[10]也對遺傳算法進行了改進，有效的避免了遺傳算法過早地找到非最優(yōu)解路徑，提高了遺傳算法的全局搜索能力.

為了減少路徑規(guī)劃算法所消耗的時間，提出快速搜索隨機樹算法（rapidly-exploration random tree， RRT），RRT算法隨機地在規(guī)劃空間內(nèi)采樣，通過判斷路徑線段是否與障礙物產(chǎn)生交集，有選擇的保留采樣點，避免了對障礙物建模，相較于搜索算法和智能算法其效率較高.但RRT算法也存在一定的局限性，其原理是隨機采樣，故而在規(guī)劃路徑時具有一定的盲目性.為了解決以上問題，文獻[11]為了簡化空間的復(fù)雜度以及提高規(guī)劃效率，提出1-0 Bg-RRT算法；文獻[12]提出了一種基于三角不等式的改進RRT-Connect算法，有效地減少了規(guī)劃所需要的時間和規(guī)劃路徑的成本；文獻[13]提出了基于CTB-RRT*的規(guī)劃算法，引入柯西函數(shù)有效地減少了采樣點的數(shù)量，增加目標引力，有效地提高了局部區(qū)域的搜索速度；文獻[14]為了提高RRT*的收斂速度，獲得更優(yōu)解，結(jié)合P-RTT*和Q-RRT*的優(yōu)點，提出了PQ-RRT*算法.

RRT-Connect算法雖然相較于RRT算法規(guī)劃路徑所需要的時間更短，但由于過早地找到路徑，往往所尋路徑并非最優(yōu)解，存在路徑成本較長的問題，同時RRT-Connect算法規(guī)劃出的路徑是由多段子線條組成的折線，因此存在著路徑曲折不平滑的問題.因此，文中提出了一種基于逆向重布線（reverse rewiring）的RRT-Connect改進算法（Improved-RRT-Connect），并對其生成的路徑采用B樣條曲線對其進行擬合.實驗表明，所提出的Improved-RRT-Connect算法所生成的路徑更短，也更加平滑.

1 RRT算法與RRT-Connect算法分析

1.1 RRT算法

RRT算法的工作原理是對探索空間進行不斷的采樣，然后像樹的生長過程一樣不斷擴張，每一個采樣點都是下一個采樣點的父節(jié)點，直到最終采樣點為目標點時停止，RRT算法的搜索過程如圖1.

生長樹的擴張過程為：設(shè)置初始節(jié)點xstart和目標節(jié)點xgoal，同時初始節(jié)點即為生長樹T的起點.隨機產(chǎn)生一個節(jié)點xrand，計算xrand到生長樹上各節(jié)點的距離，選擇最短距離的節(jié)點作為xrand的最近節(jié)點xnearst.連接xrand與xnearst，該連線即為生長樹的生長方向，在該生長方向上生成新的節(jié)點xnew，以步長step作為xnew到父節(jié)點xparent的距離（此時的xparent即為xstart），判斷xnew與xparent的連線是否與障礙物發(fā)生交集，如果不相交則保留xnew并添加到生長樹上，否則拋棄xnew，重新采樣節(jié)點.每次產(chǎn)生新節(jié)點xnew后，判斷xnew與xgoal的距離是否小于步長step，若小于步長且二者之間的連線未與障礙物發(fā)生碰撞，則直接將目標點xgoal作為生長樹的終點，并輸出找到的路徑，否則重新進行采樣.

1.2 RRT-Connect算法

RRT-Connect算法不再遵循RRT算法單一地從起點擴展一棵生長樹，而是從起點和終點分別擴展兩棵生長樹，當兩棵樹的終點重合時規(guī)劃完成，同時在隨機采樣的過程中一棵樹的生長方向總是以另外一棵樹中最近的節(jié)點作為這棵樹繼續(xù)向外擴展的依據(jù)，當在這個方向上未遇到障礙物時則繼續(xù)沿著這個方向擴展，當遇到障礙物時，則以另一個擴展樹向外擴展，因此算法可以有效地走出尋找局部最優(yōu)的困境.基于以上策略RRT-Connect算法在搜索速度上比RRT算法有著顯著的提高.圖2為RRT-Connect算法的原理圖.

2 Improved-RRT-Connect算法

2.1 逆向重布線

文中提出了一種基于逆向重布線的Improved-RRT-Connect算法.首先，在RRT-Connect算法的端部加入碰撞檢測函數(shù)，因為RRT-Connect算法采樣是隨機的，即使起點到終點之間并不存在障礙物，該算法仍然會規(guī)劃一條曲折的多線段組成的路徑，如圖3（a），而加入碰撞檢測函數(shù)后，當起點到終點之間不存在障礙物時，則規(guī)劃出一條從起點到終點的直線段，如圖3（b）.

為方便描述，障礙物的形狀設(shè)定為圓形和矩形.圓形障礙物判斷碰撞條件為：

式中：l為障礙物中心點到直線的距離；A、B、C分別為起點和終點構(gòu)成的直線方程系數(shù)；（x0，y0）、r分別為圓形障礙物的中心點坐標和半徑；矩形障礙物判斷碰撞條件為：

式中：k為直線的斜率，a、b、c、d為矩形頂點，如圖4.

接著，當起點和終點之間存在障礙物時，繼續(xù)使用RRT-Connect算法規(guī)劃出一條路徑.對于規(guī)劃出的路徑采用逆向重布線的方式對已生成的路徑點進行重新篩選并連線.圖5（a）為RRT-Connect算法規(guī)劃出的路徑；圖5（b）表示以xstart為起點，從xgoal開始沿著xstart方向逐個尋找點與xstart連線，當連線與障礙物相交時，丟棄此連線；圖5（c）表示尋找到點xnew與xstart連線未與障礙物發(fā)生碰撞，保留此路徑段；圖5（d）為以xnew為起點，從xgoal開始沿著xnew方向逐個尋找點與xnew連線，此時終點xgoal與xnew的連線未與障礙物發(fā)生碰撞，路徑優(yōu)化完成.

改進算法中修改的代碼如表3.

2.2 B樣條曲線平滑路徑

雖然利用逆向重布線的方法對RRT-Connect算法做出了改進，但改進后得到的路徑仍是由多條線段組成的折線，因此在兩條線段連接處可能是陡峭的，不平滑的，這將導(dǎo)致機械臂在運動到此處時會停頓重新更改方向移動增加運行時間，頻繁的停頓和急速運動會加劇機械臂關(guān)節(jié)處的磨損，減少機械臂的使用壽命.針對上述問題，用B樣條曲線對規(guī)劃好的路徑進行擬合處理，最終得出對機械臂運動友好的平滑路徑.

B樣條曲線具有良好的連續(xù)性，通常對生成曲線的局部控制點進行調(diào)整時，只有該區(qū)域的曲線會受到影響，其余區(qū)域的曲線將保持原樣.此外，B樣條曲線與多邊形或多線段的逼近程度較好.文獻[15-16]提出了基于B樣條曲線對路徑進行平滑處理的研究方法.文獻[17]提出在樣條函數(shù)中插入經(jīng)濟點，自動生成節(jié)點向量，有效解決了在擬合路徑過程中與障礙物碰撞的問題.

B樣條曲線的數(shù)學(xué)表達式為：

式中：Pk （k = 0， 1， 2， …，n）為第k個控制頂點，又稱de Boor點；m為階參數(shù)；t為歸一化曲線長度參數(shù).Bk，m（t）是B樣條基函數(shù)，為k次分段多項式，由Cox-de Boor遞歸公式定義為：

3 實驗驗證和分析

文中比較了RRT算法、RRT*-Smart算法、RRT-Connect算法與所提出的Improved-RRT-Connect算法在復(fù)雜環(huán)境下的執(zhí)行效率.通過在Python中使用Plotly構(gòu)造具有障礙物的二維仿真環(huán)境，障礙物設(shè)置為一定數(shù)量的矩形和圓形，設(shè)置起點的坐標為（2，2），目標點坐標為（49，24），最大迭代次數(shù)為5 000，步長Step為0.8.搭載仿真環(huán)境的硬件設(shè)備CPU為：I7 - 6700HQ，主頻為：2.60 GHz 四核，顯卡為：NVIDIA GeForce 940MX，顯存為：2 GB，主硬盤為：128 GB 固態(tài)硬盤，內(nèi)存為：12 GB，操作系統(tǒng)為：Windows 10 Enterprise 64位.

3.1 逆向重布線驗證

分別對4種算法進行50次實驗，對各算法規(guī)劃的時間和規(guī)劃出的路徑成本以及路徑解中的節(jié)點數(shù)取平均值.圖7展示了4種算法在仿真環(huán)境中規(guī)劃路徑的情形，圖7中隨機擴展的線條為各算法的采樣過程，圖7中連接起點和終點的線條為路徑解.表4中列出了各個算法中的一些性能指標，包括路徑規(guī)劃所耗時間、路徑規(guī)劃的總成本、最終路徑中的節(jié)點數(shù).

從表4中可以得知Improved-RRT-Connect算法在時間和路徑長度上的性能都優(yōu)于RRT算法，在規(guī)劃時間和路徑長度上Improved-RRT-Connect算法比RRT算法分別減少了52.14%，19.39%.雖然Improved-RRT-Connect算法在規(guī)劃路徑的長度上略高于RRT*-Smart算法，但在規(guī)劃時間上卻大大低于RRT*-Smart算法，其減少的時間達98.53%.相較于RRT-Connect算法Improved-RRT-Connect算法在規(guī)劃時間上性能稍遜，但不可否認其規(guī)劃速度仍屬快速，而在路徑長度上Improved-RRT-Connect算法比RRT-Connect算法減少了17.15%.

綜合考慮4種算法的性能指標，Improved-RRT-Connect算法優(yōu)于RRT算法、RRT*-Smart算法和RRT-Connect算法.同時經(jīng)過逆向重布線后的RRT-Connect算法，其路徑解中的節(jié)點數(shù)更少，為B樣條曲線平滑路徑奠定良好基礎(chǔ).

3.2 B樣條曲線擬合路徑驗證

對改進算法規(guī)劃出的路徑進行平滑處理.規(guī)劃出的路徑是由多段線段組成的路徑，在用B樣條曲線對路徑進行擬合處理時，將每段線段的端點作為B樣條曲線的控制點，如圖8.

通過采用B樣條曲線對原曲折路徑進行擬合處理，避免了拼接處不連續(xù)的問題，最終生成一條有利于機械臂運動的路徑.

4 結(jié)論

為進一步提高RRT-Connect算法的搜索效率，減少所規(guī)劃路徑的長度，文中結(jié)合逆向重布線理論在RRT-Connect算法采取了進一步的改進，提出了Improved-RRT-Connect算法.具體改進方法如下：

（1） 在算法的起始位置加入碰撞檢測函數(shù)，當起點和終點之間不存在障礙物時，能夠快速生成一條路徑.

（2） 通過對生成的路徑采取逆向重布線的措施，對生成的路徑中的節(jié)點進行重新篩選，達到縮短生成路徑長度，減少有效節(jié)點數(shù)量的目的.

（3） 通過結(jié)合B樣條曲線對生成的路徑進行擬合處理，得到一條平滑連續(xù)的路徑.

為了驗證改進算法的優(yōu)越性，分別對RRT算法、RRT*-Smart算法、RRT-Connect算法和Improved-RRT-Connect算法進行了50次實驗，并對結(jié)果取平均值，得出結(jié)論：

（1） 在規(guī)劃時間上，Improved-RRT-Connect算法比RRT算法、RRT*-Smart算法分別減少了52.14%、98.53%.

（2） 在路徑成本上比RRT算法、RRT-Connect算法分別減少了19.39%、17.15%.

（3） 改進后的路徑規(guī)劃算法生成的路徑更加平滑連續(xù).

參考文獻（References）

［1］ 林韓熙，向丹，歐陽劍，等.移動機器人路徑規(guī)劃算法的研究綜述[J].計算機工程與應(yīng)用，2021，57（18）：38-48.

[2] 官金炫，林桂潮，張世昂，等.水果采摘機械臂避障運動規(guī)劃優(yōu)化算法[J].現(xiàn)代農(nóng)業(yè)裝備，2021，42（3）：49-55.

[3] TANG G， TANG C， CLARAMUNT C， et al. Geometric A-star algorithm： an improved A-star algorithm for AGV path planning in a port environment[J]. IEEE Access， 2021， 9： 59196-59210.

[4] LE A V， PRABAKARAN V， SIVANANTHAM V， et al. Modified a-star algorithm for efficient coverage path planning in tetris inspired self-reconfigurable robot with integrated laser sensor[J]. Sensors， 2018， 18（8）： 2585.

[5] SHEN H， LI P. Unmanned aerial vehicle （UAV） path planning based on improved pre-planning artificial potential field method[C]∥2020 Chinese control and decision conference （CCDC）. USA：IEEE， 2020： 2727-2732.

[6] HU J， CHENG C， WANG C， et al. An improved artificial potential field method based on DWA and path optimization[C]∥2019 IEEE International Conference on Unmanned Systems （ICUS）.USA： IEEE， 2019： 809-814.

[7] MIAO C， CHEN G， YAN C， et al. Path planning optimization of indoor mobile robot based on adaptive ant colony algorithm[J]. Computers amp; Industrial Engineering， 2021， 156： 107230.

[8] PAN C， WANG H， LI J， et al. Path planning of mobile robot based on an improved ant colony algorithm[C]∥International Conference on Convergent Cognitive Information Technologies. Cham： Springer， 2018： 132-141.

[9] GUO H， MAO Z， DING W， et al. Optimal search path planning for unmanned surface vehicle based on an improved genetic algorithm[J]. Computers amp; Electrical Engineering， 2019， 79： 106467.

[10] LI Y， HUANG Z， XIE Y. Path planning of mobile robot based on improved genetic algorithm[C]∥2020 3rd International Conference on Electron Device and Mechanical Engineering （ICEDME）. USA：IEEE， 2020： 691-695.

[11] GAN Y， ZHANG B， KE C， et al. Research on robot motion planning based on RRT algorithm with nonholonomic constraints[J]. Neural Processing Letters， 2021， 53（4）： 3011-3029.

[12] KANG J G， LIM D W， CHOI Y S， et al. Improved RRT-connect algorithm based on triangular inequality for robot path planning[J]. Sensors， 2021， 21（2）： 333.

[13] 張勤， 樂曉亮， 李彬， 等. 基于 CTB-RRT～*的果蔬采摘機械臂運動路徑規(guī)劃[J]. 農(nóng)業(yè)機械學(xué)報， 2021，52（10）：129-136.

[14] LI Y， WEI W， GAO Y， et al. PQ-RRT*： An improved path planning algorithm for mobile robots[J]. Expert Systems with Applications， 2020， 152： 113425.

[15] CHIARAVALLI D， CALIFANO F， BIAGIOTTI L， et al. Physical-consistent behavior embodied in B-spline curves for robot path planning[J]. IFAC-Papers on Line， 2018， 51（22）： 306-311.

[16] 呂恩利， 阮清松， 劉妍華， 等. 基于動態(tài)識別區(qū)和 B 樣條曲線的智能叉車避障路徑規(guī)劃[J]. 農(nóng)業(yè)機械學(xué)報， 2019， 50（1）：359-366.

[17] NOREEN I. Collision free smooth path for mobile robots in cluttered environment using an economical clamped cubic B-spline[J]. Symmetry， 2020， 12（9）： 1567.