朱永浩 ，李 丹 ，龔 旭 ，金仁才

(安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院, 安徽 馬鞍山 243032)

隨著機(jī)器人技術(shù)的發(fā)展，機(jī)械臂具有工作空間大、操作簡便、靈活性和自由度高等優(yōu)點(diǎn)[1]，在焊縫焊接、制造加工、拆卸裝配、噴漆和碼垛等領(lǐng)域得到越來越廣泛的應(yīng)用[2-4]。與此同時(shí)，各項(xiàng)工作對機(jī)械臂的工作狀態(tài)有更高的要求，路徑規(guī)劃作為機(jī)械臂工作的關(guān)鍵技術(shù)，受到越來越多的關(guān)注和研究。機(jī)械臂的路徑規(guī)劃和智能小車類似，均是以機(jī)器人當(dāng)前位置為起點(diǎn)，規(guī)劃出1 條運(yùn)動(dòng)到終點(diǎn)、無碰撞的最優(yōu)路徑[5]。但是，機(jī)械臂的運(yùn)動(dòng)規(guī)劃主要用于多維空間，規(guī)劃難度更大。

近年，機(jī)械臂路徑規(guī)劃算法主要分為基于圖搜索、基于勢場、基于采樣等類別[6]。其中基于采樣的算法無需預(yù)處理，也無需提前建圖，在多維空間中得到更廣泛的應(yīng)用。快速擴(kuò)展隨機(jī)樹(rapidly-exploring random tress，RRT)算法是機(jī)械臂路徑規(guī)劃中最常用的算法[7]，但該算法在復(fù)雜環(huán)境下會(huì)出現(xiàn)效率降低且無法得到最優(yōu)路徑的問題。基于RRT 的改進(jìn)算法，Kuffner 等[8]提出讓起點(diǎn)和終點(diǎn)同時(shí)生成隨機(jī)樹來進(jìn)行雙向拓展的RRT-connect 算法；Ferguson等[9]提出動(dòng)態(tài)快速搜索樹(dynamic-RRT，DRRT)算法，通過給定目標(biāo)拓展方向，可較大程度地降低規(guī)劃時(shí)陷入局部最優(yōu)的情況；Karaman[10]提出改進(jìn)的RRT*算法，通過添加圖優(yōu)化和剪枝理論，保留傳統(tǒng)RRT 算法的完備性，同時(shí)兼具漸進(jìn)最優(yōu)性，但大大增加了搜索時(shí)間；Islam 等[11]提出RRT*-smart 算法，利用智能采樣使搜索路徑快速收斂；Klemm 等[12]結(jié)合RRT*的漸進(jìn)最優(yōu)和RRT-connect 的雙向搜索，提出使搜索路徑朝理論最優(yōu)解收斂的RRT*-connect算法；朱宏輝等[13]通過加入規(guī)避步長延伸法改進(jìn)RRT*算法，但改進(jìn)算法用于復(fù)雜路徑的規(guī)劃時(shí)，難以獲得最優(yōu)解；陳肇星等[14]提出的DSSP-RRT 算法通過動(dòng)態(tài)采樣減少迭代次數(shù)，但存在可能陷入局部最優(yōu)的情況。綜上可看出：改進(jìn)的RRT 算法均在一定程度上優(yōu)化了路徑規(guī)劃性能，但依然存在一些不足，且在高維空間下的機(jī)械臂路徑規(guī)劃通常是不穩(wěn)定的，需在不同層面上做出改進(jìn)。鑒于此，結(jié)合RRT*和RRT-connect 算法的優(yōu)點(diǎn)，提出1 種改進(jìn)的RRT算法對六自由度機(jī)械臂進(jìn)行路徑規(guī)劃，并分別利用Python 中的matplotlib 功能包以及ROS 平臺(tái)中的動(dòng)態(tài)規(guī)劃庫(open motion planning library，OMPL)，對睿爾曼RM-65 機(jī)械臂進(jìn)行避障路徑規(guī)劃仿真實(shí)驗(yàn)，結(jié)果顯示該算法規(guī)劃出的路徑更優(yōu)且用時(shí)更少。

1 機(jī)械臂運(yùn)動(dòng)學(xué)分析

1.1 運(yùn)動(dòng)學(xué)模型

以六自由度機(jī)械臂為對象，對睿爾曼RM-65機(jī)械臂進(jìn)行正運(yùn)動(dòng)學(xué)分析。機(jī)械臂可視為由關(guān)節(jié)和連桿組成，求解機(jī)械臂末端相對于基準(zhǔn)坐標(biāo)系的位姿，即為機(jī)械臂的正運(yùn)動(dòng)學(xué)建模[15]。機(jī)械臂結(jié)構(gòu)尺寸如圖1，其中d1=240.5 mm，d2=102.0 mm，d3=256.0 mm，d4=210.0 mm，d5=144.0 mm，d6=850.5 mm，d7=81.5 mm。

圖1 RM-65 機(jī)械臂結(jié)構(gòu)尺寸Fig.1 Structural dimensions of RM-65 robotic arm

基于改進(jìn)的MD-H 參數(shù)建模，建立的連桿坐標(biāo)系如圖2。

圖2 連桿坐標(biāo)系示意圖Fig.2 Schematic diagram of the connecting rod coordinate system

圖3 Matlab 仿真結(jié)果示意圖Fig.3 Schematic diagram of Matlab simulation results

x軸、y軸、z軸如圖2 中標(biāo)注所示，機(jī)械臂最底部的坐標(biāo)系base_link 為機(jī)械臂的基坐標(biāo)系，Link1～Link6 分別對應(yīng)關(guān)節(jié)1～6 的坐標(biāo)系。根據(jù)機(jī)械臂的尺寸得到MD-H 參數(shù)，如表1。

表1 RM-65 機(jī)械臂MD-H 參數(shù)Tab.1 RM-65 robotic arm MD-H parameter

根據(jù)MD-H 參數(shù)與各關(guān)節(jié)角度，確定機(jī)械臂末端執(zhí)行器基于機(jī)械臂基座的空間位姿，矩陣ii-1T為連桿i坐標(biāo)系到連桿i-1 坐標(biāo)系的MD-H變換矩陣，該矩陣經(jīng)以下四次運(yùn)動(dòng)變換得到，具體變換公式如下：

由式(2)～(7)可得出機(jī)械臂末端坐標(biāo)系相對于底端基坐標(biāo)系的運(yùn)動(dòng)學(xué)位姿變換矩陣，即正運(yùn)動(dòng)學(xué)方程06T可表示為：

圖中：RPY為矩陣60T中的旋轉(zhuǎn)矩陣轉(zhuǎn)化得到的歐拉角，q1～q6為對應(yīng)6 個(gè)關(guān)節(jié)角度；取3 組關(guān)節(jié)角度，分別通過Robotic Toolbox 工具箱讀取和建模得到正運(yùn)動(dòng)學(xué)方程計(jì)算，結(jié)果如表2。由表2 可看出：Robotic Toolbox 工具箱讀取所得位姿與運(yùn)動(dòng)學(xué)方程計(jì)算所得位姿結(jié)果基本一致，表明由MD-H 參數(shù)法建模計(jì)算得到的正運(yùn)動(dòng)學(xué)方程正確。

表2 位姿對比結(jié)果Tab.2 Pose comparison results

逆運(yùn)動(dòng)學(xué)建模是通過已知的機(jī)械臂末端位姿來確定滿足期望位姿的機(jī)械臂各關(guān)節(jié)的狀態(tài)[16]，常用的解法有數(shù)值求解法和解析求解法。其中數(shù)值求解法采用非線性優(yōu)化求解，適用范圍較廣但計(jì)算量較大、速度較慢；解析求解法速度快但往往會(huì)得出多組奇異解。因此，文中選擇數(shù)值求解法計(jì)算得到若干組逆解，根據(jù)關(guān)節(jié)角度范圍和碰撞關(guān)系的檢測在逆解中找出最優(yōu)解。

1.2 碰撞檢測

機(jī)械臂各連桿的形狀不規(guī)則，在進(jìn)行碰撞檢測時(shí)常使用包圍盒法，利用簡單幾何體將機(jī)械臂包絡(luò)起來。常用的包圍盒法有軸對齊邊界框包圍盒(axis-aligned bounding box，AABB)、包圍球、方向包圍盒(oriented bounding box，OBB)等[17-18]。AABB 法采用平行于坐標(biāo)軸的最小六面體包絡(luò)對象，包圍球法采用半徑最小的球體包絡(luò)對象，OBB 法采用最小長方體包絡(luò)對象。李智[19]提出了子彈形的包圍盒(bullet bounding box，BBB)，包圍盒模型為圓柱體搭配半球體的組合。使用該方法對機(jī)械臂進(jìn)行包絡(luò)時(shí)，對于旋轉(zhuǎn)不會(huì)影響相鄰連桿模型的軸體，采用圓柱體來包絡(luò)；對于旋轉(zhuǎn)會(huì)影響相鄰連桿的軸體，采用半球來包絡(luò)，這樣可使包絡(luò)盒更緊湊，且不會(huì)大量增加后續(xù)計(jì)算量。

文中采用BBB 方法和包圍球方法對機(jī)械臂包圍盒進(jìn)行包絡(luò)，將機(jī)械臂與空間障礙物的碰撞檢測簡化為求解圓柱中心線與球體半徑的關(guān)系問題以及圓柱中心線與AABB 包圍盒各平面的距離問題[20]。根據(jù)機(jī)械臂連桿旋轉(zhuǎn)是否會(huì)影響相鄰連桿，對第一軸、第四軸和第六軸采用圓柱包絡(luò)，對第二軸、第三軸和第五軸采用半球和球形包絡(luò)。具體為：將第一軸、第一連桿和第二軸轉(zhuǎn)換為子彈形包絡(luò)，第二連桿和第三軸轉(zhuǎn)換為球形包絡(luò)，第三連桿、第四軸、第四連桿和第五軸轉(zhuǎn)換形成子彈包絡(luò)，第五連桿、第六軸和第六連桿視作1 個(gè)圓柱體，末端執(zhí)行器視為1 個(gè)球體。使用包圍盒法可對機(jī)械臂包絡(luò)進(jìn)行簡化，根據(jù)簡化的機(jī)械臂模型，對各包絡(luò)體間距離進(jìn)行求解，各包絡(luò)體之間的最短距離可作為判斷是否發(fā)生碰撞的指標(biāo)，距離大于0 則未發(fā)生碰撞，否則判斷為發(fā)生碰撞。

2 RRT 算法的改進(jìn)

RRT 算法是1 種基于隨機(jī)采樣的快速路徑規(guī)劃算法，通過采樣在工作空間全局搜索，以給定的起點(diǎn)為根節(jié)點(diǎn)，利用隨機(jī)取點(diǎn)生成新的子節(jié)點(diǎn)使擴(kuò)展樹不斷生長，具有出色的空間搜索能力，對高維空間的路徑規(guī)劃也具有出色的性能[21]。RRT 算法的主要步驟如圖4，包括采樣、尋找鄰近節(jié)點(diǎn)、拓展根節(jié)點(diǎn)、生成候選節(jié)點(diǎn)、碰撞檢測以及生成鄰近節(jié)點(diǎn)。具體過程為：確定起點(diǎn)(S)與終點(diǎn)(F)，并明確起點(diǎn)與終點(diǎn)之間的障礙物信息；以起點(diǎn)為根節(jié)點(diǎn)，隨機(jī)擴(kuò)展采樣，遍歷樹中所有節(jié)點(diǎn)，生成候選節(jié)點(diǎn)Tn，并以固定步長生成1 個(gè)鄰近節(jié)點(diǎn)Tn+1，若Tn與Tn+1之間不存在障礙物，則Tn及Tn+1加入擴(kuò)展樹，否則放棄此次采樣點(diǎn)并重新采樣；循環(huán)上一步驟，直到采樣得到1 個(gè)節(jié)點(diǎn)Tn+1為終點(diǎn)F或該節(jié)點(diǎn)距離終點(diǎn)F的距離小于設(shè)定的閾值，則規(guī)劃路徑成功并退出采樣。

圖4 RRT 算法示意圖Fig.4 Schematic diagram of RRT algorithm

RRT 算法在多維空間中適用性很強(qiáng)，但算法本身在搜索路徑的過程中具有隨機(jī)性，導(dǎo)致每次規(guī)劃出的路徑各不相同，規(guī)劃出的是可行路徑，并不是最優(yōu)路徑，且在復(fù)雜狹小環(huán)境中難以搜索到路徑。為此，文中結(jié)合RRT*算法和RRT-connect算法的優(yōu)點(diǎn)，在搜索過程中，應(yīng)用目標(biāo)采樣思想對路徑進(jìn)行調(diào)整與重新布線，使每次采樣趨近給定的擴(kuò)展方向，改進(jìn)RRT 算法。

2.1 目標(biāo)采樣

RRT 算法在隨機(jī)采樣的過程中，搜索樹時(shí)常會(huì)擴(kuò)展到離目標(biāo)很遠(yuǎn)的地方，即“無用區(qū)域”。為提高算法的效率，應(yīng)用目標(biāo)采樣的思想加強(qiáng)算法向目標(biāo)點(diǎn)搜索的趨向性，減少無用搜索，即人為引導(dǎo)采樣過程中隨機(jī)點(diǎn)的生成，在生成1 個(gè)隨機(jī)節(jié)點(diǎn)Tn時(shí)，以一定概率將節(jié)點(diǎn)生成在終點(diǎn)處，即Tn=TF。在提高算法效率的前提下，為保持搜索樹對于未知空間的搜索能力，一般將概率p設(shè)定在0.05～0.10 之間。具體步驟為：生成1 個(gè)(0,1)的隨機(jī)數(shù)rand，若rand≤p，則節(jié)點(diǎn)生成在終點(diǎn)處，即Tn=TF；若rand＞p，則新節(jié)點(diǎn)保持隨機(jī)生成，圖5 為加入目標(biāo)采樣前后的路徑規(guī)劃效果。

圖5 加入目標(biāo)采樣前后的路徑規(guī)劃效果Fig.5 Path planning effect before and after adding target sampling

從圖5 可看出：加入目標(biāo)采樣后的算法既能保證搜索樹擴(kuò)展時(shí)的隨機(jī)性，又能一定程度上減免對“無用區(qū)域”搜索耗費(fèi)的時(shí)間，在障礙物較少的空間效果較好。

2.2 貪婪思想

當(dāng)隨機(jī)采樣到目標(biāo)點(diǎn)附近區(qū)域時(shí)，受固定步長的限制，即使采樣點(diǎn)與目標(biāo)點(diǎn)之間無障礙物，搜索樹也無法直接到達(dá)目標(biāo)點(diǎn)，轉(zhuǎn)而會(huì)繼續(xù)進(jìn)行無效搜索，使路徑復(fù)雜、規(guī)劃時(shí)長增加。針對這些采樣點(diǎn)，引用貪婪思想，當(dāng)搜索樹生成的新節(jié)點(diǎn)與目標(biāo)點(diǎn)間的距離在設(shè)定的閾值內(nèi)時(shí)，判斷新節(jié)點(diǎn)Tn1～Tn5與目標(biāo)點(diǎn)F直接相連的可行性，如圖6。當(dāng)新節(jié)點(diǎn)與目標(biāo)點(diǎn)之間存在障礙物時(shí)(如Tn1，Tn3，Tn4，Tn5)，該節(jié)點(diǎn)繼續(xù)隨機(jī)采樣拓展；若之間不存在障礙物時(shí)(如Tn2)，則該節(jié)點(diǎn)停止隨機(jī)采樣，且不受固定步長的限制，將該節(jié)點(diǎn)Tn2與目標(biāo)點(diǎn)F直接相連，并將兩點(diǎn)間的路徑放入最終規(guī)劃出的路徑中。

圖6 引入貪婪思想的示意圖Fig.6 Schematic diagram of introducing greedy thinking

2.3 路徑平滑處理

在機(jī)械臂避障路徑規(guī)劃過程中，易在路徑拐點(diǎn)出現(xiàn)急加速的情況，使規(guī)劃過程的平穩(wěn)性降低，故需對路徑進(jìn)行平滑處理。對于改進(jìn)的RRT 算法，選取路徑中的若干拐點(diǎn)，使用B 樣條函數(shù)對拐點(diǎn)間的路徑進(jìn)行平滑處理?？紤]到B 樣條次數(shù)越高，平滑處理后的路徑越平滑，為保證最終規(guī)劃出的路徑效果，文中選擇五次B 樣條函數(shù)進(jìn)行平滑優(yōu)化。根據(jù)需要平滑優(yōu)化的拐點(diǎn)坐標(biāo)，構(gòu)造出1 條參數(shù)化的B 樣條曲線，使用取值范圍為[0，1]的參數(shù)t表示曲線上的點(diǎn)；將參數(shù)t映射到實(shí)際的自變量范圍，根據(jù)拐點(diǎn)個(gè)數(shù)將自變量范圍劃分成若干區(qū)間，并對每個(gè)區(qū)間使用五次B 樣條插值；根據(jù)插值結(jié)果可得到各區(qū)間的插值函數(shù)。位置表達(dá)式為

其中，將式(13)代入式(10)～(12)，解得：

2.4 改進(jìn)RRT 算法的實(shí)現(xiàn)

改進(jìn)的RRT 算法流程圖如圖7。

圖7 改進(jìn)的RRT 算法流程圖Fig.7 Flowchart of improved RRT algorithm

由圖7 可看出：采用改進(jìn)的RRT 算法進(jìn)行路徑規(guī)劃，其主要步驟如下：

1) 確定起點(diǎn)與終點(diǎn)，將起點(diǎn)和終點(diǎn)分別作為根節(jié)點(diǎn)向空間中擴(kuò)展生成搜索樹，2 棵樹分別為TS，TF。

2) 加入目標(biāo)采樣，使搜索樹TS按照給定方向擴(kuò)展，采樣得到1 個(gè)新節(jié)點(diǎn)Tr1，檢查搜索樹TS與Tr1最近節(jié)點(diǎn)Tn1之間是否存在障礙物。若不存在障礙物，則將節(jié)點(diǎn)Tn1儲(chǔ)存進(jìn)搜索樹；若存在障礙物則重復(fù)步驟2)，直到找到1 個(gè)無碰撞的節(jié)點(diǎn)Tn1。

3) 以Tn1附近規(guī)定的半徑范圍內(nèi)尋找路徑代價(jià)更小的鄰近節(jié)點(diǎn)作為父節(jié)點(diǎn)，并重新布線，調(diào)整經(jīng)過新節(jié)點(diǎn)Tn1的路徑。

4) 與步驟2)類似，使搜索樹TF按照給定方向擴(kuò)展，通過碰撞檢測，采樣得到1 個(gè)無碰撞的節(jié)點(diǎn)Tn2。

5) 以Tn2附近規(guī)定的半徑范圍內(nèi)尋找路徑代價(jià)更小的鄰近節(jié)點(diǎn)作為父節(jié)點(diǎn)，并重新布線，調(diào)整經(jīng)過節(jié)點(diǎn)Tn2的路徑。

6) 循環(huán)步驟2)到5)，直到采樣得到的節(jié)點(diǎn)Tn1與Tn2重合或2 個(gè)節(jié)點(diǎn)間的距離小于設(shè)定的閾值，則規(guī)劃路徑成功并退出采樣，得到的TS至Tn1的路徑、TF至Tn2的路徑加上Tn1至Tn2的路徑即為最終規(guī)劃出的路徑。

3 仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證改進(jìn)RRT 算法的路徑規(guī)劃效果，利用Python 中的matplotlib 功能包以及ROS 平臺(tái)的動(dòng)態(tài)規(guī)劃庫(open motion planning，OMPL)，對改進(jìn)RRT算法與傳統(tǒng)RRT 算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn)。

3.1 三維空間仿真實(shí)驗(yàn)與結(jié)果分析

三維空間地圖范圍單位大小為100×100×100，設(shè)置路徑規(guī)劃的起點(diǎn)坐標(biāo)為(10,10,10)，終點(diǎn)坐標(biāo)為(95,95,95)，如圖8。圖中冗余線段為拓展樹的樹結(jié)構(gòu)，從起點(diǎn)連接到終點(diǎn)的線段為傳統(tǒng)RRT 算法和改進(jìn)RRT 算法最終規(guī)劃出的路線。地圖中的障礙物為8 塊正方體，初始設(shè)定采樣長度為5、步長為5，當(dāng)采樣次數(shù)超過1 000 次時(shí)認(rèn)定此次路徑規(guī)劃失敗，放棄采樣。

圖8 三維空間仿真實(shí)驗(yàn)示意圖Fig.8 Schematic diagram of three-dimensional space simulation experiment

在相同仿真環(huán)境下，分別使用傳統(tǒng)和改進(jìn)的RRT 算法進(jìn)行50 次路徑規(guī)劃實(shí)驗(yàn)，對比2 種算法規(guī)劃的平均路徑長度、計(jì)算時(shí)長和采樣點(diǎn)數(shù)，結(jié)果如表3。

表3 2 種算法的路徑規(guī)劃實(shí)驗(yàn)結(jié)果Tab.3 Experimental results of path planning for two algorithms

由表3 可看出：與傳統(tǒng)RRT 算法相比，改進(jìn)RRT 算法平均采樣點(diǎn)數(shù)略高，但規(guī)劃出的單位平均路徑長度從292.6 減少至206.2，即規(guī)劃出的路徑代價(jià)大幅減少，且平均計(jì)算時(shí)長從1.264 s 減至1.156 s，改進(jìn)RRT 算法規(guī)劃出的路徑更優(yōu)且用時(shí)更少。

3.2 ROS 仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證改進(jìn)的RRT 算法在加入真實(shí)機(jī)械臂后的路徑規(guī)劃效果，使用六自由度睿爾曼RM-65 機(jī)械臂在ROS 平臺(tái)下進(jìn)行仿真實(shí)驗(yàn)，操作系統(tǒng)為Ubuntu18.04。使用Moveit! Setup Assistant 對機(jī)械臂進(jìn)行配置，具體流程如圖9。

圖9 Moveit! Setup Assistant 配置流程圖Fig.9 Flowchart of Moveit! Setup Assistant configuration

在ROS 平臺(tái)下的動(dòng)態(tài)規(guī)劃庫(open motion planning library，OMPL)中進(jìn)行算法匹配，配置完成利用Moveit!及其插件接口實(shí)現(xiàn)機(jī)械臂在仿真環(huán)境下的避障運(yùn)動(dòng)規(guī)劃。如圖10 所示綠色機(jī)械臂為機(jī)械臂起始位姿，橙色機(jī)械臂為目標(biāo)位姿，綠色長方體為障礙物，機(jī)械臂從起始位姿開始經(jīng)過路徑規(guī)劃并避開障礙物最終到達(dá)目標(biāo)位姿視為實(shí)驗(yàn)成功。

圖10 避障運(yùn)動(dòng)規(guī)劃示意圖Fig.10 Schematic diagram of obstacle avoidance movement planning

使用傳統(tǒng)RRT 算法和改進(jìn)的RRT 算法分別進(jìn)行30 次實(shí)驗(yàn)，2 種算法的運(yùn)動(dòng)規(guī)劃過程如圖11。在選用軌跡可視化功能后，圖中連續(xù)出現(xiàn)的機(jī)械臂即為運(yùn)動(dòng)規(guī)劃過程中的路徑軌跡，機(jī)械臂軌跡越多說明規(guī)劃時(shí)間越長、路徑越復(fù)雜。

圖11 ROS 平臺(tái)下避障運(yùn)動(dòng)規(guī)劃示意圖Fig.11 Schematic diagram of obstacle avoidance motion planning on ROS platform

從圖11 可看出：在相同實(shí)驗(yàn)環(huán)境和運(yùn)動(dòng)參數(shù)下，相比于傳統(tǒng)RRT 算法，改進(jìn)的RRT 算法規(guī)劃時(shí)間和更短、規(guī)劃的路徑更優(yōu)，且路徑規(guī)劃成功率提高了10 個(gè)百分點(diǎn)，2 種算法平均規(guī)劃時(shí)間和規(guī)劃成功率統(tǒng)計(jì)如表4。

表4 避障運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)結(jié)果Tab.4 Experimental results of obstacle avoidance movement planning

4 結(jié)論

提出1 種改進(jìn)的RRT 算法對六自由度機(jī)械臂進(jìn)行路徑規(guī)劃，結(jié)合RRT*和RRT-connect 算法的優(yōu)點(diǎn)，應(yīng)用目標(biāo)采樣的思想加強(qiáng)算法向目標(biāo)點(diǎn)搜索的趨向性，引入貪婪思想提高算法效率，結(jié)合五次B樣條插值對路徑平滑優(yōu)化，使機(jī)械臂可在較短時(shí)間規(guī)劃出更優(yōu)路徑。利用Python 中的matplotlib 功能包統(tǒng)計(jì)傳統(tǒng)與改進(jìn)RRT 算法規(guī)劃路徑所需的時(shí)間、采樣點(diǎn)數(shù)量與路徑長度，且在ROS 平臺(tái)中利用動(dòng)態(tài)規(guī)劃庫對所提改進(jìn)與傳統(tǒng)RRT 算法進(jìn)行仿真實(shí)驗(yàn)。結(jié)果表明：采用所提改進(jìn)算法對六自由度機(jī)械臂進(jìn)行路徑規(guī)劃，可規(guī)劃出路徑代價(jià)更小的路徑且用時(shí)更少，最終路徑也更為平滑，驗(yàn)證了該算法在實(shí)際機(jī)械臂避障路徑規(guī)劃中的可行性。