基于RRT?A*融合算法的機械臂避障路徑規(guī)劃

李明，趙金龍，袁逸萍，晁永生

（新疆大學(xué)機械工程學(xué)院，新疆 烏魯木齊 830047）

1 引言

近年來，不同類型的機械臂廣泛的應(yīng)用于各個行業(yè)。然而，隨著復(fù)雜作業(yè)、智能作業(yè)的要求不斷提高，機械臂面臨的工作環(huán)境越來越復(fù)雜，與工作環(huán)境中的障礙物碰撞的可能性大大增加。因此，機械臂避障路徑規(guī)劃成為研究熱點：（1）機械臂避障路徑規(guī)劃是機械臂按時完成操作的前提；（2）避障路徑規(guī)劃能提高機械臂的安全性。針對機械臂避障路徑規(guī)劃，大量學(xué)者做了相關(guān)研究。文獻[1]為提升機械臂在障礙環(huán)境下運動時的安全性，先使用A*算法規(guī)劃一條避障路徑，在考慮平穩(wěn)性的基礎(chǔ)上，使用2次B樣條平滑避障路徑。文獻[2]提出了一種基于改進A*算法的變步長搜索方法。文獻[3]將障礙物轉(zhuǎn)化到機械臂關(guān)節(jié)空間，并利用A*算法完成六自由度機械臂關(guān)節(jié)空間無碰撞路徑規(guī)劃。文獻[4]提出了一種改進A*算法，在縮短路徑長度的同時，提高了路徑規(guī)劃的成功率。文獻[5]針對雙臂機器人避障路徑規(guī)劃問題，提出一種改進快速擴展隨機樹算法，應(yīng)用于協(xié)同避障路徑規(guī)劃。文獻[6]提出一種自適應(yīng)步長RRT 方法，通過建立構(gòu)型空間與工作空間的范數(shù)相容不等式，實現(xiàn)有效的碰撞檢測，完成雙機器人協(xié)同路徑規(guī)劃。文獻[7]針對荔枝采摘機器人的避障問題，提出了一種改進的RRT算法。文獻[8]提出一種適用于裝配任務(wù)系統(tǒng)的改進快速擴展隨機樹算法。文獻[9]提出一種將高斯分布用于RRT算法的方法。文獻[10]針對RRT算法搜索效率低的問題，利用高斯分布采樣的方式提出一種GB_RRT算法。

上述學(xué)者針對機械臂在障礙環(huán)境避障的問題，應(yīng)用和改進A*算法、RRT算法、人工勢場法等算法完成了避障路徑的規(guī)劃，但在改進RRT算法的過程中忽略了該算法在避障路徑規(guī)劃過程中會出現(xiàn)耗時長、路徑長度不是最優(yōu)、且轉(zhuǎn)折點多的問題。針對這些問題，首先建立障礙物模型，其次針對RRT算法在三維避障路徑規(guī)劃過程中會出現(xiàn)耗時長的問題，應(yīng)用雙向RRT算法完成避障路徑規(guī)劃，降低路徑規(guī)劃耗時。針對RRT算法規(guī)劃的避障路徑存在路徑長、且轉(zhuǎn)折點多的問題，使用A*算法選取關(guān)鍵節(jié)點，并建立三次B樣條插值函數(shù)，完成對避障路徑的平滑處理，并縮短避障路徑的長度。

2 碰撞檢測模型

2.1 障礙模型

在實際的工作場景中，障礙物的形式以及形狀難以確定。因此，為避免機械臂本體及各連桿在避障時與障礙物發(fā)生碰撞，利用便于計算的空間規(guī)則體近似包絡(luò)障礙物，雖然形成的規(guī)則體障礙物體積較大，但是使用空間規(guī)則模型提高了碰撞檢測及搜索路徑的效率，同時也降低了末端執(zhí)行器在實際運行過程中與障礙物發(fā)生碰撞的可能性。障礙物可以采用長方體包絡(luò)，如圖1所示。將障礙物近似包絡(luò)為長方體的方法，簡化了距離的計算，提升路徑規(guī)劃效率。

圖1 長方體包絡(luò)的障礙物Fig.1 Cuboid Envelope Obstacle

2.2 機械臂模型

以滿足Pieper準則的六自由度機械臂為研究對象，其三個相鄰關(guān)節(jié)軸交于一點或三軸線平行。根據(jù)結(jié)構(gòu)特點，將機械臂各桿件使用圓柱體包絡(luò)，關(guān)節(jié)使用球形包絡(luò)。六自由度機械臂結(jié)構(gòu)簡圖，如圖2所示。

圖2 機械臂模型Fig.2 The Model of Manipulator

3 基于RRT?A*融合算法的路徑搜索

3.1 雙向RRT算法

快速擴展搜索樹（RRT）算法是Steven M.LaValle 于1998 年首次提出的。該算法通過采樣點在二維或三維障礙環(huán)境進行碰撞檢測，能在高維空間、復(fù)雜約束的條件下解決路徑規(guī)劃問題?；镜腞RT算法的搜索過程，如圖3所示。

圖3 基本RRT算法原理圖Fig.3 Basic RRT Algorithm Schematic

設(shè)qinit為隨機樹的起始點，qgoal隨機樹的目標點，qrand為每次迭代產(chǎn)生的隨機點，qnear為擴展時離qrand最近的節(jié)點，設(shè)每次擴展的步長為s，當qnear與qrand之間的距離大于s時，按照步長s產(chǎn)生新節(jié)點qnew，之后需要對新產(chǎn)生的節(jié)點進行碰撞檢測，如果發(fā)現(xiàn)未發(fā)生碰撞，保留新節(jié)點加入到隨機樹，反之舍去。

如果qnear與qrand之間的距離小于s時，取qrand等于qnew，在進行碰撞檢測，如果未發(fā)生碰撞，保留新節(jié)點加入到隨機樹，反之舍去。直到隨機樹新產(chǎn)生的節(jié)點qnew與qgoal小于設(shè)置的閾值，隨機樹停止生長。

針對基本RRT算法搜索效率低，搜索時間長的問題，采用雙向RRT算法來降低搜索耗時。雙向RRT算法的主要思想是兩顆隨機樹Ta和Tb同時從起始點和目標點開始擴展，假設(shè)Ta從qinit開始擴展，Tb從qgoal開始擴展直到Ta和Tb新產(chǎn)生的點之間的距離小于設(shè)定的閾值時，隨機樹停止生長。雙向RRT算法的搜索過程，如圖4所示。

圖4 雙向RRT算法原理Fig.4 Principle of RRT?Connect Algorithm

3.2 A＊算法

A*算法是一種典型的啟發(fā)式方法，可在平面或空間障礙環(huán)境中完成避障路徑的搜索，是在障礙信息已知的條件下較為有效的路徑搜索方法。

f(n)函數(shù)是避障路徑搜索時的評價指標，每進行一次路徑搜索都需計算所有子節(jié)點的f(n)值，并將下一個父節(jié)點定義為子節(jié)點中估價值最小的點，直至搜索到目標位置。A*算法在路徑規(guī)劃過程中只需按照f(n)函數(shù)值選取子節(jié)點，解決了需要遍歷空間點的難題。A*算法公式如下：

式中：f(n)—實際代價與預(yù)估代價的和；g(n)—起始位置到節(jié)點n之間的實際代價；h(n)—節(jié)點xn到目標位置的預(yù)估代價。

節(jié)點xn到終點xd的預(yù)估代價值會影響A*算法的路徑搜索速度，如果計算節(jié)點xn到終點xd的預(yù)估代價較小，會導(dǎo)致A*算法在搜索過程中形成的子節(jié)點數(shù)量過多，降低計算速度和搜索效率。若h(n)計算的預(yù)估代價值與節(jié)點xn到終點xd實際耗費的代價相等，則A*算法搜索時不會形成額外的子節(jié)點，搜索效率也會相應(yīng)的提高。預(yù)估代價函數(shù)h(n)為從節(jié)點xn到終點xd之間的歐式距離，即：

RRT?A*融合算法的搜索流程，如圖5 所示。先使用雙向RRT算法完成在障礙環(huán)境下避障路徑的規(guī)劃，再利用A*算法選取該避障路徑上的關(guān)鍵節(jié)點，完成對避障路徑的優(yōu)化。

圖5 RRT?A*融合算法規(guī)劃避障路徑流程圖Fig.5 Flow Chart of Obstacle Avoidance Path Planning by RRT?A* Fusion Algorithm

4 構(gòu)造三次B樣條插值函數(shù)

機械臂在避障過程中，為避免機械臂在起始、終止以及避障路徑轉(zhuǎn)折點出現(xiàn)急加速的情況，就要保證避障路徑的平滑性。

首先取出A*算法選取的RRT算法規(guī)劃的避障路徑上的關(guān)鍵節(jié)點，并在關(guān)鍵節(jié)點之間使用三次B樣條函數(shù)平滑優(yōu)化。

對于空間位置—時間序{θi，ti}，i=0，1，…，n其一般形式如下：

式中：Qi（i=0，1，…，n）—控制頂點；

Ni，k(t)（i=0，1，…，n）—是k次規(guī)范B樣條基函數(shù)，且有：

式中：k—B樣條函數(shù)的階數(shù)；

i—B樣條函數(shù)的序列號，記0/0=0。

通過障礙空間中對應(yīng)的避障位置—時間序列點，將其作為型值點去反算控制點，并建立三次B樣條插值函數(shù)，保證平滑后的避障軌跡經(jīng)過A*算法選取的避障路徑上的關(guān)鍵節(jié)點，其第i段曲線的函數(shù)表達式為：

5 仿真結(jié)果及分析

仿真試驗的空間機械臂是以工業(yè)上最常用的PUMA560 型機器人為研究對象，其D?H參數(shù)，如表1所示。利用Matlab中機器人工具箱建立的模型，如圖6所示。

表1 機械臂D-H參數(shù)Tab.1 D-H Parameters of Manipulator

圖6 PUM560機器人模型Fig.6 The Model of PUM560 Robot

避障路徑規(guī)劃仿真實驗是在長寬高為100cm的環(huán)境中進行的，雙向RRT算法和RRT?A*算法規(guī)劃的避障路徑對比結(jié)果，如圖7 所示。折線為雙向RRT 算法規(guī)劃的避障路徑，路徑長度為168cm，有16個轉(zhuǎn)折點，雖然雙向RRT算法和傳統(tǒng)RRT算法相比可有效的提高搜索速度，但不能解決路徑長度較長，轉(zhuǎn)折點多的問題。因此，使用A*算法對避障路徑的節(jié)點進行選取，A*算法對該避障路徑選取的關(guān)鍵節(jié)點構(gòu)成的避障路徑，如圖7虛線所示。經(jīng)過A*算法對避障路徑節(jié)點的選取，路徑長度縮短為136cm，但路徑仍然有3個轉(zhuǎn)折點。故使用三次B樣條函數(shù)完成進一步的避障路徑優(yōu)化，經(jīng)過三次B樣條平滑后的避障路徑，如圖8所示。從圖中可以看出，經(jīng)過三次B樣條曲線平滑后，生成了一條平滑的避障路徑，從而可提升機械臂在避障運動過程中的位移、速度、加速度曲線的平滑性。

圖7 雙向RRT算法和RRT?A*算法規(guī)劃避障路徑對比Fig.7 The Comparison of RRT?connect Algorithm and RRT?A* Algo?rithm Planning Obstacle Avoidance Path

圖8 三次B樣條平滑避障路徑Fig.8 Obstacle Avoidance Path Smoothed by Cubic B?Spline

通過對機械臂避障運動過程中的位移、速度、加速度的計算求解，得到其位移、速度、加速度變化圖，如圖9所示。從圖中可以看出，機械臂在避障運動過程中位移、速度、加速度曲線均平滑變化、且速度、加速度無突變，這保證了機械臂在運動過程中的穩(wěn)定性和準確性，同時可提高機械臂的安全性以及完成避障運動的速度。

圖9 三次B樣條平滑路徑各關(guān)節(jié)位移、速度、加速度曲線Fig.9 Displacement，Velocity，Acceleration Curve of Each Joint of Cubic B?Spline Smooth Path

6 結(jié)論

針對RRT算法在三維避障路徑規(guī)劃過程中會出現(xiàn)耗時長、路徑長度不是最優(yōu)、且轉(zhuǎn)折點多的問題，提出并實現(xiàn)了一種基于RRT?A*融合算法和三次B樣條的機械臂避障路徑規(guī)劃方法。首先建立空間障礙物三維模型，在構(gòu)建的障礙環(huán)境中應(yīng)用雙向RRT算法規(guī)劃避障路徑，降低搜索避障路徑的耗時。然后使用A*算法選取該避障路徑的關(guān)鍵節(jié)點，建立三次B樣條插值函數(shù)，生成平滑的機械臂避障路徑，解決路徑長度不是最優(yōu)且轉(zhuǎn)折點多的問題。從仿真結(jié)果可以看出，通過該方法可規(guī)劃出平滑的避障路徑且保證機械臂避障時位移、速度、加速度曲線的平滑性，驗證了該方法的有效性。