鄒宇星, 李立君, 高自成
(中南林業(yè)科技大學(xué) 機(jī)電工程學(xué)院,湖南 長沙 410000)
由于在工作空間中進(jìn)行避障路徑規(guī)劃存在局部最小點(diǎn)和振蕩等問題[1],機(jī)械臂避障路徑規(guī)劃通常在構(gòu)形空間[2,3]內(nèi)進(jìn)行。國內(nèi)外學(xué)者基于構(gòu)形空間對(duì)避障路徑規(guī)劃算法開展了大量研究[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ī)劃算法。研究過程中傳統(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ī)劃,算法只要通過采樣獲得部分機(jī)械臂關(guān)節(jié)構(gòu)形空間無撞點(diǎn),連接無撞點(diǎn)即可獲得機(jī)械臂避障路徑。
本文針對(duì)PRM算法需要大量耗時(shí)以構(gòu)建無撞點(diǎn)網(wǎng)絡(luò)圖的問題,引進(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ī)劃。
根據(jù)PRM算法的基本原理不難發(fā)現(xiàn),該算法路標(biāo)圖的質(zhì)量取決于隨機(jī)采樣點(diǎn)的個(gè)數(shù)。若將PRM算法應(yīng)用于高自由度機(jī)械臂避障路徑規(guī)劃,那么在構(gòu)建路標(biāo)圖的過程中,對(duì)于任意隨機(jī)采樣點(diǎn)都需要檢測(cè)該采樣點(diǎn)所對(duì)應(yīng)的機(jī)械臂位姿與工作空間障礙物的碰撞情況。而空間三維模型的碰撞檢測(cè)問題十分復(fù)雜,且計(jì)算量巨大。為保證PRM算法的性能,本文引入構(gòu)形空間構(gòu)建算法對(duì)其進(jìn)行改進(jìn),通過構(gòu)形空間構(gòu)建算法快速建立明確的構(gòu)形空間,使PRM算法采樣點(diǎn)可行性檢測(cè)由復(fù)雜三維模型碰撞檢測(cè)轉(zhuǎn)化為簡單的查表操作和布爾運(yùn)算。
1.1.1 碰撞查詢數(shù)據(jù)庫的構(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,通過計(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ù)庫(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é)步長遍歷機(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ù)庫中存儲(chǔ)了機(jī)械臂工作空間中任意離散單元在構(gòu)形空間C中的映射模型,在線階段只需要根據(jù)空間障礙物所占據(jù)的微小單元對(duì)應(yīng)的序號(hào)檢索CQD即可快速構(gòu)建障礙物在空間C中的映射模型。
1.1.2 機(jī)械臂碰撞檢測(cè)模型及碰撞檢測(cè)算法
在建立CQD的過程中需要對(duì)機(jī)械臂與工作空間離散單元wi之間的碰撞關(guān)系進(jìn)行檢測(cè)。由于采摘機(jī)械臂曲面造型較為復(fù)雜,若以精確的曲面模型為基礎(chǔ)進(jìn)行碰撞檢測(cè),CQD建立速度過慢,難以滿足使用性能要求,因此,需要對(duì)機(jī)械臂模型進(jìn)行一定簡化,以提高碰撞檢測(cè)速度。
方向包圍盒(oriented bounding box,OBB)作為機(jī)械臂常見的簡化模型,其模型擬合程度較高,碰撞檢測(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ǔ)上,通過齊次變換矩陣可以將表示在局部坐標(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所示為簡化后機(jī)械臂碰撞檢測(cè)模型。
圖3 采摘機(jī)器人機(jī)械臂碰撞檢測(cè)三維模型
由于機(jī)械臂精確三維模型被簡化成OBB模型,而且工作空間被分割成各邊長尺寸一致的離散立方體單元,因此,機(jī)械臂與工作空間單元之間的碰撞檢測(cè)問題可以等效為OBB-OBB碰撞檢測(cè)問題。
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ù)庫建立構(gòu)形空間
在線階段機(jī)器人雙目視覺系統(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)。
之后,通過式(1)求取障礙物點(diǎn)云占據(jù)的工作空間單元序號(hào),并通過序號(hào)索引CQD可以快速建立與當(dāng)前障礙物模型對(duì)應(yīng)的明確構(gòu)形空間。
為了避免PRM算法復(fù)雜的碰撞檢測(cè),提高避障路徑規(guī)劃效率,先快速建立當(dāng)前場景構(gòu)形空間, PRM算法在判斷隨機(jī)采樣點(diǎn)是否可行的過程中,只需要進(jìn)行簡單的布爾運(yùn)算。融合了構(gòu)形空間構(gòu)建算法的PRM算法主要步驟包括:
1)在離線階段構(gòu)建CQD。
2)將工作空間障礙物Wobs分割成離散單元集合wobs。
3)根據(jù)離散單元集合wobs中的單元序號(hào)i檢索CQD,建立與當(dāng)前場景對(duì)應(yīng)的明確構(gòu)形空間C。
4)在構(gòu)形空間C中隨機(jī)生成采樣點(diǎn)c,通過計(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)通過歐拉公式計(jì)算獲得集合V中與采樣點(diǎn)c鄰近的節(jié)點(diǎn)集合M。
7)點(diǎn)c與M中任意點(diǎn)m的連線按一定步長分割成若干節(jié)點(diǎn),利用節(jié)點(diǎn)占據(jù)的單元序號(hào)來檢索構(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通過最短歐拉距離連接到集合V中。
10)最后通過Dijkstra算法從集合V中搜尋一條連接點(diǎn)cs和點(diǎn)cg的距離最短路徑,完成避障路徑規(guī)劃。
為驗(yàn)證本文所提算法的有效性和可行性,首先將算法應(yīng)用于一種平面2R機(jī)械臂的避障路徑規(guī)劃,再將算法擴(kuò)展應(yīng)用至高自由度采摘機(jī)器人機(jī)械臂避障路徑規(guī)劃問題。圖5為平面2R機(jī)械臂及其避障路徑規(guī)劃場景俯視圖。
圖5 平面2R機(jī)械臂避障路徑規(guī)場景俯視
根據(jù)表1中的工作空間單元尺寸參數(shù),可將實(shí)驗(yàn)場景中的工作空間劃分成400個(gè)離散工作空間單元。
表1 平面2R機(jī)械臂避障路徑規(guī)劃CQD參數(shù)
通過式(1)可以獲得工作空間障礙物所占據(jù)的5個(gè)單元序號(hào)為115,147,235,333,348。
同樣,可將圖6所示的構(gòu)形空間劃分成5 198個(gè)離散構(gòu)形空間單元。利用遍歷的方法建立CQD數(shù)據(jù)庫后通過工作空間障礙物序號(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ǔ)上,通過PRM算法在構(gòu)形空間中成功搜尋到一條連接代表機(jī)械臂起始位姿和目標(biāo)位姿的無碰撞路徑,該路徑中各節(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。
利用無碰撞路徑節(jié)點(diǎn)的坐標(biāo)驅(qū)動(dòng)2R機(jī)械臂各關(guān)節(jié),可以獲得如圖7所示的機(jī)械臂運(yùn)動(dòng)過程??梢杂^察到平面2R機(jī)械臂從起始位姿運(yùn)動(dòng)到了目標(biāo)位姿,且在此過程中未與工作空間中的障礙物發(fā)生碰撞。
為了進(jìn)一步驗(yàn)證本文所提出的算法在高自由度(degree of freedom,DOF)機(jī)械臂避障路徑規(guī)劃中的有效性,將改進(jìn)算法應(yīng)用至6 DOF機(jī)械臂避障路徑規(guī)劃場景中。
由于6 DOF采摘機(jī)器人機(jī)械臂對(duì)應(yīng)的構(gòu)形空間達(dá)到六維,因此,要繪制六維構(gòu)形空間以描述路徑搜索過程具有相當(dāng)大的困難,這里僅對(duì)避障路徑規(guī)劃完成后機(jī)器人的運(yùn)動(dòng)過程進(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é)角度序列安全,無碰撞地將機(jī)械臂末端執(zhí)行器從起始位姿移動(dòng)到目標(biāo)采摘位姿。
分別利用傳統(tǒng)PRM算法和本文所提出的改進(jìn)算法在當(dāng)前場景下進(jìn)行50次避障路徑規(guī)劃,算法路徑規(guī)劃平均耗時(shí)分別為0.81 s和 0.63 s,相比傳統(tǒng)PRM算法,改進(jìn)算法的耗時(shí)降低了約22.2 %,可見改進(jìn)算法規(guī)劃速度提升較為明顯。值得指出的是,改進(jìn)算法在構(gòu)建CQD的過程中耗時(shí)約為5.36 h,但是由于CQD的建立是在離線階段完成的,不會(huì)對(duì)在線避障路徑規(guī)劃耗時(shí)造成不利影響。
圖8 6 DOF采摘機(jī)器人機(jī)械臂避障運(yùn)動(dòng)狀態(tài)
針對(duì)概率地圖法在避障路徑規(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ī)劃要求。