鄒宇星， 李立君， 高自成

(中南林業(yè)科技大學(xué) 機(jī)電工程學(xué)院，湖南 長(zhǎng)沙 410000)

0 引 言

由于在工作空間中進(jìn)行避障路徑規(guī)劃存在局部最小點(diǎn)和振蕩等問(wèn)題[1]，機(jī)械臂避障路徑規(guī)劃通常在構(gòu)形空間[2，3]內(nèi)進(jìn)行。國(guó)內(nèi)外學(xué)者基于構(gòu)形空間對(duì)避障路徑規(guī)劃算法開(kāi)展了大量研究[4～8]，提出了包括概率地圖(probabilistic roadmap,PRM)算法[9]、快速擴(kuò)展隨機(jī)樹(rapidly-exploring random tree,RRT)算法[10,11]和快速擴(kuò)展樹(fast marching tree，F(xiàn)MT)算法[12]等在內(nèi)的路徑規(guī)劃算法。研究過(guò)程中傳統(tǒng)算法不斷獲得改進(jìn)，文獻(xiàn)[13]提出了一種改進(jìn)的bi-RRT算法，算法能夠自動(dòng)選擇合適的采樣點(diǎn)作為目標(biāo)節(jié)點(diǎn)，提高了算法搜索效率；文獻(xiàn)[14]基于關(guān)節(jié)空間提出一種改進(jìn)的A*算法對(duì)空間機(jī)械臂進(jìn)行避障路徑規(guī)劃，驗(yàn)證了A*算法應(yīng)用于高自由度機(jī)械臂的可行性；文獻(xiàn)[15]利用改進(jìn)PRM算法對(duì)機(jī)械臂進(jìn)行避障路徑規(guī)劃，算法只要通過(guò)采樣獲得部分機(jī)械臂關(guān)節(jié)構(gòu)形空間無(wú)撞點(diǎn)，連接無(wú)撞點(diǎn)即可獲得機(jī)械臂避障路徑。

本文針對(duì)PRM算法需要大量耗時(shí)以構(gòu)建無(wú)撞點(diǎn)網(wǎng)絡(luò)圖的問(wèn)題，引進(jìn)一種快速構(gòu)形空間構(gòu)建算法，避免PRM算法在構(gòu)建網(wǎng)絡(luò)圖時(shí)所需要的復(fù)雜碰撞檢測(cè)。實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法能夠快速、有效地為平面2R機(jī)械臂和三維空間高自由度機(jī)械臂進(jìn)行避障路徑規(guī)劃。

1 PRM算法的融合改進(jìn)

根據(jù)PRM算法的基本原理不難發(fā)現(xiàn)，該算法路標(biāo)圖的質(zhì)量取決于隨機(jī)采樣點(diǎn)的個(gè)數(shù)。若將PRM算法應(yīng)用于高自由度機(jī)械臂避障路徑規(guī)劃，那么在構(gòu)建路標(biāo)圖的過(guò)程中，對(duì)于任意隨機(jī)采樣點(diǎn)都需要檢測(cè)該采樣點(diǎn)所對(duì)應(yīng)的機(jī)械臂位姿與工作空間障礙物的碰撞情況。而空間三維模型的碰撞檢測(cè)問(wèn)題十分復(fù)雜，且計(jì)算量巨大。為保證PRM算法的性能，本文引入構(gòu)形空間構(gòu)建算法對(duì)其進(jìn)行改進(jìn)，通過(guò)構(gòu)形空間構(gòu)建算法快速建立明確的構(gòu)形空間，使PRM算法采樣點(diǎn)可行性檢測(cè)由復(fù)雜三維模型碰撞檢測(cè)轉(zhuǎn)化為簡(jiǎn)單的查表操作和布爾運(yùn)算。

1.1 快速構(gòu)形空間構(gòu)建算法

1.1.1 碰撞查詢數(shù)據(jù)庫(kù)的構(gòu)建

在機(jī)械臂路徑規(guī)劃研究中，機(jī)械臂實(shí)際運(yùn)動(dòng)所處的空間一般稱為工作空間W，以機(jī)械臂各關(guān)節(jié)角度值為坐標(biāo)所建立的空間通常稱為機(jī)械臂的構(gòu)形空間C。因此，工作空間W中的障礙物可在空間C中映射為一個(gè)不可達(dá)區(qū)域，通常稱為構(gòu)形空間障礙物Cobs?？臻g障礙物Cobs在空間C中的補(bǔ)集通常稱為自由區(qū)域Cfree。若空間C中點(diǎn)cs和點(diǎn)cg分別表示機(jī)械臂的初始位姿qs和目標(biāo)位姿qg，那么，機(jī)械臂避障路徑規(guī)劃就轉(zhuǎn)化成在空間C中搜索一條路徑Cpath=[cs,c1,…,cn,cg]，且滿足Cpath?Cfree。ci表示構(gòu)形空間C中某點(diǎn)的第i軸坐標(biāo)值。

若n自由度(degree of freedom,DOF)機(jī)械臂的第i個(gè)關(guān)節(jié)角度值為qi(mm/(°))，那么該機(jī)械臂的某位姿可以表示為q=[q1,q2,…,qn]。

該機(jī)械臂構(gòu)形空間C中與q唯一對(duì)應(yīng)的點(diǎn)可以表示為c=[c1,c2,…,cn]，且滿足qi=ci。

將機(jī)械臂工作空間W分割成各個(gè)方向尺寸均為dw(mm)的離散化單元wi，那么分割后的工作空間可以表示為W，其中單元序號(hào)i為

i=i1+i2+…+in-(n-1)+(hn-1)·(in-1-1)+

(hn·hn-1-1)·(in-2-1)+…+

(hn·hn-1·…·h2-1)·(i1-1)

(1)

式中in為離散單元在空間中的坐標(biāo)值,hn為空間最大坐標(biāo)值。

對(duì)于工作空間任意離散單元wi，通過(guò)計(jì)算獲得所有與該單元相交的機(jī)械臂位姿qwi={q|wi∩Aq≠0}。qwi為工作空間某點(diǎn)狀障礙物處于離散單元wi中時(shí)，構(gòu)形空間C中所對(duì)應(yīng)的障礙物模型,Aq為機(jī)械臂A處于位姿q狀態(tài)。

如圖1所示為x，y軸范圍均為[-100,100]mm的二維工作空間W，該空間被分割成400個(gè)尺寸一致的正方形單元，圖中灰色正方形為離散單元w125。圖2表示單元w125在機(jī)械臂構(gòu)形空間C中的映射模型qw125。

圖1 離散化的二維空間和機(jī)械臂模型

圖2 離散單元在構(gòu)形空間中的映射模型

若能夠計(jì)算獲得任意離散單元wi所對(duì)應(yīng)的qwi，則qwi的集合就構(gòu)成了碰撞查詢數(shù)據(jù)庫(kù)(collision query database,CQD)，即

CQD={q|Aq∩W≠0}={q|Aq∩(∪wi)≠0}

=∪{q|Aq∩wi≠0}=∪(qwi)

(2)

為構(gòu)建CQD，以dc為關(guān)節(jié)步長(zhǎng)遍歷機(jī)械臂位姿，若某位姿q對(duì)應(yīng)的機(jī)械臂三維模型與單元wi發(fā)生干涉時(shí)，則將該位姿數(shù)據(jù)q=[q1,q2,…,qn]保存至離散單元序號(hào)所對(duì)應(yīng)的數(shù)據(jù)空間di中。對(duì)工作空間W中的所有離散化單元完成上述遍歷操作后，數(shù)據(jù)空間di的集合∪di就構(gòu)成了CQD。

在離線階段建立CQD，該數(shù)據(jù)庫(kù)中存儲(chǔ)了機(jī)械臂工作空間中任意離散單元在構(gòu)形空間C中的映射模型，在線階段只需要根據(jù)空間障礙物所占據(jù)的微小單元對(duì)應(yīng)的序號(hào)檢索CQD即可快速構(gòu)建障礙物在空間C中的映射模型。

1.1.2 機(jī)械臂碰撞檢測(cè)模型及碰撞檢測(cè)算法

在建立CQD的過(guò)程中需要對(duì)機(jī)械臂與工作空間離散單元wi之間的碰撞關(guān)系進(jìn)行檢測(cè)。由于采摘機(jī)械臂曲面造型較為復(fù)雜，若以精確的曲面模型為基礎(chǔ)進(jìn)行碰撞檢測(cè)，CQD建立速度過(guò)慢，難以滿足使用性能要求，因此,需要對(duì)機(jī)械臂模型進(jìn)行一定簡(jiǎn)化，以提高碰撞檢測(cè)速度。

方向包圍盒(oriented bounding box，OBB)作為機(jī)械臂常見(jiàn)的簡(jiǎn)化模型，其模型擬合程度較高，碰撞檢測(cè)算法成熟。因此，本文采用OBB模型擬合機(jī)械臂精確三維模型。

為了在基礎(chǔ)坐標(biāo)系Tbase中明確描述OBB模型的尺寸和空間位姿，在OBB模型上以相互平行的兩個(gè)面的中心o1和o2為原點(diǎn)分別建立局部坐標(biāo)系Ta和Tb，且局部坐標(biāo)系Ta和Tb的各坐標(biāo)軸與OBB模型對(duì)應(yīng)棱邊平行。

在建立了局部坐標(biāo)系的基礎(chǔ)上，通過(guò)齊次變換矩陣可以將表示在局部坐標(biāo)系中的點(diǎn)的坐標(biāo)轉(zhuǎn)換為在基礎(chǔ)坐標(biāo)系中表示的點(diǎn)的坐標(biāo)。

齊次變換矩陣的一般形式為

(3)

式中T為坐標(biāo)系TA與坐標(biāo)系TB的齊次變換矩陣,R為姿態(tài)變換矩陣,P為位置變換矩陣。

如圖3所示為簡(jiǎn)化后機(jī)械臂碰撞檢測(cè)模型。

圖3 采摘機(jī)器人機(jī)械臂碰撞檢測(cè)三維模型

由于機(jī)械臂精確三維模型被簡(jiǎn)化成OBB模型，而且工作空間被分割成各邊長(zhǎng)尺寸一致的離散立方體單元，因此，機(jī)械臂與工作空間單元之間的碰撞檢測(cè)問(wèn)題可以等效為OBB-OBB碰撞檢測(cè)問(wèn)題。

Gottschalk在文獻(xiàn)[16]中系統(tǒng)地闡述了采用軸向投影方法確定空間OBB模型相交性的分離軸算法(separating axis theorem，SAT)。如圖4所示為一對(duì)平面OBB模型。

圖4 SAT算法在平面OBB模型中的應(yīng)用

對(duì)于三維空間中任一對(duì)OBB模型，只需對(duì)15條分離軸進(jìn)行測(cè)試，即可確定兩者之間的碰撞關(guān)系。這15條分離軸中有6條為兩OBB模型不同方向面的法線，另外9條為兩OBB模型棱邊的叉乘組合。若兩OBB模型在15條潛在軸線上的投影都相交，則可以確定該對(duì)OBB模型必然相交。

1.1.3 索引CQD數(shù)據(jù)庫(kù)建立構(gòu)形空間

在線階段機(jī)器人雙目視覺(jué)系統(tǒng)所獲得的障礙物點(diǎn)云模型為避障路徑規(guī)劃提供障礙物信息。障礙物點(diǎn)云模型中任一點(diǎn)obsi所占據(jù)的工作空間離散單元wi在x軸方向序號(hào)sx為

sx=(x-xmin)/dw

(4)

式中x為該點(diǎn)在x軸方向的坐標(biāo)值，xmin為工作空間在x軸方向的最小坐標(biāo)值，同理可以求取離散單元wi在y和z軸方向上的序號(hào)。

之后，通過(guò)式(1)求取障礙物點(diǎn)云占據(jù)的工作空間單元序號(hào)，并通過(guò)序號(hào)索引CQD可以快速建立與當(dāng)前障礙物模型對(duì)應(yīng)的明確構(gòu)形空間。

1.2 融合構(gòu)形空間的PRM算法

為了避免PRM算法復(fù)雜的碰撞檢測(cè)，提高避障路徑規(guī)劃效率，先快速建立當(dāng)前場(chǎng)景構(gòu)形空間， PRM算法在判斷隨機(jī)采樣點(diǎn)是否可行的過(guò)程中，只需要進(jìn)行簡(jiǎn)單的布爾運(yùn)算。融合了構(gòu)形空間構(gòu)建算法的PRM算法主要步驟包括：

1)在離線階段構(gòu)建CQD。

2)將工作空間障礙物Wobs分割成離散單元集合wobs。

3)根據(jù)離散單元集合wobs中的單元序號(hào)i檢索CQD，建立與當(dāng)前場(chǎng)景對(duì)應(yīng)的明確構(gòu)形空間C。

4)在構(gòu)形空間C中隨機(jī)生成采樣點(diǎn)c，通過(guò)計(jì)算求得該采樣點(diǎn)所占據(jù)的構(gòu)形空間單元ck及其序號(hào)k。

5)根據(jù)序號(hào)k檢索構(gòu)形空間C，確定單元ck是否滿足條件ck∈Cfree：若不滿足該條件，則重復(fù)步驟(4)、步驟(5),重新生成采樣點(diǎn)并判斷其可行性；若滿足條件，則將采樣點(diǎn)c加入到概率地圖路標(biāo)點(diǎn)集合V中。

6)通過(guò)歐拉公式計(jì)算獲得集合V中與采樣點(diǎn)c鄰近的節(jié)點(diǎn)集合M。

7)點(diǎn)c與M中任意點(diǎn)m的連線按一定步長(zhǎng)分割成若干節(jié)點(diǎn)，利用節(jié)點(diǎn)占據(jù)的單元序號(hào)來(lái)檢索構(gòu)形空間C以確定采樣點(diǎn)c與點(diǎn)m之間連線e是否滿足條件e∈Cfree。

8)以步驟(7)的方法遍歷集合M中的所有點(diǎn)，并在與采樣點(diǎn)c相關(guān)聯(lián)的數(shù)據(jù)空間中存儲(chǔ)M中所有滿足條件e∈Cfree的節(jié)點(diǎn)序號(hào)。

9)當(dāng)集合V中點(diǎn)的個(gè)數(shù)達(dá)到設(shè)置的閾值時(shí)，將機(jī)械臂的初始構(gòu)形qs和目標(biāo)構(gòu)形qg在構(gòu)形空間C中所對(duì)應(yīng)的點(diǎn)cs和點(diǎn)cg通過(guò)最短歐拉距離連接到集合V中。

10)最后通過(guò)Dijkstra算法從集合V中搜尋一條連接點(diǎn)cs和點(diǎn)cg的距離最短路徑，完成避障路徑規(guī)劃。

2 仿真結(jié)果與分析

2.1 平面2R機(jī)械臂避障路徑規(guī)劃

為驗(yàn)證本文所提算法的有效性和可行性，首先將算法應(yīng)用于一種平面2R機(jī)械臂的避障路徑規(guī)劃，再將算法擴(kuò)展應(yīng)用至高自由度采摘機(jī)器人機(jī)械臂避障路徑規(guī)劃問(wèn)題。圖5為平面2R機(jī)械臂及其避障路徑規(guī)劃場(chǎng)景俯視圖。

圖5 平面2R機(jī)械臂避障路徑規(guī)場(chǎng)景俯視

根據(jù)表1中的工作空間單元尺寸參數(shù)，可將實(shí)驗(yàn)場(chǎng)景中的工作空間劃分成400個(gè)離散工作空間單元。

表1 平面2R機(jī)械臂避障路徑規(guī)劃CQD參數(shù)

通過(guò)式(1)可以獲得工作空間障礙物所占據(jù)的5個(gè)單元序號(hào)為115，147，235，333，348。

同樣，可將圖6所示的構(gòu)形空間劃分成5 198個(gè)離散構(gòu)形空間單元。利用遍歷的方法建立CQD數(shù)據(jù)庫(kù)后通過(guò)工作空間障礙物序號(hào)對(duì)其進(jìn)行索引，即可獲得所有構(gòu)形空間障礙物單元序號(hào)以及單元總個(gè)數(shù)，其中單元總數(shù)為1 713。

圖6 利用PRM算法在構(gòu)形空間搜尋避障路徑

利用MATLAB軟件在構(gòu)形空間中繪制障礙物單元，即可構(gòu)建如圖7所示的平面2R機(jī)械臂的明確構(gòu)形空間。

圖7 平面2R機(jī)械臂的避障運(yùn)動(dòng)

圖中字母S和G表示的構(gòu)形空間單元分別對(duì)應(yīng)工作空間中平面2R機(jī)械臂起始位姿和目標(biāo)位姿。

在此基礎(chǔ)上，通過(guò)PRM算法在構(gòu)形空間中成功搜尋到一條連接代表機(jī)械臂起始位姿和目標(biāo)位姿的無(wú)碰撞路徑，該路徑中各節(jié)點(diǎn)的坐標(biāo)可以構(gòu)成一組關(guān)節(jié)角度序列。PRM算法主要控制參數(shù)為:算法迭代次數(shù)閾值為500，路標(biāo)地圖節(jié)點(diǎn)數(shù)為100，節(jié)點(diǎn)連接距離閾值為80，節(jié)點(diǎn)連線分割距離為50。

利用無(wú)碰撞路徑節(jié)點(diǎn)的坐標(biāo)驅(qū)動(dòng)2R機(jī)械臂各關(guān)節(jié)，可以獲得如圖7所示的機(jī)械臂運(yùn)動(dòng)過(guò)程?？梢杂^察到平面2R機(jī)械臂從起始位姿運(yùn)動(dòng)到了目標(biāo)位姿，且在此過(guò)程中未與工作空間中的障礙物發(fā)生碰撞。

2.2 6自由度采摘機(jī)器人機(jī)械臂避障路徑規(guī)劃

為了進(jìn)一步驗(yàn)證本文所提出的算法在高自由度(degree of freedom,DOF)機(jī)械臂避障路徑規(guī)劃中的有效性，將改進(jìn)算法應(yīng)用至6 DOF機(jī)械臂避障路徑規(guī)劃場(chǎng)景中。

由于6 DOF采摘機(jī)器人機(jī)械臂對(duì)應(yīng)的構(gòu)形空間達(dá)到六維，因此，要繪制六維構(gòu)形空間以描述路徑搜索過(guò)程具有相當(dāng)大的困難，這里僅對(duì)避障路徑規(guī)劃完成后機(jī)器人的運(yùn)動(dòng)過(guò)程進(jìn)行可視化操作。表2為6DOF機(jī)械臂避障路徑規(guī)劃相關(guān)實(shí)驗(yàn)參數(shù)。

表2 6 DOF機(jī)械臂避障路徑規(guī)劃CQD參數(shù)

如圖8所示，為利用本文算法所獲得的6自由度機(jī)械臂避障運(yùn)動(dòng)狀態(tài)。可以看出，本文算法所求得的關(guān)節(jié)角度序列安全，無(wú)碰撞地將機(jī)械臂末端執(zhí)行器從起始位姿移動(dòng)到目標(biāo)采摘位姿。

分別利用傳統(tǒng)PRM算法和本文所提出的改進(jìn)算法在當(dāng)前場(chǎng)景下進(jìn)行50次避障路徑規(guī)劃，算法路徑規(guī)劃平均耗時(shí)分別為0.81 s和 0.63 s，相比傳統(tǒng)PRM算法,改進(jìn)算法的耗時(shí)降低了約22.2 %，可見(jiàn)改進(jìn)算法規(guī)劃速度提升較為明顯。值得指出的是，改進(jìn)算法在構(gòu)建CQD的過(guò)程中耗時(shí)約為5.36 h，但是由于CQD的建立是在離線階段完成的，不會(huì)對(duì)在線避障路徑規(guī)劃耗時(shí)造成不利影響。

圖8 6 DOF采摘機(jī)器人機(jī)械臂避障運(yùn)動(dòng)狀態(tài)

3 結(jié) 論

針對(duì)概率地圖法在避障路徑規(guī)劃中存在的不足，本文通過(guò)融入快速構(gòu)形空間構(gòu)建算法對(duì)其進(jìn)行了改進(jìn)。結(jié)果表明，本文的改進(jìn)算法相比傳統(tǒng)PRM算法速度提高22.2 %，能夠滿足復(fù)雜環(huán)境下機(jī)械臂的實(shí)時(shí)避障路徑規(guī)劃要求。