演算法細部解析

開始

計算終點平面對首節的斥力，

以力的反方向前進距離δ

結束 碰撞偵測並

處理

圖 4.5 首節前進的工作流程圖，δ為可變參數，視情況

圖 4.5 說明首節前進的工作流程，為了在三度空間中將手臂往終點平 面方向拉，我們利用廣義位能場計算

調整。

出終點平面對第一節的吸引力方向

(i.e. 推斥力的反方向)，並將首節往此吸引力方向移動距離δ，前進之 後，將會判斷移動過程中有無碰撞發生，如果有，則將手臂退回先前的狀 態，並以δ的一半作為前進的距離，以此重複，直到沒有碰撞發生為止。

其中碰撞的偵測與處理，在以下討論會時常發生，因此我們將在 4.3 節一 併作解說。

面(Goal Plane)的引力，並在以引力為法向量的平面

圖 4.6 手臂受到終點平 (V_Plane)上作調整。

N_Loop : 二分搜尋的次

向移動ε單位並作碰撞偵測處理，然後對移動後的物體在計算一次合力，

並將之投影到F上得到F＇。如果F＇與F的方向相同則往F的方向繼續 移動ε單位和作碰撞偵測處理；反之，表示物體受力的方向已經改變，

我們將移動的步伐減半，並且將物體移動方向反向，利用這樣的二分搜尋 法就可以在F方向上找到位能最小的位置，在我們的設計中，反向後的二 分搜尋次數固定為五次，直接獲得的好處是可以提高運算的效率，且事實 上在經過一定的次數搜尋，物體已經能夠相當接近位能低點，且位置已在 計算誤差之內。在每次針對一個方向調整完成之後，演算法將會比對調整 前後的位置，若位置相差很小，則我們判斷物體已經到達或極接近在 V_Plane 上的位能最低點，因此結束調整動作，反之則繼續尋找下一個調 整的方向，重複以上的動作，直到結束為止。

除了移動平衡調整之外，各節還會有轉動平衡調整的動作。若我們將 物體的一點固定，此點我們取連結物體該節前端的中心位置(支點)，取樣 點所受推斥力的合力會對物體產生一個力矩，以力矩為旋轉軸，我們可以 在單一自由度上求得轉動平衡。如同移動平衡的計算方式，我們先求出所 有取樣點所受到的合力矩Τ，然後以合力矩Τ為轉軸，將物體旋轉角度 α 並作碰撞偵測處理，對調整過後的物體在計算合力矩，並將之投影到 Τ 上我們可以得到 Τ＇，如果 Τ＇與Τ 方向相同時，我們繼續對以 Τ 為旋轉軸旋轉 α 並作碰撞偵測處理；反之則將旋轉角度減半，且旋轉軸 倒轉(可直接取反向的 T＇為新旋轉軸)，利用這樣的二分搜尋法找到轉動 平衡點。與移動平衡相同，反向後的二分搜尋固定為五次，詳細的演算法 流程如圖 4.8。

N_Loop : 二分搜尋的次

作。

最後兩節的調整類似於上述的旋轉調整，而不同之處如圖 4.9 所示，

由於最後一節以及倒數第二節在此時都有一端已經被固定 (最末節尾端 固定於基座，倒數第二節的前端固定於第三節尾端)，因此可在調整中可 以活動的範圍僅剩三個自由度，包括圖中的圓 O，調整時先求出環境對末 兩節的合力矩，投影在該手臂基座與第三節尾端的連線向量，稱為 T，再

以T 為旋轉軸，該手臂基地為支點，兩節同時旋轉角度 α 並作碰撞偵測處

理，之後再計算合力矩，並投影在T 上稱為 T’，若 T 與 T’ 同向，則繼續

旋轉角度α 並作碰撞偵測處理，若反向則進入二元搜尋，方式與旋轉調整

相同，差別在於最末兩節只對一個自由度作調整，找到平衡點後就算完成 最末兩節調整。