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

趙 凱，儲澤楠，張修太

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

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

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

1 背景算法

1.1 蒙特卡洛算法

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

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

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

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

式中:rk是[0，1]上的隨機值和分別為第k個關節的下限值和上限值。

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

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

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

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

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

因多自由度機械臂末端多在三維空間內活動，在這里，將三維空間中的標準RRT 算法敘述如下:

①構造一棵隨機樹R，并將起點Ps作為樹的第一個節點。

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

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

④生成新的隨機樹節點。給定步長h，產生新的節點

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

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

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

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

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

2.1 基本思想

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

圖1 工作空間產生

2.2 算法步驟

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

2.2.1 非實時部分

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

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

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

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

2.2.2 實時部分

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

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

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

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

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

3 實驗及分析

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

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

圖2 蒙特卡洛算法產生的工作空間

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

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

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

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

4 結論

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