基于改進RRT-Connect算法的機械臂抓取路徑規(guī)劃

李磊 吳翔 包祥威 張景濤 方翔 曹毅

（1河南工業(yè)大學(xué)電氣工程學(xué)院，鄭州，450001；2河南工業(yè)大學(xué)機電工程學(xué)院，鄭州，450001）

0 引言

操作臂的避障路徑規(guī)劃是在操作臂的工作空間內(nèi)完成起止位姿之間安全連續(xù)路徑搜索，一般以路徑最短、搜索時間最短、或者能量消耗最優(yōu)其中之一作為評價標準。目前按照操作臂所處環(huán)境信息已知狀態(tài)，避障路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃?；谌致窂揭?guī)劃的方法主要有自由空間法、構(gòu)型空間法、拓撲法和柵格法等。基于局部路徑規(guī)劃的方法有人工勢場法、遺傳算法、模糊邏輯算法、神經(jīng)網(wǎng)絡(luò)法、蟻群禁忌搜索算法[1]，粒子群算法[2]等等。這些算法有的適合靜態(tài)環(huán)境，有的適用于動態(tài)環(huán)境，有的搜索速度快但易陷入局部極優(yōu)，若對一些算法進行改進和結(jié)合，可以使改進后的算法達到更理想的效果。

通過對比國內(nèi)外規(guī)劃算法研究分析發(fā)現(xiàn)[3-4]，遺傳算法、神經(jīng)網(wǎng)絡(luò)等智能仿生學(xué)算法應(yīng)用于三維空間模型避障運動規(guī)劃較為困難，且搜索效率較低；人工勢場法等傳統(tǒng)算法容易早熟收斂；多項式軌跡插值擬合計算量大，耗時長；基于隨機采樣的避障路徑規(guī)劃算法具有強適應(yīng)性和概率完備性，高維空間避障路徑采用隨機采樣可以有效減少精確建模時間。

基于隨機采樣的應(yīng)用最多的算法為概率路標法和快速擴展隨機樹法。

概率路標法是在操作臂的可行空間內(nèi)進行隨機采樣，連接鄰近點構(gòu)建“路標圖”，查詢路標圖中起始位置到目標位置最短可行路徑[5]。該方法雖實用性強，但是最終得到的路徑可能不是最優(yōu)路徑。

快速擴展隨機樹法模擬樹枝生長產(chǎn)生枝葉進行空間擴展路徑搜索，是一種增量式采樣算法[6]，對于在線和離線路徑規(guī)劃都適用，在高維空間路徑規(guī)劃該算法時效性較好，且具有概率完備性，但每次規(guī)劃所得路徑隨機性大，且得到的路徑存在不光滑現(xiàn)象。

本文針對傳統(tǒng)操作臂避障路徑規(guī)劃算法進行分析比較，選擇雙樹快速隨機擴展連接樹RRT-Connect算法進行改進，并利用Dijkstra[7]算法和B樣條對改進算法規(guī)劃路徑進行優(yōu)化處理，以達到減少規(guī)劃路徑長度、時間和平滑路徑的目的。

1 RRT算法

RRT(快速隨機擴展樹)算法是一種簡單采樣算法。為了算法描述方便，引入一些參數(shù)，這些參數(shù)的意義如表1所示。

表1 RRT算法各參數(shù)意義

2 改進RRT-Connect算法

2.1 RRT-Connect算法步驟

2.2 RRT-Connect算法的改進

3 路徑優(yōu)化

3.1 Dijkstra算法

通過 算法搜索最短路徑時，首先指定起始節(jié)點s，此外引進兩個集合S和U。S是用來存放已求出最短路徑節(jié)點的集合，U是用來存放剩余節(jié)點的集合。算法初始時，將起始節(jié)點s加入集合S中，并用鄰近矩陣D記錄各個節(jié)點之間距離。通過不斷循環(huán)尋找U集合中距離起始節(jié)點最近的節(jié)點，將該節(jié)點放入到集合S中，并修改起始節(jié)點經(jīng)過該節(jié)點到其他節(jié)點的鄰近矩陣的值。

在算法中，采用 表示i節(jié)點與起始節(jié)點s之間的距離，兩個節(jié)點之間若連通，則w(i,j)表示i節(jié)點和j節(jié)點之間的權(quán)值，否則w(i,j)=∞。根據(jù)節(jié)點圖構(gòu)造鄰近矩陣Dn×n，n表示節(jié)點圖中節(jié)點個數(shù)。首先求出鄰近矩陣的第一行除去起始節(jié)點之外的最小值，并記為k，則第k個節(jié)點就是距離起始節(jié)點最近的節(jié)點，將k節(jié)點加入集合S中。然后從集合U中求出距離起始節(jié)點最近的節(jié)點m，在將m節(jié)點加入S集合之前，需要做以下比較：

根據(jù)式（4）計算出m節(jié)點到起始節(jié)點的最短距離，并對鄰近矩陣D內(nèi)的值進行替換。依次循環(huán)，直至U中節(jié)點都加入集合S為止。Dijkstra算法流程圖如下；

3.2 路徑平滑處理

改進RRT-Connect算法優(yōu)化后的路徑，在節(jié)點連接處存在折點、不平滑現(xiàn)象。操作臂沿此路徑執(zhí)行任務(wù)時，操作臂會出現(xiàn)快速變向現(xiàn)象，該現(xiàn)象必然會影響執(zhí)行任務(wù)的精度，并減少操作臂的使用周期。為了避免出現(xiàn)這種現(xiàn)象，必須對規(guī)劃路徑進行平滑處理。B樣條曲線在路徑平滑方面效果顯著，其優(yōu)點之一便是修改局部路徑而不會對全部路線變動。

一般情況下，K階B樣條曲線方程可表示為：

本文采用三次B樣條對路徑進行優(yōu)化處理，為使操作臂的運動更加平穩(wěn)，盡可能多地插入平滑路徑點。

4 算法驗證

為驗證本文算法的有效性，筆者進行了算法仿真試驗。在每組實驗中，起點均用符號‘S’表示，坐標為(10,10)，終點均用符號‘G’表示，坐標為(490,490)，坐標點單位為cm。實驗中各類算法的路徑規(guī)劃結(jié)果如圖2-圖6所示。

圖2 原始RRT算法仿真結(jié)果

圖6 B 樣條曲線仿真結(jié)果

實驗1：原始RRT算法的路徑規(guī)劃。

實驗2：在實驗1的基礎(chǔ)上引入雙向搜索策略，采用RRT-Connect算法路徑規(guī)劃策略，并與RRT仿真結(jié)果進行對比。

圖3中，規(guī)劃路徑分別由1號藍色路徑和2號紅色路徑拼接組成，3號路徑是去除無效節(jié)點之后完整規(guī)劃路徑。

圖3 RRT-Connect算法仿真結(jié)果

為了避免采樣算法的隨機性誤差，進行40次仿真驗證，得到驗證結(jié)果如表2所示。

表2 RRT與RRT-Connect算法仿真結(jié)果對比

由表2可知，RRT-Connect算法在平均搜索時間、平均節(jié)點和平均路徑長度上遠遠優(yōu)于原始RRT算法。

實驗3：在實驗2的基礎(chǔ)上采用改進RRT-Connect算法進行路徑規(guī)劃。

圖4中，規(guī)劃路徑分別由1號藍色路徑和2號紅色路徑拼接組成，3號粉紅色路徑是去除無效節(jié)點之后完整規(guī)劃路徑。

圖4 改進RRT-Connect算法仿真結(jié)果

由圖3和圖4對比可知改進RRT-Connect算法減少了了無效節(jié)點的出現(xiàn)，且路徑長度優(yōu)于傳統(tǒng)RRT-Connect算法。

實驗4：在實驗3的基礎(chǔ)上引入Dijkstra算法搜索最短路徑。

圖5中，1號紅色路徑表示優(yōu)化之后的路徑，2號粉紅色路徑是改進RRT-Connect規(guī)劃路徑。明顯看出，紅色路徑比粉紅色路徑長度更短。

圖5 融合Dijkstra算法仿真結(jié)果

為了避免采樣算法的隨機性誤差，進行40次仿真驗證，得到驗證結(jié)果如表3所示。

表3 RRT-Connect與融合算法仿真結(jié)果對比

由表3可知，改進RRT-Connect與Dijkstra融合算法IRRT-Connect在平均搜索時間、平均節(jié)點和平均路徑長度優(yōu)于改進RRT-Connect算法。

實驗5：在實驗4的基礎(chǔ)上采用B樣條曲線對路徑進行平滑處理。

圖6中，1號粉紅色路徑表示首次規(guī)劃路徑，2號紅色路徑表示Dijkstra算法優(yōu)化路徑，3號黑色路徑表示B樣條函數(shù)平滑處理后路徑。

由圖6中三條路徑對比可知，平滑處理后的黑色路徑在節(jié)點連接處比紅色路徑曲率更小。

5 仿真實驗

仿真實驗以越疆科技有限公司生產(chǎn)的Dobot魔術(shù)師為機械臂模型進行三維空間路徑規(guī)劃，該機械臂三個自由度?？臻g環(huán)境包括操作臂模型和障礙物模型。

為易于觀察，圖7所示的規(guī)劃場景去掉障礙物上面一部分。在X方向上，障礙物中心點與機械臂的距離為25，在Y方向上為25，在Z方向上為0，長度單位均為cm。

圖7 操作臂規(guī)劃場景

在操作臂規(guī)劃場景內(nèi)應(yīng)用改進RRT-Connect算法進行避障路徑規(guī)劃，確定操作臂的起始坐標和目標坐標分別為(20，10，18)和(25，10，20)并在空間中用“start”和“goal”標注。

操作臂完成從起始目標點向目標節(jié)點的運動路徑規(guī)劃如下。

首次規(guī)劃路徑如圖8中紅色路徑所示。

圖8 操作臂首次規(guī)劃路徑

首次規(guī)劃路徑呈鋸齒狀，存在多余路徑節(jié)點，利用Dijkstra算法進行二次路徑優(yōu)化。優(yōu)化路徑如圖9中綠色路徑所示。

圖9 操作臂規(guī)劃優(yōu)化路徑

雖然優(yōu)化路徑與首次規(guī)劃路徑相比，減少了路徑長度，但路徑在節(jié)點連接處存在折角。采用B樣條曲線對路徑進行光滑處理，平滑后路徑如圖10中3號粉紅色路徑所示。

圖1 Dijkstra算法流程圖

圖10 操作臂規(guī)劃平滑路徑

選取相同的操作臂避障空間環(huán)境，分別進行原算法和改進的融合算法路徑規(guī)劃。路徑規(guī)劃結(jié)果如圖11所示。

圖11 算法規(guī)劃路徑對比

在操作臂三維空間配置同樣的規(guī)劃場景，進行RRT算法、RRT-Connect算法和改進RRT-Connect多次仿真驗證，取驗證平均結(jié)果作為對比標準，仿真結(jié)果如表4所示。

通過表4可以發(fā)現(xiàn)，本文提出的規(guī)劃算法對串聯(lián)機械臂的實際使用具有參考價值。

表4 三維空間算法仿真結(jié)果對比

6 結(jié)論

針對機械臂在抓取目標物體的路徑規(guī)劃問題,本文提出了一種改進RRT-Connect和Dijkstra融合算法的路徑規(guī)劃策略。引入雙樹目標偏執(zhí)、極致貪婪策略提高路徑搜索速率，減少了傳統(tǒng)RRT-Connect算法中無效節(jié)點數(shù)量、搜索時間和路徑長度，再利用Dijkstra算法進行首次路徑優(yōu)化，得到最短路徑。為了避免優(yōu)化后的路徑在節(jié)點連接處存在折點、不平滑現(xiàn)象，必須對規(guī)劃路徑進行平滑處理，其中利用B樣條差值的方法得到一條光滑平緩的完整運動曲線。仿真表明，該算法更加適合機械臂的實際運行與操作，進而證明該算法的可行性與實用性。

本文是在障礙物位置信息已知的情況下，對空間中機械臂的避障軌跡規(guī)劃進行研究，但在實際應(yīng)用環(huán)境中，障礙物環(huán)境位置往往處于未知狀態(tài)，因此對復(fù)雜環(huán)境下的機械臂軌跡規(guī)劃有待進一步研究和改善。