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

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

1 引言

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

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

2 碰撞檢測(cè)模型

2.1 障礙模型

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

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

2.2 機(jī)械臂模型

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

圖2 機(jī)械臂模型Fig.2 The Model of Manipulator

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

3.1 雙向RRT算法

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

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

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

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

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

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

3.2 A＊算法

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

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

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

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

RRT?A*融合算法的搜索流程，如圖5 所示。先使用雙向RRT算法完成在障礙環(huán)境下避障路徑的規(guī)劃，再利用A*算法選取該避障路徑上的關(guān)鍵節(jié)點(diǎn)，完成對(duì)避障路徑的優(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ù)

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

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

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

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

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

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

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

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

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

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

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

圖6 PUM560機(jī)器人模型Fig.6 The Model of PUM560 Robot

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

圖7 雙向RRT算法和RRT?A*算法規(guī)劃避障路徑對(duì)比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

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

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

6 結(jié)論

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