基于改進PRM的采摘機器人機械臂避障路徑規劃*

鄒宇星， 李立君， 高自成

(中南林業科技大學 機電工程學院，湖南 長沙 410000)

0 引 言

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

本文針對PRM算法需要大量耗時以構建無撞點網絡圖的問題，引進一種快速構形空間構建算法，避免PRM算法在構建網絡圖時所需要的復雜碰撞檢測。實驗結果表明，改進算法能夠快速、有效地為平面2R機械臂和三維空間高自由度機械臂進行避障路徑規劃。

1 PRM算法的融合改進

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

1.1 快速構形空間構建算法

1.1.1 碰撞查詢數據庫的構建

在機械臂路徑規劃研究中，機械臂實際運動所處的空間一般稱為工作空間W，以機械臂各關節角度值為坐標所建立的空間通常稱為機械臂的構形空間C。因此，工作空間W中的障礙物可在空間C中映射為一個不可達區域，通常稱為構形空間障礙物Cobs。空間障礙物Cobs在空間C中的補集通常稱為自由區域Cfree。若空間C中點cs和點cg分別表示機械臂的初始位姿qs和目標位姿qg，那么，機械臂避障路徑規劃就轉化成在空間C中搜索一條路徑Cpath=[cs,c1,…,cn,cg]，且滿足Cpath?Cfree。ci表示構形空間C中某點的第i軸坐標值。

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

該機械臂構形空間C中與q唯一對應的點可以表示為c=[c1,c2,…,cn]，且滿足qi=ci。

將機械臂工作空間W分割成各個方向尺寸均為dw(mm)的離散化單元wi，那么分割后的工作空間可以表示為W，其中單元序號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為離散單元在空間中的坐標值,hn為空間最大坐標值。

對于工作空間任意離散單元wi，通過計算獲得所有與該單元相交的機械臂位姿qwi={q|wi∩Aq≠0}。qwi為工作空間某點狀障礙物處于離散單元wi中時，構形空間C中所對應的障礙物模型,Aq為機械臂A處于位姿q狀態。

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

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

圖2 離散單元在構形空間中的映射模型

若能夠計算獲得任意離散單元wi所對應的qwi，則qwi的集合就構成了碰撞查詢數據庫(collision query database,CQD)，即

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

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

(2)

為構建CQD，以dc為關節步長遍歷機械臂位姿，若某位姿q對應的機械臂三維模型與單元wi發生干涉時，則將該位姿數據q=[q1,q2,…,qn]保存至離散單元序號所對應的數據空間di中。對工作空間W中的所有離散化單元完成上述遍歷操作后，數據空間di的集合∪di就構成了CQD。

在離線階段建立CQD，該數據庫中存儲了機械臂工作空間中任意離散單元在構形空間C中的映射模型，在線階段只需要根據空間障礙物所占據的微小單元對應的序號檢索CQD即可快速構建障礙物在空間C中的映射模型。

1.1.2 機械臂碰撞檢測模型及碰撞檢測算法

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

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

為了在基礎坐標系Tbase中明確描述OBB模型的尺寸和空間位姿，在OBB模型上以相互平行的兩個面的中心o1和o2為原點分別建立局部坐標系Ta和Tb，且局部坐標系Ta和Tb的各坐標軸與OBB模型對應棱邊平行。

在建立了局部坐標系的基礎上，通過齊次變換矩陣可以將表示在局部坐標系中的點的坐標轉換為在基礎坐標系中表示的點的坐標。

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

(3)

式中T為坐標系TA與坐標系TB的齊次變換矩陣,R為姿態變換矩陣,P為位置變換矩陣。

如圖3所示為簡化后機械臂碰撞檢測模型。

圖3 采摘機器人機械臂碰撞檢測三維模型

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

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

圖4 SAT算法在平面OBB模型中的應用

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

1.1.3 索引CQD數據庫建立構形空間

在線階段機器人雙目視覺系統所獲得的障礙物點云模型為避障路徑規劃提供障礙物信息。障礙物點云模型中任一點obsi所占據的工作空間離散單元wi在x軸方向序號sx為

sx=(x-xmin)/dw

(4)

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

之后，通過式(1)求取障礙物點云占據的工作空間單元序號，并通過序號索引CQD可以快速建立與當前障礙物模型對應的明確構形空間。

1.2 融合構形空間的PRM算法

為了避免PRM算法復雜的碰撞檢測，提高避障路徑規劃效率，先快速建立當前場景構形空間， PRM算法在判斷隨機采樣點是否可行的過程中，只需要進行簡單的布爾運算。融合了構形空間構建算法的PRM算法主要步驟包括：

1)在離線階段構建CQD。

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

3)根據離散單元集合wobs中的單元序號i檢索CQD，建立與當前場景對應的明確構形空間C。

4)在構形空間C中隨機生成采樣點c，通過計算求得該采樣點所占據的構形空間單元ck及其序號k。

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

6)通過歐拉公式計算獲得集合V中與采樣點c鄰近的節點集合M。

7)點c與M中任意點m的連線按一定步長分割成若干節點，利用節點占據的單元序號來檢索構形空間C以確定采樣點c與點m之間連線e是否滿足條件e∈Cfree。

8)以步驟(7)的方法遍歷集合M中的所有點，并在與采樣點c相關聯的數據空間中存儲M中所有滿足條件e∈Cfree的節點序號。

9)當集合V中點的個數達到設置的閾值時，將機械臂的初始構形qs和目標構形qg在構形空間C中所對應的點cs和點cg通過最短歐拉距離連接到集合V中。

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

2 仿真結果與分析

2.1 平面2R機械臂避障路徑規劃

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

圖5 平面2R機械臂避障路徑規場景俯視

根據表1中的工作空間單元尺寸參數，可將實驗場景中的工作空間劃分成400個離散工作空間單元。

表1 平面2R機械臂避障路徑規劃CQD參數

通過式(1)可以獲得工作空間障礙物所占據的5個單元序號為115，147，235，333，348。

同樣，可將圖6所示的構形空間劃分成5 198個離散構形空間單元。利用遍歷的方法建立CQD數據庫后通過工作空間障礙物序號對其進行索引，即可獲得所有構形空間障礙物單元序號以及單元總個數，其中單元總數為1 713。

圖6 利用PRM算法在構形空間搜尋避障路徑

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

圖7 平面2R機械臂的避障運動

圖中字母S和G表示的構形空間單元分別對應工作空間中平面2R機械臂起始位姿和目標位姿。

在此基礎上，通過PRM算法在構形空間中成功搜尋到一條連接代表機械臂起始位姿和目標位姿的無碰撞路徑，該路徑中各節點的坐標可以構成一組關節角度序列。PRM算法主要控制參數為:算法迭代次數閾值為500，路標地圖節點數為100，節點連接距離閾值為80，節點連線分割距離為50。

利用無碰撞路徑節點的坐標驅動2R機械臂各關節，可以獲得如圖7所示的機械臂運動過程。可以觀察到平面2R機械臂從起始位姿運動到了目標位姿，且在此過程中未與工作空間中的障礙物發生碰撞。

2.2 6自由度采摘機器人機械臂避障路徑規劃

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

由于6 DOF采摘機器人機械臂對應的構形空間達到六維，因此，要繪制六維構形空間以描述路徑搜索過程具有相當大的困難，這里僅對避障路徑規劃完成后機器人的運動過程進行可視化操作。表2為6DOF機械臂避障路徑規劃相關實驗參數。

表2 6 DOF機械臂避障路徑規劃CQD參數

如圖8所示，為利用本文算法所獲得的6自由度機械臂避障運動狀態。可以看出，本文算法所求得的關節角度序列安全，無碰撞地將機械臂末端執行器從起始位姿移動到目標采摘位姿。

分別利用傳統PRM算法和本文所提出的改進算法在當前場景下進行50次避障路徑規劃，算法路徑規劃平均耗時分別為0.81 s和 0.63 s，相比傳統PRM算法,改進算法的耗時降低了約22.2 %，可見改進算法規劃速度提升較為明顯。值得指出的是，改進算法在構建CQD的過程中耗時約為5.36 h，但是由于CQD的建立是在離線階段完成的，不會對在線避障路徑規劃耗時造成不利影響。

圖8 6 DOF采摘機器人機械臂避障運動狀態

3 結 論

針對概率地圖法在避障路徑規劃中存在的不足，本文通過融入快速構形空間構建算法對其進行了改進。結果表明，本文的改進算法相比傳統PRM算法速度提高22.2 %，能夠滿足復雜環境下機械臂的實時避障路徑規劃要求。