劉建宇，范平清

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

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

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

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

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

1 RRT＊-connect算法

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

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

圖1 自由空間采樣

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

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

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

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

圖3 兩棵隨機樹連接過程

2 改進RRT＊-connect算法

2.1 采樣約束

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

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

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

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

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

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

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

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

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

2.2 平滑處理

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

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

將路徑看成曲線并用函數(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)?，實際寬度由微分步長α決定，通過改變θ的值直至損失值趨于0。

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

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

圖6 平滑處理對比

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

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

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

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

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

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

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

圖7 算法測試對比

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

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

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

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

3.2 平滑處理效果分析

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

圖10 四種地圖下平滑處理

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

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

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

3.3 仿真測試

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

圖11 機械臂位姿

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

圖12 機械臂運動軌跡

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

4 結(jié)論

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