利用位能場模型規劃雙機械手臂之移動路徑

全文

(1)國立交通大學資訊科學系碩士論文. 利用位能場模型規劃雙機械手臂之移動路徑. Path Planning for Two Manipulators Using Potential Fields. 研究生：周存厚指導教授：莊仁輝. 教授. 中華民國九十三年六月.

(2) 利用位能場模型規劃雙機械手臂之移動路徑 Path Planning for Two Manipulators Using Potential Fields. 研究生：周存厚. Student：Tsun-Hou Chou. 指導教授：莊仁輝. Advisor：Jen-Hui Chuang. 國立交通大學資訊科學系碩士論文. A Thesis Submitted to Institute of Computer and Information Science College of Electrical Engineering and Computer Science National Chiao Tung University in partial Fulfillment of the Requirements for the Degree of Master in Computer and Information Science June 2004 Hsinchu, Taiwan, Republic of China. 中華民國九十三年六月.

(3) 利用位能場模型規劃雙機械手臂之移動路徑. 研究生：周存厚. 指導教授：莊仁輝博士. 國立交通大學資訊科學系. 摘要. 本論文針對三維空間中的雙機械手臂路徑規劃問題提出一演算法。該演算法在工作空間中，利用三度空間的廣義位能場模型計算障礙物對手臂所產生之推斥力，以保證手臂前進時所經過的路徑不會與障礙物或是兩個手臂互相碰撞。由於我們的方法完全是利用工作空間中的資訊，不需透過大量計算將相關資訊轉到組態空間作處理，因此規劃速度相當快速。另外，本論文採用「主僕式架構」來處理兩個機械手臂之間的協調問題，讓僕臂在規劃路徑的過程中視主臂為障礙物並閃避之，而主臂則完全忽視僕臂之存在。電腦模擬顯示，透過交錯式的規劃方式，本論文所提出之演算法在大部分的情況之下都能讓兩個手臂規劃完成各自的路徑，所規劃出來的路徑也能夠平滑且連續。. i.

(4) Path Planning for Two Manipulators Using Potential Fields. Student: Tsun-Hou Chou. Advisor: Dr. Jen-Hui Chuang. Institute of Computer and Information Science College of Electrical Engineering and Computer Science National Chiao Tung University. ABSTRACT. An algorithm for path planning of two manipulators is presented in this thesis. The proposed algorithm utilizes a generalized potential model to evaluate repulsion between manipulators 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. In this thesis, the “master-slave＂ architecture is adopted to deal with the coordination of two manipulators, while the master manipulator is regarded as an obstacle by the slave manipulator, the existence of the latter is completely ignored by the former. Simulation results show that by alternately planning configurations of the two manipulators, safe and spatially smooth paths can be obtained successfully almost always. ii.

(5) 致謝. 本論文得以完成，最需要感謝指導教授莊仁輝老師，兩年來不但在學業上給予悉心的指導，對於生活上的種種問題，老師也給予相當多的建議與鼓勵，因此這兩年的研究生涯，我過得愉快又充實。此外，由於三位口試委員，林志清教授、雷欽隆教授以及顏嗣鈞教授的意見及指教，讓本篇論文得以更加充實以及完善，在此衷心感謝他們。另外我要感謝智慧型系統實驗室的學長以及同學和學弟們，還有許多從大學就一起相伴奮鬥到現在的交大資科同學，你們的砥礪讓我一路奮鬥到現在，而可貴的有情更是我最要珍惜的。最後要感謝我的家人，尤其是我的母親，感謝您在我求學過程中不斷的給予各種建議和關懷，如今論文完成，碩士畢業，希望能讓您感到欣慰並以我為榮。僅以此篇獻給愛我及我愛的家人，以及許多給予過我幫助的朋友和同學們，誠摯感謝。. iii.

(6) 目錄摘要 ..........................................................i ABSTRACT .....................................................ii 致謝 ........................................................iii 目錄 .........................................................iv 表目錄 .......................................................vi 圖例目錄 ....................................................vii 第一章. 簡介.................................................1. 1.1 機械手臂的路徑規劃 ....................................1 1.2 研究目的及論文結構 ....................................2 第二章. 背景.................................................4. 2.1 路徑規劃演算法.........................................4 2.1.1 骨架法 ...........................................5 2.1.2 切割法 ...........................................6 2.1.3 數學規劃法 .......................................6 2.1.4 次要目的圖形法 ...................................7 2.1.5 位能場模型法 .....................................7 2.1.6 廣義位能場模型...................................8 2.1.7 其他方法 .........................................9 2.2 雙機械手臂避碰方法之回顧整理 .........................10 2.2.1 手臂間的避碰處理 ................................10 2.2.2 廣義位能場模型在手臂間的避碰處理的應用..........11 第三章. 廣義位能場模型及其應用..............................13. 3.1 三維空間的廣義位能場 ..................................13 3.2 推導向量函數 .........................................15 iv.

(7) 3.3 廣義位能場的解析式....................................16 3.3.1 牛頓位能場 (當m = 1 時) .........................17 3.3.2 廣義位能場模型 (當m > 2 時) .....................18 3.4 推斥力 ...............................................19 3.5 廣義位能場模型在路徑規劃問題的應用 ...................20 3.5.1 力矩 ...........................................20 第四章. 三度空間雙機械手臂路徑規劃演算法 ....................21. 4.1 環境設定..............................................21 4.2 路徑規劃演算法........................................24 4.2.1 演算法細部解析 ..................................25 4.2.2 兩個手臂之間的協調 ..............................31 4.3 手臂與障礙物間的碰撞偵測與處理 ........................32 第五章. 實驗結果與問題討論..................................34. 5.1 移動平衡以及轉動平衡 .................................34 5.2 三度空間雙機械手臂範例實測 ...........................38 5.3 運算複雜度與效能 .....................................47 5.4 問題與討論 ...........................................51 第六章. 結論................................................53. v.

(8) 表目錄表 5.1 例一的動作資訊統計 .............................47 表 5.2 例二的動作資訊統計 .............................48 表 5.3 例三的動作資訊統計 .............................49 表 5.4 例四的動作資訊統計 .............................50. vi.

(9) 圖例目錄圖 3.2 三維空間中的一點、線段C(圖 3.1 中多邊形S的其中一邊)以及平面Q包含了多邊形S。 ..................................14 圖 3.3 牛頓位能場在三維空間中的例子。 (a)一個正立方體模型。根據牛頓位能場模型的假設，該正方體的表面均勻地帶著正電荷。 (b) 當z = 0 時，圖示面OABC所在處，其位能場分布的等位線圖。 (c) 當x = y，. w∆ ( x 2 + y 2 ) / 2. 時，圖示面OBDE所在處，其位能場分布. 的等位線圖。圖中顏色越深的部分，其位能值越小。........17 圖 4.1 雙機械手臂環境基本模型，包含左邊的主臂(Master)、右邊的僕臂(Slave)、以及主臂終點(Goal of Master)和僕臂終點(Goal of Slave)。...............................................22 圖 4.2 物體的取樣點(m=7,n=2)，m和n可是情況作改變。 .........22 圖 4.3 三維自由度關節例子 - 球形關節。 .....................23 圖 4.4 單一手臂路徑規劃流程圖，其中N代表手臂的節數。........24 圖 4.5 首節前進的工作流程圖，δ為可變參數，視情況調整。 ....25 圖 4.6 手臂受到終點平面(Goal Plane)的引力，並在以引力為法向量的平面(V_Plane)上作調整。..............................26 圖 4.7 首節移動調整的工作流程圖，N_Loop為可變參數，代表二分搜尋的次數...............................................27 圖 4.8 轉動調整的工作流程圖，N_Loop為可變參數，代表二分搜尋的次數...................................................29 圖 4.9 最末兩節的調整示意圖，其中圓O是可活動的區域 .........30 圖 4.10 最末兩節調整工作流程圖，N_Loop為可變參數，代表二分搜尋的次數.................................................30 vii.

(10) 圖 4.11 沒有碰撞偵測時，如果物體與障礙物發生碰撞，會因為取樣點受力方向不同而卡在障礙物上。...........................32 圖 4.12 碰撞的偵測與處理流程圖。 ...........................33 圖 5.1 環境中有一個在角落中的小方塊，將會受力影響而運動。 ..34 圖 5.2 轉動平衡調整步驟(a)第一次調整，共七步(b)第二次調整，共六步(c)第三次調整，共六步(d)到達目的地，環境方塊的中央.35 圖 5.3 小方塊經過調整後正確到達環境中央。 ..................36 圖 5.5 例一的環境及結果(a)初始狀態，包含手臂及外圍障礙物(b)調整軌跡。...............................................38 圖 5.6 例一軌跡圖比較(a)例一俯視角軌跡(b)固定例一中的主臂所形成的軌跡。.............................................39 圖 5.8 例二中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。......................................40 圖 5.9 例二的行進軌跡圖(a)前視角度(b)俯視角度。 ............40 圖 5.11 例三中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。......................................42 圖 5.12 例三的行進軌跡圖(a)前視角度(b)俯視角度。 ...........43 圖 5.13 例四中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。......................................44 圖 5.14 (續) ...............................................46 圖 5.15 例四的行進軌跡圖(a)前視角度(b)俯視角度。 ...........46 圖 5.16 解決位能場先天問題的方式，對特定平面的授與不同權重值。 .......................................................51. viii.

(11) 第一章. 簡介. 路徑規劃的問題起源於行動機器人 (mobile robot) 在空間中的移動，一般這類的問題可以描述為：在空間中，機器人由某個給定的初始組態 (initial configuration)，在不與障礙物碰撞的情況下，找出到達另一個給定的最終組態 (final configuration) 的方法，並說明中間的移動過程[1]。過去研究許多都是在組態空間 (configuration-space，以下簡稱 C-space) 作處理[2][3][4]，而 C-space 中的禁止區域 (forbidden regions) 則對應著被規劃的物體在工作空間 (work space) 中被障礙物所阻擋的區域，因此原本的在工作空間中的路徑規劃問題即被簡化為在 C-space 中作規劃。另一方面，不少研究則是藉由紀錄以及運用空間中的座標資訊，直接在工作空間中作路徑的規劃[5][6][7][8][9][10][11]，在有障礙物的條件影響之下，找出代價最少的路徑。. 1.1 機械手臂的路徑規劃多節物體路徑規劃問題，即在空間中為多節物體找一個無碰撞 (collision-free) 的路徑，多節物體即為連結物體 (articulated object)，由數節剛體連結而成，障礙物則存在於環境當中。隨著多節物體節數和各節連結處 (關節) 的自由度 (degree of freedom，其縮寫為 DOF)的提高，或是障礙物的複雜度愈高，都會讓路徑規劃的運算工作繁複許多，因此多節物體路徑規劃問題遠比單純的剛體路徑規劃問題更為複雜。機械手臂可以看作是多節物體的一種，而就單一機械手臂路徑規劃問題而言，常以機械手臂的末端操作點 (end-effector) 的位置作為整個手臂的位置參考點，此時給定一個末端操作點的位置，當機械手臂的自由度大於其工作空間 (work-space) 的維度時，此機械手臂將有無限多組機械 1.

(12) 手臂組態 (configuration) 的解，這種機械手臂我們稱為 redundant manipulator。相較於 non-redundant 的手臂，這類手臂的路徑規劃問題較不容易，由於要在無限多組機械手臂組態的解中求得最佳解 (最短路徑或最安全路徑)，因此在計算上所要付出的代價是要多出許多，但其優點是在於動作的控制上，可以表現其靈活的自由度。. 1.2 研究目的及論文結構本篇論文在探討雙機械手臂的路徑規劃問題，而這類的問題可以看作是傳統單機械手臂路徑規劃問題的推廣，也就是兩個機械手臂，各自有其初始組態. (initial. configuration). 和最終組態. (final. configuration)。大部分有關這類問題的研究，例如[12]和[13]，是利用主僕式架構 (Master and Slave) 來解決這種問題，本論文亦採用這種架構，優點是能夠將整個問題簡化成兩個單一的機械手臂的問題並分別討論之。而這兩個機械手臂在以下論文中，分別稱作主臂 (Master) 以及僕臂 (Slave)，簡而言之，主臂 (Master) 的運動過程可以簡單的參考或沿用先前有關單一機械手臂的路徑規劃演算法，而僕臂 (Slave) 則必須將主臂 (Master) 視為環境中的障礙物，並且設法閃躲之。本論文引用廣義位能場模型[6]作避碰處理，進而規劃路徑，我們假設所探討的多節物體是建構在三維的環境裡，因而以多邊型或線段來描述物體以及障礙物，事先假設已知工作空間有關手臂及障礙物的所有幾何形狀，以及其電荷分佈的狀態等資訊進行參數設定，並以文中所提出的演算法進行雙機械手臂路徑規劃的模擬與驗證，而關節的連結機制是屬於序列式 (serial) 的 redundant 多節物體。本研究的目的是希望藉由提出一套高效率的運算及適應性高的雙機械手臂路徑規劃演算法，以創新的方式為這樣的問題提供一個安全且實用性高的解答，除了可以解決各節物體移動時所潛在可能發生的避碰問題，甚至可以在即時運算的條件下回應，以便 2.

(13) 更廣泛的運用到實務領域。根據上述目的，在接下來的論文中，第二章將會對基本路徑規劃方法作一些背景探討以及回顧；第三章則介紹本篇論文中主要所使用的廣義位能場模型；第四章會針對雙機械手臂路徑規劃問題，對於我們所提出的演算法作詳盡解說；第五章將列出一些實驗以及模擬的結果，並針對問題作作一些分析及探討；第六章則是結論以及對未來可以深入研究的問題作展望。. 3.

(14) 第二章. 背景. 雙機械手臂的路徑規劃問題是由一般路徑規劃問題的研究推廣而來，本章將對先前既有的路徑規劃演算法作介紹，並在第二節對於雙機械手臂路徑規劃當中，所會碰到的避碰問題，作先前文獻的討論以及整理。. 2.1 路徑規劃演算法一般對『路徑規劃（ path planning ）』和『動作規劃（ motion planning）』的區別，在於『路徑規劃』試圖找尋工作環境中一條存在的無碰撞路徑，而『動作規劃』則需進一步考慮到時間、動態的限制、工作空間裡的互動等等。 70 年代的末期，Lozano-Pérez 和 Wesley 在[14]裡提出了一個完全演算法（complete algorithm），他們的方法是將障礙物所存在的空間，以多邊形或是多面體表示，形成一個禁止區之網路圖，而機械手臂的路徑規劃問題轉變為避開這些點，而在圖中找尋一條安全的路徑，這個方式被認為是 C-space 的前身。到了 1983 年，Lozano-Pérez 和 Wesley 在[15]裡首度引進了機械人組態空間（configuration space，簡稱 C-space）的觀念，影響了後來這領域的研究方向。在 C-space 裡，機械手臂的組態被表示成一個點（一個點即是一個動作組態，而該點的所在空間的維度由手臂關節的數目所決定），因此機械人的自由度即為 C-space 的維度。如此，組態空間裡的禁止區域即為機械人碰撞障礙物的組態的集合，而禁止區以外的自由空間（free space）則對應機械人所有安全的動作組態的集合。因此整個避碰問題的核心被轉變成──『在自由空間裡，找一條連接初始點和終點的一維路徑。』組態空間描述了所有的組態，且幾乎把所有解都算了出來，因此解決了 redundant solution 的問題，但是為了要描述所有機械手臂的組態， 4.

(15) 必花費很多時間，這樣的前置工作代價太大，尤其是對高自由度的機械手臂而言影響更重大。在 80 年代，微分幾何也被應用到組態空間，不少方法研究 C-space 裡的 manifold 結構，伴隨著是更豐富的拓樸、幾何、代數的特性。這樣的問題在數值分析、複雜度研究日趨成熟之後，演算法的方向轉向了經驗法則式的規劃。這大致有兩個熱門的方向，一是細格的切割法（approximate cell decomposition），另一個是位能場（potential field）的應用。如今在路徑規劃這個領域，已經有相當多的研究成果。大部分的方法將問題轉換到組態空間，有些則直接在工作空間進行。基本上路徑規劃的方法可分為五大類:骨架法. (skeleton) 、細胞切割法. (cell. decomposition) 、數學規劃法 (mathematical programming) 、次要目的圖形法 (subgoal graph) 以及位能場模型法。底下針對這五大類以及其他各式方法作個概述。. 2.1.1 骨架法骨架法又稱為路線圖法 (roadmap)，收縮法 (retraction)，高速公路法 (highway)。這個方法是將原有的自由空間轉換成一為線段所組成的網路線段，也就是骨架，依據這些線段或骨架找出所要的路徑。因此所要找的路徑就會被侷限在這些網路線段當中，而原先的路徑規劃問題被巧妙的轉化成圖形的搜尋問題[16]。一般而言骨架法可分為三個步驟 : 首先將物體起始位置移至位於骨架上的一點開始位置，接著再將物體最終位置移至骨架上的一點為目的地，最後透過我們所建構出來的骨架網路用線段將這兩個點連接起來。其中兩個最著名的骨架法，為視線可見圖形法 (visibility graph) 與 Voronoi 圖形法 (Voronoi diagram)。. 5.

(16) 2.1.2 切割法 Brooks & Lozano-Pérez 在 1983 年提出的切割法[17]是把安全空間（free space）切割成大小不同的細格，利用這些切出來的格子依據其相鄰關係組成一條安全的路徑，而格子的大小是決定結果的關鍵。而位能場的應用始於 1986 年的[18]，Khatib 創新地利用人工位能場對機械人的排斥力而達到避碰的目的，值得一提的是它能做到即時的運算。雖然這些方法都很有效率，但是這兩種方法均在 C-space 裡討論，而且只適合總自由度二或三的機械手臂，對於總自由度四或五以上的 redundant manipulator，這些方法很顯然都無法表現的良好，例如切割法可能會因為細格切割出來的數量太大，在高自由度的機械手臂的路徑規劃中效能無法提高；又例如位能場法，其機械人有可能陷入人工位能場的非最終目的地的位能極小值（local minima）。雖然之後有很多經驗法則式的演算法對某些缺點做了改良[19]，但是卻無法保證其效率，且不夠穩定。. 2.1.3 數學規劃法在數學規劃法中，主要利用組態參數或不等式來達到避開障礙物的要求。在 Yong 等人的研究中，探討了路徑規劃問題的數學最佳化模式，亦即找出一條從起點到目的地組態的最短長度曲線。但是這些最佳化問題可能是非線性的，而且由多個不等式組成，因此常運用數值方法來幫助尋找最佳解。此外，還有一種利用線性代數作機械手臂動作規劃的方式，由 Cartesian 空間中末端操作點的速度（或加速度），以便求得機械手臂各關節所需的轉動變量，稱之為反運動學（inverse kinematics）。一般利用線性代數作機械手臂動作規劃的方法多為利用 Jacobian matrix，現今已實際應用到多種機械手臂。Jacobian matrix 可以很方便的對關節變數空間（joint variable space）和末端操作點移動空間（end-effector 6.

(17) space）兩者之間作線性轉換，可以快速的求得每個關節所需之轉動變量，而作即時的動作規劃。不過這個方法必須用在工作空間為已知一條無碰撞的末端操作點的移動路徑之後，才可對手臂的關節求得細部的轉速調整，所以屬於動作規劃的控制，而非路徑規劃的避碰問題。此外任何矩陣都有 singular 的問題，這一直都是動作規劃中有待解決的重點。. 2.1.4 次要目的圖形法次要目的圖形法則是利用工作空間中重要的中途站或是組態空間中最重要的組態，即次要目的 (subgoal)，可以用來幫助找到路徑。不過次要目的 (subgoal) 並不是強迫必須經過的，而是視情況而定。因此次要目的圖形法通常分成兩個階段，分別為全域規劃 (global planner) 和局部規劃 (local planner)。由全域規劃產生一個包含次要目的地的空間，而局部規劃則是用來確認每一個次要目的地中的可到達性 (reachability)。與細胞分割法相似的是，其兩個次要目的地之間也可以依實際情形需要再細分更多的次要目的地。Henningy 等人曾在[20]提出蛇行機器人的運行方式，建立一數學規劃模式，取代傳統的機器人運動軌跡。尤其在求解大型問題時，成效相當好。. 2.1.5 位能場模型法位能場的應用至今仍常被採用於機械手臂路徑規劃，在[21]中， Laliberté 和 Gosselin 把工作空間作骨架（skeleton）抽取決定大致的路徑，再用離散的位能場計算（discrete potential field）來減低 local minima 的數目。另外在[22]中，Ralli 和 Hirzinger 把 C-space 離散化成格狀，並計算機械手臂於 C-space 中各點的數值位能值（numerical potential），然後再用快速的格狀搜尋（grid-searching），找出一條完 7.

(18) 整的組態路徑，作為手臂動作的過程，不過缺點是最多只能到五個自由度的機械手臂。Gill & Zomaya 於[23]中，結合了位能場以及基因演算法，利用平行計算的方式作機械手臂的避碰，可是這個方式需要的硬體需求太大，而且後置處理(路徑的平順化)是必要的。Vadakkepat 等人在[24]提出人工位能場 (Evolutionary Artificial Potential Field, EAPF)來即時地機器人路徑規劃，其中融合了基因演算法來產生最佳的位能場函數。 1991 年，位能場的應用開始加入了隨機方法（randomized planner）來解決高自由度的手臂問題。Barraquand 和 Latombe 在[20]裡利用力場的負階度（negated gradient）決定機械人的移動方向，當落入 local minima 時，則利用隨機亂數來決定運動的方向。1996 年 Kavraki 等人，於[25] 裡提出了 Probabilistic Roadmap（PRM），很成功的把其應用範圍提高至高自由度的機械手臂，此演算法在 C-space 裡利用亂數，灑下一定數目的點，每一個點即是指一個手臂的動作組態，然後區域性地 (locally) 把一些沒有碰到障礙物的點連起來，形成一個圖（graph），而之後在這圖裡找一條連接起點和終點的路徑，來描述機械手臂的整個運動過程。雖然這方法可以在數十秒內建好圖，在不到一秒的時間裡找到一條路徑，但是機率的操控的因素太大，成功率沒有絕對的保證，而且求出來的路徑可能需要再平順化（smooth），再者，此方法只適合用於靜態的工作空間。. 2.1.6 廣義位能場模型除了最短距離 (minimum distance) 以及最短時間 (minimum time) 路徑之外，最安全 (safest) 路徑也是另一項用來評量路徑規劃演算法的標準，1998 年 Jen-Hui Chuang 所發表提出的廣義位能場模型[6]，在三維的工作空間中，經由計算廣義位能場中多面體的每個面所產生的推斥力，可以找出障礙物與場景的位能谷底以及位能最小值，運用此推斥力，可將運動中的手臂推往力的平衡點，這個平衡點就是可視為再當時刻最安全的 8.

(19) 位置，而由每個時間點所計算出的力平衡點，連結起來的路徑，則可視為最安全路徑，由於本篇論文即利用此方法作為依據，進而延伸到雙機械手臂的路徑規劃，因此有關廣義位能場模型的推導及論述，將在下一章作詳盡解說。. 2.1.7 其他方法除了上述的五大類路徑規劃方法以外，還有其它各式不同的方法被應用到路徑規劃的領域，底下概述幾個作法。 Pires and Machado 於 [26] 提出基因演算法(GA) 產生機器人路徑規劃的控制器。其目標為最小化機器人位置與速度所產生的波動。此外，在不碰撞規劃空間中的障礙條件下，此控制器亦可達到事先預定之目標。 Maron and Lozano-Pieerez 於 [27] 利用視覺化的分解法，在擁有大量障礙物的規劃環境下，即時地計算出無碰撞的移動路徑。此法將規劃空間分解成數個區域性的視覺化圖形，以保證所有的的作業都在區域中進行。由於搜尋的子空間的個數少，所以可使搜尋時間最小化。 Ando 等人在 [28] 提出一個分配式的演算法使有自由移動的機器人環節朝向單一點運行。每一個機器人都朝向同一個方向重複運動。只有在特定的距離 V 下，機器人才可看到彼此，而且中間沒有其他的機器人。此外，這個演算法是無記憶性的，下一個位置的決定只與現在機器人現在所處的位置有關。 Camel 等人在 [29] 則展示了如何從多台電腦上以隨機搜尋的方式，在數秒或更短的時間內產生的多個環節的機器人的路徑。 Lu and Akella 在 [30] 所提出的方法利用固定點的內部交換 (inter-changeable)來達到快速交換的目的。其演算法透過運動力學的方式找出機器人運行過程的相鄰節點與連結處，來求解多向度的機器手臂路徑規劃問題。 9.

(20) Ahmad 等人在 [31] 提出以位能場的方法找出正朝向障礙物運行的目標物。其中，運用了時間相依的位能場來產生一線性相關的波形函數。在 Kuffner [32] 的研究中，回顧了近來學術上研究複合式動態機器人路徑規劃的演算法。並對其中的問題定義、假設、主要概念、實例驗證結果與未來可行研究作進一步的分析和探討。. 2.2 雙機械手臂避碰方法之回顧整理多機械手臂系統比起單手臂系統，在應用上有顯著的優點，這樣的系統可以輕易地應付許多複雜的工作，而在它的應用方面，大致可以分成以下兩類： (1). 兩個手臂在同一個環境當中分別獨立運作，稱為 Co-operative。. (2). 兩個手臂同時運作，而定義在這種情況下的動作通常會是一個封閉迴圈，重複著動作，稱為 Co-ordinated。. 由於雙機械手臂系統中的兩個手臂是在同一個環境中工作，因此先前為單機械手臂系統，所提出的一些有關位置以及位能場演算法，並不能直接套用。在多機械手臂的環境當中，主要不同且需要處理的問題，就是手臂間的避碰問題，規劃過程的路徑必須確認為無碰撞 (collision free)，因此就必須不斷的作碰撞偵測 (collision detection) 以及碰撞避免 (collision avoidance)，基於這些問題的重要性，先前有關多手臂環境的研究，也提出了一些解決方法。以下是對於手臂間的避碰問題，對先前提出方法作所的整理。. 2.2.1 手臂間的避碰處理 Cameron [34]曾經整理出有關碰撞偵測的技術，並將之歸類為主要三 10.

(21) 個方法： (1). 將機器人的動作對時間作平均取樣，而每個取樣後的片段再單獨對環境作比對，偵測有無碰撞發生，取樣的密集度則必須確定在取樣片段跟取樣片段之間不會有碰撞發生。. (2). 將機器人在時間範圍內掃過的區域記錄起來，並將此區域對環境作比對，偵測有無碰撞發生。. (3). 不將動作對時間作取樣，而是直接把時間當作除了空間之外的第四個維度作處理，因此障礙物資訊將隨時間而累積，這種方法所求出的解接近完美，然而缺點是資料量以及計算時間過於龐大。. 利用以上的方法為依據，接下來的研究即可以之為工具，做好手臂間的避碰處理，而後將重點轉移到路徑規劃剩下的問題當中。且綜觀以上，各個方法多是將機器人或機器手臂，或者其通過的空間視為物體 (object)，進而去判斷物體與環境是否有交錯 (intersection)，通常在接下來所規劃出的路徑，可被看成擁有最短距離或是最短時間的路徑。. 2.2.2 廣義位能場模型在手臂間的避碰處理的應用廣義位能場模型的特性，在於能將三維空間中活動的物體，推向力的平衡點，在行動機器人或機械手臂的路徑規劃應用上，可視為是盡可能的遠離障礙物 (因為障礙物表面對機器人或手臂具有推斥力)，而手臂間的互動，亦與此特性有關。兩個機械手臂在環境中動作時，當其中一個手臂，或是兩個手臂互相把對方看成是環境中的一部分 (障礙物)，在每一個時間點都去重新考慮環境中的障礙物資訊而計算出新的位能低點，此時規劃路徑中的手臂將同時受到環境以及另一個手臂的斥力，到達新的位能低點，而處於新的安全點。 11.

(22) 廣義位能場模型不但應用在單一機器人或手臂的路徑規劃上效果顯著，由於其推斥力的特性，我們認為若應用在雙機械手臂，甚至多機械手臂都會有很好的效果，因此下一章我們針對將要應用的廣義位能場模型，作詳細的介紹。. 12.

(23) 第三章. 廣義位能場模型及其應用. 在本章中，我們將介紹在一個三維空間中，用以計算物體推斥力的廣義位能場模型。在參考文獻[35]中，以牛頓位能場 (Newtonian potential) 模型取代尋找二維空間的距離函數。從當中可以發現，位能谷點 (potential valley) 相當地近似中心軸 (medial axis)，而且可以由簡單的推斥力的計算演算法求得；開始時，從物體角點的內部為起始點，延著推斥力的方向走，直到到達位能極小點 (potential minima) 為止。其中位能谷點到鄰近的位能極小點只需在一維方向搜尋。在本論文中，我們將上述觀念推廣而以廣義位能場應用在三維空間中推斥力的計算。但由於使用牛頓位能場來計算推斥力時，會使種子點往物體的邊線或是角落方向走，故牛頓位能場並不不能保證解決三維空間中物體的避碰問題，因此，我們建議採用廣義位能場模型。. 3.1 三維空間的廣義位能場. 圖 3.1 在三維空間中一個多邊形平面. S。. 如圖 3.1，考慮在三維空間的一個平面 S，在點 r 因平面 S 所產生的位能可被定義成下式 ( m = 1 為牛頓位能場)：. 13.

(24) 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)中位能場積分方程式中的積分元，寫成某一向量函數的表面散度 (surface divergence)。步驟 2：利用表面發散定理 (Surface Divergence Theorem)，將二重 (面) 積分轉成對 ∆S 之線積分。步驟 3：對多邊形週界 ∆S 的每個邊做直線積分，而我們的面積分結果即是將這些線積分加總起來。. s 圖 3.2 三維空間中的一點、線段 C(圖 3.1 中多邊形 S 的其中一邊)以及平面 Q 包含了多邊形 S。. 多邊形 S 的某一邊線 C 與包含多邊形 S 之平面 Q 的相關幾何量，請參見圖 14.

(25) 3.2，其中 r ′ ∈ C。在不失其一般性的原則下，我們假設 d ∆ nˆ (r – r ′ ) > 0. (3-2). 以上式子也相當於由 r 到 Q 的距離。. 3.2 推導向量函數假設步驟 1 中的向量函數可被表示為如下面的式子： f m (R)P. (3-3). 其中 P 是 S 中某點相對於 rQ 的位置向量，而 rQ 表示 r 在 Q 上的投影。步驟 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 P2 ∫. log R ⎧ , ⎪ R2 − d 2 =⎨ −1 , ⎪ m −2 2 2 ⎩ ( m − 2) R ( R − d ). m=2 m≠2. ( f1 (R)相當於在[36]中用於牛頓位能積分的向量函數。) 15. (3-6).

(26) 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 m ∫S R m ε →0 ⎢S −∫S ∫ R m ⎥⎥ S ε ε ⎦ ⎣ =. ⎤ ⎡ α α ε pdpdθ ⎥ 2 2 2 ⎢ ˆ ( l ) P u l lim ε ε θ f d f d d ⋅ + − + + m ∫ m ∫0 ∫0 2 2 m ⎥ ε →0 ⎢ ∫ ∆S 0 (p + d )2 ⎦ ⎣. (. = ∑ 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 , ⎪ α = ( ) α gm ⎨ , ⎪⎩ ( m − 2)d m−2. m=2 m≠2. (3-8) (3-9). 在式 (3-8) 中， Pi0 是 rQ 與 Ci 之間的距離， l i 則是 r 在 Ci 上的投影沿著 ˆli 方向的分量， α 則是當 ε 趨近於 0 時在 S 內部的 Sε 之圓周角。舉例來說，當 α = 2π 時， rQ 在 S 內部，而 α = π 時， rQ 在 S 的邊上。如果 rQ 在 S 中視兩. 條邊的接點，則 α 就等於這兩邊的夾角。(為求簡明，往後註標 i 將省略). 16.

(27) 圖 3.3 牛頓位能場在三維空間中的例子。 (a)一個正立方體模型。根據牛頓位能場模型的假設，該正方體的表面均勻地帶著正電荷。 (b)當 z = 0 時，圖示面 OABC 所在處，其位能場分布的等位線圖。 (c)當 x = y，. w∆ ( x 2 + y 2 ) / 2. 時，圖示面 OBDE 所在處，其位能場分. 布的等位線圖。圖中顏色越深的部分，其位能值越小。. 3.3.1 牛頓位能場 (當 m = 1 時) 在[36]裡證明了在牛頓位能場中當 m = 1 時，得到如下的等式：. ∫. C. R + + l + d ⎡ −1 l + d ⎤ f1 (l )dl = log − − + 0 ⎢ tan R +l P ⎣ P 0 R − ⎥⎦. (3-10). 然而上式位能場並不適合使用來找尋軸的計算力 (force) 的程序。考慮一個正立方體，根據牛頓位能場模型，我們假設此一正立方體的邊界均勻帶 17.

(28) 著正電荷 (如圖 3.3(a)) ，並假設正立方體的頂點座標為( ± 10， ± 10，. ± 10)。我們可以計算出正立方體內部區域的位能場 (參見[36])。圖 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 的部分)。我們可以輕易觀察得到，遵循此位能場的力 (force) 將會往正立方體的某一邊，或是往角落的方向移動。在路徑規劃的領域當中，我們希望物體能在環境中盡量遠離障礙物，此牛頓位能場特性並不符合要求，因此我們採用以下提出的廣義位能場模型。. 3.3.2 廣義位能場模型 (當 m > 2 時) 對於 m = 2 以外的其他偶數， f m (l ) 為有理函數，而對於 m 為奇數時，. f m (l ) 則可被有理化。因此除了 m = 2 外，線積分可由封閉式子來求得。例如，當 P 0 ≠ 0 ，則得到下面式子：(註標 i 省略). C. ∫. 1 f 3 (l )dl = 0 Pd. ∫. f 4 (l )dl =. C. 1 2d 2. 1 − 0 P. + ⎡ −1 l − d −1 l d ⎤ ⎢ tan P 0 R − − tan P 0 R + ⎥ ⎣ ⎦. ⎡1 ⎢ 0 ⎣R. (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. 18. α z. (3-13).

(29) 在式 (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. 19. (3-18).

(30) 3.5 廣義位能場模型在路徑規劃問題的應用一般而言，根據廣義位能場模型，一個點在多面物體各個面所受的力，可以藉由物體每個面所蓄的電荷予以加總求得，而在路徑規劃的問題當中，若把障礙物用這些帶有電荷的面表示之，而被規劃的物體則用許多取樣點(sample points)來表示(取樣點的取樣方式必須盡可能分佈平均)，將這些平面對於這些取樣點所產生的推斥力相加，進而推擠物體，理論上物體將在工作空間中處於最安全的位置，由於擁有此特性，我們認為廣義位能場模型極適合運用在此領域。. 3.5.1 力矩在多節物體的運動當中，除了移動，各節還會以節與節之間的連接處 (關節)為支點，作轉動的動作，每個取樣點先分別計算出對於環境的所受的推斥力總合，再與力臂向量作外積，最後把此節所有取樣點所計算出的合力矩相加，即為我們所要的旋轉合力矩，關於這部分以及涉及雙機械手臂路徑規劃問題的運用，將在下一章作詳細討論。. 20.

(31) 第四章. 三度空間雙機械手臂路徑規劃演算法. 在這一章當中我們將會說明以廣義位能場模型作為基礎的雙機械手臂路徑規劃演算法。不同於一般在組態空間規劃的方式，本篇論文所提出的演算法並不需要花費時間在建構組態空間上，也沒有搜尋空間龐大的問題，所有需要的資訊都來自工作空間，其中包含機械手臂以及環境障礙物的幾何資訊。首先我們將說明演算法中所需要的環境設定，接下來則從整體的路徑規劃，講解兩個機械手臂如何從起始位置到達終點，之後會從細節上，例如手臂的移動和轉動等動作，為我們的演算法作說明。. 4.1 環境設定本論文所討論的環境主要是在封閉的空間中，兩個手臂的路徑規劃，如圖 4.1 所示，我們用程式模擬出雙機械手臂的工作環境，空間中主要有四個元素，分別為主臂 (Master)、僕臂 (Slave)、主臂的終點 (Goal of Master)以及僕臂的終點 (Goal of Slave)，而兩個終點均用一平面來表示之，且兩個手臂的最後一節均固定於環境底部的某一位置 (基座)。在整個規劃的過程當中，我們將設法讓主臂以及僕臂分別逐步前進，最後到達其終點平面。. 21.

(32) 圖 4.1 雙機械手臂環境基本模型，包含左邊的主臂(Master)、右邊的僕臂(Slave)、以及主臂終點 (Goal of Master)和僕臂終點(Goal of Slave)。. 圖 4.2 物體的取樣點(m=7,n=2)，m 和 n 可是情況作改變。. 如圖 4.1 所示，我們假設機械手臂的各節為長方體。環境中障礙物與物體之間的推斥力，如果是計算面與面之間的推斥力，那麼將需要作複雜的四重積分。因此如同前一章所討論到，我們可以為這些長方體選出取樣點，以取樣點來代表各節，圖 4.2 繪出了其中一種取樣點的取樣方法，其參數 (m 表示長邊的取樣點數，n 表示短邊的取樣點數) 可視情況改變，一般而言，愈複雜的環境需要愈多的取樣點以獲得較好的結果，而若是單純的環境，甚至取樣八個頂點即可。透過取樣點，就可以將環境中障礙物與物體之間的推斥力，簡化成許多面對點的推斥力的合力，也因此可以利用前述所提到的廣義位能場模型作計算。 22.

(33) 一般作連結物體或機械手臂的路徑規劃時，常使用的關節有下列幾種： (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 三維自由度關節例子 ─ 球形關節. 23.

(34) 4.2 路徑規劃演算法以前一節所列出的資訊為基礎，我們提出的演算法，將設法讓兩個手臂由原來的初始狀態，最終雙雙到達其終點平面，而更深入地來看，這一連串的動作包括了要設法讓手臂逐步前往終點的「前進」動作，讓手臂在受到推斥力的影響下，最終移動到位能低點的「移動平衡調整」動作，以及如同前一章所述的力矩觀念，有就是以關節為支點，同樣在推斥力影響下所作的「轉動平衡調整」動作。開始. N : 手臂的節數 No. n=N. 首節前進首節移動調整首節轉動調整. 到達終點?. n=n-1. 結束. Yes. 第 N 節往前連接第 N 節轉動調整. n=n-1. No. n=2? Yes 最後兩節調整. 圖 4.4 單一手臂路徑規劃流程圖，其中 N 代表手臂的節數。. 24.

(35) 雖然演算法中必須考慮兩個手臂間複雜的避碰問題，但若先忽略避碰問題，並分開來看，其實兩個手臂的基本路徑規劃法是大同小異的，圖 4.4 簡易繪出單一手臂的基本路徑規劃演算法，其中包含了不少的細部演算法，將在下一小節分別詳述。由圖中可以看出，整個演算法其實可以大致分開為三個處理區域，第一個處理方塊處理手臂第一節的前進，移動調整，和轉動調整，第二個處理方塊則是規劃從第二節一直到倒數第三節的動作，由於除了第一節以外的的每一節手臂都需要連接前一節，所以每一節的端點都是固定在某一個位置，由於無法移動，物體的動作只需要作轉動調整以及連接的動作，但最後兩節的情況特殊，由於最末一節是固定於其基座(basement)，因此我們將最後兩節獨立出來處理，也就是圖中的第三個處理方塊，詳細過程將在以下小節中討論。. 4.2.1 演算法細部解析. 開始. 計算終點平面對首節的斥力，以力的反方向前進距離δ. 碰撞偵測並處理. 結束圖 4.5 首節前進的工作流程圖，δ為可變參數，視情況調整。. 圖 4.5 說明首節前進的工作流程，為了在三度空間中將手臂往終點平面方向拉，我們利用廣義位能場計算出終點平面對第一節的吸引力方向 25.

(36) (i.e. 推斥力的反方向)，並將首節往此吸引力方向移動距離δ，前進之後，將會判斷移動過程中有無碰撞發生，如果有，則將手臂退回先前的狀態，並以δ的一半作為前進的距離，以此重複，直到沒有碰撞發生為止。其中碰撞的偵測與處理，在以下討論會時常發生，因此我們將在 4.3 節一併作解說。. 圖 4.6 手臂受到終點平面(Goal Plane)的引力，並在以引力為法向量的平面(V_Plane)上作調整。. 26.

(37) N_Loop : 二分搜尋的次. 開始. 數，預設為 5. 求出以推斥力為法向量的平面(V_Plane). N_Loop = 5; 求出環境對首節的力總合，投. N_Loop = 0?. 影在 V_Plane 稱作 F. No 往 F 方向移動ε單位，並作碰. F = F’; ε=ε/ 2;. 撞偵測處理，重新算出新的合. 往 F 方向移動ε單位，並重新算出新. 力，投影在 F 上並將之稱作 F’. 的合力，投影在 F 上並將之稱作 F’. N_Loop = N_Loop - 1;. Yes. No F 與 F’同方向?. Yes 調整前後位置. No. 是否相同. Yes 結束. 圖 4.7 首節移動調整的工作流程圖，N_Loop 為可變參數，代表二分搜尋的次數. 當首節完成前進的動作之後，接下來我們要在前進後的位置找出以受力方向為法向量的平面(以下稱 V_Plane)，並在這個平面上作移動調整，圖 4.6 繪出終點平面和調整平面，以及機械手臂的相對關係，圖中虛線代表該處受終點平面的吸引力方向。圖 4.7 說明首節移動調整的工作流程。在每一次計算移動平衡時，我們先求出物體所受的合力，投影在 V_Plane 上稱作 F，並針對 F 的方向上尋找位能的最低點。我們先將物體往 F 的方 27.

(38) 向移動 ε 單位並作碰撞偵測處理，然後對移動後的物體在計算一次合力，並將之投影到 F 上得到 F＇。如果 F＇與 F 的方向相同則往 F 的方向繼續移動 ε 單位和作碰撞偵測處理；反之，表示物體受力的方向已經改變，我們將移動的步伐減半，並且將物體移動方向反向，利用這樣的二分搜尋法就可以在 F 方向上找到位能最小的位置，在我們的設計中，反向後的二分搜尋次數固定為五次，直接獲得的好處是可以提高運算的效率，且事實上在經過一定的次數搜尋，物體已經能夠相當接近位能低點，且位置已在計算誤差之內。在每次針對一個方向調整完成之後，演算法將會比對調整前後的位置，若位置相差很小，則我們判斷物體已經到達或極接近在 V_Plane 上的位能最低點，因此結束調整動作，反之則繼續尋找下一個調整的方向，重複以上的動作，直到結束為止。除了移動平衡調整之外，各節還會有轉動平衡調整的動作。若我們將物體的一點固定，此點我們取連結物體該節前端的中心位置(支點)，取樣點所受推斥力的合力會對物體產生一個力矩，以力矩為旋轉軸，我們可以在單一自由度上求得轉動平衡。如同移動平衡的計算方式，我們先求出所有取樣點所受到的合力矩 Τ，然後以合力矩 Τ 為轉軸，將物體旋轉角度. α 並作碰撞偵測處理，對調整過後的物體在計算合力矩，並將之投影到 Τ 上我們可以得到 Τ＇，如果 Τ＇與 Τ 方向相同時，我們繼續對以 Τ 為旋轉軸旋轉 α 並作碰撞偵測處理；反之則將旋轉角度減半，且旋轉軸倒轉(可直接取反向的 T＇為新旋轉軸)，利用這樣的二分搜尋法找到轉動平衡點。與移動平衡相同，反向後的二分搜尋固定為五次，詳細的演算法流程如圖 4.8。. 28.

(39) N_Loop : 二分搜尋的次開始. 數，預設為 5. N_Loop = 5; 求出環境對首節的合力矩 T. N_Loop = 0? No 以 T 為旋轉軸，該節前端為支點旋轉角度 α，並作碰撞偵測處理之後再計算合力矩，並投. T = T’; α= α / 2; 以 T 為旋轉軸，該節前端為支點選轉角度 α，之後再計算合力矩，並投影. 影在 T 上稱為 T’. 在 T 上稱為 T’. N_Loop = N_Loop - 1; No T 與 T’同方向?. Yes. Yes 調整前後位置是否相同. No. Yes 結束. 圖 4.8 轉動調整的工作流程圖，N_Loop 為可變參數，代表二分搜尋的次數. 在圖 4.4 中的第二個方塊中，有個往前連接的動作，也就是在調整完之後，由於前一節的位置已經產生了變化，在實際機械手臂的應用時，後端的手臂小節將會直接受到牽引移動，由於前一節在調整姿勢時，物體的位置受到位能場的影響會盡量遠離障礙物，所以在前進步伐δ不大的情況之下，物體連接的動作可以看作是剛體運動。在我們的模擬系統中，直接用往前連接，也就是後面小節的前端，接到前一節的最末端來完成這個動 29.

(40) 作。. 圖 4.9 最末兩節的調整示意圖，其中圓 O 是可活動的區域. N_Loop : 二分搜尋的次. 開始. 數，預設為 5 求出最末兩節可以活動的圓，並在圓上找一連接點 P，分別將最末兩節的頭尾接上 P 求出環境對末兩節的合力矩，投影在該手臂基地與第三. N_Loop = 0?. 節尾端的連線向量，稱為 T. No 以 T 為旋轉軸，該手臂基地為支. T = T’; α= α / 2;. 點，兩節同時旋轉角度 α，並作. 以 T 為旋轉軸，該手臂基地為支點，. 碰撞偵測處理，之後再計算合力. 兩節同時旋轉角度 α，之後再計算合. 矩，並投影在 T 上稱為 T’. 力矩，並投影在 T 上稱為 T’. N_Loop = N_Loop - 1; No T 與 T’同方向?. Yes. Yes 結束. 圖 4.10 最末兩節調整工作流程圖，N_Loop 為可變參數，代表二分搜尋的次數. 30.

(41) 最後兩節的調整類似於上述的旋轉調整，而不同之處如圖 4.9 所示，由於最後一節以及倒數第二節在此時都有一端已經被固定 (最末節尾端固定於基座，倒數第二節的前端固定於第三節尾端)，因此可在調整中可以活動的範圍僅剩三個自由度，包括圖中的圓 O，調整時先求出環境對末兩節的合力矩，投影在該手臂基座與第三節尾端的連線向量，稱為 T，再以 T 為旋轉軸，該手臂基地為支點，兩節同時旋轉角度 α 並作碰撞偵測處理，之後再計算合力矩，並投影在 T 上稱為 T’，若 T 與 T’ 同向，則繼續旋轉角度 α 並作碰撞偵測處理，若反向則進入二元搜尋，方式與旋轉調整相同，差別在於最末兩節只對一個自由度作調整，找到平衡點後就算完成最末兩節調整。. 4.2.2 兩個手臂之間的協調上述的演算法著重在單一手臂的路徑規劃上面，而在雙機械手臂的運動環境下，各個手臂的位置是不斷地在變動，我們必須在運動的過程中同時紀錄並運用現有的資訊，才能在下一個時間點規劃出正確的步伐。如同先前所提到，我們是利用主僕式架構來處理這樣的問題，除此之外，我們的規劃方法是以交錯的方式進行，在圖 4.4 的流程當中，若我們作以下定義：從最上方的前進動作，一直到最末兩節的調整，這樣的的過程進行一次，則稱作完成一步 (Step)。我們的協調方式將是，主臂走一步之後，僕臂接著依照規劃演算法也走一步，緊接著又換到主臂為下一步作規劃，以此類推，直到到達終點為止。在我們的演算法中，有關力與力矩的計算都是利用廣義為能場模型，也就是計算手臂上的取樣點和環境平面的作用力，而在主臂的前進過程當中，環境因素不包含僕臂在內，也就是主臂對於僕臂僅需作碰撞偵測，而不需要對僕臂計算推斥力，因此過程中主臂將以類似單一手臂路徑規劃的方法往終點平面移動；僕臂所需要的環境因素則必須包含主臂，在僕臂每 31.

(42) 一次計算推斥力時，需將主臂視為障礙物，並閃躲之，因此我們必須同時計算原有的障礙物，以及主臂對僕臂所產生的推斥力，相加才是所求合力。又因為主臂是持續地在變化，僕臂必須不斷的獲悉主臂的幾何位置，因此才有前述主臂走一步，僕臂緊接著走一步的設計。. 圖 4.11 沒有碰撞偵測時，如果物體與障礙物發生碰撞，會因為取樣點受力方向不同而卡在障礙物上。. 4.3 手臂與障礙物間的碰撞偵測與處理當我們在移動或旋轉連結物體時，難以避免和障礙物發生碰撞。當發生碰撞而沒有偵測出來時，我們的位能場計算公式會使物體卡在穿過障礙物的面上動彈不得，如圖 4.11 所示，因此我們必須在發生碰撞前防止碰撞的發生。如果碰撞發生在連結物體第一節的移動與各節的轉動時，為了提高計算效率，我們利用檢查連結物體的取樣點，在移動或旋轉前、後所在位置形成的線段，與障礙物的面是否有交點完成碰撞偵測。當發生碰撞時，處理的方式是將參數 (前進距離δ或旋轉角度α) 減半，並以減半後的參數再一次前進或旋轉，重複此步驟直到不發生碰撞為止。圖 4.12 繪 32.

(43) 出碰撞的偵測與處理流程圖。此外為了防止機械手臂的邊穿過障礙物的面，我們必須檢查機械手臂的各邊與各個障礙物的面是否有交點，如果有表示有碰撞發生。由於機械手臂各節的實際行進方向可能是曲線而非直線，但是為求計算簡單我們只以直線路徑來逼近曲線路徑，並檢查此直線是否穿過障礙物。由於直線與曲線當中所產生的誤差，導致可能會有某些碰撞無法被偵測出，我們可以利用將障礙物體積膨脹擴大一些或縮小每次前進的距離來盡可能減少可能發生的問題。. 開始. 有碰撞發生?. Yes. 參數(δ或α)減半，並重新前進δ或旋轉角度α. No. 結束. 圖 4.12 碰撞的偵測與處理流程圖。. 33.

(44) 第五章. 實驗結果與問題討論. 本章內容將依照前一章所提及的雙機械手臂路徑規劃演算法，利用程式模擬、實驗。我的的程式主要是用 Borland C++ Builder 6.0 開發，配合 Microsoft Visual Studio .NET 及其 MFC，測試的電腦環境為 PENTIUM IV 2.4G 和 512M RAM，作業系統則是 Windows XP。第一節我們將針對核心的移動平衡以及轉動平衡，對簡單的例子作測試，之後則會利用我們的系統，對各種不同環境作模擬與實驗，最後則是各項的比較與探討。. 5.1 移動平衡以及轉動平衡由於移動平衡以及轉動平衡是我們演算法中的核心調整動作，所有其他的流程都是以這兩種動作為基礎而延伸，因此本節我們將利用簡單的例子，測試演算法的適用性。. 圖 5.1 環境中有一個在角落中的小方塊，將會受力影響而運動。. 34.

(45) 首先我們對移動平衡演算法作實驗，圖 5.1 是我們設計的一個簡單例子，圖中外圍是一個擁有六個平面的大方塊，包含在其中為一個小的長方形方塊，在這個例子當中，依照廣義位能場模型。我們取樣長形方塊的八個頂點分別對環境中的六個平面作推斥力的運算，並依照二分搜尋法作調整，圖 5.2 顯示調整時以及調整後的結果。. 圖 5.2 轉動平衡調整步驟(a)第一次調整，共七步(b)第二次調整，共六步(c)第三次調整，共六步 (d)到達目的地，環境方塊的中央。. 35.

(46) 圖 5.2 中的 (a) 顯示在角落中的方塊，第一次調整的結果，包括二元搜尋前調整兩步，二分搜尋五步，加起來共調整七步；因為第一次的調整過後並未到達我們預期中的環境中央位置，因此會有第二以及第三次的調整，其過程如圖 5.2 的 (b) 跟 (c)，最後終於到達終點 (環境中央)，如 (d) 所示。圖 5.3 則是俯視圖，證明我們的移動平衡演算法確實讓小方塊正確到達終點。. 圖 5.3 小方塊經過調整後正確到達環境中央。. 圖 5.4 則是我們測試轉動平衡演算法，測試方法是將一個對 X、Y、Z 軸分別旋轉 30 度的小方塊，放進一個正方且沒有旋轉過的大方塊，且處於正中央，如圖 5.4 的 (a) 所示，類似於移動平衡調整，這個例子總過作了四個方向的調整，其調整過程如圖中的 (b) (調整共十二步)、(c) (調整共八步)，(d) (調整共六步)以及 (e) (調整共六步)。最終到達終點狀態，以此例而言，終點狀態為：小方塊旋轉到各邊與大方塊平行為止。圖中的 (f) 顯示調整後的結果。由調整的過程中可以發現，第一次的調整將是對偏差最多的方向或角度作調整，也因此通常第一次調整的步數都是最多，之後逐次減少，而經過測試之後，移動平衡及轉動平衡演算法確實能達到我們的要求而順利到達終點，之後的實驗將利用之，並著重在完整雙手臂系統的實驗。 36.

(47) 圖 5.4 轉動平衡範例(a)一個分別對三個軸旋轉 30 度，在正中央的方塊(b)第一次調整，共 12 步(c)第二次調整，共 8 步(d)第三次調整，共六步(e)第四次調整，共六步(f)調整結束。. 37.

(48) 5.2 三度空間雙機械手臂範例實測. 圖 5.5 例一的環境及結果(a)初始狀態，包含手臂及外圍障礙物(b)調整軌跡。. 例一 (長方體通道)：本例為一個基本三度空間雙機械手臂路徑規劃問題，環境中僅有兩個機械手臂 (各七節) 和各自的終點平面，以及外圍的邊界障礙物，如圖 5.5 (a)。而圖中的 (b) 則是調整的軌跡圖，由軌跡圖可以清楚的看出，左邊的僕臂在過程中有閃避右邊的主臂的行為，而最終也如預期的雙雙到達終點平面。圖 5.6 的 (a) 是例一俯視角度的軌跡圖，而為了作比較，我們試著將主臂固定不動，另外對僕臂作了一次規劃，且軌跡如圖中的 (b)，由軌跡圖可以發現僕臂在不同情況之下的運動情況，的確都是在接近主臂時作閃避的動作。圖 5.7 逐步展示例一的調整過程。其中主臂在第 8 次調 38.

(49) 整時到達終點平面，如圖中的 (h)，僕臂則是在第九次調整 (如同中的 (i)) 到達終點平面，因而結束整個路徑規劃流程。. 圖 5.6 例一軌跡圖比較(a)例一俯視角軌跡(b)固定例一中的主臂所形成的軌跡。. 圖 5.7 例一過程中的調整細部過程，(a)~(i)分別顯示調整過程的九次調整。. 39.

(50) 圖 5.8 例二中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。. 例二 (寬度縮減通道)：圖 5.8 繪出例二的初始位置，障礙物以及其他資訊。. 圖 5.9 例二的行進軌跡圖(a)前視角度(b)俯視角度。. 40.

(51) (k) 圖 5.10 例二的調整過程，在(k)時主臂及僕臂同時到達終點平面。 41.

(52) 例二中的障礙物限制了兩個手臂的移動範圍，而在調整的過程中，主臂大致是在可移動範圍的中央位置作前進及調整的動作，而僕臂則由於可移動的範圍狹窄，且原本前進路線又與主臂有所重複，因此由軌跡圖 5.9 可以發現在調整的過程中，僕臂的閃避以及轉彎的動作，最後前進路線大致是在主臂以及障礙物之間，在第 11 次規劃時，主臂以及僕臂雙雙到達其終點平面而完成整個流程。圖 5.10 繪出例二在每一次規劃後的情形，其中可以明顯看出第 7 步開始 (圖中的 (g))，僕臂開始作出向左邊，也就是向內閃躲的動作。. 圖 5.11 例三中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。. 例三 (有障礙物通道)：本例場景類似於例二，不同之處是我們在場景的中央附近放置了一個正立方體障礙物，而這個障礙物切斷了原本例二結果中兩個手臂的前進軌跡，在本例中我們將觀察在前進路線有障礙物的情況之下，兩個手臂的移動情形。圖 5.12 為例三從開始到結束的移動軌跡圖，由圖中的前視圖 (a) 可以發現，受到了圖中央小方塊的影響，兩個手臂規劃的路徑偏向下方，也就是從下方繞過方塊而到達終點平面，另外僕臂由於同時要閃躲主臂以 42.

(53) 及障礙物方塊，因此軌跡震盪較大，俯視圖 (b) 顯示主臂除了從下方，軌跡也偏向外側，另外也發現僕臂的移動情形，顯然要比例二中的軌跡要複雜許多，事實上在這個例子當中，主臂以及僕臂分別到第 14 次和第 13 次才分別到達終點平面，顯示環境愈複雜，確實是需要更多的調整以到達目標。. 圖 5.12 例三的行進軌跡圖(a)前視角度(b)俯視角度。. 43.

(54) 圖 5.13 例四中的初始位置以及障礙物資訊(a)前視圖(b)側視圖(c)俯視圖(d)斜角視圖。. 例四 (正方體工作空間)：本例場景為一正方體外圍方塊，如圖 5.13 所示，兩個手臂的基地分別固定在兩邊，而由側視圖 (b) 及俯視圖 (c) 可以知道，所設計的兩個手臂初始狀態，各節以及終點平面的中心點均是在同一個平面上 (平面 x =. 0)，也就是說它幾乎可以看成是一個二維的路徑規劃問題。但由於僕臂會受到主臂各平面的推斥力，實際的運動過程當中，主臂除了最末兩節的調整之外，的確是作了類似於二維平面的路徑規劃動作，這個路徑是其最短且最安全的路徑，相較於主臂，僕臂的規劃過程則複雜許多，本例主臂在第 9 步即到達終點平面，如圖 5.14 中的 (i)，但僕臂則因為受到主臂的推擠，一直到第 17 次規劃才到達終點 (圖 5.14 中的 (q))，另外從圖 5.15 的移動軌跡圖可以發現，僕臂受力而繞過主臂到達終點的過程，由俯視圖 (b) 尤其明顯，此外觀察主臂移動過程，的確除了最末兩節的特殊調整之外，其餘各節的調整路徑均在同一平面 (平面 x = 0) 上。 44.

(55) 圖 5.14 例四的調整過程，在(i)時主臂到達終點平面，在(q)時僕臂到達終點平面。. 45.

(56) 圖 5.14 (續). (b) 圖 5.15 例四的行進軌跡圖(a)前視角度(b)俯視角度。. 46.

(57) 5.3 運算複雜度與效能本節將分別對上述四個例子有關調整次數以及計算時間作統計整理與比較。我們將統計首節的平移調整次數，其他各節的旋轉調整次數，以及各項運算時間統計和平均等資訊，並依照所獲得的資訊，作討論以及演算法的檢討。表 5.1 例一的動作資訊統計. 主臂動作資訊： Total number of end link translation 197 Total number of link rotation Link [1] : 110 Link [2] : 74 Link [3] : 67 Link [4] : 81 Link [5] : 111 Total number of rotation of final two links 48. Computation time (sec) 0.188 Number of steps 8. Computation time per step (sec) 0.024. 僕臂動作資訊： Total number of end link translation 189 Total number of link rotation Link [1] : 109 Link [2] : 139 Link [3] : 132 Link [4] : 165 Link [5] : 171 Total number of rotation of final two links 54. Computation time (sec) 2.093 Number of steps 9. Computation time per step (sec) 0.233. 表一列出了例一的各項動作統計，並針對主臂以及僕臂作比較，其中僕臂由於必須在過程中閃躲主臂，因此調整的次數較多，且也影響到最後的運算時間，另外值得注意的是，僕臂所計算的時間遠較主臂為多，且比例大過調整的次數比，原因是因為僕臂將主臂的每個平面視為障礙物，因此在每次計算推斥力時花了相對較多的時間，因此我們可以判斷，推斥力的計算在整個規劃的過程中佔了大多數的計算時間，雖然如此，不論是主臂和僕臂，其平均每一步的運算時間都在 0.3 秒以下，如此快速的運算時 47.

(58) 間對於將來若要應用到即時的規劃系統，仍然有很大的優勢。. 表 5.2 例二的動作資訊統計. 主臂動作資訊： Total number of end link translation 260 Total number of link rotation Link [1] : 126 Link [2] : 104 Link [3] : 151 Link [4] : 179 Link [5] : 157 Total number of rotation of final two links 66. Computation time (sec) 0.797 Number of steps 11. Computation time per step (sec) 0.072. 僕臂動作資訊： Total number of end link translation 266 Total number of link rotation Link [1] : 148 Link [2] : 164 Link [3] : 156 Link [4] : 174 Link [5] : 171 Total number of rotation of final two links 66. Computation time (sec) 3.203 Number of steps 11. Computation time per step (sec) 0.291. 例二中的場景是將兩個障礙物放置於例一的場景當中，讓兩個手臂的活動範圍受到限制，我們可以預期由於受到更多障礙物的影響，無論主臂或是僕臂均會有更高的計算量和計算時間，表 5.2 為例二的各項統計資訊。由表中可以觀察出，僕臂相較於主臂，雖然規劃的次數同為 11 次，然而運算時間還是僕臂較多。另外最後的運算時間較例一有所增加，但並不僅僅是規劃的次數增加，且每一節的調整次數和時間也相對增加，也因此最後平均出來每一步規劃所花的時間要比例一要多出約 0.05 秒的差距，儘管如此，這樣的速度仍然被認為是非常迅速。. 48.

(59) 表 5.3 例三的動作資訊統計. 主臂動作資訊： Total number of end link translation 543 Total number of link rotation Link [1] : 272 Link [2] : 213 Link [3] : 179 Link [4] : 191 Link [5] : 178 Total number of rotation of final two links 84. Computation time (sec) 1.703 Number of steps 14. Computation time per step (sec) 0.122. 僕臂動作資訊： Total number of end link translation 437 Total number of link rotation Link [1] : 191 Link [2] : 189 Link [3] : 162 Link [4] : 214 Link [5] : 169 Total number of rotation of final two links 78. Computation time (sec) 4.203 Number of steps 13. Computation time per step (sec) 0.323. 例三是將一個障礙物方塊，放在例二場景中兩個手臂的前進方向，表三列出各項資訊。在這個例子當中，主臂要比僕臂早一步到達終點平面，且調整的次數甚至多過僕臂的調整次數，然而僕臂的規劃時間仍然多出主臂規劃時間不少，證明計算時間多花在推斥力的計算。此外，相較於例二，本例僅多一個六個面的方塊障礙物，卻對計算時間產生一定程度的影響 (各增加約 0.5 秒和 1 秒)，顯示手臂的前進過程中若存在障礙物，對於計算時間和效率的影響會非常顯著。但平均每一步的計算時間(0.122 秒和 0.323 秒)，仍然相當迅速。. 49.

(60) 表 5.4 例四的動作資訊統計. 主臂動作資訊： Total number of end link translation 175 Total number of link rotation Link [1] : 117 Link [2] : 158 Link [3] : 170 Link [4] : 132 Link [5] : 165 Total number of rotation of final two links 36. Computation time (sec) 0.235 Number of steps 9. Computation time per step (sec) 0.026. 僕臂動作資訊： Total number of end link translation 433 Total number of link rotation Link [1] : 329 Link [2] : 524 Link [3] : 634 Link [4] : 602 Link [5] : 434 Total number of rotation of final two links 102. Computation time (sec) 6.312 Number of steps 17. Computation time per step (sec) 0.371. 例四較為特殊，在先前的實驗中，我們提到這個例子對主臂而言是近似於非常簡單的二維路徑規劃問題，從表四的統計資料可以發現，主臂在這個實驗中確實僅僅花了非常少的時間，相較之下，僕臂在此例的調整次數和計算時間都是主臂的好幾倍，此因僕臂在過程當中有作出向後倒退並閃躲的動作，因此一直到第 17 步才到達終點平面，也才會造成計算複雜程度暴增，而計算平均之後，主臂因為空間中沒有其他障礙物，因此規劃的時間與例一幾乎相同，而僕臂平均規劃時間則約 0.4 秒，稍稍多於前三個例子。. 50.

(61) 5.4 問題與討論在[37]有討論到用二度空間的位能場模型在某些情況下，機械手臂會因為順著位能場的方向而產生不自然的結果，這個問題在我們使用的三度空間位能場模型依舊存在。由於物體愈靠近邊界所受的推斥力愈大，因此我們若把兩個手臂的基座設在靠近與終點反向邊界的位置，規劃過程中有可能發生整個手臂的扭曲變形，甚至因為被推擠太過嚴重，造成最後兩節無法接上而使得手臂斷裂。解決方式是針對各個手臂，在計算推斥力時，對於終點相對方向的平面所算出來的推斥力，以較少的權值相加，如圖 5.16，對於右邊的主臂而言，由於終點平面在左邊，因此主臂在對最右邊的平面作推斥力的計算時，就可以較小的權重來計算，本篇論文均採用原來力的五分之一，因此主臂在往左邊移動時，會以類似首節牽引拉動後面的小節前進，而不是受到力的不當影響，造成整個手臂往中間推擠。圖中左邊的僕臂調整則是類似主臂的計算方式，受到更改權重的平面為最左邊的平面。. 圖 5.16 解決位能場先天問題的方式，對特定平面的授與不同權重值。. 51.

(62) 在大部分的環境中，由於僕臂需要閃避主臂而調整，因此僕臂到達終點平面時間會較主臂的到達時間晚，但仍然有可能會有僕臂先到達的情況發生(如例三)，而在這些僕臂先到達終點的例子當中，有一部分會因為僕臂在到達後不再調整，主臂則因繼續前進而有可能與僕臂相撞的情形。解決這種問題的方式有幾種： 1. 捨棄交錯式的規劃方法，讓主臂先規劃其路徑，到達終點之後，再讓僕臂開始規劃。 2. 主臂先規劃前進路徑，僕臂則在主臂規劃的過程中先不前進，僅作各節轉動調整來閃避主臂，其運動過程類似兩端固定的手臂，在主臂規劃完成到達終點之後，僕臂開始作前進路徑規劃。 3. 利用原本的演算法先規劃完成，但僕臂到達之後，仍繼續作各節的轉動調整，其運動過程類似兩端固定的手臂。然而這樣的情況並不常見，因此可當作是特殊情況來處裡。. 52.

(63) 第六章. 結論. 本論文的目的在於對三維空間中的雙機械手臂作路徑規劃。我們所提出的演算法，利用三度空間的廣義位能場模型，在工作空間中使障礙物對手臂產生推斥力，不但保證手臂前進時所經過的路徑不會與障礙物或是彼此碰撞，所規劃出來的路徑也能夠近乎平滑且連續。相對於一般以組態空間為基礎的演算法，我們所提出的方法對於具有高自由度的物體，並不需要花費大量的時間與資源建構組態空間，因此在一般的環境之下，搜尋速度相當快速而且正確。本論文利用「主僕式架構」以及「交錯式運行」來解決兩個機械手臂的協調問題，由實驗證明，主僕式架構的確適合應用於雙機械手臂的環境當中，透過推斥力讓僕臂在規劃路徑的過程中閃避主臂，可以讓兩個手臂雙雙到達目的地，且透過交錯式的運行方法，在大部分的情況之下也都能讓兩個手臂規劃完成各自的路徑。. 53.

(64) 參考文獻 [1]. J.C. Latombe, “Motion planning: a journey of robots, molecules, digital actors, and other artifacts,” International Journal of Robotics Research, vol. 18, no.11, pp. 1119-1128, Nov. 1999. [2] T. Lozano-Perez, “Spatial planning: a configuration space approach,” IEEE Trans. on Computers, vol. C-32, no. 2, pp. 108-120, Feb.1983. [3] R. Brooks and T. Lozano-Preze, “A subdivision algorithm in configuration space for findpath with rotation,” IEEE Trans. on Sys., Man, Cybern., vol. 15, no.2, pp. 224-233, March-April 1985. [4] L. E. Kavraki, P. Sverstka, J. C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Trans. on Robotics and Automation, vol. 12, no. 4, pp. 566-580, Aug. 1996. [5] J-H. Chuang and N. Ahuja, “An analytically tractable potential field model of free space and its application in obstacle avoidance,” IEEE Trans. on Sys., Man, Cybern., Part B: Cybernetics, vol. 28, no. 5, pp. 729-736, 1998. [6] J-H. Chuang, “Potential-based modeling of three-dimensional workspace of the obstacle avoidance,” IEEE Trans. on Robotics and Automation, vol. 14, no. 5, pp. 778-785, 1998. [7] Y.K. Hwang and N. Ahuja, “Gross motion planning a survey,” ACM Computer Survey, vol. 24, no. 3, pp. 219-291, 1992. [8] P. Khosla and R. Volpe, “Superquadric artificial potentials for obstacle avoidance and approach,” Proc. IEEE Intl. Conf. Robot. And Auto., pp. 1778-1784, 1988. [9] J-H. Chuang, C-H. Tsai, W-H. Tsai, and C-Y. Yang, “Potential-based modeling of 2-D regions using nonuniform source distribution,” IEEE Trans. on Sys., Man, Cybern., Part A: Systems and Humans, vol. 3, no. 2, pp. 197-202, 2000. [10] K. P. Valavanis, T. Herbert, and N. C. Tsourveloudis, “Mobile robot navigation in 2-D dynamic environments using electrostatic potential fields,” IEEE Trans. on Sys., Man, Cybern., Part A: Systems and Humans, vol. 30, no. 2, pp. 187-196, 2001. [11] C-H. Tsai, J-S. Lee, and J-H. Chuang, “Path planning of 3-D objects using a new workspace model,” IEEE Trans. on Sys., Man, Cybern., Part B: Cybernetics, vol. 31, no. 3, pp. 405-410, Aug. 2001. [12] C. Chang, M-J Chung, and B-H Lee, “Collision Avoidance of Two 54.