利用位能場規劃以機械手臂傳遞之路徑

全文

(1)國立交通大學資訊科學與工程研究所碩士論文. 利用位能場規劃以機械手臂傳遞之路徑 Potential Based Path Planning for an Object Convey by Multiple Manipulators. 研究生：劉開國指導教授：莊仁輝. 教授.

(2) 利用位能場規劃以機械手臂傳遞之路徑 Potential Based Path Planning for an Object Convey by Multiple Manipulators. 研究生：劉開國. Student：Kai-Guo Liu. 指導教授：莊仁輝. Advisor：Jen-Hui Chuang. 國立交通大學資訊科學與工程研究所碩士論文. A Thesis Submitted to Institute of Computer Science and Engineering College of Computer Science National Chiao Tung University in partial Fulfillment of the Requirements for the Degree of Master in Computer Science June 2006 Hsinchu, Taiwan, Republic of China. 中華民國九十五年六月.

(3) 利用位能場規劃以機械手臂傳遞之路徑. 研究生：劉開國. 指導教授：莊仁輝博士. 國立交通大學資訊科學與工程研究所. 摘要. 本論文針對三維空間中的多機械手臂傳遞物體之路徑規劃問題提出一演算法。該演算法在工作空間中，利用三度空間的廣義位能場模型計算障礙物對手臂所產生之推斥力，以保證手臂或物體前進時所經過的路徑不會與障礙物互相碰撞。由於我們的方法完全是利用工作空間中的資訊，不需透過大量計算將相關資訊轉到組態空間作處理，因此規劃速度相當快速。我們在空間中加入可移動的物體，並且利用數隻手臂以互相傳遞的方式，來將物體搬運到較遠的終點平面或達成一些單機械手臂無法完成的任務。. ii.

(4) Potential Based Path Planning for an Object Convey by Multiple Manipulators Student: Kai-Guo Liu. Advisor: Dr. Jen-Hui Chuang. Institute of Computer Science and Engineering College of Computer Science National Chiao Tung University. ABSTRACT An algorithm for path planning of an object conveyed by multiple manipulators is presented in this thesis. The proposed algorithm utilizes a generalized potential model to evaluate repulsion between manipulators and obstacles and between object and obstacles in workspace, so collision avoidance of the planned path can be guaranteed. Without converting information in workspace into C-space, which is computationally expensive, the proposed path planning approach is very efficient. We add a movable object into the workspace. By using the manner of convey between several manipulators, we can transport the object to a remote goal plane, and also make possible some tasks which can not accomplish by a single manipulator.. iii.

(5) 致謝感謝指導教授莊仁輝老師在研究所兩年的耐心指導，讓我在研究所生涯中得到很多的協助以及指導，在此深表感謝。而除了莊教授之外，我尚要感謝實驗室三位學長：林建州、林泓宏、高肇宏，有諸位學長的照顧以及專業方面的琢磨，才能促成此篇論文的順利完成，而同學詹庭瑋、黃仁信、江柏頡、高丈淵、吳思慧、許志高亦對我在完成論文的過程中提供了不少的建議以及幫助。在此我還要感謝親愛的父母親在生活上的支持，以及精神上的慰藉，讓我在繁忙的工作以及壓力下，能夠心無旁鶩，專心在自己的研究上，所以在此我特別將此論文的成果分享給所有支持我的人，也致意給我身邊的所有人。. iv.

(6) 目錄目錄................................................................................................................................v 表目錄...........................................................................................................................vi 圖例目錄......................................................................................................................vii 第一章研究背景與動機............................................................................................1 1.1 研究背景與動機..............................................................................................1 第二章背景............................................................................................................3 2.1 路徑規劃演算法..............................................................................................3 2.1.1 路線圖法...............................................................................................4 2.1.2 細胞切割法...........................................................................................4 2.1.3 數學規劃法...........................................................................................5 2.1.4 次要目的圖形法...................................................................................5 2.1.5 位能場模型法.......................................................................................6 2.1.6 廣義位能場模型..................................................................................7 2.2 多手臂傳遞物體之回顧..................................................................................8 第四章三度空間多機械手臂傳遞路徑規劃..........................................................17 4.1 機械手臂工作環境設定...............................................................................17 4.2 單手臂路徑規劃演算法...............................................................................19 4.2.1 首節前進............................................................................................21 4.2.2 移動平衡調整....................................................................................23 4.2.3 轉動平衡調整....................................................................................25 4.2.4 機械手臂末兩節平衡調整................................................................26 4.3 多手臂傳遞系統規劃....................................................................................28 4.3.1 傳遞系統第一階段工作....................................................................29 4.3.2 傳遞系統第二階段工作....................................................................30 4.3.3 傳遞系統第三階段工作....................................................................32 4.4 基座吸引力...................................................................................................34 4.5 碰撞偵測與處理............................................................................................37 第五章實驗結果與問題討論..................................................................................40 5.1 移動平衡與轉動平衡...................................................................................40 5.2 基座吸引力...................................................................................................47 5.3 多機械手臂範例實測...................................................................................50 5.4 運算複雜度與效能.......................................................................................72 5.5 問題與討論....................................................................................................80 第六章結論..............................................................................................................83. v.

(7) 表目錄表 5.1 表 5.2 表 5.3 表 5.4 表 5.5 表 5.6. 例一的動作資訊統計 ....................................................................................72 例二的動作資訊統計 ....................................................................................74 例三的動作資訊統計 ....................................................................................76 例四的動作資訊統計 ....................................................................................77 例五的動作資訊統計 ....................................................................................78 例七的動作資訊統計 ....................................................................................79. vi.

(8) 圖例目錄圗 3.1 在三維空間的一個多邊型平面 S。 .............................................................10 圗 3.2 三維空間中的一點、線段 C（圖 3.1 中多邊形 S 的其中一邊）及包含多邊形 S 的平面 Q。 .................................................................................................11 圗 3.3 牛頓位能場在三維空間中的例子。(A)為一個正立方體模型。根據牛頓位能場模型的假設，該正方體的表面均勻帶著正電荷。(B)為當 Z = 0 時，圖示 wΔ ( x 2 + y 2 ) / 2 面 OABC 所在處，其位能場分布的等位線圖。(C)為當 X = Y，. 時，圖示面 OBDE 所在處，其位能場分布的等位線圖。圖中顏色越深的部分，其位能值越小。..........................................................................................13 圖 4.1 機械手臂工作空間示意圖。 ........................................................................18 圖 4.2 物體的取樣點（M = 7, N = 2），M 和 N 可視情況作改變。.........................18 圖 4.3 三維自由度關節的例子 – 球形關節。 .......................................................19 圖 4.4 單一手臂路徑規劃流程圖，其中 N 為手臂的總節數。............................21 圖 4.5 首節前進的規劃流程圖。 ............................................................................22 圖 4.6 手臂受到目標平面（GOAL PLANE）的吸引力，並在以引力為法向量的平面（V_PLANE）上作調整。 ..............................................................................23 圖 4.7 首節移動調整的工作流程圖。 ....................................................................24 圖 4.8 五階二分搜尋的示意圖。 ............................................................................25 圖 4.9 轉動調整的工作流程圖。 ............................................................................26 圖 4.10 最末兩節的調整示意圖，其中圓 O 是可活動的區域軌跡，Ｔ是旋轉軸。 ..............................................................................................................................27 圖 4.11 末兩節調整的工作流程圖。 ......................................................................27 圖 4.12 多手臂傳遞系統工作流程圖。 ..................................................................28 圖 4.13 手臂由數個瓶頸面導引至瓶頸面 B4 的示意圖。 .....................................29 圖 4.14 手臂取得被傳遞物的工作流程圖 ..............................................................30 圖 4.15 手臂受到目標點（GOAL POINT）的吸引力，並在以引力為法向量的平面（V_PLANE）上作調整。 ..................................................................................30 圖 4.16 傳遞系統第二階段工作流程圖 ..................................................................32 圖 4.17 傳遞系統第三階段工作流程圖 ..................................................................33 圖 4.18 一個手臂無法連結至基座的例子 ..............................................................34 圖 4.19 長方形場景的骨架示意圖，虛線部分就是整個場景的骨架，也是位能最低點的軌跡..........................................................................................................35 圖 4.20 （A）未受基座吸引力影響前的結果（B）加上基座吸引力後的結果 ..35 圖 4.21 在基座吸引力的影響下，原本單純推斥力所形成的位能最低點軌跡已經 vii.

(9) 整個朝基座偏移。..............................................................................................36 圖 4.22 交點偵測示意圖 ............................................................................................39 圖 4.23 碰撞偵測流程圖 ............................................................................................39 圖 5.1 環境中有一個在角落中的小方塊，將會受力影響而運動。 ....................40 圖 5.2 移動平衡調整步驟：(A)長方體尚未調整前 (B)第一次調整，共十一步 (C) 第一次調整後的結果 (D)第二次調整，共六步 (E)第二次調整的結果 (F)第三次調整，共六步 (G)第三次調整的結果(H) 第四次調整，共六步 (I) 第四次調整的結果 (J)最後結果的鳥瞰圖，顯示長方體到達場景的正中央。 ............41 圖 5.2(續).....................................................................................................................42 圖 5.2(續).....................................................................................................................43 圖 5.3 轉動平衡調整步驟：(A)長方體尚未調整前(B)第一次調整，共十一步 (C) 第一次調整後的結果 (D)第二次調整，共九步 (E)第二次調整的結果 (F)第三次調整，共九步 (G)第三次調整的結果 (H)第四次調整，共八步 (I)第四次調整的結果 (J)第五次調整，共六步 (K)第五次調整的結果 (L)最後結果的鳥瞰圖，顯示長方體與場景相互平行。..................................................................44 圖 5.3(續).....................................................................................................................45 圖 5.3(續).....................................................................................................................46 圖 5.4 移動調整與轉動調整相互配合調整之示意圖。 ........................................47. G 在參數不同的情況下，對每節長度為 3（L = 3）的機械手 k nL 臂之影響。..........................................................................................................48 圖 5.5(續).....................................................................................................................49 圖 5.6 例一中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖 (D)斜角視圖 ...................................................................................................50 圖 5.7 兩隻手臂將物體從左側搬運到右側的細部過程 ........................................51 圖 5.8 例二中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................52 圖 5.9 物體從Ｕ型通道左側被搬運到右側終點平面的細部過程 ........................53 圖 5.9(續).....................................................................................................................54 圖 5.9(續).....................................................................................................................55 圖 5.10 例三中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................56 圖 5.11 V 型物體從長方型通道左側被搬運到右側終點平面的細部過程...........56 圖 5.11(續) ...................................................................................................................57 圖 5.11(續) ...................................................................................................................58 圖 5.12 例四中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................58 圖 5.13 雙手臂交替將物體從左側搬運到右側終點平面的細部過程 ..................59 圖 5.5 吸引力 F =. viii.

(10) 圖 5.13(續)...................................................................................................................60 圖 5.14 單手臂獨自將物體從左側搬運到右側終點平面的細部過程 ..................60 圖 5.14(續)...................................................................................................................61 圖 5.14(續)...................................................................................................................62 圖 5.15 例五中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................63 圖 5.16 手臂在開放與封閉空間中穿梭，將物體搬運到右側終點平面的細部過程 ..............................................................................................................................64 圖 5.16(續)...................................................................................................................65 圖 5.16(續)...................................................................................................................66 圖 5.17 例六中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................67 圖 5.18 手臂在不同障礙物間移動的細部過程 ......................................................67 圖 5.18(續)...................................................................................................................68 圖 5.19 例七中機械手臂以及物體的初始位置資訊 (A)前視圖 (B)俯視圖 (C)側視圖(D)斜角視圖.....................................................................................................69 圖 5.20 機械手臂在障礙物間移動的細部過程 ......................................................69 圖 5.20(續)...................................................................................................................70 圖 5.21 機械手臂在障礙物間移動的細部過程(不同視角)....................................71 圖 5.22 一個通道由窄突然變寬的例子 ..................................................................80 圖 5.23 一個手臂姿勢不自然的例子 ......................................................................81 圖 5.24 兩個障礙物凸出角度不同的例子 ..............................................................81 圖 5.25 物體交接點的一個示意圖 ..........................................................................82. ix.

(11) 第一章. 研究背景與動機. 1.1 研究背景與動機路徑規劃問題起源於行動機器人（mobile robot）在空間中的移動，目的是找出行動機器人從初始狀態到最終狀態所經過的路徑，而在一般研究中所尋找的路徑通常分為兩種，一種是最短路徑，另一種則是最安全路徑。最短路徑之規劃，主要是在可移動的空間中，尋找出從起點至終點距離最短的路徑，以達到節省時間或資源的目的；最安全路徑之規劃，則是讓機器人在移動的過程中，和障礙物保持最遠或最適當的距離，而本論文中所要找的路徑，雖然不是一般所認為的最安全路徑，但是是在某些條件限制下的最安全路徑，此限制是為了確保機器人可以朝目標前進，所以我們只在垂直於前進方向的平面上，找出最安全的位置。路徑規劃除了在所尋找的路徑有不同類型之外，在何種空間中規劃也不只有一種選擇，而主要的規劃空間大致分為兩種，一種是工作空間（work space），另一種為組態空間（configuration-space，簡稱 C-space）。在工作空間中進行的路徑規劃方法，主要是仰賴空間中座標的資訊，如三度空間的工作空間就包含了 X、. Y、Z 三個座標的資訊，利用這些座標資訊所描述物體和障礙物的位置來做路徑規劃；而在組態空間中的每一個組態，都代表機器人不同的狀態，如果是圓形機器人，我們可以單純用一點來描述它，如果是一隻機械手臂，我們則可以用機械手臂每個關節的角度所組成的空間來描述它。所以在組態空間中的一條軌跡，就代表機器人一連串的動作，而組態空間中的禁止區域（forbidden regions），則對應著物體在工作空間中被障礙物阻擋的區域，表示機器人在這些狀態下是會和障礙物產生碰撞的，所以在組態空間中的路徑規劃，是在禁止區域以外的自由空間（free space）中進行，也就是先屏除了會和障礙物碰撞的因素。路徑規劃除了最常被討論的行動機器人之外，機械手臂也常被討論到。機械 1.

(12) 手臂乃由數個剛體連結而成，且有一個固定基座（base），一般常以機械手臂的末端操作點（end-effector）的位置作為整個手臂位置的參考點。當給定一個末端操作點的位置，若機械手臂的自由度大於其工作空間的維度時，此機械手臂都會有無限多組機械手臂組態的解，這種手臂稱為 redundant manipulator；相對地，機械手臂的自由度若小於工作空間維度，則稱為 non-redundant manipulator。不難發現 redundant manipulator 的路徑規劃，必須在無限多組機械手臂組態的解中尋找一組最佳解，所要付出的代價也相對地要比 non-redundant manipulator 高出許多。因此在組態空間中要處理 redundant manipulator 的路徑規劃，就必須先付出龐大的資源與代價，來將其組態轉換出來，但卻也因為 redundant manipulator 擁有高自由度的關節，使得它在動作的表現上更為靈活。本論文主要目的是要討論如何利用高自由度的機械手臂，在三度空間中做物體傳遞工作，將可移動的物體安全送達終點。在此我們假設物體是無法自己移動的，必須藉助機械手臂的提攜才能移動。在機械手臂長度有限的情況下，可能無法單獨將物體搬運到終點，但是透過多隻機械的手臂相互傳遞，便可將物體傳送至較遠的終點。再者，機械手臂也有可能因為其他幾何的限制，例如手臂太大，無法穿過通道，此時也可以透過手臂在通道的另一端接應，來將物體傳送至終點。在物體傳送或機械手臂運動的過程中，我們引用廣義位能場來為物體和機械手臂在工作空間裡找出一條無碰撞的安全路徑，並為理論與現實上的最安全路徑，提供一座橋樑。根據上述的研究目的與研究問題，以下列出本研究的研究步驟，並說明研究的論文結構。第二章中將探討以及回顧基本路徑規劃方法；第三章將概述我們所採用的廣義位能場；第四章會針對多機械手臂傳遞的系統，做演算法的詳盡解說；第五章將列出一些實驗和模擬的結果，並做分析與討論；第六章則是結論以及未來的展望。. 2.

(13) 第二章. 背景. 2.1 路徑規劃演算法路徑規劃所指的是在工作空間（workspace）中從起點到終點間尋找一條不會與周圍發生碰撞的安全路徑，除了要求路徑的安全性之外，我們還希望所找出來的能夠是距離最短、計算最有效率的路徑。1970 年代末期，Lozano-Pérez 和 Wesley 在[1]裡提出了一個完全演算法（complete algorithm），他們將障礙物所存在的空間，以多邊形或是多面體來表示，形成一個禁止區之網路圖，而機械手臂的路徑規劃問題便轉變為避開這些點，在圖中找尋一條安全的路徑，這個方式被認為是 C-space 的前身。到了 1983 年，Lozano-Pérez 和 Wesley 在[2]裡首度引進了機械人組態空間（configuration space，簡稱 C-space）的觀念，影響了後來這領域的研究方向。組態空間是機械手臂在工作空間中可能處於的所有狀態所形成的空間，這個空間的維度由手臂關節數目及各關節的自由度個數決定，因此機械人的自由度即為組態空間的維度。在組態空間中，機械手臂的組態被表示成一個點，一個點即代表一個動作組態。如此，組態空間裡的禁止區域即為機械人碰撞障礙物的組態的集合，而禁止區以外的自由空間（free space）則對應機械人所有安全的動作組態的集合。因此整個避碰問題的核心便轉變成在組態空間的自由空間裡，找一條連接起點組態和終點組態的路徑。如今在路徑規劃這個領域，大部分的方法都是將問題轉換到組態空間處理，但仍然有些是直接在工作空間進行。基本上路徑規劃的方法可分為五大類:骨架法（skeleton）、細胞切割法（cell decomposition）、數學規劃法（mathematical programming）、次要目的圖形法（subgoal graph）以及位能場模型法。底下針對這五大類以及其他各式方法作個概述。. 3.

(14) 2.1.1 路線圖法路線圖法（roadmap）是把原有的自由空間（free space）轉換成為某些線段所組成的網路，也就是路線圖，依據這些路線圖找出所要的路徑。因此我們所要找的路徑就會被侷限在這些網路線段當中，使得原先的路徑規劃問題被巧妙地轉化成圖形的搜尋問題[3]。1996 年 Kavraki 等人，在[4]裡提出了 Probabilistic Roadmap（PRM），很成功的把其應用範圍提高至高自由度的機械手臂，此演算法在 C-space 裡利用亂數，灑下某種數量的點，每一個點即指一個手臂的動作組態，然後區域性地（locally）把一些沒有碰到障礙物的點連起來，形成一個圖（graph），而之後在這圖裡找一條連接起點和終點的路徑，來描述機械手臂的整個運動過程。雖然這方法可以在數十秒內建好圖，在不到一秒的時間裡找到一條路徑，但是機率的操控的因素太大，沒有辦法保證能夠順利到達目的，而求出來的路徑可能需要再平順化（smooth），且此方法只可用於靜態的工作空間。一般而言，路線圖法可分為三個步驟：首先將物體起始位置移至位於路線圖上的一點開始位置，接著再將物體最終位置移至路線圖上的另一點為目的地，最後透過我們所建構出來的路線網路，找出數個相連的線段，把這兩個點連接起來。其中較著名建構路線圖的方法有：機率路線圖法（probabilistic roadmap, PRMs）、視線可見圖形法（visibility graph）、Voronoi 圖形法（Voronoi diagram）和隨機快速搜尋樹法（RRTs）等。. 2.1.2 細胞切割法 Brooks & Lozano-Pérez 在 1983 年提出的細胞切割法[5]是把自由空間切割成大小不同的細格（cell），將這些切出來的格子依據其相鄰關係組成一條安全的路徑，格子的大小是決定結果的關鍵。位能場應用始 1986 年的[6]，Khatib 創新地利用人工位能場對機械人的排斥力而達到避碰的目的，且能做到即時的運算。雖然這些方法都很有效率，但是這兩種方法均被侷限於 C-space 裡討論，而且只適 4.

(15) 合總自由度二或三的機械手臂，對於總自由度在四或五以上的 redundant manipulator ，這些方法顯然都無法有良好的表現，例如細胞切割法（ cell decomposition）可能會因為細格切割出來的數量過大，導致在高自由度的機械手臂的路徑規劃中效能不彰。雖然之後有很多經驗法則式的演算法對某些缺點做了改良[7]，但是仍然無法保證其效率，且不夠穩定。. 2.1.3 數學規劃法在數學規劃法中，主要利用組態參數和不等式來達到避開障礙物的要求。在 Hwang 等人的研究[8]中，試著將路徑規劃問題的數學最佳化，亦即找出一條從起點到目的地組態的最短長度曲線。但是這些最佳化問題可能是非線性的，轉而由多個不等式組成，因此常運用數值方法來幫助尋找最佳解。此外，還有一種利用線性代數方法，來做機械手臂動作規劃的方式，由 Cartesian 空間中末端操作點的速度（或加速度），以便求得機械手臂各關節所需的轉動變量，稱之為反運動學（inverse kinematics）。一般線性代數式的機械手臂動作規劃方法，大多會利用到 Jacobian matrix，且現今已實際應用到各式不同的機械手臂。Jacobian matrix 可以很方便的對關節變數空間（joint variable space）和末端操作點移動空間（end-effector space）做兩者之間的線性轉換，可以快速求得每個關節所需之轉動變量，而做即時的動作規劃。不過此法必須用在工作空間為一條無碰撞的末端操作點的移動路徑，始可對手臂的關節求得細部的轉速調整，所以屬於動作規劃的控制，而不是路徑規劃的避碰問題。此外任何矩陣都有 singular 的問題，這一直都是動作規劃中有待解決的重點。. 2.1.4 次要目的圖形法次要目的圖形法是利用次要目的（subgoal）來幫助找到路徑，而次要目的即工作空間中重要的中途站，或是組態空間中最重要的組態。不過次要目的並不 5.

(16) 是強迫必須經過的，而是視情況而定。所以次要目的圖形法通常分成兩個階段，分別為全域規劃（global planner）和局部規劃（local planner）。全域規劃目的是要產生一個包含次要目的地的空間，而局部規劃則是用來確認每一個次要目的地的可到達性（reachability）。與細胞分割法相似的是，任兩個次要目的地之間也可以依實際情形需要，再細分為更多的次要目的地。Henning 等人曾在[9]提出蛇行機器人的運行方式，建立一數學規劃模式，取代傳統的機器人運動軌跡。此法在求解大型問題時，成效相當好。. 2.1.5 位能場模型法位能場模型法主要是利用磁場同性相吸、異性相斥的原理，將之應用在路徑的規劃上，藉由目標對機器人產生吸引力以及障礙物對機器人產生推斥力，使得機器人可以在朝著目標前進的同時遠離周圍的障礙物，避免與之發生碰撞。但是使用位能場模型進行路徑規劃卻存在著數個問題，第一是機器人可能會陷在位能區域極小值（local minima）之中的問題，第二是機器人在目標吸引力的影響之下，可能會穿過或碰撞到障礙物（obstacle collision）的問題，第三是機器人因障礙物的排斥力作用而無法到達目標（unreachable goal）的問題。 local minima 形成的主要原因是因為位能的分布存在四周位能都較高的位能谷，當機器人移至其中便會被困在裡頭而走不出來；機器人會穿過、碰撞到物體或是機器人無法到達目標的原因較多，但主要是因為障礙物與目標兩者對機器人的推斥力及吸引力相互干擾所導致的結果。在 local minima 的問題處理上，Laliberté 和 Gosselin 於 1994 年在[10]中把工作空間作骨架（skeleton）抽取形成大致的路徑，再利用離散的位能場計算（discrete potential field）來降低遇到 local minima 機率；Barraquand 和 Latombe 於 1997 年在[11]裡利用力場的負階度（negated gradient）決定機械人的移動方向，當機器人落入 local minima 時，便利用隨機亂數來決定運動的方向。近年來則有 Park 和 Lee 在[12]中根據時間與位置的關係設計 local minima 偵測 6.

(17) 器，若偵測到機器人已落於 local minima 中，便會製造虛擬的位能丘（virtual hill），提高該處的位能以改善 local minima 位能較四周低的情況，讓機器人能夠脫離 local minima。還有 DING 等在[13]中則是利用在障礙物的邊緣產生虛擬的位能場，沿著障礙物邊緣來引導機器人走出 local minima 的位置。在機器人會碰撞到障礙物或無法到達目標的問題上，2000 年 Ge 和 Cui 兩人在[14]中透過設置特殊的位能函數，讓目標位置的位能大小永遠為零，並讓機器人在相當靠近障礙物時，位能大小趨近於無限大，使得目標位置的位能不會因為障礙物在附近而提高，導致機器人可能無法到達目標，且因為機器人在相當靠近障礙物時因位能趨近無限大，使得機器人無法碰撞到障礙物。2002 年 Chuang 等人在[15]中可以讓機械手臂在非常靠近障礙物時，產生趨近於無窮大的推斥力，且透過瓶頸面的設置，將導引的力量與障礙面所產生的推斥力分離，排除其相互干擾的情況；雖然位能場模型法在路徑規劃的應用常會遇到上述的幾種限制，但是它可以在工作空間中直接且快速地規劃路徑，因此位能場模型法在解決路徑規劃問題上仍廣泛的被使用。. 2.1.6 廣義位能場模型在路徑規劃上，除了最短距離（minimum distance）以及最短時間（minimum time）路徑之外，最安全（safest）路徑也是一項用來評量路徑規劃演算法的標準， 1998 年 Chuang 所發表提出的廣義位能場模型[16]，在三維工作空間中，藉由計算廣義位能場中多面體的每個面所產生的推斥力，可以找出障礙物與場景的位能谷底以及位能最小值。簡言之，就是運用推斥力，將運動中的手臂推往力的平衡點，這個平衡點就可視為在當時刻最安全的位置，而由每個時間點所計算出的力平衡點，連結起來的路徑，則可視為最安全路徑，由於本篇論文即利用此方法作為依據，進而延伸到多機械手臂的路徑規劃，因此有關廣義位能場模型的推導及論述，將在下一章作詳盡解說。 7.

(18) 2.2 多手臂傳遞物體之回顧多機械手臂之合作模式通常分成兩種，一種是共持物體，另一種則是傳遞物體，而前者通常是在機械手臂無法單獨支撐或是提起物體時，所必須進行的合作方式，例如一般的鍋子通常就需要兩隻手才能被平穩的移動；而傳遞物體通常是在機械手臂或是附近環境有所限制的情況下，所必須進行的合作模式，例如單隻機械手臂可能因長度不足而無法將物體送至目的地，或是受到地形的阻擋，必須在兩個較獨立的空間做物體的交換。機械手臂會因為長度以及關節數不同，產生支撐強度的問題，也就是如果手臂長度越長，手臂整體的支撐強度就越弱，手臂關節數越多，也同樣會造成整體支撐力的下降，所以在機械手臂長度或關節數受到限制的情況下，在較長或廣的工作空間中，就會有多隻機械手臂的需求。在 1987 年，Faverjon 等人在[17]中便提出多手臂合作的方式，其主要方法是利用簡單的幾何來描述所有物體以及機械手臂，並在障礙物與機械手臂、物體建立一個測量距離的機制，並利用此機制建立組態空間的禁制區域，達到避碰的效果，之後再利用手臂間的相對位置以及方向關係來進行手臂間的合作。以物體的傳遞為例，此論文將已經拿著物體的手臂當作 leader，而要接收物體的手臂當作 follower，在 leader 完成一個步驟之後，follower 便會根據目前抓取點（grasp point）做適當移動，在此 leader 並不會考慮 follower 的存在，所以 follower 的除了目標點並不固定之外，還必須閃躲 leader 所在的位置，所以運算複雜度也較 leader 提高不少。1994 年 Koga 等人也在[18]中提出多機械手臂合作的方法，文中廣泛討論機械手臂重新抓取（regrasp）、共持、傳遞的方法，讓機械手臂的合作更趨靈活，但仍然是在組態空間中做規劃，比較特別的是他們將自由空間再細分為穩定空間、抓取空間，因此必須將多隻手臂同時考量，建立起一個維度相當高的組態空間，因此前至處理量相當龐大。在機械手臂合作的領域上，一般都是在組態空間中做討論，因為組態空間可以規避掉複雜的碰撞問題，並且在前置處理做完之後，可以快速的得到路徑或動作規劃的結果，但相對的在龐大前置運算的代價 8.

(19) 下，卻也有很多不適合的情況，尤其是在即時的路徑規劃。. 9.

(20) 第三章廣義位能場模型及其應用在本章中，我們將介紹在一個三維空間中，用以計算物體推斥力的廣義位能場模型。在參考文獻[19]中，以牛頓位能場（Newtonian potential）模型取代尋找二維空間的距離函數。但由於使用牛頓位能場來計算推斥力時，與物體的邊線或角落接觸並不會導致無限大的推斥力，使種子點傾向往物體的邊線或是角落方向走，故牛頓位能場並不不能保證解決三維空間中物體的避碰問題，因此我們建議採用廣義位能場模型。. 3.1 三維空間的廣義位能場. 圗 3.1 在三維空間的一個多邊型平面 S。. 如圖 3.1，考慮在三維空間的一個平面 S，在點 r 因平面 S 所產生的位能可被定義成下式( m = 1 為牛頓位能場)：. dS. ∫ Rm. ， m≥2. (3-1). S. 其中 R = | r ′ – r |， r ′ ∈ S，整數 m 為位能函數的次方（order）。週界 ΔS 的方向由表面法向量 nˆ 利用右手定則來決定，也就是 nˆ = uˆ × ˆl ，其中 uˆ 沿著 ΔS 的向外法向量方向， ˆl 則沿著切線方向。計算點 r 的位能可分為以下幾個基本步驟：步驟 1：將式(3-1)中位能場積分方程式中的積分元，寫成某一向量函數的表面散 10.

(21) 度（surface divergence）。步驟 2：利用表面發散定理（Surface Divergence Theorem），將二重（面）積分轉成對 ΔS 之線積分。步驟 3：對多邊形週界 ΔS 的每個邊做直線積分，而我們的面積分結果即是將這些線積分加總起來。. 圗 3.2 三維空間中的一點、線段 C（圖 3.1 中多邊形 S 的其中一邊）及包含多邊形 S 的平面 Q。. 多邊形 S 的某一邊線 C 與包含多邊形 S 之平面 Q 的相關幾何量，請參見圖 3.2，其中 r ′ ∈ C。在不失其一般性的原則下，我們假設 d Δ nˆ (r – r ′ ) > 0. (3-2). 以上式子也相當於由 r 到 Q 的距離。. 3.2 推導向量函數假設步驟 1 中的向量函數可被表示為如下面的式子：. f m (R)P 其中 P 是 S 中某點相對於 rQ 的位置向量，而 rQ 表示 r 在 Q 上的投影。. 11. (3-3).

(22) 步驟 1 的目的是要求出 f m (R)使得. 1 = ∇ S ⋅ ( f m (R)P) Rm. (3-4). 在以上的式子中， ∇ S (…)表示相對於面的座標做發散（divergence）的動作。式. (3-4)又可化簡成線性一階常微分方程式： 1 = ∇f m ( R ) ⋅ P + f m ( R)∇ S ⋅ P Rm ˆ ⋅ P) f m′ ( R ) + 2 f m ( R ) = (R. =. P2 f m′ ( R ) + 2 f m ( R ) R. (3-5). 藉由解出上面的微分方程式，我們可以得到下面的式子（將多出來的常數予以忽略）：. f m (R ) =. 1 R 1−m dR 2 ∫ P. log R ⎧ , 2 2 ⎪ R − d =⎨ −1 , ⎪ m −2 2 2 ⎩ ( m − 2) R ( R − d ). m=2 m≠2. (3-6). （ f1 (R)相當於在[20]中用於牛頓位能積分的向量函數。）. 3.3 廣義位能場的解析式根據前面 3.2 小節所導出的 f m (R)，對某些 r' ' = rQ ，R= d 時則 f m (R)會形成有發散的情況。我們令 Sε 表示 S 跟 Q 中以 rQ 為圓心， ε 為半徑的圓相交的部分，那麼由 S 於 r 點所產生的位能可計算如下：. ⎡ dS dS ⎤ f R P dS = ∇ ⋅ + lim ( ( ) ) ⎢ ∫S R m ε →0 ⎢S −∫S S m ∫ R m ⎥⎥ S ε ⎣ ε ⎦ ⎡ α ⎤ α ε pdpdθ ⎥ 2 2 2 ⎢ ˆ ε ε θ ( l ) P u l lim fm = ∫ fm ⋅ d + − +d d +∫∫ m ε →0 ⎢ ∫ ⎥ 0 0 0 (p 2 + d 2 )2 ΔS ⎣ ⎦. (. 12. ).

(23) = ∑ Pi0 ⋅ uˆ i ∫ f m ,i (l i )dl + g m (α ), i. (3-7). Ci. 其中， f m ,i (l i ) = f m ( R = l i2 + d 2 + ( Pi0 ) 2 ),. ⎧ a log d , ⎪ α g m (α ) = ⎨ , ⎪⎩ ( m − 2)d m−2. m=2 m≠2. (3-8). (3-9). 在式(3-8)中， Pi0 是 rQ 與 C i 之間的距離， l i 則是 r 在 Ci 上的投影沿著 ˆli 方向的分量，α 則是當 ε 趨近於 0 時在 S 內部的 Sε 之圓周角。舉例來說，當 α = 2π 時， rQ 在 S 內部，而 α = π 時， rQ 在 S 的邊上。如果 rQ 在 S 中視兩條邊的接點，則 α 就等於這兩邊的夾角。（為求簡明，往後註標 i 將省略）. 圗 3.3 牛頓位能場在三維空間中的例子。(a)為一個正立方體模型。根據牛頓位能場模型的假設，該正方體的表面均勻帶著正電荷。(b)為當 z = 0 時，圖示面 OABC 所在處，其位能場分布的等位線圖。(c)為當 x = y，. wΔ ( x 2 + y 2 ) / 2. 時，圖示面 OBDE 所在處，其位能場分布的等位線圖。. 圖中顏色越深的部分，其位能值越小。. 3.3.1 牛頓位能場（當 m=1 時）在[20]裡證明了在牛頓位能場中當 m = 1 時，得到如下的等式： 13.

(24) ∫ C. R + + l + d ⎡ −1 l + d ⎤ f1 (l )dl = log − − + 0 ⎢ tan R +l P ⎣ P 0 R − ⎥⎦. (3-10). 然而上式位能場並不適合使用來找尋軸的計算力（force）的程序。考慮一個正立方體，根據牛頓位能場模型，我們假設此一正立方體的邊界均勻帶著正電荷（如圖 3.3(a)），並假設正立方體的頂點座標為( ± 10, ± 10, ± 10)。我們可以計算出正立方體內部區域的位能場（參見[20]）。圖 3.3(b)及圖 3.3(c)分別代表當 z = 0 及 x =. y 時，正立方體內部的等位線圖。（由於對稱的緣故，圖 3.3(b)只畫出 x > 0 及 y > 。我們可以輕易觀察 0 的部分。圖 3.3(c)則是只畫出 x > 0，y > 0 及 z > 0 的部分）得到，遵循此位能場的力將會往正立方體的某一邊，或是往角落的方向移動。在路徑規劃的領域當中，我們希望物體能在環境中盡量遠離障礙物，此牛頓位能場的特性並不符合要求，因此我們採用以下所提出的廣義位能場模型。. 3.3.2 廣義位能場模型（當 m>2 時）對於 m=2 以外的其他偶數， f m (l ) 為有理函數，而對於 m 為奇數時， f m (l ) 則可被有理化。因此除了 m=2 外，線積分可由封閉式子來求得。例如，當 P ≠ 0 時， 0. 可得到下面式子：（註標 i 省略）. ∫ C. ∫ C. 1 f 3 (l )dl = 0 Pd. f 4 (l )dl =. 1 2d 2. + ⎡ −1 l − d −1 l d ⎤ ⎢ tan P 0 R − − tan P 0 R + ⎥ ⎦ ⎣. ⎡1 ⎢ 0 ⎣R. 1 − 0 P. (3-11). − ⎡ −1 l + −1 l ⎤ tan tan − ⎢ R0 R 0 ⎥⎦ ⎣. − ⎡ −1 l + −1 l ⎤ ⎤ ⎢ tan P 0 − tan P 0 ⎥ ⎥ ⎦⎦ ⎣. (3-12). 藉由式子(3-7)，我們計算多面體中的每一平面之位能值，並累加起來，便可得到這個多面體所產生的位能場。例如，當 m=3 時，式(3-7)可寫成以下形式：. ∫ R 3 Δ ∑i [Φ 3,i (xi = li , yi , z ) − Φ 3,i (xi = li , yi , z )]+ dS. −. +. S. 14. α z. (3-13).

(25) 在式(3-13)中，對每個 Ci 來講，它的 xi ， yi ， z ( z = d > 0) 將分別沿著 lˆi ， − uˆi 及. nˆ 計算，該座標系的原點則位在 r 對於 Ci 的投影上，而上式中的 Φ 3 ( x, y, z ) 則為：. 1 xz Φ 3 ( x, y , z ) = tan −1 z y x2 + y2 + z2. (3-14). 3.4 推斥力面 S 對單一點電荷 ( x, y , z ) 產生的推斥力可以利用式(3-13)，求其梯度值，如下面的式子所示：. − ∑ ∇i [Φ 3,i (xi = li+ , yi , z ) − Φ 3,i (xi = li− , yi , z )] +. α z2. iz. (3-15). 其中， ∇i 是以 Ci 所在的座標系統為準。因此，推斥力可以利用下列各式子由解析法求得：. ∂Φ 3 y = 2 ∂x ( x + y2 ) x2 + y2 + z2. ∂Φ 3 − x( x 2 + 2 y 2 + z 2 ) = 2 ∂y ( x + y 2 )( y 2 + z 2 ) x 2 + y 2 + z 2. ∂Φ 3 =− ∂z +. tan −1. (3-16). (3-17). xz y x2 + y2 + z2 z2 xy ( x 2 + y 2 ). z ( x 2 + y 2 )( y 2 + z 2 ) x 2 + y 2 + z 2. (3-18). 3.5 廣義位能場在路徑規劃問題的應用根據廣義位能場模型，一個點因多面物體各個面所產生的推斥力可以藉由物 15.

(26) 體表面每個平面所蓄的電荷予以加總求得。在路徑規劃的問題當中，我們常常需要利用廣義位能場模型計算手臂與障礙物之間的斥力，若手臂與障礙物均以平面表示，推斥力的計算將需要複雜的四重積分。為了簡化起見，我們在手臂的表面選取適當的取樣點，以這些取樣點代表手臂，障礙物則仍以平面表示，手臂所受的總推斥力便是所有取樣點所受推斥力的加總，而手臂的合力矩則等於各取樣點所受力矩的總和。我們將在第四章介紹的路徑規劃演算法便是以廣義位能場所求出的合力與合力矩做為依據而進行的。. 16.

(27) 第四章. 三度空間多機械手臂傳遞路徑規劃. 在這章我們要介紹的是在三度空間中，多機械手臂將待傳遞物搬運到終點的整個流程，以及細部的演算法。目的是要將單手臂路徑規劃，推廣到多手臂的應用上，並利用廣義位能場的特性，達到安全且即時的路徑規劃。首先我們會介紹機械手臂的工作環境，接下來是單手臂路徑規劃的方法，最後介紹到整個傳遞系統的運作方式，詳細細節會在本章做完整的交代。. 4.1 機械手臂工作環境設定本論文所討論的環境主要是在工作空間裡，多個機械手臂傳遞物體的路徑規劃，如圖 4.1 所示，首先，我們用程式模擬出機械手臂的工作空間，在此空間中主要有四個元素，分別為機械手臂（manipulator）、被傳遞物（object）、瓶頸面（bottleneck）、障礙物（obstacle）。其中，機械手臂有多個關節，每個關節均有三個旋轉自由度，且機械手臂均各自被固定在一個基座之上，無法任意移動；而被傳遞物是無法自行移動的，需要被動的用機械手臂來移動之；瓶頸面通常被置於較狹窄的通道上，是用來引導機械手臂前進方向的重要因子，所以當瓶頸面被選作導引的目標時，我們就稱它為（guide plane），以及一個被指定為終點的瓶頸面，我們稱它為終點平面（goal plane）。而整個路徑規劃的過程，就是要利用多個機械手臂，在不發生碰撞的情況下，把被傳遞物從起始點搬運到終點平面。. 17.

(28) 圖 4.1 機械手臂工作空間示意圖。. 在環境中，物體與障礙面的推斥力，若是計算面與面之間的推斥力，就必須用到複雜的四重積分，所以為了節省計算量，我們改為計算點與面的推斥力。也就是在機械手臂和被傳遞物上取點，來和障礙面做推斥力的計算，再從每點所受的推斥力求得合力，作為該機械手臂所受之力。透過假設機械手臂以及傳遞物為長方體模型，我們使用了一種取點方式，如圖 4.2 所示，我們在長邊上各取 m 個點，而在短邊上各取 n 個點，m、n 的大小可以視情況而改變。一般而言，取樣點的多寡要視場景複雜度而定，如果在一般單純簡單的場景，則不需要用到太多取樣點，有時只需要在長方體的八個頂點取樣即可；而在較為複雜的場景，為求精確安全的表現，則可能必須將取樣點數提高，但相對的也會影響到整體的運算速度。. 圖 4.2 物體的取樣點（m = 7, n = 2），m 和 n 可視情況作改變。. 18.

(29) 在一般的機械手臂或是連結物中所使用到的關節，通常可以分為以下幾種：（1）旋轉關節（Revolute joint） - 1-DOF joint （2）滑動關節（Sliding joint） - 1-DOF joint （3）圓柱形關節（Cylindrical joint） - 2-DOF joint （4）螺旋關節（Helical joint） - 1-DOF joint （5）球形關節（Spherical joint） - 3-DOF joint （6）平面關節(Planer joint) - 3-DOF joint 在三度空間中，機械手臂為達到靈活的運動以及突顯位能場在高自由度的優勢，所以我們採用三維自由度的球形關節來做手臂的連結。如圖 4.3 即為球形關節的一個例子。. 圖 4.3 三維自由度關節的例子 – 球形關節。. 4.2 單手臂路徑規劃演算法以 4.1 節做為基礎，我們所要提出的演算法，目的是要利用機械手臂將被傳遞物，由初始的起點，搬運到終點平面，這一連串的「搬運」動作包括了要使被傳遞物或手臂首節「前進」的動作，讓手臂首節（end link）或被傳遞物受到推斥力的影響下，能夠移動到位能低點的「移動平衡調整」動作，然後再利用推斥力影響下，以各節關節為支點，做「轉動平衡調整」，目的是要促使整隻手臂和 19.

(30) 被傳遞物都處於位能低點，即遠離障礙較安全的地方。但是除了受到障礙面的推斥力影響之外，我們在「轉動平衡調整」時，另外加入了手臂基座的吸引力，以克服機械手臂太過於受到障礙面限制的困難，詳細原因會在第 4.4 節做說明。雖然我們模擬的是一個多機械手臂的系統，但若單就每隻手臂的運動行為看來，其實每隻手臂的基本路徑規劃法是相同的，圖 4.4 簡易繪出了單一手臂在未拿著被傳遞物時的基本路徑規劃演算法，若手臂拿著被傳遞物，則可將被傳遞物視為手臂的首節（n = N，N 為手臂總節數），且手臂的總節數加一。而演算法的細部，會在下一節詳述。由圖 4.4 大致可見，每隻手臂基本的路徑規劃只要分為四個處理方塊，第一個處理方塊處理手臂首節朝目標瓶頸面前進；第二個處理方塊則是首節的移動調整和轉動調整；第三個處理方塊則是依序處理從第二節到倒數第三節的連接和轉動調整，在此所謂的連接是將每一節手臂接到前一節的關節，所以在此的每節手臂有一端其實是被固定住的，所以只能做轉動調整；第四個處理方塊則處理手臂最後兩節的連接和轉動調整，將最後兩節獨立出來做處理的主要原因，是因手臂的末節必須連接到手臂的基座，不能任意隨推斥力來調整。後面的幾小節，我們將對首節前進、移動調整、轉動調整和手臂末兩節的調整做特別說明。. 20.

(31) 圖 4.4 單一手臂路徑規劃流程圖，其中 N 為手臂的總節數。. 4.2.1 首節前進如圖 4.5，在三度空間中，為了迫使手臂往目標瓶頸面方向前進，我們利用廣義位能場計算出目標瓶頸面對首節的吸引力方向（即推斥力的反向），並將首節手臂朝此吸引方向移動距離δ，隨即判斷是否和障礙面產生碰撞，若有碰撞發生，則將首節恢復原本的狀態，重新以δ/2 作為前進的距離，重覆此步驟直至沒有碰撞為止，但若δ的值小至某值之後仍然有碰撞發生，則視首節已無法前進。前面所提到的碰撞偵測與處理在之後的討論時常會見到，因此我們將在 4.5 節一併作說明。 21.

(32) start. Calculate the attractive force. Move The end link by toward guide plane. Collision?. = /2. yes. no end 圖 4.5 首節前進的規劃流程圖。. 22.

(33) 4.2.2 移動平衡調整 V_Plane. goal plane. forward direction. 圖 4.6 手臂受到目標平面（goal plane）的吸引力，並在以引力為法向量的平面（V_Plane）上作調整。. 在首節完成前進的動作之後，我們要在前進後的位置找出以吸引力方向為法向量的平面（稱為 V_Plane），並在 V_Plane 上做移動調整，而移動調整的目的是在搜尋 V_Plane 上的位能最低點，並將手臂首節平移到該點上。如圖 4.6 所示，朝向目標平面的虛線方向代表受目標平面吸引的方向，該圖為手臂做移動調整前的示意圖，顯示出 V_Plane、機械手臂、目標平面的相對關係。如圖 4.7 除了求出 V_Plane 之外，移動調整主要分為兩部份，第一階段是順著推斥力的投影方向將物體移動到位能最低點附近，第二階段是進行二分搜尋法來逼近位能最低點。首先我們算出物體所受的推斥力，並將此力投影到 V_Plane 上，得到一個分力 F，以ε為前進單位，朝Ｆ方向移動，在每次移動ε距離後，我們就重算一次推斥力，並將其投影在 F 上，得到力 F＇，若 F 與 F＇同向則繼續前進，若反向則將 F 重設為 F＇並進入移動調整的第二階段。移動調整的第二階段是先把前進的步伐ε減半，再朝 F 方向移動，在移動完後算推斥力，將此力投影在 F 上，得到力 F＇，若 F 與 F＇同向則繼續將步伐減半後前進一步，若 23.

(34) 反向則將 F 重設為 F＇，步伐減半後朝 F 方向前進。循環第二階段五次後，如圖. 4.8，位能最低點的實際位置與我們得到的結果，最多相差ε/32 的距離。因移動調整只侷限在一個方向上，為了更精確地找出位能最低點，我們重覆找出調整後手臂首節所受的推斥力，並在新的推斥力方向上做調整，直到首節在調整前後的位移量小於某個精確值之後，首節的移動調整才算完成。 start. Find V_Plane using attractive force of guide plane. end. no. Calculate the total repulsive force toward the end link and find its projection on V_Plane, F. D > accuracy. yes. Record present position as p. Record position p of the end link and calculate the movement D=|p-p |. Move along the direction of F, and Check force direction after each translation until the force direction is opposite. Continue 1D potential minimization, each time halve the translation distance. 圖 4.7 首節移動調整的工作流程圖。. 24.

(35) 圖 4.8 五階二分搜尋的示意圖。. 4.2.3 轉動平衡調整手臂除了首節要做移動調整之外，其它各節還必須向前一節連接，導致各節一端被固定，所以為了達到位能低點的狀態，我們使用轉動調整的動作來完成目的。若我們將物體的一點固定，此點是我們取連結物體該節前端的中心位置（支點），取樣點所受環境推斥力與基座吸引力的合力會對支點產生一個力矩，而在 4.4 節說明為何要加上基座吸引力，以力矩為旋轉軸，我們可以在單一自由度上求得轉動平衡。類似移動平衡的調整方式，我們先求出所有取樣點所受到的合力矩 Τ，然後以合力矩 Τ 為旋轉軸，以旋轉角度 α 為單位，把物體順著推斥力的投影方向轉動至位能最低點附近，之後再做二分搜尋。我們先將物體旋轉角度. α 減半後，對物體再計算合力矩，並將其投影到 Τ 上我們可以得到 Τ＇，如果 Τ＇與 Τ 方向相同時，我們繼續對以 Τ 為旋轉軸旋轉 α 並作碰撞偵測處理；反之則反向旋轉，利用這樣的二分搜尋法找到轉動平衡點。與移動平衡相同，我們把進行五次二分搜尋後的所在位置當做位能最低點，並且也考慮到整個二分搜尋前後的位移量，當位移量達到某精確值之內，轉動調整才算完成，詳細的演算法流程如圖 4.9。. 25.

(36) start. end no. Record present position as p. yes. D > accuracy. Record position p of the present link and calculate the movement D=|p-p |. Calculate the torque T of the present link. Use T as the rotation axis and check torque direction after each rotation until direction is opposite. Continue 1D potential minimization, each time halve the rotation angle. 圖 4.9 轉動調整的工作流程圖。. 4.2.4 機械手臂末兩節平衡調整在手臂前幾節調整完後，最後剩下手臂末兩節必須進行調整，將末兩節獨立出來討論的原因是因為其它各節均可以自由的做轉動調整，可是末兩節為了連結至基座，因此在轉動上是有所限制的。在這裡我們將末兩節的調整分為同時調整和分開調整兩部分，如圖 4.10 所示末兩節在同時調整時，只能以基座與倒數第二節前端的連線Ｔ為軸，在Ｏ的軌跡上做轉動調整；在分開調整時，則以各自的前端與末端連線為軸，做轉動調整。如圖 4.11 的流程，調整時先求出環境對末兩節的合力矩，投影在該手臂基座與第三節末端的連線向量，稱為 T，再以 T 為旋轉軸，該手臂基地為支點，兩節同時旋轉角度α並作碰撞偵測處理，之後再計算合力矩，並投影在 T 上稱為 T＇，若 T 與 T＇同向，則繼續旋轉角度α並作碰撞偵測處理，若反向則我們當作末兩節已經在位能最低點附近，於是進入二分搜尋來逼近位能最小值。在做完同時的調整之後，手臂末兩節分開的調整，則是以自身的前端中心與末端中心連線為軸，做轉動調整，調整完之後，末兩節的調 26.

(37) 整才算完成。. base. 圖 4.10 最末兩節的調整示意圖，其中圓 O 是可活動的區域軌跡，Ｔ是旋轉軸。. start. end. Connect the last two links with collision detection. Adjust the joint angle of the last two links with fixed joint locations. Use T as rotation axis and check torque direction after each rotation until direction is opposite. Continue 1D potential minimization, each time halve the rotaion angle. 圖 4.11 末兩節調整的工作流程圖。. 27.

(38) 4.3 多手臂傳遞系統規劃 start. Send the closest manipulator to get object. end. Transport the object by manipulators. Retreat all manipulators. 圖 4.12 多手臂傳遞系統工作流程圖。. 我們假設被傳遞物在環境中是無法自行移動的，可以移動的只有機械手臂，所以被傳遞物必須仰賴機械手臂的搬運，才能移動。如圖 4.12 所示，我們將整個傳遞的流程分成三階段：第一階段，必須先指派距離被傳遞物最近的手臂去取得被傳遞物；第二階段，透過數隻手臂的協調，將被傳遞物搬運至終點；第三階段，將所有手臂恢復為初始的狀態。在整個流程中，會牽涉到數個問題，第一是手臂該如何移動，這一部分已經在 4.2 節單機械手臂路徑規劃作說明；第二是被傳遞物搬運或是機械手臂該受到哪些瓶頸面的導引，才能順利到達終點？第三是在搬運的過程有可能遇到手臂長度不足的情況其它手臂該如何做支援的動作？這些問題都會在此節做說明。在我們的系統裡面，單手臂的路徑規劃必須靠瓶頸面的導引來完成。所以我們在場景中放置一連串的瓶頸面，來當作手臂在每個通道中的導引，協助其前進。然後我們把某些瓶頸面先後串連，建立一套動線系統，目的是協助手臂選擇在每一時刻的目標瓶頸面。每次給定每隻手臂一個初始的瓶頸面，以及終點平面 28.

(39) 後，便可以從所有動線中找出必經的中繼瓶頸面。如圖 4.13 所示，整個場景中含有九個瓶頸面，我們簡單設計三條動線，第一條由瓶頸面 b1、b2、b3、b4 組成；第二條由瓶頸面 b6、b7、b8、b9 組成；第三條由瓶頸面 b6、b7、b5、b3、b4 組成。也就是說整個場景中只有三種主要的流動路線，在 b6 附近的手臂是絕對無法延伸至 b1 的，但是若要延伸到 b4 則有第三條動線可以選擇。. 圖 4.13 手臂由數個瓶頸面導引至瓶頸面 b4 的示意圖。. 4.3.1 傳遞系統第一階段工作整個傳遞系統的第一個工作，是挑選最近的手臂去取得被傳遞物，在此所謂最近的手臂，是指手臂基座距離被傳遞物最近的手臂，而圖 4.14 顯示此一手臂之工作流程，這一流程可以視為很多段的單手臂路徑規劃，每一段皆擁有不同的目標瓶頸面（第 a1, …, aM 個目標瓶頸面），但一旦手臂接近傳遞物至某距離 d 之後，手臂受導引的方式才會產生變化，而改以手臂首節前端朝被傳遞物最近的可攜點（我們假設被傳遞物有兩個可攜點）的方向作為導引方向，其目的是要確保手臂首節前端與被傳遞物的可攜點能夠相互連結，在連結之後，我們便當作被傳遞物已經被取得。而連結的方法是改以可攜點當作導引，進行單手臂的路徑規劃，如圖 4.15 所示。這裡跟以往單手臂路徑規劃不同的地方，是改以點當做手臂前進的導引來源，而非之前的平面。在取得被傳遞物後，這階段的工作即告完成。 29.

(40) start. m =m+1 m=1 yes Single-arm path planning using the bottleneck am as guide plane. no. Distance between end effector and nearest grasp point is less than d. am is arrived. no. yes Single-arm path planning using grasp point as guide point. Connecting to the grasp point. no. Connected?. yes. end. 圖 4.14 手臂取得被傳遞物的工作流程圖. 圖 4.15 手臂受到目標點（Goal point）的吸引力，並在以引力為法向量的平面（V_Plane）上作調整。. 4.3.2 傳遞系統第二階段工作 30.

(41) 在取得被傳遞物之後，傳遞系統的下一個目標是要將被傳遞物安全送達終點平面。從第一隻機械手臂出發去取得傳遞物開始，系統就會先估算手臂是否有足夠的長度，可以將被傳遞物送至終點平面，如果長度不足，則會啟動附近的另一隻可以支援的手臂來支援該手臂。我們在這裡將已經取得被傳遞物的手臂，稱作工作機械手臂（Working Manipulator）；而前來支援工作手臂，稱為支援機械手臂（Supporting Manipulator）；完成搬運工作後的手臂，成為退休機械手臂（Retired. Manipulator）；剩下其它的手臂則稱為休眠機械手臂群（Group of Sleeping Manipulator）。第一隻去取得被傳遞物的手臂較為特別，在剛開始的時候並未攜著被傳遞物，要在取得被傳遞物之後才算是工作手臂，但是那是傳遞系統第一階段的工作，所以在進入第二階段後，該手臂已經成為工作手臂。如此一來，第二階段我們可以得到如同圖 4.16 的工作流程。每次在有手臂身份轉為工作手臂的同時，我們都會先評估此工作手臂是否有足夠長度將被傳遞物搬運至終點平面，也就是看工作手臂完全伸直的總長是否能夠到達終點平面，如果可以的話，我們就無須啟動其他手臂來支援；相反地，如果工作手臂的總長度無法延伸至終點平面時，便需要從休眠手臂群中挑選出一個鄰近手臂來支援它，支援方法就是要工作手臂將被傳遞物傳遞給支援手臂，在支援手臂取得物體後，身份就轉為工作手臂，繼續進行搬運被傳遞物的任務，而原來的工作手臂則成為退休手臂，並進入傳遞系統的第三階段工作。工作手臂在被評估為無需支援之後，也有可能發生無法到達終點平面的困境，主要是因為場景地形的限制，使得工作手臂受到障礙物的阻礙，導致工作手臂無法繼續前進，此時我們的處理方法是將工作手臂改為需要支援的狀態，系統便會依循之前的方法，在休眠手臂群中挑選出一個支援手臂，來做接應被傳遞物的工作。在傳遞系統第二階段的工作就是要不斷地利用工作手臂把被傳遞物往終點平面傳送，但是目標達成之後，傳遞系統並未完全結束，而是要所有手臂縮回至基座附近，這些便是傳遞系統的第三階段工作。. 31.

(42) start. Working manipulator is long enough. Working manipulator retire and transfer one supporting manipulator. yes. yes. Working manipulator moves toward goal plane. end. no. Supporting manipulator grasped the object. Object reached goal plane. yes. Supporting manipulator moves toward the object. no no. no Working manipulator can not go further. no. Supporting manipulator is working. yes. yes Supporting manipulator is working. yes. no Select a manipulator as supporting one from the group of sleeping manipulators. 圖 4.16 傳遞系統第二階段工作流程圖. 4.3.3 傳遞系統第三階段工作在工作手臂把被傳遞物交給支援手臂，或是工作手臂把被傳遞物送達終點平面後，該手臂就進入傳遞系統的第三階段，在這階段的目的是要將手臂回復到初始狀態。在這個階段，為了將機械手臂完全恢復成為初始狀態（initial state），我們利用類似將時間倒轉的方法，把手臂從初始狀態，走到目前位置，並將每一步都記錄下來，然後再將時間倒置，變成手臂從目前位置退回到初始狀態。使用這 32.

(43) 個方法的主要原因是手臂的初始狀態並不一定是處於位能最低的狀態，所以在廣義位能場的影響下是無法到達相同的組態，可是手臂目前狀態（present state，剛進入傳遞系統第三階段工作之狀態）是處於位能最低的狀態，所以我們可以確保手臂走到相同位置時，會幾乎擁有相同的組態。如圖 4.17，我們把手臂從初始狀態走到目前狀態，並且用堆疊（stack）將每個狀態存放在（push in）堆疊裡，等到達目前狀態後再將堆疊裡的狀態讀取出來（pop out），直到初始狀態被讀取出來後，任務即告完成。 start. Set the manipulator to initial state. end yes. Plan the manipulator to present state step by step and push each step. Stack is empty. no. no The present state is arrived. yes. Pop out each step and apply to the manipulator. 圖 4.17 傳遞系統第三階段工作流程圖. 33.

(44) 4.4 基座吸引力. 圖 4.18 一個手臂無法連結至基座的例子. 本論文的單手臂路徑規劃，若單純只受到推斥力的影響，在調整上會產生一些先天上的限制。圖 4.18 顯示出一個例子，我們將一隻共七節的機械手臂，放在障礙面附近，加上廣義位能場的影響後，導致手臂朝遠離基座的方向延伸，手臂倒數第三節與基座的距離Ｄ超過末兩節可以連接的距離。會造成這種結果的原因是因為在單純只考慮到障礙面時，推斥力擁有絕對的影響，自然手臂的延伸方向也完全是仰賴場景而定，使得整隻手臂會盡量依附在場景的骨架（中心線）上，也就是位能最低點的軌跡，圖 4.19 就是一個場景為長方形的例子，我們刻意將骨架位置用虛線標示出來。若基座與所在骨架點的最近距離超過末兩節長度的和，手臂斷裂的機率便會相當的高。另一個原因則是因為手臂的長度較長，受到推斥力的影響後，手臂倒數第三節已經延伸至遠離基座的地方，導致手臂末兩節也無法順利連結。. 34.

(45) 圖 4.19 長方形場景的骨架示意圖，虛線部分就是整個場景的骨架，也是位能最低點的軌跡. 為了克服手臂會斷裂的困難，我們試圖讓手臂愈後面的節數愈有朝向基座傾向，也就是在做轉動調整時，加上一個朝向基座的吸引力，但是此舉會讓機械手臂偏離位能最小值的軌跡，且吸引力愈大，偏移愈多。因此我們讓手臂愈前面的節數，所受基座吸引力的影響愈小，使得機械手臂愈前面的節數，愈受到地形場景的影響；愈靠近基座的節數，愈能安全連結回基座。圖 4.20（a）是未受基座吸引力影響前的結果，（b）是加上基座吸引力後的結果，我們可以清楚看出手臂在受到基座吸引力前後的結果。. (a). (b) 圖 4.20 （a）未受基座吸引力影響前的結果（b）加上基座吸引力後的結果. 在本論文我們要討論是最安全的路徑，也就是必須遠離障礙物，若將手臂可 35.

(46) 以完整連結也視為一個安全的因素，那基座的吸引力的加入並不突兀，一旦加入後，場景中位能最低點的軌跡也將隨之改變，以圖 4.19 為例，我們在加入基座吸引力後便產生了類似圖 4.21 的效果。但是根據廣義位能場的特性，在物體與障礙面距離很近時，所產生的推斥力會趨近無限大，所以即使有基座吸引力的加入，只要沒有吸引力無窮大的情況出現，便不會有碰撞的情況發生。. 圖 4.21 在基座吸引力的影響下，原本單純推斥力所形成的位能最低點軌跡已經整個朝基座偏移。. 根據之前的討論，我們希望基座的吸引力是根據手臂節數的大小，來決定吸引力的大小，當節數愈小時，吸引力愈小（首節吸引力最小）；節數愈大時，吸引力愈大，並且吸引力不能有無窮大的情況發生。依此特性我們設計了一個簡單的吸引力 F 的計算式（4-1）：. F=. G k nL. （4-1）. 其中Ｇ是可調整的參數，用來控制手臂整體的吸引力大小；k 代表的是第 k 節（和基座相連者為第一節）手臂；Ｌ則是每節手臂的長度；n 則是拿來調整的吸引力衰減的程度，n 愈大，第 i 節與第 i-1 節手臂所受的吸引力差距愈大。在第 5.2 節我們會以實例分析討論到基座吸引力大小對整個路徑規劃的影響為何。. 36.

(47) 4.5 碰撞偵測與處理在三維空間中，理想的碰撞偵測應該是透過檢查兩物體體積的交集來做判斷，但在位能場模型中，障礙物和手臂都是用多個平面包圍的體積來表示，而為求高效率的方式做碰撞偵測，可以考慮將之簡化成檢查兩物體的平面之間檢查是否有相交。但這麼作仍顯麻煩，故在手臂的部份再做簡化，亦即不拿其上的平面來做檢查，而只取其頂點，因為頂點是手臂最突出的部位。以本論文所使用的例子而言，長方體的手臂就只有八個頂點需要做檢查，障礙物部分仍維持以平面來檢查，所以最後即是以手臂各節上的頂點和障礙物的表面做碰撞檢查，看看手臂在運動時是否有穿過障礙物上的面。另外檢查碰撞時應該是沿著連續的運動軌跡檢查，為了簡化計算，我們改由運動的頂點與終點連線來代替運動的軌跡，希望能夠盡量使用最少的時間得到夠好的檢查結果。實際碰撞的偵測可分為兩個部份，一是如前述由物體運動前後位置連線來判斷的動態部份，另一部份則是由運動後物體本身的稜、邊線來判斷的靜態部份，不論是動態還是靜態部分都是基於檢查一個平面和一條線段之間是否有交點的步驟，稱為交點偵測。碰撞偵測的動態部分即是把運動(不論是移動還是轉動) 物體的每個頂點拿來做檢查，每個頂點在移動前的位置為起點，在移動後的位置為終點。以機械手臂(長方體)為例，長方體總共有八個頂點，因此就會有八條起點終點連線，把所有的連線和場景中所有的障礙面一一拿去做交點偵測，即可得知物體在運動時是否有和障礙物相撞。但這麼做有個缺點，即只有當頂點穿過障礙面時才偵測得到，若是兩頂點之間的線段部份穿過障礙就無法偵測出來。而碰撞偵測的靜態部分即是為了彌補動態部分的不足之處，把物體在運動後本身的所有稜、邊拿去和所有障礙物平面作交點偵測，以期對碰撞偵測的正確性做補強。交點偵測是拿一個面(平面 A)和兩個點(起點 S、終點 E)來判斷兩點連線是否有穿過該平面的步驟，這是碰撞偵測最基本也是最重要的部分，快速判斷的步驟如下，如圖 4.22 所示： 37.

(48) 1. 首先，利用平面 A 上的點(P1~P5)產生兩向量如 P1 P2 和 P1 P5 ，做外積以求得. v 平面法向量 n 。. v 2. 找出 P1 S 和 P1 E 兩個向量，拿這兩個向量分別跟 n 做內積，若兩個內積值同號則代表 S 和 E 在平面 A 的同一邊，SE 不會穿過平面 A；若兩個內積值異號，則代表 S 和 E 在平面 A 的兩側，需要更進一步做檢查。 3. 把起點 S 和平面上各點 P1~P5 連起來成一喇叭狀的結構，喇叭除了底部是原平面 A 的形狀之外，其他的面都是呈三角形，這些三角形所圍成的空間就是會和平面 A 產生交點的線段可能出現的範圍。 4. 把喇叭中每個側面的三角形都拿來產生指向喇叭外的法向量，以 ΔSP1P5 為例，找出的法向量為 n15 ，由於產生出來的法向量必須是要指向喇叭外，所以需要平面 A 上任意第三個點來保證法向量的指向。由於我們考慮的平面都是凸多邊形，所以向量 SP2 必定是在 ΔSP1P5 的內側，因此只要將 SP2 和 n15 做內積，若內積值大於或等於 0，則代表剛剛求出的法向量 n15 指向內，要將之轉向 180 度改為指向外。 5. 拿起點 S 和終點 E 的連線向量 SE 和步驟 4 找到的每個法向量做內積，若有一個內積值大於 0 則代表 SE 在某一三角形之外，即 SE 在喇叭外，也就是不可能有交點，若每個內積值都小於 0，則代表 SE 在喇叭之內，連線和平面之間有交點產生。圗 4.23 是我們碰撞偵測的流程圖。偵測的順序是先檢查動態部份的碰撞，我們讓物體先試探性的走一步，然後拿走過之後的位置和走之前的位置連線去做動態碰撞偵測，若是檢查結果有碰撞產生，即以發生碰撞回報並結束偵測工作，反之若是沒有偵測出動態的碰撞，我們還得進一步檢查靜態的碰撞。為了檢查靜態碰撞，我們讓物體實際前進一步，然後拿物體移動後本身上的線段檢查靜態碰 38.

(49) 撞，所得到的結果即碰撞偵測最終的結果，將結果回報即完成碰撞的偵測工作。一旦有碰撞發生，我們將前進或旋轉步伐減半，重新試探性的走一步，直到沒有碰撞發生為止。. 圖 4.22 交點偵測示意圖. 圖 4.23 碰撞偵測流程圖 39.

(50) 第五章. 實驗結果與問題討論. 本章內容將依照前一章所提及的三度空間多機械手臂傳遞路徑規劃，利用程式模擬、實驗。我們的程式主要是用 Microsoft Visual Studio C++.NET 2003 開發，測試的電腦環境為 PENTIUM IV 1.6G 和 1.0G RAM，作業系統則是 Windows XP。第一節我們將針對單手臂路徑規劃的核心，移動平衡以及轉動平衡，來做簡單的例子測試；第二節則是討論基座吸引力的影響，以及簡單的例子測試；再來會利用我們的系統，對各種不同環境作模擬和實驗；最後，是各個例子的比較與探討。. 5.1 移動平衡與轉動平衡在這節我們利用簡單的例子，來測試移動平衡與轉動平衡調整的適用性，看兩種調整是否能將物體調整至位能的最低點。如圖 5.1，我們建立一個封閉的正方體當作場景，並利用一個小長方體，來觀察它受廣義位能場影響下，以及配合我們的二分搜尋法所得到的每一步結果。. 圖 5.1 環境中有一個在角落中的小方塊，將會受力影響而運動。 40.