基于改進(jìn)的RRT*-connect算法機(jī)械臂路徑規(guī)劃

劉建宇，范平清

上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海 201620

隨著時(shí)代的飛速發(fā)展，高度自主化的機(jī)器人在人類社會中的地位與作用越來越大。而機(jī)械臂作為機(jī)器人的一個(gè)最主要操作部件，其運(yùn)動規(guī)劃問題，例如準(zhǔn)確抓取物體，在運(yùn)動中躲避障礙物等，是現(xiàn)在研究的熱點(diǎn)，對其運(yùn)動規(guī)劃的不斷深入研究是非常必要的。

機(jī)械臂的運(yùn)動規(guī)劃主要在高維空間中進(jìn)行。RRT（Rapidly-exploring Random Tree）算法[1]基于隨機(jī)采樣的規(guī)劃方式，無需對構(gòu)型空間的障礙物進(jìn)行精確描述，同時(shí)不需要預(yù)處理，因此在高維空間被廣為使用。

近些年人們對于RRT 算法的研究很多，2000 年Kuffner 等提出 RRT-connect 算法[2]，通過在起點(diǎn)與終點(diǎn)同時(shí)生成兩棵隨機(jī)樹，加快了算法的收斂速度，但存在搜索路徑步長較長的情況。2002 年Bruce 等提出了ERRT（Extend RRT）算法[3]。2006年Ferguson等提出DRRT（Dynamic RRT）算法[4]。2011 年 Karaman 和 Frazzoli 提出改進(jìn)的RRT*算法[5]，在繼承傳統(tǒng)RRT 算法概率完備性的基礎(chǔ)上，同時(shí)具備了漸進(jìn)最優(yōu)性，保證路徑較優(yōu)，但是會增加搜索時(shí)間。2012 年Islam 等提出快速收斂的RRT*-smart 算法[6]，利用智能采樣和路徑優(yōu)化來迫近最優(yōu)解，但是路徑采樣點(diǎn)較少，使得路徑棱角較大，不利于實(shí)際運(yùn)用。2013 年Jordan 等通過將RRT*算法進(jìn)行雙向搜索，提出B-RRT*算法[7]，加快了搜索速度。同年Salzman 等提出在下界樹LBT-RRT 中連續(xù)插值的漸進(jìn)優(yōu)化算法[8]。2015 年Qureshi 等提出在B-RRT*算法中插入智能函數(shù)提高搜索速度的IB-RRT*算法[9]。同年Klemm等結(jié)合RRT*的漸進(jìn)最優(yōu)和RRT-connect的雙向搜索，提出使搜索路徑朝理論最優(yōu)解收斂的RRT*-connect算法[10]。2016年王道威等提出動態(tài)步長的RRT算法[11]，但是只考慮簡單障礙物環(huán)境。2017 年朱宏輝等提出加入規(guī)避步長延伸法的改進(jìn)RRT*算法[12]，雖然防止陷入局部最小解，但是對于復(fù)雜狹小路徑，不容易獲得較優(yōu)解。2018 年陳波芝等人提出用于雙機(jī)械臂避障的改進(jìn)RRT算法[13]，但只考慮了球形障礙物環(huán)境。2019年王坤等提出在B-RRT*中加入采樣函數(shù)，并結(jié)合快速擴(kuò)展策略使搜索速度加快的EB-RRT*算法[14]。

本文基于上述研究成果，針對RRT*-connect算法的采樣速度較慢以及產(chǎn)生路徑較粗糙的問題，首先在目標(biāo)偏向采樣的基礎(chǔ)上加入約束條件，保證每次采樣趨近目標(biāo)點(diǎn)，減少不必要區(qū)域的搜索，使采樣更加高效。然后采用梯度下降法對搜索形成的路徑做平滑處理，對比分析改進(jìn)前后的RRT*-connect算法，證明經(jīng)過優(yōu)化處理的路徑更平滑。最后在ROS 平臺進(jìn)行仿真，驗(yàn)證該算法更適合實(shí)際機(jī)器人操作，路徑更短且用時(shí)更少。

1 RRT＊-connect算法

2015 年 Klemm 提出了 RRT*-connect 算法，該算法利用RRT*算法的漸進(jìn)最優(yōu)思想，在傳統(tǒng)的RRT 基礎(chǔ)上進(jìn)行優(yōu)化，使得搜索出的路徑是漸進(jìn)最優(yōu)的。

如圖1所示，qinit為路徑的起始點(diǎn)，向目標(biāo)點(diǎn)qgoal搜索，qrand是在自由空間得到的隨機(jī)采樣點(diǎn)，qnearest是隨機(jī)樹上找到離qrand最近的節(jié)點(diǎn)，qnew為qnearest向qrand延伸一定距離l得到的下一個(gè)拓展點(diǎn)。利用算法的漸進(jìn)最優(yōu)特性，為qnew重新選擇父節(jié)點(diǎn)。

圖1 自由空間采樣

如圖2（a）所示，首先以qnew為圓心，以一定半徑作圓，將隨機(jī)樹上落在圓內(nèi)的節(jié)點(diǎn)作為潛在父節(jié)點(diǎn)形成集合Qnear。其次對路徑進(jìn)行代價(jià)判斷，將從起始點(diǎn)經(jīng)過潛在父節(jié)點(diǎn)到qnew的路徑長度與經(jīng)過原qnearest節(jié)點(diǎn)的長度比較，不斷對比篩選出路徑代價(jià)最小的節(jié)點(diǎn)qnew_parent。然后將路徑添加進(jìn)隨機(jī)樹并刪除原qnearest與qnew連接路徑，如圖2（b）所示。同時(shí)該算法利用RRT-connect算法的貪婪搜索來實(shí)現(xiàn)雙向拓展。

圖2 父節(jié)點(diǎn)重構(gòu)

如圖3 所示，從起點(diǎn)qinit向目標(biāo)點(diǎn)qgoal搜索的過程中，目標(biāo)點(diǎn)qgoal也在向起始點(diǎn)qinit搜索。在兩棵隨機(jī)樹空間V1、V2中，qopt為起始點(diǎn)隨機(jī)樹V1中的一個(gè)拓展點(diǎn)，V2隨機(jī)樹將設(shè)法拓展到qopt節(jié)點(diǎn)。qopt節(jié)點(diǎn)利用路徑代價(jià)函數(shù)進(jìn)行選擇，如式（1）所示：

通過代價(jià)判斷得到最優(yōu)節(jié)點(diǎn)qopt，將兩棵隨機(jī)樹連接，路徑搜索完成。

圖3 兩棵隨機(jī)樹連接過程

2 改進(jìn)RRT＊-connect算法

2.1 采樣約束

上述RRT*-connect算法利用雙向隨機(jī)樹搜索，加快搜索速度。但是在搜索過程中存在采樣點(diǎn)的隨機(jī)性較大和采樣區(qū)域太分散的問題，容易造成搜索效率低和路徑較粗糙。

針對這個(gè)問題，本文將目標(biāo)偏向思想[15]加入到RRT*-connect中，目標(biāo)偏向思想是指在采樣過程中首先設(shè)定一個(gè)基準(zhǔn)值Pstandard，然后隨機(jī)樹每次在自由空間中采樣，按均勻概率分配，隨機(jī)產(chǎn)生一個(gè)概率值P。如果P小于原先設(shè)定的基準(zhǔn)值，則采樣點(diǎn)選擇為目標(biāo)點(diǎn)：

然后本文在P大于基準(zhǔn)值時(shí)，對隨機(jī)樹搜索的隨機(jī)點(diǎn)進(jìn)行控制（此處僅以起始點(diǎn)拓展隨機(jī)樹為例，目標(biāo)點(diǎn)拓展隨機(jī)樹的設(shè)置相同），如圖4所示，利用采樣約束策略，具體設(shè)置如下。

圖4 采樣設(shè)置示意圖

采樣約束設(shè)置：首先將起始點(diǎn)與目標(biāo)點(diǎn)進(jìn)行連接，將兩者之間的區(qū)域定義為全局的采樣區(qū)域Vq，即圖4黃色區(qū)域。然后當(dāng)P大于基準(zhǔn)值時(shí)，開始在空間進(jìn)行隨機(jī)采樣，并對采樣點(diǎn)的位置進(jìn)行判斷，保證每一次的采樣要比前一次更接近終點(diǎn)，否則重新采樣。即通過函數(shù)約束，使每次的采樣點(diǎn)都在采樣區(qū)間Vq內(nèi)，并且每次的采樣都在前一次采樣點(diǎn)的一定角度內(nèi)，左右角度θ≤900。如圖4中的qrand2為第二次隨機(jī)采樣點(diǎn)，位于第一次采樣點(diǎn)qrand1的上方θ角度內(nèi)，qrand3位于qrand2的下方θ角度內(nèi)，如下所示：

采樣約束設(shè)置確保每次采樣趨于目標(biāo)點(diǎn)，有效防止隨機(jī)采樣點(diǎn)的反向搜索，從而保證采樣的方向性，減少資源浪費(fèi)。同時(shí)在約束環(huán)境下不限制采樣點(diǎn)在縱向空間的隨機(jī)選擇，保留了采樣的隨機(jī)性，確保遇到障礙物時(shí)，能夠高效搜索出可行路徑。

最后在加入采樣約束后，整個(gè)改進(jìn)的RRT*-connect算法流程如圖5所示。

圖5 優(yōu)化算法流程圖

改進(jìn)的RRT*-connect算法，首先從起始點(diǎn)和目標(biāo)點(diǎn)初始化兩棵隨機(jī)樹，然后開始隨機(jī)獲取概率值并進(jìn)行判斷。如果小于基準(zhǔn)值，則將目標(biāo)點(diǎn)設(shè)置為采樣點(diǎn)，否則在約束區(qū)域進(jìn)行采樣，將經(jīng)過約束設(shè)置的采樣點(diǎn)加入隨機(jī)樹。最后利用算法的漸進(jìn)最優(yōu)特性重新選擇父節(jié)點(diǎn)，重構(gòu)隨機(jī)樹，不斷重復(fù)操作，直到某一節(jié)點(diǎn)同時(shí)存在兩隨機(jī)樹上，則路徑完成。

2.2 平滑處理

上文將RRT*-connect算法的采樣點(diǎn)進(jìn)行約束設(shè)置，明確采樣方向性，從而節(jié)約搜索資源，使得路徑更優(yōu)且用時(shí)更短。但依舊存在規(guī)劃出的路徑較陡，大角度彎折較多的問題。針對這個(gè)問題，采用梯度下降法對搜索出的路徑進(jìn)行優(yōu)化，使其在實(shí)際運(yùn)用中更加合理，進(jìn)而增強(qiáng)在實(shí)際運(yùn)行中的可操作性。

梯度下降法常應(yīng)用在A*、D*算法上，用于將搜索出的路徑做平滑處理，其優(yōu)點(diǎn)為計(jì)算量較少，能夠最小化所有樣本的損失函數(shù)值，并且使結(jié)果是全局最優(yōu)解或者在最優(yōu)解附近。梯度下降法是一種依靠迭代求解最小值的算法，主要是通過誤差最小化求解出最佳數(shù)據(jù)，從而可以達(dá)到擬合曲線的目的。

將路徑看成曲線并用函數(shù)表示，如式（2）所示：

式中，xi為處理數(shù)據(jù)中的特征值，θi為權(quán)重值。

引入假設(shè)函數(shù)，使用一組特征值代入式（2），由于權(quán)重值θi未知，令得到結(jié)果為y′，如式（3）所示：

引入損失函數(shù)概念，即式（3）得到的y′與真正的y值間存在誤差。損失函數(shù)ΔT(θ)如式（4）所示：

此差值為方差，損失ΔT(θ)越小意味兩者越接近，故ΔT(θ)越小越好。針對路徑存在數(shù)據(jù)較多的問題，將采用均方差科學(xué)表示損失值，如式（5）所示：式中匯總所有樣本數(shù)據(jù)，J(θ)為均方損失差。

梯度下降法利用微分思想求解，將θ取值范圍劃分多份，每份寬度為動態(tài)?，實(shí)際寬度由微分步長α決定，通過改變θ的值直至損失值趨于0。

根據(jù)微分公式變形得到的迭代公式如式（6）所示：

迭代使得每次損失最小，對于路徑而言只需控制權(quán)重值θi的大小和迭代次數(shù)，使得目標(biāo)函數(shù)取最小值，即可以對路徑進(jìn)行平滑處理，如圖6所示。

圖6 平滑處理對比

圖6紅色圈劃線為原始曲線，黑色虛線為經(jīng)過梯度下降法平滑處理后的曲線，可以明顯看出處理后的曲線弱化了原路徑的棱角，同時(shí)能夠控制平滑處理的程度，保證路徑的可靠性。

3 仿真結(jié)果對比分析

3.1 規(guī)劃時(shí)間與路徑長度分析

本文提出RRT*-connect 改進(jìn)算法并運(yùn)用仿真軟件對比原規(guī)劃算法。首先選擇兩幅復(fù)雜程度較高的地圖進(jìn)行測試，如圖7，地圖范圍為500×500，地圖中的黑色為障礙物，起點(diǎn)坐標(biāo)（0，0），目標(biāo)坐標(biāo)（500，500），紅色部分為搜索形成的隨機(jī)樹，藍(lán)色部分為規(guī)劃出的路徑。然后對比分析兩者搜索時(shí)間與路徑長度上的差異，來驗(yàn)證改進(jìn)算法的性能。

通過兩個(gè)算法在不同地圖中的對比，可以很明顯看出改進(jìn)后RRT*-connect算法在復(fù)雜環(huán)境下迭代次數(shù)更少，同時(shí)減少無效區(qū)域搜索，使得搜索更加高效，目的性更強(qiáng)。

經(jīng)過20 次重復(fù)實(shí)驗(yàn)，將路徑搜索時(shí)間和路徑長度匯總成表格。

圖8為兩幅測試圖20次實(shí)驗(yàn)的搜索時(shí)間對比，可以很明顯看出，改進(jìn)后RRT*-connect算法在不同的地圖環(huán)境下，搜索時(shí)間都有較大程度的縮小，并且更加穩(wěn)定，不存在原RRT*-connect 算法搜索時(shí)間隨機(jī)性大的問題。測試環(huán)境1 中原算法20 次搜索的平均耗時(shí)2.88 s，改進(jìn)RRT*-connect算法平均耗時(shí)為1.70 s，時(shí)間縮短40.97%。測試環(huán)境2 中原算法20 次搜索的平均耗時(shí)4.35 s，改進(jìn)RRT*-connect 算法平均耗時(shí)2.68 s，時(shí)間縮短38.39%。通過數(shù)據(jù)可以清晰看出，改進(jìn)后的RRT*-connect算法路徑搜索時(shí)間縮短40%左右，減少搜索資源的浪費(fèi)。

圖7 算法測試對比

圖8 兩種算法時(shí)間數(shù)據(jù)對比圖

圖9為兩幅測試圖20次實(shí)驗(yàn)的路徑長度對比，可以看出改進(jìn)后的RRT*-connect算法路徑長度普遍更短，路徑更優(yōu)，并且多次搜索的路徑相比原算法更加穩(wěn)定。測試環(huán)境1 中原算法20 次的平均路徑長度為858.41 mm，改進(jìn)RRT*-connect 算法平均路徑長度為758.29 mm，路徑縮短11.66%。地圖2中原算法20次的平均路徑長度860.38 mm，改進(jìn)RRT*-connect 算法平均路徑長度為780.36 mm，路徑縮短9.30%。通過數(shù)據(jù)可以清晰看出，改進(jìn)后的RRT*-connect 算法搜索得出的路徑長度減少10%左右，從而表明改進(jìn)后算法搜索出的路徑更優(yōu)。

圖9 兩種算法路徑長度數(shù)據(jù)對比圖

綜上，本文通過設(shè)置采樣約束，使改進(jìn)后的路徑在不同復(fù)雜環(huán)境下都更具目的性和方向性，且比原RRT*-connect 算法搜索更加精確，路徑長度更短，耗時(shí)更少。但在實(shí)際的運(yùn)行操作中還是存在路徑較粗糙的問題，因此需要進(jìn)行平滑處理。

3.2 平滑處理效果分析

針對上述問題，采用梯度下降法對路徑進(jìn)行優(yōu)化處理，如圖10 所示，分別為四種不同障礙物地形，設(shè)置如上文所述，黑色路徑為改進(jìn)后RRT*-connect 算法路徑，藍(lán)色路徑為經(jīng)過平滑處理后的路徑。

圖10 四種地圖下平滑處理

通過圖10 可以明顯看出，在不同地圖環(huán)境下經(jīng)過平滑處理，路徑大角度棱角明顯減少，更加平坦，且路徑長度也明顯縮短。具體數(shù)據(jù)見表1。

表1 不同地圖下路徑長度數(shù)據(jù)

通過表1 可以明顯看出，在不同的障礙物環(huán)境下，經(jīng)過平滑處理，路徑在原有的基礎(chǔ)上進(jìn)一步改善，路線長度有明顯減少，平均減少4%左右，路徑趨于最優(yōu)。證明本優(yōu)化處理是十分有效且必要的。

3.3 仿真測試

本文改進(jìn)的RRT*-connect算法在ROS開發(fā)平臺下，通過使用UR5 機(jī)械臂在moveit 環(huán)境下進(jìn)行運(yùn)動規(guī)劃測試。如圖11所示，圖中綠色柱體為障礙物，將改進(jìn)算法作為插件加載進(jìn)moveit 進(jìn)行測試，指定其由平置的down 位姿，如圖11（a）所示，繞過障礙物到達(dá)上方指定的up位姿，如圖11（b）所示。

圖11 機(jī)械臂位姿

利用ROS平臺，可以直觀看出算法的規(guī)劃路線，并對其進(jìn)行分析，如圖12所示。

圖12 機(jī)械臂運(yùn)動軌跡

通過圖12 可以明顯看出，該算法很好地繞過障礙物，且路徑較優(yōu)，證明該改進(jìn)算法的實(shí)用性與有效性。

4 結(jié)論

本文將RRT 的變種算法RRT*-connect 算法加以改進(jìn)，首先通過引入目標(biāo)偏向思想，對隨機(jī)采樣點(diǎn)加以控制，使得全局的采樣點(diǎn)趨向目標(biāo)點(diǎn)進(jìn)行采樣，確保采樣更加精確，避免了不必要區(qū)域的搜索，大大節(jié)省搜索時(shí)間。同時(shí)通過概率偏置，防止出現(xiàn)局部最小解，使得路徑漸進(jìn)更優(yōu)。其次通過仿真軟件在多種復(fù)雜地圖環(huán)境下進(jìn)行測試，將改進(jìn)的RRT*-connect 算法與原算法相比，搜索時(shí)間減少40%左右，路徑長度縮短10%左右，從而表明路徑更優(yōu)，搜索速度更快。然后再利用梯度下降法對路徑進(jìn)行平滑處理，減少路徑的大角度棱角，同時(shí)路徑長度減少4%左右。最后在ROS平臺，使用UR5機(jī)械臂在多個(gè)障礙物的仿真環(huán)境下進(jìn)行了無碰撞運(yùn)動規(guī)劃。綜上得出結(jié)論，本文改進(jìn)的RRT*-connect 算法搜索時(shí)間相比原RRT*-connect 算法大幅度減少，同時(shí)路徑長度更短，趨于最優(yōu)解。仿真實(shí)驗(yàn)表明改進(jìn)算法更加適合機(jī)械臂的實(shí)際運(yùn)行與操作，進(jìn)而證明該算法的可行性與實(shí)用性。