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

趙 凱，儲(chǔ)澤楠，張修太

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

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

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

1 背景算法

1.1 蒙特卡洛算法

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

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

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

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

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

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

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

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

1.2 快速搜索隨機(jī)樹(shù)(RRT)算法

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

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

①構(gòu)造一棵隨機(jī)樹(shù)R，并將起點(diǎn)Ps作為樹(shù)的第一個(gè)節(jié)點(diǎn)。

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

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

④生成新的隨機(jī)樹(shù)節(jié)點(diǎn)。給定步長(zhǎng)h，產(chǎn)生新的節(jié)點(diǎn)

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

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

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

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

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

2.1 基本思想

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

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

2.2 算法步驟

具體來(lái)說(shuō)，算法包括非實(shí)時(shí)部分和實(shí)時(shí)部分，非實(shí)時(shí)部分離線完成一次即可，實(shí)時(shí)部分可支持在線進(jìn)行路徑規(guī)劃。整個(gè)算法的步驟歸納如下:

2.2.1 非實(shí)時(shí)部分

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

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

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

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

2.2.2 實(shí)時(shí)部分

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

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

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

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

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

3 實(shí)驗(yàn)及分析

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

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

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

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

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

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

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

4 結(jié)論

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