張云峰，馬振書，孫華剛，陸繼山

（1軍械工程學(xué)院，石家莊　050003；2軍械技術(shù)研究所2室，石家莊　050003）

?

基于改進(jìn)快速擴(kuò)展隨機(jī)樹的機(jī)械臂路徑規(guī)劃

張云峰1，馬振書2，孫華剛2，陸繼山1

（1軍械工程學(xué)院，石家莊050003；2軍械技術(shù)研究所2室，石家莊050003）

摘要：針對機(jī)械臂路徑規(guī)劃問題，提出一種基于改進(jìn)RRT算法的路徑規(guī)劃方法。改進(jìn)RRT結(jié)合了目標(biāo)偏置策略和貪婪生長策略的優(yōu)點，在隨機(jī)采樣時，以一定概率使采樣點偏置為目標(biāo)節(jié)點，降低隨機(jī)采樣的盲目性，在目標(biāo)節(jié)點方向上采用貪婪式擴(kuò)展策略，增加隨機(jī)樹局部方向上的生長速度。RRT法規(guī)劃路徑結(jié)果并非最優(yōu)，提出改進(jìn)GPP法刪除多余路徑節(jié)點，優(yōu)化機(jī)械臂運動路徑。通過與Biased-RRT和Greedy-RRT數(shù)值仿真結(jié)果對比，證明了改進(jìn)RRT在計算時間、迭代次數(shù)、擴(kuò)展節(jié)點數(shù)上均優(yōu)于以上方法。在機(jī)械臂兩種典型工作環(huán)境中的仿真結(jié)果表明，使用該方法可以較好解決排爆機(jī)械臂避障路徑規(guī)劃問題。

關(guān)鍵詞：路徑規(guī)劃，快速擴(kuò)展隨機(jī)樹，RRT，機(jī)械臂，最優(yōu)路徑

0　引言

機(jī)器人路徑規(guī)劃是機(jī)器人學(xué)的重要議題，它自20世紀(jì)70年代提出以來，逐漸成為人們的研究熱點。機(jī)械臂路徑規(guī)劃是其中的重要內(nèi)容，機(jī)械臂路徑規(guī)劃方法主要有細(xì)胞分解法［1］（Cell Decomposition）、人工勢場法［2］（Artificial Potential Field）、隨機(jī)路標(biāo)法［3］（PRM，Probabilistic Roadmap）和快速擴(kuò)展隨機(jī)樹法［4-6］（RRT，Rapidly-exploring Random Tree）等。

細(xì)胞分解法等基于靜態(tài)地圖分析的方法需要大量計算內(nèi)存且不適用于動態(tài)環(huán)境中的路徑規(guī)劃。人工勢場法可以實時在線規(guī)劃，但容易陷入局部最小值，算法效率較低。近年來，基于隨機(jī)采樣的方法如PRM和RRT，越來越多地被用于機(jī)器人路徑規(guī)劃問題中［7-8］。RRT算法基本思想為在樣本空間中隨機(jī)采樣并不斷擴(kuò)展樹狀結(jié)構(gòu)直至到達(dá)目標(biāo)，可適用于高維空間中的路徑規(guī)劃問題，但其隨機(jī)采樣的方式使節(jié)點遍布于整個采樣空間，隨著采樣空間增長其計算效率會降低。Dmitry Berenson［9］使用Cost-RRT的方法引導(dǎo)隨機(jī)樹向代價低的方向生長，從而增加通過狹窄障礙的成功率并提高了算法效率。Piet Stroeven［10］、Wei Wang［11］等提出了多樹同時擴(kuò)展的方法，提高了RRT算法在樣本空間中的擴(kuò)展效率。Rahul Kala［12］根據(jù)隨機(jī)采樣的思想發(fā)展了RRG算法，變樹狀結(jié)構(gòu)為圖結(jié)構(gòu)。為提高隨機(jī)樹在局部區(qū)域的生長速度，周芳等［13］提出了貪婪式生長方法（Greedy-RRT），提高了隨機(jī)樹的擴(kuò)展效率。徐娜等［14］引入目標(biāo)偏向的思想（Biased-RRT），減小了在全局狀態(tài)空間均勻采樣耗費的代價。但貪婪算法和目標(biāo)偏置算法的固有缺點導(dǎo)致樹的盲目生長和低效擴(kuò)展，同時RRT算法的路徑并非最優(yōu)結(jié)果的問題沒有得到解決。

本文針對某型機(jī)械臂路徑規(guī)劃的問題，提出了改進(jìn)RRT算法，通過數(shù)值仿真與基本RRT、Greedy-RRT、Biased-RRT和改進(jìn)RRT算法進(jìn)行比較，并提出改進(jìn)GPP法（Global path pruning）優(yōu)化機(jī)械臂運動路徑，最后在兩種典型工作環(huán)境中通過虛擬樣機(jī)仿真驗證方法的有效性。

1　問題描述

研究機(jī)械臂的路徑規(guī)劃問題，首先要對機(jī)械臂和障礙物進(jìn)行簡化，研究它們之間的碰撞條件。本文以某型機(jī)器人6關(guān)節(jié)機(jī)械臂為研究對象，其中5個轉(zhuǎn)動關(guān)節(jié)，1個棱柱關(guān)節(jié)，機(jī)械臂末端位置由棱柱關(guān)節(jié)和3個轉(zhuǎn)動關(guān)節(jié)決定，因此，該機(jī)械臂雖為六自由度機(jī)械臂，但卻是一種冗余結(jié)構(gòu)。機(jī)械臂的D-H坐標(biāo)系如圖1所示，其D-H參數(shù)如表1所示。

圖1　機(jī)械臂示意圖

表1　機(jī)械臂D-H參數(shù)

本文使用幾何包絡(luò)法簡化機(jī)械臂與障礙物的碰撞模型，如圖2所示，機(jī)械臂連桿由圓柱體完全包裹，障礙物由多個球陣列包裹，最終機(jī)械臂與障礙物的碰撞檢測問題變成一系列判斷球心到圓柱中心線距離的問題。若球心到圓柱體中心線的距離小于球與圓柱體半徑之和，則出現(xiàn)機(jī)械臂與障礙物干涉。該模型簡化方法會損失一部分機(jī)械臂運動的自由空間，但簡化了碰撞模型，提高了計算效率。

圖2　模型簡化示意圖

為避免計算過程中多次調(diào)用迭代法求排爆機(jī)械臂逆運動學(xué)解［15］，本文采用J空間（Joint Space）中隨機(jī)采樣后通過正向運動學(xué)映射到C空間（Configuration Space）中檢測碰障并規(guī)劃路徑的方法。J空間由所有可能關(guān)節(jié)角度的集合構(gòu)成，C空間則是機(jī)械臂所有可能位姿的集合。Cobs空間表示因障礙物遮擋，機(jī)械臂不可到達(dá)的位姿集合，則Cfree空間｛Cfree=C/Cobs｝為自由空間，路徑規(guī)劃即在J空間找到一條映射到Cfree空間中的路徑Γ（0，1），使得Γ（0）= Nstart，Γ（1）=Ngoal。

2　基于改進(jìn)擴(kuò)展隨機(jī)樹的路徑規(guī)劃

2.1RRT算法基本原理

RRT是一種基于隨機(jī)采樣的快速搜索算法，它把采集到的隨機(jī)樣本存儲于一個樹狀結(jié)構(gòu)中，并繼續(xù)向未知空間搜索，直到接觸目標(biāo)點為止?；綬RT算法的過程如圖3所示，紅色區(qū)域為障礙空間，空白區(qū)域為Cfree空間。首先以初始節(jié)點Nstart作為樹根，隨機(jī)采樣得到Nrand；然后找到隨機(jī)樹中離Nrand最近的節(jié)點Nnearest，在方向上以一定步長生成新節(jié)點Nnew，判斷Nnew是否碰障，若否，則加入隨機(jī)樹，若碰障如節(jié)點1，則重新采樣；最后，若隨機(jī)樹生長至目標(biāo)節(jié)點Ngoal處，則算法結(jié)束，否則繼續(xù)生長。為滿足機(jī)械臂的運動學(xué)和動力學(xué)條件，以式（1）、式（2）生成新節(jié)點。其中J為機(jī)械臂的雅可比矩陣，它隨關(guān)節(jié)位置的改變而改變，△q為關(guān)節(jié)改變量，q˙為機(jī)械臂的控制輸入變量并在一定范圍內(nèi)取值，△t為時間步長。

圖3　基本RRT原理

采用基本RRT在3維空間中規(guī)劃機(jī)械臂路徑，結(jié)果如圖4所示，為表示清晰，畫面上只顯示了機(jī)械臂末端位置，并未顯示連桿機(jī)構(gòu)，圖4（a）、圖4（b）、圖4（c）分別為x-y、x-z、y-z視角。圖中紅色實體即為障礙物，藍(lán)色樹狀圖為隨機(jī)樹，青色節(jié)點為Nstart，綠色節(jié)點為Ngoal。路徑規(guī)劃結(jié)果用紅色線段連接，可以看到，樹狀圖的節(jié)點和邊都繞開了障礙，證明了碰障模型的有效性。但基本RRT法采樣具有盲目性，節(jié)點分布于機(jī)械臂整個工作空間中［16］，降低了算法的效率。

圖4　基本RRT路徑規(guī)劃

2.2目標(biāo)偏置策略和貪婪策略

針對RRT算法的盲目性問題，文獻(xiàn)［12］引用了Biased-RRT算法，在隨機(jī)樹生長過程中以p為偏置概率使Nrand等于Ngoal，以（1-p）的概率隨機(jī)產(chǎn)生Nrand。該方法實際上變隨機(jī)搜索為相關(guān)于p的目標(biāo)最優(yōu)搜索，使隨機(jī)樹以概率p向目標(biāo)節(jié)點生長，同時以概率（1-p）隨機(jī)采樣以避開障礙。P的取值決定搜索過程和搜索結(jié)果，當(dāng)障礙物密集時P應(yīng)較小，反之P取值較大。采用Biased-RRT規(guī)劃機(jī)械臂空間路徑，P取0.2，結(jié)果如圖5所示，圖5（a）為理想情況，由于加入了偏置概率，Biased-RRT所需節(jié)點數(shù)更少，提高了搜索效率。但其搜索過程中局部方向連續(xù)擴(kuò)展能力不足，也會出現(xiàn)圖5（b）所示的一般情況，隨機(jī)樹依然搜索了整個自由空間。

圖5　Biased-RRT路徑規(guī)劃

傳統(tǒng)RRT與Biased-RRT局部擴(kuò)展速度較慢，為提高樹枝生長速度，文獻(xiàn)［11］提出了一種貪婪算法，在節(jié)點擴(kuò)展時，若生成的新節(jié)點不與障礙物干涉，則在該方向上繼續(xù)擴(kuò)展節(jié)點，直到碰障或達(dá)到運動學(xué)極限。采用Greedy-RRT規(guī)劃機(jī)械臂運動路徑，結(jié)果如圖6所示，由于貪婪算法特殊的節(jié)點生長方式，其樹圖具有不同的形態(tài)，每一樹枝都較長，由于關(guān)節(jié)在特定方向上連續(xù)轉(zhuǎn)動而使末端圖像呈現(xiàn)螺旋形，Greedy-RRT以這種方法快速擴(kuò)展至整個Cfree空間，如圖6（b）。但其擴(kuò)展具有盲目性，算法結(jié)果不穩(wěn)定，圖6（a）為較理想的情況，擴(kuò)展過程中Nrand正好位于近似Ngoal方向，因此，減小了擴(kuò)展節(jié)點數(shù)量。

圖6　Greedy-RRT路徑規(guī)劃結(jié)果

2.3改進(jìn)RRT路徑規(guī)劃

通過上面的分析，Biased-RRT會使隨機(jī)樹向目標(biāo)方向生長，減小了采樣盲目性，Greedy-RRT會加快隨機(jī)樹在局部區(qū)域的擴(kuò)展速度，增加算法效率。結(jié)合兩者的優(yōu)點，隨機(jī)樹生長過程中以偏置策略采集樣本，當(dāng)Nrand以概率p等于Ngoal時啟用貪婪策略快速擴(kuò)展該方向上的樹枝，此時相當(dāng)于目標(biāo)最優(yōu)搜索，在與目標(biāo)距離的梯度方向上快速擴(kuò)展隨機(jī)樹。而當(dāng)Nrand隨機(jī)采樣時，不進(jìn)行貪婪擴(kuò)展，減少迭代次數(shù)，保留隨機(jī)采樣跳出局部最小值的能力。通過這種結(jié)合方式，改善了貪婪算法在不需要的區(qū)域擴(kuò)展大量節(jié)點和偏置算法局部生長速度慢的缺點，改進(jìn)RRT算法的偽碼如下：

Algorithm.1：Improved-RRT Initialize（rrt）Add（rrt，Nstart）While Distance（Nnew，Ngoal）＞Dim p=rand If p＜=Biased Parameter Nrand=Ngoal Else Sampling（Nrand）End If NearestNode（rrt，Nrand）GenerateNew（Nnearest，Nrand）If～CollisionCheck（obs，Nnew）Add（rrt，Nnew）While Nrand=Ngoal GreedyExtend（rrt，Ngoal，Nnew）End While End If End While Add（rrt，Ngoal）Return rrt TracePath（rrt）

（1）初始化樹狀數(shù)據(jù)結(jié)構(gòu)，把起始節(jié)點Nstart作為樹根加入隨機(jī)樹中。

（2）在J空間中以目標(biāo)偏置策略采集樣本，即以概率p使隨機(jī)節(jié)點Nrand等于目標(biāo)節(jié)點Ngoal，以概率（1-p）隨機(jī)采樣。

（3）搜索樹中與Nrand距離最近的節(jié)點Nnearest，在方向上以一定步長生成新節(jié)點，利用正向運動學(xué)計算機(jī)械臂各個連桿位姿，進(jìn)行碰撞檢測，判斷其是否位于Cfree空間中。若是，把Nnew加入樹中，若否，返回（2）重新采樣。

（4）若Nrand等于Ngoal且Nnew不碰障，則采用貪婪擴(kuò)張策略，在方向上不斷生長新節(jié)點并加入樹中，直至碰障或超出機(jī)械臂運動學(xué)限制。

（5）判斷Nnew與Ngoal的距離，若小于距離極限D(zhuǎn)im，則認(rèn)為隨機(jī)樹已經(jīng)擴(kuò)展至Ngoal，把Ngoal加入樹中，回溯父子節(jié)點關(guān)系，返回隨機(jī)樹和規(guī)劃路徑，若距離大于Dim則樹繼續(xù)生長。

圖7為改進(jìn)RRT路徑規(guī)劃結(jié)果，圖7（a）為理想情況，隨機(jī)樹擴(kuò)展碰障后快速找到目標(biāo)節(jié)點方向，再次貪婪擴(kuò)展，快速到達(dá)了目標(biāo)節(jié)點。圖7（b）為一般情況，隨機(jī)樹貪婪擴(kuò)展遇到了障礙，此時由于以概率（1-p）多次隨機(jī)采樣，最終能夠找到避障路徑，跳出局部最小值，再次貪婪擴(kuò)展得到一條無碰路徑。與前述方法直觀比較可知，改進(jìn)RRT算法擴(kuò)展節(jié)點更少，提高了算法效率。

圖7　改進(jìn)RRT路徑規(guī)劃

表2為使用4種RRT方法經(jīng)過30次數(shù)值仿真試驗后的對比結(jié)果。為在相同條件下比較算法效果，本文統(tǒng)一使用單樹RRT，通過對比可以看到，由于加入了目標(biāo)位置信息，Biased-RRT所用的計算時間和迭代次數(shù)均優(yōu)于基本RRT。Greedy-RRT最終得到的隨機(jī)樹節(jié)點數(shù)與基本RRT相當(dāng)，但所需計算時間和迭代次數(shù)則少得多，證明了Greed-RRT具有局部快速擴(kuò)展能力。而改進(jìn)RRT算法由于結(jié)合了Biased-RRT和Greedy-RRT兩者的優(yōu)點，在計算時間，迭代次數(shù)和擴(kuò)展節(jié)點數(shù)上均優(yōu)于前三種方法，證明改進(jìn)RRT具有較高的計算效率。

表2　數(shù)值仿真結(jié)果對比

3　路徑優(yōu)化

3.1改進(jìn)GPP法

RRT法的優(yōu)點是計算效率高，但由于其在全狀態(tài)空間隨機(jī)搜索可行路徑，路徑規(guī)劃結(jié)果并非最優(yōu)。大量多余節(jié)點導(dǎo)致路徑長而曲折，必須刪除規(guī)劃結(jié)果的多余節(jié)點。節(jié)點刪除通常有LOS（Line of sight path pruning）和GPP兩種方法，GPP克服了LOS短視的缺點且有較高的計算效率，因此，采用GPP法刪除多余節(jié)點。傳統(tǒng)GPP方法主要針對二維環(huán)境下移動機(jī)器人的路徑優(yōu)化，改進(jìn)GPP使其適用于三維條件下機(jī)械臂的運動路徑優(yōu)化：

（1）初始化優(yōu)化路徑Opath為空集用以存儲優(yōu)化路徑，令BNode為Nstart，ENode為Ngoal，并把BNode加入Opath。

（2）判斷BNode、ENode間運動路徑是否碰障，若是，令ENode等于前一個節(jié)點，若否，把ENode加入Opath并令BNode等于ENode，ENode等于Ngoal開始下一輪循環(huán)，當(dāng)Opath包含Ngoal時算法結(jié)束。

如圖8所示，為判斷BNode、ENode間路徑是否碰障，在BNode、ENode間使關(guān)節(jié)定步長運動生成St個過渡節(jié)點，判斷每個節(jié)點對應(yīng)的機(jī)械臂構(gòu)型是否碰障，通過這種方法把判斷復(fù)雜曲面與球體是否干涉轉(zhuǎn)化為判斷若干不連續(xù)的線段與球體的位置關(guān)系，簡化了碰障檢測過程，其中St由式（3）求得：

圖8　GPP法示意圖

3.1路徑優(yōu)化結(jié)果

使用GPP法刪除某次路徑規(guī)劃結(jié)果的多余節(jié)點，如圖9（a）所示，紅色曲線為原始路徑，包含29個節(jié)點，路徑的笛卡爾空間長度和關(guān)節(jié)總運動量為2.910 1 m和4.233 5 rad（0.744 7 m），綠色曲線為優(yōu)化后的路徑，僅包含4個節(jié)點，路徑的笛卡爾空間長度和關(guān)節(jié)總運動量為2.181 9 m和3.839 7 rad （0.621 0 m），分別為前者的75.0%和90.7%（83.4%），可以看到節(jié)點刪除后路徑的笛卡爾空間長度和關(guān)節(jié)運動量都得到大幅優(yōu)化。圖9（b）為機(jī)械臂沿優(yōu)化路徑運動示意圖，可以清楚看到連桿結(jié)構(gòu)的運動軌跡，優(yōu)化后路徑與障礙物并不干涉，證明了改進(jìn)GPP的有效性。

圖9　路徑優(yōu)化結(jié)果

4　仿真實驗

為檢驗改進(jìn)RRT算法路徑規(guī)劃的效果，對機(jī)械臂在兩種不同環(huán)境下的路徑規(guī)劃結(jié)果進(jìn)行運動學(xué)仿真。利用Solidworks建立機(jī)械臂虛擬樣機(jī)模型與MATLAB生成的虛擬環(huán)境特征矩陣導(dǎo)入RecurDyn中，構(gòu)建機(jī)械臂工作環(huán)境，保證了環(huán)境特征的一致性。使用MATLAB規(guī)劃機(jī)械臂在兩種環(huán)境中的工作路徑，并把數(shù)值仿真結(jié)果導(dǎo)入RecurDyn中進(jìn)行運動學(xué)仿真，使用MarkerTrace技術(shù)跟蹤機(jī)械臂末端以觀察其運動軌跡。

圖10為稀疏障礙環(huán)境中機(jī)械臂路徑規(guī)劃仿真結(jié)果，該環(huán)境模擬機(jī)械臂在空曠環(huán)境中作業(yè)時可能遇到的樹枝、巖石等稀疏障礙。圖10（a）為初始位姿，在圖10（b）、圖10（c）中機(jī)械臂從上方繞過了黃色障礙物、綠色障礙物，在圖10（d）中機(jī)械臂從前側(cè)方繞過了粉色障礙物，最終到達(dá)目標(biāo)位姿，圖10（e）為目標(biāo)位姿俯視圖，圖10（f）為目標(biāo)位姿正視圖。圖中紅色曲線為機(jī)械臂末端運動軌跡，可以看到機(jī)械臂良好地避開了所有障礙物，完成了從初始位姿到目標(biāo)位姿的運動。

圖10　稀疏障礙路徑規(guī)劃仿真結(jié)果

圖11為狹窄通路障礙路徑規(guī)劃仿真結(jié)果，狹窄通路障礙環(huán)境模擬機(jī)械臂作業(yè)時可能遇到的車窗、車門、縫隙內(nèi)有目標(biāo)物的情況。圖中藍(lán)色有孔墻壁為障礙物模型，圖11（a）為初始位姿，圖11（b）為機(jī)械臂接近障礙運動軌跡，由于貪婪擴(kuò)張策略，該段軌跡較為平滑。在圖11（c）、圖11（d）中機(jī)械臂接近墻面后與其保持一定距離的畫面，由于算法以（1-p）的概率隨機(jī)采樣，機(jī)械臂得以避開墻體障礙。圖11（e）、圖11（f）為目標(biāo)位姿的頂視圖及正視圖，可以看到機(jī)械臂的避障到達(dá)點的運動軌跡，機(jī)械臂較好地完成了通過狹窄通路障礙的任務(wù)。

圖11　狹窄通路障礙路徑規(guī)劃結(jié)果

5　結(jié)論

本文對比了傳統(tǒng)RRT算法，Biased-RRT算法和Greedy-RRT算法在機(jī)械臂路徑規(guī)劃問題上的應(yīng)用效果，結(jié)合它們的優(yōu)點對RRT算法進(jìn)行改進(jìn)；針對RRT算法的不足，提出改進(jìn)GPP法優(yōu)化機(jī)械臂路徑。在兩種典型工作環(huán)境中的仿真結(jié)果表明，該算法可以有效規(guī)劃機(jī)械臂運動路徑，引導(dǎo)機(jī)械臂避障并完成任務(wù)。改進(jìn)RRT算法的優(yōu)點是效率高、節(jié)點擴(kuò)展迅速、路徑短且平滑，作為一種通用方法，它也可被應(yīng)用于雙樹、多樹以及帶價樹等其他RRT方法的節(jié)點擴(kuò)展策略中，其中的路徑優(yōu)化方法也可用于PRM、RRT等路徑規(guī)劃方法的結(jié)果優(yōu)化。

參考文獻(xiàn)：

［1］TING Y，WEI W I，JAR H C. A path planning algorithm for industrial robots［J］. Computers&Industrial Engineering，2002，42：299-308.

［2］CONKUR E S. Path planning using potential fields for highly redundant manipulators［J］. Robotics and Autonomous Systems，2005（52）：209-228.

［3］RANTANEN M T. A connectivity-based method for enhancing sampling in probabilistic roadmap planners［J］. J Intell Robot Syst，2011，64：161-178.

［4］謝碧云，趙京，劉宇.基于快速擴(kuò)展隨機(jī)樹的7R機(jī)械臂避障達(dá)點運動規(guī)劃［J］.機(jī)械工程學(xué)報，2012，48（3）：63-69.

［5］JAILLET L，JOSEP M P. Path planning under kinematic constraints by rapidly exploring manifolds［J］.IEEE Transactions on Robotics，2013，29（1）：105-117.

［6］BERTRAM D，KUFFNER J，DILLMANN R，et al. An integrated approach to inverse kinematics and path planning for redundant manipulators［C］// IEEE International Conference on Robotics and Automation，2006：1874-1879.

［7］GERAERTS R，OVERMARS M H. Sampling and node adding in probabilistic roadmap planners［J］. Robotics and Autonomous Systems，2006，54：165-173.

［8］DASGUPTA B，GUPTA A，SINGLA E. A variational approach to path planning for hyper-redundant manipulators［J］. Robotics and Autonomous Systems，2009，57：194-201.

［9］BERENSON D，SIMEON T，SRINIVASA S. Addressing cost-space chasms in manipulation planning［C］// ICRA，2010：4561- 4568.

［10］STROEVEN P，LE N L，SLUYS L J. Porosimetry by double-random multiple tree structing in virtual concrete［J］. Image Anal Stereol，2012，31：55-63.

［11］WANG W，XU X，LI Y，et al. Triple RRTs：An effective method for path planning in narrow passages［J］. Advanced Robotics，2010，24：943-962.

［12］KALA R. Rapidly exploring random graphs：motion planning of multiple mobile robots［J］. Advanced Robotics，2013，27：1113-1122.

［13］周芳，朱齊丹，趙國良.基于改進(jìn)快速搜索隨機(jī)樹法的機(jī)械手路徑優(yōu)化［J］.機(jī)械工程學(xué)報，2011，47（11）：30-35.

［14］徐娜，陳雄，孔慶生.非完整約束下的機(jī)器人運動規(guī)劃算法［J］.機(jī)器人，2011，33（6）：666-672.

［15］介黨陽，劉敬明，梁曉峰，等.機(jī)械臂技術(shù)在艦艇發(fā)射裝置領(lǐng)域的應(yīng)用［J］.四川兵工學(xué)報，2015，26（5）：20-23.

［16］ZHANG P F，MU X H，MA Z S. A PSGO-based method for inverse kinematics analysis of serial dangerous articles disposal manipulator［J］.Information-An International Interdisciplinary Journal，2012（12）：06-07.

［17］卞學(xué)良，杜清，馬振書，等.危險彈藥機(jī)器人機(jī)械臂運動學(xué)仿真分析［J］.現(xiàn)代制造工程，2010，32（6）：137-140.

Path Planning of Manipulators
Based on Improved Rapidly- Exploring Random Tree

ZHANG Yun-feng1，MA Zhen-shu2，SUN Hua-gang2，LU Ji-shan1
（1. Mechanical Engineering College，Shijiazhuang 050003，China；2. Ordnance Technology Institute，Shijiazhuang 050003，China）

Abstract：A certain improved rapidly -exploring random tree is proposed to solving the path planning problem of EOD manipulators. The improved RRT combines advantages of biased strategy and greedy strategy. By selecting goal node with some probability when sampling，improved RRT reduces the blindness of RRT. Greedy strategy is used for extending trees in the direction of goal node to enhance the extending rapid. The results by RRT are not optimal，an improved GPP technique is developed to prune the redundant nodes of planned paths，optimize the motion path of manipulators. By comparing the numerical simulation results of Biased-RRT and Greedy-RRT，the high efficiency of improved RRT is proved in computation time，iterations and number of extended nodes. The simulation results in two typical environments show that the algorithm can achieved path planning tasks well in obstacle circumstances.

Key words：path planning，rapidly-exploring random tree，RRT，manipulators，optimal path

中圖分類號：TP241.3

文獻(xiàn)標(biāo)識碼：A

文章編號：1002-0640（2016）05-0025-06

收稿日期：2015-03-15修回日期：2015-05-20

*基金項目：國家“863”計劃基金資助項目（2001AA422420）

作者簡介：張云峰（1991-），男，河北無極人，碩士。研究方向：機(jī)器人運動規(guī)劃。