工作空間限制下的機械臂路徑規(guī)劃方法?

趙 凱，儲澤楠，張修太

(1.安陽工學(xué)院計算機科學(xué)與信息工程學(xué)院，河南 安陽 455000；2.安陽工學(xué)院電子信息與電氣工程學(xué)院，河南 安陽 455000；3.安陽市異構(gòu)大數(shù)據(jù)分析與處理重點實驗室，河南 安陽 455000)

隨著科技的進步，機械臂作為一種有力的輔助工具，被廣泛應(yīng)用于工業(yè)和康復(fù)領(lǐng)域[1]。一般情況下，機械臂被安裝在固定的底座上，完成固定的工作。然而，隨著機械臂可從事的工作越來越多，有些工作內(nèi)容具有不確定性的，這就對機械臂能否順利完成該工作提出了要求。每臺機械臂因機械設(shè)計不同，能夠工作的范圍也不同。因此，確定機械臂的工作范圍非常重要，也非常有助于幫助機械臂完成不確定性的工作，如人機對弈[2]。機械臂的工作范圍又稱為工作空間或能力地圖，很多學(xué)者都對其進行了深入的研究。Porges 等[3]對空間機器人的能力地圖進行了建模和分析，并對幾種特定機械臂的工作空間進行了可視化和存儲。對機械臂工作空間的建模包括幾何方法、理論分析和數(shù)值方法。其中，數(shù)值方法因靈活方便，可操作性強，是目前的研究的趨勢。蒙特卡洛方法是其中一種較為常用的數(shù)值方法。Peidró 等[4]基于高斯增長提出了一種改進的蒙特卡洛方法來獲取機器人的工作空間，以改進傳統(tǒng)蒙特卡洛方法依靠增加隨機點數(shù)提高精度的缺點。Chaudhury 等[5]基于蒙特卡洛方法獲取固定的工作空間，結(jié)合梯度下降方法設(shè)計多自由度并行機械臂。其他的眾多學(xué)者針對工作空間的建模和分析也提出了一些其他的數(shù)值方法，如分支修剪技術(shù)[6]，神經(jīng)網(wǎng)絡(luò)技術(shù)[7]，六維空間劃分技術(shù)[8]，啟發(fā)式方法[9]等。

工作空間確定以后，就可以進行下一步的工作，如機器人的設(shè)計[4]等。Porges 等[10]將建立的機械臂能力地圖，用于估計機械手的抓取位姿。Isiah 等[11]根據(jù)工作空間分析，解決冗余機械臂的逆運動學(xué)閉環(huán)求解問題。Vahrenkamp 等[12]則利用工作空間解決機械臂的放置問題。Liu 等[13]則根據(jù)關(guān)節(jié)工作空間的劃分，解決機械臂的軌跡規(guī)劃問題。Zacharias 等[14]采用混合方法建立了工作空間模型，并指出可用于機器人的位置放置和任務(wù)規(guī)劃。然而，正如我們所提到的，機械臂的許多工作并不確定，這將涉及到機械臂末端的移動，即笛卡爾空間的路徑規(guī)劃。很明顯，路徑點是否可行，不僅僅受到障礙物影響，還會受制于工作空間。針對這個問題，本文提出了一種基于工作空間和障礙物限制的快速搜索隨機樹(Rapidly-Exploring Random Trees，RRT)的路徑規(guī)劃方法。

1 背景算法

1.1 蒙特卡洛算法

使用蒙特卡洛方法確定機械臂工作空間的思想非常直觀，是一種基于采樣的方法，可以概括為:

式中:Ω代表機械臂在笛卡爾坐標(biāo)系下的工作空間；x∈?3代表機械臂末端使用坐標(biāo)表示的笛卡爾空間位置；q∈?n代表n自由度機械臂關(guān)節(jié)空間中的各關(guān)節(jié)的值，一般為弧度值；qmin和qmax分別表示關(guān)節(jié)值的下限和上限；f(?):?n→?3表示將關(guān)節(jié)空間映射到笛卡爾空間的變換，對于機械臂來說，實際上指的就是正向運動學(xué)變換。

典型的蒙特卡洛算法有如下2 個步驟:

①獲取機械臂各關(guān)節(jié)的活動范圍qmin≤q≤qmax，其中q＝[q1，q2，…，qn]∈?n。在活動范圍內(nèi)，對每個關(guān)節(jié)進行隨機采樣，采樣方法如下:

式中:rk是[0，1]上的隨機值和分別為第k個關(guān)節(jié)的下限值和上限值。

②將式(2)隨機產(chǎn)生的關(guān)節(jié)值進行n自由度機械臂正向運動學(xué)變換，可以得到笛卡爾空間的機械臂末端位置。

③大量重復(fù)步驟①和步驟②，即可得到機械臂的工作空間。

從整個算法過程，很容易知道，該工作空間是以點云的形式存在的。由于隨機產(chǎn)生關(guān)節(jié)向量，蒙特卡洛算法也無法保證生成一個完整的工作空間。因而，要覆蓋整個工作空間，隨機采樣次數(shù)應(yīng)盡可能地多。另外，要完成如路徑規(guī)劃這樣的任務(wù)，產(chǎn)生的點云數(shù)據(jù)需要按照一定的方式進行存儲。例如，將點云數(shù)據(jù)分布到邊長很小的正方體中，以適應(yīng)路徑規(guī)劃等任務(wù)的需要。這樣的分布過程計算量非常大。

1.2 快速搜索隨機樹(RRT)算法

快速搜索隨機樹(RRT)算法是一種基于采樣的路徑規(guī)劃算法，廣泛用于平面、立體以及高維空間的路徑產(chǎn)生。給定起始點和目標(biāo)點，RRT 算法可以快速有效地找到一條無障礙路徑[15]。因其算法簡單，健壯性強，可適用于多種機器人的路徑規(guī)劃，如導(dǎo)航車，機械臂，輪式機器人等。

因多自由度機械臂末端多在三維空間內(nèi)活動，在這里，將三維空間中的標(biāo)準(zhǔn)RRT 算法敘述如下:

①構(gòu)造一棵隨機樹R，并將起點Ps作為樹的第一個節(jié)點。

②在笛卡爾空間中進行采樣，尋找合適的滿足條件的節(jié)點Sample。獲取Sample 的方法一般使用隨機采樣，當(dāng)然也可以應(yīng)用其他方法，如低差異采樣。

③在隨機樹R上尋找最近節(jié)點Pn，滿足‖Sample－Pn‖≤‖Sample－Pr‖，其中Pr是任意一個隨機樹上的節(jié)點。最近的度量‖?‖也可以采用多種度量方式，如馬氏距離，歐氏距離。

④生成新的隨機樹節(jié)點。給定步長h，產(chǎn)生新的節(jié)點

⑤檢測新產(chǎn)生的節(jié)點Pz與最近節(jié)點Pn的連線是否與障礙物重疊。如果重疊，則放棄Pz，返回步驟②；否則將Pz作為新節(jié)點加入隨機樹R中，進入步驟⑥。

⑥檢查新節(jié)點Pz與終點Pe之間的距離是否滿足給定的閾值。如果滿足閾值，則路徑規(guī)劃完成；否則，返回步驟②。

RRT 算法雖然不能保證生成的路徑是最優(yōu)的，但尋找速度比較快。標(biāo)準(zhǔn)RRT 算法雖然也采用了隨機的采樣方法，但采樣的位置為整個笛卡爾空間，并不適用于有工作空間限制的多自由度機械臂的末端路徑規(guī)劃。

2 工作空間限制下的RRT 方法

為了使RRT 算法能夠適用于帶有工作空間限制的多自由度機械臂的末端路徑規(guī)劃，需要為工作空間的生成和存儲設(shè)計合適的方法，并對RRT 進行改進。

2.1 基本思想

本文提出的工作空間建立方法與蒙特卡洛方法不同，基本思想如圖1 所示。將整個空間離散化為許多小正方體，如圖1(a)所示。其中任意一個小正方體中包含三個點，A，B，M，其中M為線段的中點，為小正方體任意一條空間對角線，如圖1(b)所示。對多自由度機械臂來說，A，B，M三點中任意一點不可達(dá)，則該小正方體被標(biāo)記為不可達(dá)空間，反之則為可達(dá)，如圖1(c)所示。所有可達(dá)小正方體就構(gòu)成了機械臂的能力地圖，如圖1(d)所示。

圖1 工作空間產(chǎn)生

2.2 算法步驟

具體來說，算法包括非實時部分和實時部分，非實時部分離線完成一次即可，實時部分可支持在線進行路徑規(guī)劃。整個算法的步驟歸納如下:

2.2.1 非實時部分

步驟1:將整個既定空間按照要求離散為小正方體，數(shù)量為D。既定空間的獲取可以根據(jù)機械臂連桿和關(guān)節(jié)的總長度確定，也可根據(jù)工作環(huán)境確定。

步驟2:獲取所有小正方體頂點序列A＝{A1，A2，…，AD}和B＝{B1，B2，…，BD}，并獲取中點序列

步驟3:對集合A，B，M中的元素進行遍歷，求解多自由度機械臂的逆運動學(xué)，對于無法獲得逆運動學(xué)解的元素進行標(biāo)記。第k小正方體中，Ak、Bk、Mk中任意一點無法求出逆運動學(xué)解，則整個小正方體標(biāo)記為不可達(dá)，從工作空間中移除。剩余的所有小正方體就被作為機械臂的工作空間。

步驟4:去掉不可達(dá)小正方體后，所有可達(dá)小正方體的個數(shù)為E。將E個小正方體的頂點和中點構(gòu)成一個新的集合Ξ＝{A1，A2，…，AE，B1，B2，…，BE，M1，M2，…，ME}，該集合將作為RRT 算法的搜索空間。

2.2.2 實時部分

步驟5:給定起點Ps和終點Pe，檢查兩點是否在工作空間內(nèi)。具體做法為遍歷集合Ξ，檢查Ps和Pe三個坐標(biāo)值是否落在某個可達(dá)小正方體內(nèi)部。

步驟6:構(gòu)造一棵隨機樹R，并將起點Ps加入樹中，隨機給定Ξ中元素的索引，得到采樣值Sample。

步驟7:在隨機樹R中查找與Sample 的歐氏距離最小的節(jié)點Pn。并給定步長h，生成新的隨機樹節(jié)點Pz。

步驟8:檢查新節(jié)點Pz與Pn的連線是否與障礙物重疊。為保證算法的通用性，對所有障礙物采用球形包絡(luò)。如果重疊，則放棄Pz，返回步驟6；否則將Pz作為新節(jié)點加入隨機樹R中，進入步驟9。

步驟9:給定閾值ε，失敗次數(shù)λ，檢查終止條件①‖Pe－Pz‖≤ε，②λ≤20 000。如果兩個條件均滿足，則路徑已經(jīng)找到；如果僅滿足條件②則轉(zhuǎn)入步驟7 繼續(xù)查找；如果不滿足條件②則路徑尋找失敗。

3 實驗及分析

為驗證算法的有效性，利用機器人工具箱[16]中的機械臂對算法進行了測試。該機械臂為PUMA560，有6 個自由度。為方便對比，本文還實現(xiàn)了蒙特卡洛算法和標(biāo)準(zhǔn)的RRT 算法。

如圖2(a)所示，蒙特卡洛算法對每個關(guān)節(jié)隨機采樣，使用正向運動學(xué)求解后，得到了共27 000 點的點云數(shù)據(jù)，為了方便觀察和對比，圖2(b)給出了整個工作空間在XY平面上的投影，是一個近似的圓環(huán)形狀。周圍的空白區(qū)域是由于機械臂本身的限制導(dǎo)致不可達(dá)區(qū)域，中間的空白區(qū)域是機械臂基座的放置位置，因距離基座和本體太近而不可達(dá)。

圖2 蒙特卡洛算法產(chǎn)生的工作空間

在執(zhí)行本文提出方法的實驗中，環(huán)境空間假定為長寬高均2 m 的大正方體，設(shè)定離散化的小正方體的棱長為0.1 m，如圖3(a)所示。如果想獲取更高的精度，可減小小正方體的棱長。通過求解逆運動學(xué)，將不可達(dá)的小正方體去除，得到圖3(b)所示的工作空間。為了方便對比，將工作空間投影到XY平面上，如圖3(c)所示。

圖3 本文方法生成的工作空間

對比圖2(b)和圖3(c)，可以看出，在XY平面上的投影相差不大，造成差別的主要原因是本文的方法使用的基本元素是小正方體，而蒙特卡洛算法使用的是點云數(shù)據(jù)。嚴(yán)格來說，蒙特卡洛算法的精度更高，但計算成本偏高，而且點云格式的數(shù)據(jù)也不適合接下來的基于采樣的路徑避障和規(guī)劃算法。

有了工作空間后，就可以執(zhí)行路徑的實時規(guī)劃。為了公平對比，兩種搜索算法的步長h均設(shè)定為0.05 m，新節(jié)點與終點之間的閾值ε也均設(shè)置為0.05 m。如圖4(a)所示，本文所提出的基于工作空間限制的RRT 算法的搜索空間，實際是所有小正方體的頂點和中點的集合Ξ，這將最大限度地確保搜索的采樣值和新節(jié)點都在工作空間內(nèi)部。標(biāo)準(zhǔn)的RRT 算法的搜索空間是一個棱長為2 的空間正六面體，因而觀察圖4(b)，搜索的節(jié)點很多都超出了工作空間的范圍，甚至有的節(jié)點超出了設(shè)定的整個環(huán)境空間。

4 結(jié)論

本文為完成多自由度機械臂末端的路徑規(guī)劃任務(wù)，考慮機械臂工作空間的限制，提出了一種帶有工作空間限制的RRT 算法。在產(chǎn)生工作空間時，首先將整個工作環(huán)境進行離散，然后使用逆運動求解確定工作空間。這種方法所確定的工作空間可以很容易地應(yīng)用采樣型的路徑規(guī)劃算法，如RRT 等。通過實驗對比，我們發(fā)現(xiàn)這種方法與蒙特卡洛算法產(chǎn)生的工作空間相差不大，但計算量小，且在路徑規(guī)劃時能有效避免搜索到的節(jié)點超出工作空間，確保機械臂路徑規(guī)劃的成功。