左國(guó)玉，陳國(guó)棟，劉月雷，龔道雄，李劍鋒

(1.北京工業(yè)大學(xué)信息學(xué)部，北京 100124；2.北京工業(yè)大學(xué)材料與制造學(xué)部，北京 100124)

在機(jī)械臂將末端或目標(biāo)物體安全平穩(wěn)地移動(dòng)到目標(biāo)位置并且能夠有效地避開障礙物的過(guò)程中，機(jī)械臂的機(jī)構(gòu)設(shè)計(jì)及運(yùn)動(dòng)規(guī)劃方法對(duì)機(jī)械臂實(shí)際作業(yè)場(chǎng)景中的有效應(yīng)用就顯得非常重要，提高機(jī)械臂的運(yùn)動(dòng)規(guī)劃效率是機(jī)器人技術(shù)研究中的重要課題[1].運(yùn)動(dòng)規(guī)劃分為路徑規(guī)劃和軌跡規(guī)劃2種[2-4].在運(yùn)動(dòng)規(guī)劃研究中，連接起點(diǎn)位置和終點(diǎn)位置的序列點(diǎn)或曲線稱之為路徑，構(gòu)成路徑的策略稱之為路徑規(guī)劃.在路徑規(guī)劃的基礎(chǔ)上加入時(shí)間序列信息稱之為軌跡規(guī)劃[5]，路徑規(guī)劃是運(yùn)動(dòng)規(guī)劃中的主要研究?jī)?nèi)容之一.國(guó)內(nèi)外學(xué)者提出了一些經(jīng)典的路徑規(guī)劃方法，主要有A*法[6]、人工勢(shì)場(chǎng)法[7]、快速擴(kuò)展隨機(jī)樹(rapidly-exploring random trees，RRT)法[8]等.A*算法主要應(yīng)用于二維平面的運(yùn)動(dòng)規(guī)劃問(wèn)題中，并且在此過(guò)程中需要花費(fèi)大量的時(shí)間與精力構(gòu)建可視圖；人工勢(shì)場(chǎng)法在對(duì)環(huán)境進(jìn)行精確建模后，通過(guò)將目標(biāo)點(diǎn)的引力場(chǎng)與障礙物之間的斥力場(chǎng)進(jìn)行疊加得到人工勢(shì)場(chǎng)，在該勢(shì)場(chǎng)的作用下進(jìn)行路徑規(guī)劃.但是，人工勢(shì)場(chǎng)法難以獲得環(huán)境的精確建模，人工勢(shì)場(chǎng)構(gòu)建難度大，并且存在容易陷入局部極小值的缺點(diǎn).

RRT是一種基于隨機(jī)采樣策略的運(yùn)動(dòng)規(guī)劃算法.相對(duì)于其他全局路徑規(guī)劃算法，它不需要對(duì)環(huán)境進(jìn)行精確建模，可以減少大量的空間建模工作，從而更加適用于高維空間下的路徑規(guī)劃，并且具有較快的擴(kuò)展和搜索速度，因此，被研究者廣泛關(guān)注[9-10].然而，由于工作空間中RRT方法采用的是隨機(jī)抽樣和全局抽樣策略，所以RRT方法具有較大的隨機(jī)性和盲目性[11]，并且很難在短時(shí)間內(nèi)生成更優(yōu)的路徑[12].針對(duì)快速擴(kuò)展隨機(jī)樹的上述缺點(diǎn)，許多學(xué)者基于不同的研究對(duì)象和性能要求提出了一些改進(jìn)的RRT算法，如RRT-Connect[13]、RRT*[14]、S-RRT[15]、NoD-RRT[16]、LBT-RRT[17]等.除此之外，李瑋等[18]通過(guò)使用啟發(fā)式隨機(jī)路徑近似方法來(lái)規(guī)劃?rùn)C(jī)械臂的運(yùn)動(dòng)軌跡.Cao等[19]在RRT算法中加入了目標(biāo)重力的思想，并且采用遺傳算法對(duì)路徑進(jìn)行平滑處理等優(yōu)化，能夠縮短路徑長(zhǎng)度，并通過(guò)六自由度機(jī)械臂的拾取仿真實(shí)驗(yàn)驗(yàn)證了該方法的有效性.Huh等[20]提出了基于高斯混合模型的RRT算法，利用高斯混合模型部分隨機(jī)抽樣，大大減少了碰撞檢測(cè)的次數(shù)，可以有效地提高碰撞檢測(cè)的效率.Zhang等[21]通過(guò)在RRT中引入回歸機(jī)制來(lái)減少在配置空間中的過(guò)度搜索，并且使用自適應(yīng)擴(kuò)展機(jī)制避免在笛卡兒空間中不必要的迭代，規(guī)劃效率得到了顯著的提高.

根據(jù)以上研究成果，為提高機(jī)械臂的運(yùn)動(dòng)規(guī)劃效率，本文提出了基于均勻概率的目標(biāo)啟發(fā)式RRT(target heuristic RRT based on uniform probability, PH-RRT)機(jī)械臂運(yùn)動(dòng)規(guī)劃方法.一方面，通過(guò)對(duì)RRT路徑規(guī)劃方法加入均勻概率分配機(jī)制，改善傳統(tǒng)RRT在冗余空間中的盲目擴(kuò)展，并基于目標(biāo)偏差和目標(biāo)重力的目標(biāo)啟發(fā)式節(jié)點(diǎn)拓展方法，提高了路徑規(guī)劃的探索效率.另一方面，針對(duì)所規(guī)劃出來(lái)的運(yùn)動(dòng)路徑，引入廣度優(yōu)先搜索的思想進(jìn)行平滑處理.仿真實(shí)驗(yàn)表明，本文提出的方法可以有效地減少路徑規(guī)劃的路徑長(zhǎng)度和規(guī)劃時(shí)間.

1 路徑規(guī)劃算法

1.1 基本RRT路徑規(guī)劃

RRT路徑規(guī)劃方法廣泛應(yīng)用于機(jī)械臂的路徑規(guī)劃，其基于全局統(tǒng)一隨機(jī)采樣策略，不需要構(gòu)建機(jī)器人環(huán)境模型，將傳統(tǒng)規(guī)劃算法中的障礙物信息映射步驟省略，可以節(jié)約計(jì)算成本.在配置空間中，它從起點(diǎn)開始逐漸擴(kuò)展形成搜索樹，直到目標(biāo)點(diǎn)成為搜索樹的一部分或達(dá)到搜索樹的設(shè)置閾值.只要給定足夠的搜索時(shí)間，基本RRT方法就可以完成路徑規(guī)劃任務(wù).基本RRT方法保證了擴(kuò)展隨機(jī)樹的全部概率，具有很好的全局搜索性，但是全局隨機(jī)抽樣方法也帶來(lái)了隨機(jī)性和盲目性等局限，并且基本RRT方法很難在路徑規(guī)劃過(guò)程中獲得更好的路徑質(zhì)量.隨著維度的進(jìn)一步增加，這些缺點(diǎn)將會(huì)更加明顯.圖1所示為基本RRT算法擴(kuò)展方式.

圖1 基本RRT示意圖

具體擴(kuò)展過(guò)程描述如下.

首先，將狀態(tài)空間中的起始狀態(tài)點(diǎn)qstart作為起始節(jié)點(diǎn).然后，根據(jù)全局統(tǒng)一的隨機(jī)數(shù)在空間中生成隨機(jī)采樣點(diǎn)qrand，并在生成的搜索樹中找到距離隨機(jī)采樣點(diǎn)qrand最近的節(jié)點(diǎn)qnear，通過(guò)一定的搜索擴(kuò)展步長(zhǎng)在qrand和qnear之間建立新的節(jié)點(diǎn)qnew.此時(shí)，將檢測(cè)qnew是否與障礙物碰撞：如果發(fā)生碰撞，則將其舍棄；如果沒(méi)有發(fā)生碰撞，則將qnew添加到搜索樹中.檢查qnew到目標(biāo)節(jié)點(diǎn)的距離是否達(dá)到設(shè)定的閾值：如果達(dá)到設(shè)置的閾值，則找到從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑；如果未達(dá)到設(shè)置的閾值，則重復(fù)進(jìn)行上述搜索過(guò)程，直到搜索樹的節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的距離小于設(shè)置的閾值或最大樣本數(shù)達(dá)到上限為止.基本RRT方法的偽代碼如算法1所示.

算法1基本RRT擴(kuò)展方法

Tree.init(qstart)

for(Dis(qnew,qgoal)> Disset)&&∈[1,N]do

qrand← RANDOM_SAMPLE_POINT(n);

qnear← NEAREST(qrand, Tree);

qnew← Tree_EXTENDqrand, step,qnear);

if CollisionFree(qnear,qnew)then

Tree.add.point(qnew);

end if

if(‖qnew-qgoal‖

return Tree;

end if

end for

return False;

1.2 PH-RRT路徑規(guī)劃

基本RRT方法采用的是基于全局統(tǒng)一的隨機(jī)采樣策略，即擴(kuò)展過(guò)程中隨機(jī)采樣點(diǎn)在工作空間中完全隨機(jī)產(chǎn)生，從而導(dǎo)致了較大的隨機(jī)性和盲目性.為解決以上問(wèn)題并提高路徑規(guī)劃效率，提出了一種PH-RRT路徑規(guī)劃.

首先，為保證RRT原本具有的全局搜索性的同時(shí)減小在隨機(jī)點(diǎn)采樣過(guò)程中的盲目性，通過(guò)引入基于均勻概率的分配機(jī)制，即在隨機(jī)點(diǎn)生成過(guò)程中，設(shè)置一個(gè)處于0～1的閾值作為采樣閾值t，隨后隨機(jī)生成0～1隨機(jī)采樣值n，將隨機(jī)采樣值n和采樣閾值t進(jìn)行比較，根據(jù)不同情況采用不同的隨機(jī)點(diǎn)采樣策略.當(dāng)隨機(jī)采樣值n在0和采樣閾值t之間時(shí)，將隨機(jī)點(diǎn)選作目標(biāo)節(jié)點(diǎn)，即qrand=qgoal，以提高隨機(jī)樹探索效率.當(dāng)隨機(jī)采樣值n在采樣閾值t和1之間，仍采用隨機(jī)采樣策略，即在工作空間中隨機(jī)生成隨機(jī)點(diǎn)，在該概率區(qū)間內(nèi)保留了基本RRT的全局搜索性.采樣閾值t越大表示目標(biāo)導(dǎo)向性越大，但為了保證較好的全局搜索性和路徑規(guī)劃效果，t的值不宜過(guò)大，一般處于0.15～0.30.

然后，在均勻概率的分配機(jī)制的采樣策略的基礎(chǔ)上，當(dāng)n處于[0，t]的概率區(qū)間內(nèi)時(shí)，新節(jié)點(diǎn)擴(kuò)展方式為

(1)

式中qnear為隨機(jī)搜索樹中離目標(biāo)點(diǎn)最近的節(jié)點(diǎn),在qnear的基礎(chǔ)上向著qgoal的方向以特定步長(zhǎng)ρ1進(jìn)行新節(jié)點(diǎn)擴(kuò)展.

當(dāng)n處于[t，1]的概率區(qū)間內(nèi)時(shí),隨機(jī)產(chǎn)生采樣節(jié)點(diǎn)qrand,為保證在該概率區(qū)間內(nèi)進(jìn)一步提升探索效率,將目標(biāo)點(diǎn)也融入節(jié)點(diǎn)擴(kuò)展的過(guò)程中,體現(xiàn)為隨機(jī)樹擴(kuò)展方向受到目標(biāo)點(diǎn)影響,目標(biāo)點(diǎn)會(huì)啟發(fā)擴(kuò)展方向向目標(biāo)點(diǎn)靠近,可理解為目標(biāo)點(diǎn)對(duì)擴(kuò)展方向具有啟發(fā)式的重力影響.具體擴(kuò)展過(guò)程如圖2所示.

圖2 目標(biāo)重力下的目標(biāo)啟發(fā)式節(jié)點(diǎn)擴(kuò)展

具體新節(jié)點(diǎn)擴(kuò)展方式為

qnew=qnear+X1+X2(t≤n<1)

(2)

式中X1、X2分別代表在目標(biāo)重力影響和隨機(jī)點(diǎn)影響下的擴(kuò)展分量，公式為

(3)

(4)

式中：qnear為隨機(jī)搜索樹中與隨機(jī)點(diǎn)距離最近的節(jié)點(diǎn)；ρ2、ρ3分別為目標(biāo)點(diǎn)和隨機(jī)點(diǎn)對(duì)新節(jié)點(diǎn)擴(kuò)展的分量步長(zhǎng)，兩者的值越大，代表新節(jié)點(diǎn)擴(kuò)展步長(zhǎng)越大，ρ2/ρ3越大代表目標(biāo)點(diǎn)對(duì)節(jié)點(diǎn)擴(kuò)展的目標(biāo)啟發(fā)影響越大.

基于均勻概率的目標(biāo)啟發(fā)式RRT路徑規(guī)劃的偽代碼如算法2所示.

算法2PH-RRT算法偽代碼

Tree.init(qstart);

t← SET VALUE OF CRITICAL POINT(0, 1);

for(Dis(qnew,qgoal)>Disset)&&n∈[1,N]do

qrand← RANDOM_SAMPLE_POINT(n);

qnear← NEAREST(qnew, Tree);

qnew← Tree_EXTEND(qnew, step,qnear);

k← RANDOM VALUE(0,1);

if(0<=k<=t)then

qrand=qgoal;

qnew=qnear+ρ(qgoal-qnear/‖qgoal-qnear‖);

end if

else if(t<=k<=1)then

qnew=qnear+X1+X2;

end if

if CollisionFree(qnear,qnew)then

Tree.add.point(qnew);

end if

if(‖qnew-qgoal‖

return Tree;

end if

end for

pathsmooth;

return False;

2 路徑優(yōu)化處理

盡管在均勻概率下的目標(biāo)偏差和目標(biāo)重力的作用下可以有效地減少冗余空間中的搜索，但生成的路徑中的路徑點(diǎn)仍然存在許多冗余點(diǎn)，從而導(dǎo)致機(jī)械臂軌跡不平滑，使路徑規(guī)劃的距離、時(shí)間和機(jī)械臂的能耗也相應(yīng)增加.因此，在保持原始曲線特性的同時(shí)，有必要對(duì)軌跡曲線進(jìn)一步優(yōu)化.

這里通過(guò)引入廣度優(yōu)先搜索的思想來(lái)優(yōu)化路徑，如圖3所示.其中：p1是公共路徑段；p2～p4是不同的優(yōu)化路徑可能性.

圖3 路徑優(yōu)化處理示意圖

具體優(yōu)化過(guò)程如下：首先，將路徑中的所有節(jié)點(diǎn)相互連接，并刪除與障礙沖突的路徑，并且計(jì)算節(jié)點(diǎn)之間的路徑長(zhǎng)度作為權(quán)重，并記錄直接連接到起點(diǎn)的路徑的最小權(quán)重點(diǎn)i.然后，更新并比較起點(diǎn)的最小權(quán)重和最小權(quán)重點(diǎn)i的連接點(diǎn)，并循環(huán)擴(kuò)展，直到目標(biāo)點(diǎn)成為最小權(quán)重點(diǎn).根據(jù)該優(yōu)化方法對(duì)圖3中路徑進(jìn)行優(yōu)化可得到優(yōu)化路徑為p1+p3.

3 仿真實(shí)驗(yàn)

3.1 二維空間下仿真對(duì)比實(shí)驗(yàn)

首先，在二維空間進(jìn)行仿真對(duì)比實(shí)驗(yàn)，地圖范圍500×500，隨機(jī)采樣點(diǎn)均在地圖內(nèi)部，內(nèi)部設(shè)置一定障礙物，起始節(jié)點(diǎn)坐標(biāo)為(10,10)，目標(biāo)節(jié)點(diǎn)坐標(biāo)為(490,490).本實(shí)驗(yàn)中針對(duì)基本RRT、RRT-Connect與PH-RRT方法分別進(jìn)行25次實(shí)驗(yàn).其中基本RRT、RRT-Connect方法每步的拓展步長(zhǎng)均為15.PH-RRT實(shí)驗(yàn)中為保證較好的全局搜索性和路徑規(guī)劃效果，隨機(jī)閾值t=0.2，為保證實(shí)驗(yàn)結(jié)果的可比性，取ρ1=15，為保證在目標(biāo)啟發(fā)式作用下擴(kuò)展步長(zhǎng)適宜并且使得新節(jié)點(diǎn)擴(kuò)展更傾向于目標(biāo)點(diǎn)，取ρ2=10，ρ3=20，即ρ2/ρ3=1/2.各方法在采樣期間的最大采樣數(shù)均為10 000.圖4分別為3種方法的路徑規(guī)劃和路徑優(yōu)化處理后的一次實(shí)驗(yàn)結(jié)果.

圖4 二維環(huán)境下路徑規(guī)劃及路徑優(yōu)化仿真實(shí)驗(yàn)對(duì)比

如圖4所示，在二維空間中可以明顯地看出PH-RRT方法能夠有效減少在冗余空間中的搜索，大量減少了無(wú)用節(jié)點(diǎn)的生成，提高了路徑規(guī)劃效率，節(jié)省了路徑規(guī)劃時(shí)間，并且路徑優(yōu)化算法也能有效地優(yōu)化路徑，提升了路徑平滑度并縮短了路徑長(zhǎng)度.

針對(duì)25次實(shí)驗(yàn)結(jié)果，對(duì)3種方法的路徑規(guī)劃時(shí)間和所規(guī)劃出的路徑長(zhǎng)度進(jìn)行數(shù)據(jù)采集，結(jié)果如圖5、6所示.

圖5 路徑規(guī)劃時(shí)間對(duì)比

圖6 路徑長(zhǎng)度對(duì)比

在路徑規(guī)劃時(shí)間方面，因?yàn)榛綬RT方法隨機(jī)性高，穩(wěn)定性差，其平均路徑規(guī)劃時(shí)間為15.70 s.PH-RRT和RRT-Connect方法在運(yùn)動(dòng)路徑規(guī)劃時(shí)間上均有一定的改善，PH-RRT的平均路徑規(guī)劃時(shí)間為7.38 s，RRT-Connect的平均路徑規(guī)劃時(shí)間為8.59 s，由此可見，PH-RRT方法的探索效率高于雙向探索的RRT-Connect方法.通過(guò)以上數(shù)據(jù)可以得出，二維空間環(huán)境下PH-RRT方法相比于其他2種RRT方法，路徑規(guī)劃時(shí)間均有所改善，能夠提高路徑規(guī)劃效率，減少規(guī)劃時(shí)間.在規(guī)劃的路徑長(zhǎng)度方面，基本RRT的平均路徑長(zhǎng)度為1 087.4，RRT-Connect平均路徑長(zhǎng)度為1 003.5，PH-RRT的平均路徑長(zhǎng)度為851.6.通過(guò)以上數(shù)據(jù)可以看出，PH-RRT因?yàn)橐肓四繕?biāo)偏差和目標(biāo)重力的目標(biāo)啟發(fā)式思想使得擴(kuò)展節(jié)點(diǎn)更加接近目標(biāo)點(diǎn)，從而能夠得到質(zhì)量更好的路徑.

采用基于廣度優(yōu)先搜索的路徑優(yōu)化方法針對(duì)PH-RRT方法所規(guī)劃出的路徑的25次優(yōu)化結(jié)果如圖7所示.

圖7 優(yōu)化路徑與原路徑長(zhǎng)度對(duì)比

如圖7所示，PH-RRT的優(yōu)化前平均路徑長(zhǎng)度為851.6，路徑平滑后的平均路徑長(zhǎng)度為754.9，縮短了12.81%.相比于未優(yōu)化的原路徑，經(jīng)過(guò)路徑優(yōu)化處理后大大減少了路徑的冗余點(diǎn)，提高了路徑的平滑度.

3.2 三維空間下仿真對(duì)比實(shí)驗(yàn)

3.2.1 三維空間環(huán)境下的路徑規(guī)劃仿真對(duì)比實(shí)驗(yàn)

在二維空間下的仿真對(duì)比實(shí)驗(yàn)驗(yàn)證了PH-RRT方法在二維空間中的有效性及可靠性，但機(jī)械臂工作空間為三維空間，隨著空間維度增加計(jì)算量和隨機(jī)性也進(jìn)一步增加，需要在三維空間中進(jìn)行仿真對(duì)比實(shí)驗(yàn)以驗(yàn)證該方法針對(duì)機(jī)械臂的軌跡規(guī)劃具有有效性.

為了驗(yàn)證三維空間中規(guī)劃的有效性及可靠性，本實(shí)驗(yàn)設(shè)置了2個(gè)空間大小為100×100×100的三維地圖.一個(gè)是無(wú)障礙場(chǎng)景地圖，在無(wú)障礙物情況下，可以明顯觀察出三維空間中探索隨機(jī)樹的拓展情況；另一個(gè)是有9個(gè)均勻分布于空間的半徑為15的球形作為障礙物的障礙場(chǎng)景地圖，用于驗(yàn)證該方法可以在避障的同時(shí)完成軌跡規(guī)劃任務(wù).

本實(shí)驗(yàn)中，隨機(jī)采樣點(diǎn)均處于三維空間內(nèi)部，起始節(jié)點(diǎn)坐標(biāo)為(5, 5, 5)，目標(biāo)節(jié)點(diǎn)坐標(biāo)為(95, 95, 95).針對(duì)基本RRT、RRT-Connect與PH-RRT三種方法分別進(jìn)行25次仿真實(shí)驗(yàn)，其中基本RRT、RRT-Connect方法每步的拓展步長(zhǎng)均為5，PH-RRT中相關(guān)參數(shù)取t=0.2，為保證實(shí)驗(yàn)結(jié)果的可比性，取ρ1=5.為保證在目標(biāo)啟發(fā)式作用下擴(kuò)展步長(zhǎng)適宜并且使得新節(jié)點(diǎn)擴(kuò)展更傾向于目標(biāo)點(diǎn)，取ρ2=4，ρ3=8，即ρ2/ρ3=1/2.各方法在采樣期間的最大采樣數(shù)均為10 000.圖8～10分別為3種方法在無(wú)障礙場(chǎng)景下和有障礙場(chǎng)景下的一次實(shí)驗(yàn).

圖8 三維空間下基本RRT路徑規(guī)劃

由圖8～10對(duì)比可以明顯得出，在無(wú)障礙場(chǎng)景下基本RRT方法的擴(kuò)展節(jié)點(diǎn)幾乎遍布整個(gè)工作空間，難以在短時(shí)間內(nèi)進(jìn)行有效的軌跡規(guī)劃，在三維空間中基本RRT方法盲目探索的隨機(jī)性和計(jì)算冗余性隨著維度的提高進(jìn)一步放大.RRT-Connect方法因?yàn)樘剿骺臻g維度的增加，雙向探索的2棵隨機(jī)探索樹相連接的可能性減少，該方法在三維空間中探索效率不如二維空間中的實(shí)驗(yàn)結(jié)果，而PH-RRT方法就節(jié)點(diǎn)數(shù)和路徑質(zhì)量而言仍然具有較好的路徑規(guī)劃效果.

為了更加準(zhǔn)確客觀地評(píng)價(jià)3種RRT方法在三維空間中的路徑規(guī)劃的性能指標(biāo)差別，分別記錄3種RRT方法在有障礙場(chǎng)景下的25次實(shí)驗(yàn)中的平均節(jié)點(diǎn)總數(shù)、平均路徑節(jié)點(diǎn)總數(shù)、平均路徑長(zhǎng)度和平均規(guī)劃時(shí)間等數(shù)據(jù)，結(jié)果如表1所示.

圖9 三維空間下RRT-Connect路徑規(guī)劃

圖10 三維空間下PH-RRT路徑規(guī)劃

表1 有障礙場(chǎng)景下各算法性能指標(biāo)對(duì)比

由實(shí)驗(yàn)結(jié)果可以得到，在引入了均勻概率原則的分配機(jī)制，并且融入了目標(biāo)偏差和目標(biāo)力的目標(biāo)啟發(fā)式策略后，PH-RRT方法在三維空間搜索有效空間方面顯著提升，生成節(jié)點(diǎn)總數(shù)遠(yuǎn)遠(yuǎn)小于基本RRT算法，相較于RRT-Connect算法也有較大提升，在探索過(guò)程中生成的總節(jié)點(diǎn)數(shù)減少會(huì)使得探索過(guò)程中計(jì)算量減少，因此，PH-RRT的平均路徑規(guī)劃時(shí)間也明顯縮短，相對(duì)于其他2種方法具有更好的路徑規(guī)劃效率.此外，PH-RRT方法所規(guī)劃出的路徑的平均路徑節(jié)點(diǎn)總數(shù)和平均路徑長(zhǎng)度相比于其他2種方法也均明顯減小，驗(yàn)證了PH-RRT方法在三維空間中得到的路徑質(zhì)量?jī)?yōu)于其他傳統(tǒng)方法，具有較好的有效性和可靠性.

3.2.2 三維空間環(huán)境下的路徑規(guī)劃仿真對(duì)比實(shí)驗(yàn)

三維空間中PH-RRT路徑規(guī)劃方法可以有效地減少冗余空間中的搜索，但生成的路徑點(diǎn)中仍然存在許多冗余點(diǎn).為驗(yàn)證三維空間中路徑優(yōu)化處理仍然可以進(jìn)一步優(yōu)化路徑，分別在無(wú)障礙場(chǎng)景地圖和有障礙場(chǎng)景地圖下，針對(duì)已通過(guò)PH-RRT方法進(jìn)行路徑規(guī)劃得到的路徑進(jìn)行基于廣度優(yōu)先搜索思想的路徑平滑處理方法的仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖11、12所示.

圖11 無(wú)障礙場(chǎng)景下路徑平滑處理仿真實(shí)驗(yàn)

圖12 有障礙場(chǎng)景下路徑平滑處理仿真實(shí)驗(yàn)

圖中，黑線表示規(guī)劃出的路徑，紅線表示優(yōu)化后路徑.在無(wú)障礙場(chǎng)景下優(yōu)化后路徑為連接起始點(diǎn)和目標(biāo)點(diǎn)的直線路徑，優(yōu)化效果明顯.在有障礙場(chǎng)景下，處理后路徑拐點(diǎn)明顯減少，可以有效提高路徑平滑度，并且原路徑經(jīng)過(guò)路徑優(yōu)化處理后的路徑長(zhǎng)度明顯縮短.有障礙場(chǎng)景下優(yōu)化前后路徑長(zhǎng)度如圖13所示.

圖13 三維空間中優(yōu)化路徑與原路徑長(zhǎng)度對(duì)比

PH-RRT路徑規(guī)劃方法規(guī)劃出的平均路徑長(zhǎng)度為196.5，經(jīng)過(guò)路徑平滑后的平均路徑長(zhǎng)度為178.4.原路徑經(jīng)過(guò)路徑優(yōu)化處理后，路徑長(zhǎng)度縮短了9.21%，在三維空間中路徑優(yōu)化處理仍然能夠較好地減少路徑的冗余點(diǎn)，提高路徑的平滑度.

3.3 機(jī)器人操作系統(tǒng)(robot operating system，ROS)環(huán)境下機(jī)械臂仿真實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證本文提出的方法能夠很好地適用于機(jī)械臂的無(wú)碰撞路徑規(guī)劃，安全到達(dá)目標(biāo)點(diǎn)，實(shí)驗(yàn)在ROS的仿真環(huán)境中對(duì)機(jī)械臂的不同規(guī)劃路徑方法的規(guī)劃效果進(jìn)行了對(duì)比分析.

實(shí)驗(yàn)中機(jī)械臂采用自行設(shè)計(jì)的仿人移動(dòng)機(jī)器人的仿生機(jī)械臂，實(shí)驗(yàn)環(huán)境為ROS下搭建的仿真環(huán)境，機(jī)械臂的工作任務(wù)為模擬機(jī)械臂翻越桌面的任務(wù).實(shí)驗(yàn)中自行設(shè)計(jì)的仿人移動(dòng)機(jī)器人的三維單臂模型如圖14所示，其中機(jī)器人手臂的所有關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，右側(cè)是七自由度機(jī)械臂的連桿坐標(biāo)系，其中關(guān)節(jié)7用于連接機(jī)械臂的末端執(zhí)行器.

圖14 仿人機(jī)械手的三維模型及連桿坐標(biāo)系

選取機(jī)械臂的空間起始點(diǎn)為(-0.331 8,-0.340 2,-0.340 3, 0.005 9, 0.960 2, 0.250 3,-0.124 1)、機(jī)械臂的空間終止點(diǎn)為(-0.429 6,-0.396 3,-0.024 3, 0.330 0, 0.925 7, 0.094 55,-0.158 4)，在同一種起始構(gòu)型與終止構(gòu)型情況下對(duì)基本RRT、RRT-Connect、PH-RRT運(yùn)動(dòng)規(guī)劃方法進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果分別如圖15所示.

圖15 ROS環(huán)境下仿真實(shí)驗(yàn)

圖15中，藍(lán)色線表示該方法優(yōu)化前的軌跡路徑，紅色線表示該方法優(yōu)化后的軌跡路徑.由實(shí)驗(yàn)結(jié)果可見，本文的PH-RRT路徑規(guī)劃方法應(yīng)用于機(jī)械臂的效果明顯優(yōu)于傳統(tǒng)方法.PH-RRT方法有效地規(guī)劃出了可行性路徑，并使得路徑平滑度得到改善，路徑長(zhǎng)度進(jìn)一步縮短.

4 結(jié)論

1)針對(duì)高維空間的機(jī)械臂進(jìn)行路徑規(guī)劃及其優(yōu)化研究是機(jī)械臂性能提升的關(guān)鍵之一，對(duì)基本RRT路徑規(guī)劃方法加入均勻概率分配機(jī)制和目標(biāo)偏差與目標(biāo)重力的目標(biāo)啟發(fā)式策略，并引入廣度優(yōu)先搜索思想，對(duì)路徑進(jìn)行優(yōu)化.通過(guò)在二維空間、三維空間環(huán)境下與傳統(tǒng)的RRT、RRT-Connect方法進(jìn)行對(duì)比實(shí)驗(yàn)，證明了該方法在減少路徑長(zhǎng)度與提高路徑平滑度方面的有效性.

2)通過(guò)基于ROS對(duì)所建立的機(jī)械臂模型進(jìn)行相同起始點(diǎn)與終止點(diǎn)的機(jī)械臂仿真實(shí)驗(yàn)，結(jié)果表明，機(jī)械臂的運(yùn)動(dòng)規(guī)劃路徑長(zhǎng)度與平滑度得到明顯改善，在機(jī)械臂上具有較好的適用性.