單手臂路徑規劃演算法

以4.1 節做為基礎，我們所要提出的演算法，目的是要利用機械手臂將被傳 遞物，由初始的起點，搬運到終點平面，這一連串的「搬運」動作包括了要使被 傳遞物或手臂首節「前進」的動作，讓手臂首節（end link）或被傳遞物受到推 斥力的影響下，能夠移動到位能低點的「移動平衡調整」動作，然後再利用推斥 力影響下，以各節關節為支點，做「轉動平衡調整」，目的是要促使整隻手臂和

20

被傳遞物都處於位能低點，即遠離障礙較安全的地方。但是除了受到障礙面的推 斥力影響之外，我們在「轉動平衡調整」時，另外加入了手臂基座的吸引力，以 克服機械手臂太過於受到障礙面限制的困難，詳細原因會在第4.4 節做說明。

雖然我們模擬的是一個多機械手臂的系統，但若單就每隻手臂的運動行為看 來，其實每隻手臂的基本路徑規劃法是相同的，圖4.4 簡易繪出了單一手臂在未 拿著被傳遞物時的基本路徑規劃演算法，若手臂拿著被傳遞物，則可將被傳遞物 視為手臂的首節（n = N，N 為手臂總節數），且手臂的總節數加一。而演算法的 細部，會在下一節詳述。由圖4.4 大致可見，每隻手臂基本的路徑規劃只要分為 四個處理方塊，第一個處理方塊處理手臂首節朝目標瓶頸面前進；第二個處理方 塊則是首節的移動調整和轉動調整；第三個處理方塊則是依序處理從第二節到倒 數第三節的連接和轉動調整，在此所謂的連接是將每一節手臂接到前一節的關 節，所以在此的每節手臂有一端其實是被固定住的，所以只能做轉動調整；第四 個處理方塊則處理手臂最後兩節的連接和轉動調整，將最後兩節獨立出來做處理 的主要原因，是因手臂的末節必須連接到手臂的基座，不能任意隨推斥力來調 整。後面的幾小節，我們將對首節前進、移動調整、轉動調整和手臂末兩節的調 整做特別說明。

21

圖4.4 單一手臂路徑規劃流程圖，其中 N 為手臂的總節數。

4.2.1

如圖 4.5，在三度空間中，為了迫使手臂往目標瓶頸面方向前進，我們利用 廣義位能場計算出目標瓶頸面對首節的吸引力方向（即推斥力的反向），並將首 節手臂朝此吸引方向移動距離δ，隨即判斷是否和障礙面產生碰撞，若有碰撞發 生，則將首節恢復原本的狀態，重新以δ/2 作為前進的距離，重覆此步驟直至沒 有碰撞為止，但若δ的值小至某值之後仍然有碰撞發生，則視首節已無法前進。

前面所提到的碰撞偵測與處理在之後的討論時常會見到，因此我們將在4.5 節一 併作說明。

22 start

Move The end link by toward guide

plane Calculate the attractive force

end no Collision?

= /2

yes

圖4.5 首節前進的規劃流程圖。

23

4.2.2

圖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＇同向則繼續將步伐減半後前進一步，若

V_Plane

goal plane

forward direction

24

Find V_Plane using attractive force of guide

plane

Calculate the total repulsive force toward the end link and find its

projection on V_Plane, F

Record present position as p

Record position p of the end link and calculate the movement

D = | p - p | D > accuracy yes

end

no

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 首節移動調整的工作流程圖。

25

圖4.8 五階二分搜尋的示意圖。

4.2.3

手臂除了首節要做移動調整之外，其它各節還必須向前一節連接，導致各節 一端被固定，所以為了達到位能低點的狀態，我們使用轉動調整的動作來完成目 的。若我們將物體的一點固定，此點是我們取連結物體該節前端的中心位置（支 點），取樣點所受環境推斥力與基座吸引力的合力會對支點產生一個力矩，而在 4.4 節說明為何要加上基座吸引力，以力矩為旋轉軸，我們可以在單一自由度上 求得轉動平衡。類似移動平衡的調整方式，我們先求出所有取樣點所受到的合力 矩Τ，然後以合力矩 Τ為旋轉軸，以旋轉角度 α為單位，把物體順著推斥力 的投影方向轉動至位能最低點附近，之後再做二分搜尋。我們先將物體旋轉角度 α減半後，對物體再計算合力矩，並將其投影到Τ上我們可以得到Τ＇，如果 Τ＇與Τ方向相同時，我們繼續對以Τ為旋轉軸旋轉α並作碰撞偵測處理；

反之則反向旋轉，利用這樣的二分搜尋法找到轉動平衡點。與移動平衡相同，我 們把進行五次二分搜尋後的所在位置當做位能最低點，並且也考慮到整個二分搜 尋前後的位移量，當位移量達到某精確值之內，轉動調整才算完成，詳細的演算 法流程如圖 4.9。

26 start

Calculate the torque T of the present link

Record present position as p

Record position p of the present link and calculate the movement

D = | p - p | D > accuracy

end

no

yes

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 轉動調整的工作流程圖。

27

整才算完成。

圖4.10 最末兩節的調整示意圖，其中圓 O 是可活動的區域軌跡，Ｔ是旋轉軸。

start

Connect the last two links with collision detection

Use T as rotation axis and check torque direction after each

rotation until direction is opposite

end

Continue 1D potential minimization, each time

halve the rotaion angle Adjust the joint angle of

the last two links with fixed joint locations

圖4.11 末兩節調整的工作流程圖。

base

28