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

中圖分類號(hào)：TP241.3"" 文獻(xiàn)標(biāo)志碼：A"""" 文章編號(hào)：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

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

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

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

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

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

1.1 RRT算法

RRT算法的工作原理是對(duì)探索空間進(jìn)行不斷的采樣，然后像樹的生長(zhǎng)過(guò)程一樣不斷擴(kuò)張，每一個(gè)采樣點(diǎn)都是下一個(gè)采樣點(diǎn)的父節(jié)點(diǎn)，直到最終采樣點(diǎn)為目標(biāo)點(diǎn)時(shí)停止，RRT算法的搜索過(guò)程如圖1.

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

1.2 RRT-Connect算法

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

2 Improved-RRT-Connect算法

2.1 逆向重布線

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

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

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

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

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

改進(jìn)算法中修改的代碼如表3.

2.2 B樣條曲線平滑路徑

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

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

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

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

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

文中比較了RRT算法、RRT*-Smart算法、RRT-Connect算法與所提出的Improved-RRT-Connect算法在復(fù)雜環(huán)境下的執(zhí)行效率.通過(guò)在Python中使用Plotly構(gòu)造具有障礙物的二維仿真環(huán)境，障礙物設(shè)置為一定數(shù)量的矩形和圓形，設(shè)置起點(diǎn)的坐標(biāo)為（2，2），目標(biāo)點(diǎn)坐標(biāo)為（49，24），最大迭代次數(shù)為5 000，步長(zhǎng)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 逆向重布線驗(yàn)證

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

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

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

3.2 B樣條曲線擬合路徑驗(yàn)證

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

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

4 結(jié)論

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

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

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

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

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

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

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

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

參考文獻(xiàn)（References）

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

[2] 官金炫，林桂潮，張世昂，等.水果采摘機(jī)械臂避障運(yùn)動(dòng)規(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] 張勤， 樂(lè)曉亮， 李彬， 等. 基于 CTB-RRT～*的果蔬采摘機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 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] 呂恩利， 阮清松， 劉妍華， 等. 基于動(dòng)態(tài)識(shí)別區(qū)和 B 樣條曲線的智能叉車避障路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 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.