座標校正

為了校正我們推導的旋轉矩陣，我們使用每格 45mm*45mm 的校正板，如 圖 3.14 所示。將深度攝影機固定於機器人上，校正板每次往後退 45mm 的距離，

針對校正板上黑白相間的交點做記錄，直到板子離開手臂的工作範圍外，此校正 的目的在於找出影像的誤差公式，減小機器人所觀測到物體位置與實際位置的誤 差，並有利於在接下來做為手臂與攝影機之間校正的基礎。

我們利用攝影機轉換後所得在世界座標系的座標與與實際座標求出兩者之 間的關係曲線，其中實際座標是利用每格為 45*45 的校正板量出與校正板與機器 人之間的距離。我們採用一次方程式來做為此漸進曲線，在將各點帶入此方程式 求得校正後的座標，在於實際座標比較後，所得 X、Y、Z 在各個位置上的誤差 值。X、Y、Z 三個方向的校正方程式分別為(3.38)、(3.39)、(3.40)，其中 x、y、

z 是由 Kinect 取得的座標，而 Xc、Yc、Zc則是校正後的座標。

( ) 0.9458 30.372

X xc  x (3.38) ( ) 0.9834 1.3503

Y yc  y (3.39) ( ) 1.0192 2.3659

Zc z  z (3.40) 校正實驗中我們 X、Y、Z 方向取 734 個點。X 誤差方面，我們測量由距離 機器人 470mm 到 670mm，經由校正之後，如圖我們可以將平均誤差控制於 2mm 之內，由圖我們可以看出，X 方向的誤差不會隨距離的遠近產生較大的誤差。

圖 3.15 校正後影像取得與實際 X 座標的誤差

在 Y 方向誤差方面，如圖可以看出，經過以上求出來的誤差方程式，可以修正 Y 座標的誤差達到 5mm 以內，其中，左右兩邊平均誤差約為 2mm，中間誤差約在 1mm 左右。

Z 為轉換後機器人上下的方向，我們可以由圖中看出，當座標越往下，由於 攝影機傾斜角度較大的關係，造成誤差較大，而越往上誤差則越小，但整體平均 誤差仍可控制於 2mm 左右。

圖 3.16 校正後影像取得與實際 Y 座標的誤差

圖 3.17 校正後影像取得與實際 Z 座標的誤差

3.2.3 手臂與影像轉換矩陣校正

在手臂抓取的過程中，機器人必須先由影像獲得目標點位置，再根據攝影機 所給的環境資訊進行路徑規劃。雖然可以直接由 3.2.1 所提及的公式由攝影機所 得座標轉換到機器人上，但將攝影機所獲得座標，與實際機械手臂所到達的實際 位置仍然有些許的誤差，也就是說，當攝影機定位經過校正後得到一個準確的座 標，將這座標下命令給機械手臂，透過手臂逆運動學轉換，使機械手臂移動至此 座標，此時機械手臂可能因為機械結構上的誤差或攝影機安裝無法準確與平台密 合，不一定會準確的到達攝影機所定位的目標點，所以，我們希望透過校正的動 作，使機械手臂能移動至攝影機所觀察到的目標點。

我們所設計的實驗是利用機械手臂抓取一個校正球，手臂每次會向前(X 方 向)或向左(Y 方向)移動 30mm，針對校正球上的同一個點做記錄，把在影像中所 看到的點記錄下來，直到移動手臂超出其工作範圍。移動的各點表示攝影機看到 目標物的位置，影像所記錄下來的各點表示機械手臂所移動到的位置，把過程中 記錄下來的座標點，於實際值進行比較，找出其關係方程式。

(3.41)、(3.42)、(3.43)為我們所求得的校正公式，當攝影機取得位置資訊 x、

圖 3.18 X 影像取得目標物位置校正後與手臂實際移動誤差

y、z 後，為了使機械手臂能順利到達此目標點，我們將轉換成機械手臂的位置 Xr、Yr、Zr，如此，機械手臂可以準確到達抓取的目標點。

( ) 1.0637 27.513

X xr  x (3.41) ( ) 1.0515 2.0126

Y yr  y (3.42) ( ) 0.9922 0.2881

Z zr  z (3.43) 圖 3.18 為影像取得目標物位置經校正後與手臂實際移動到座標的誤差，Xr 是與機器人前後的距離，我們讓機械手臂由距離機器人 480mm 移動到 600mm，

所測得的結果，經過校正後，平均誤差在 1mm。

在 Y 方向部分，是往機器人左右的方向，我們讓機械手臂由-120mm 移動至 120mm，所獲得結果，平均誤差 1.5mm。

圖 3.19 Y 影像取得目標物位置校正後與手臂實際移動誤差

圖 3.20 Z 影像取得目標物位置校正後與手臂實際移動誤差

在 Z 方向部分，是往機器人上下的方向，我們讓機械手臂由-180mm 移動至 60mm，所獲得結果，平均誤差 1.2mm。

3.3 討論

為了使機械手臂能被引導到影像所偵測到的目標位置，我們希望在各個座標 轉換的過程中，修正兩個不同座標系轉換所造成的誤差，所以，本論文提出了一 個手眼協調的校正方法。首先取得影像校正的結果，以此為基礎，在透過手部的 移動，記錄下手臂移動到達的位置，與影像所記錄的位置，找出其誤差值，此誤 差值表示其兩個不同座標系，經由不同的轉換所造成的誤差，將其誤差修正回 來，使手臂移動位置能與影像偵測位置一致。

第四章 即時閃避障礙物設計與控制

Potential field 對於引力與斥力的大小只根據距離物體的遠近，並只針對手臂周圍 的障礙物進行規劃，本論文所提出 Potential field 會根據物體面積的大小，與在 機械手臂空間中分佈的情況來決定出 Potential field 引力與斥力的大小和方向，

其設計的目的使機械臂能依據整體的環境進行規劃，使手臂能直接經由環境空曠 的區域到達目標點。

Finding the obstacles

Finding the target

The grasping pose

4.1 機械手臂安全抓取控制架構

調整 Potential field 的大小。在藉由 Potential field 算出環境對於機械手臂的引力 和斥力，最後由機械手臂讀回編碼器訊號在經由數值轉換成每軸角度，然後再經 由順向運動學得到現在的位置資訊，算出此刻須給各軸多少位置命令。

Target detection

Sensing the obstacles

Target moving detection

4.2 Potential field 工作原理

Potential field 應用於機器人路徑規劃的主要概念是將環境模擬成一個磁力 場，假設將機械手臂磁極設定為負極，環境中的障礙物磁極則會被視為與機器人 相同的負極，兩者之間會產生排斥的力量，而目標物的磁極會被設定為相反的正 極，讓機械手臂能藉此吸力到達目標點。磁場的大小會因與障礙物與目標物距離 的不同，產生一個梯度的磁場，而障礙物會是磁場梯度最高的點，而目標物則是 環境中磁場梯度最低的一點，此時，機械手臂就會受到障礙物的排斥並順應到達 目標點。

圖 4.3 為 Potential field 的一個範例，在圖 4.3(a)有三個直立的障礙物與一目 標點，而圖 4.3(b)為環境中所建立的磁場，可以由圖中看出，此時障礙物所產生 的磁場梯度為最高，而目標物則是最低點，機械手臂會順應所建立的磁場，到達 梯度最低的目標點。

然而 Potential field 主要的問題在於當機械手臂移動至梯度相對較小值(local minima)[23]時，這會使機器人或機械手臂在到達目標點前就停滯不動或原地擺 動，此問題的起源來自於 Potential field 的規劃路徑都是區域規劃，並非全域規 劃，所以會造成，機械手臂進入磁力平衡的區域時，會使得機械手臂離開此區域 到達目

圖 4.3(a)障礙物與目標點的示意圖(b)Potential field 所產生的梯度示意圖[26]

標點，為了解決 local minima 的問題，本論文提出一個方法，先將空間的各個區 域分類，每一個區域都有屬於該區域的權重，權重的越大表示該區域越安全，所 產生至此方向的斥力越大，利用分析全域的方法，來解決 local minima 的問題。

4.3 安全指標的設計

由於傳統的 Potential field 在路徑規劃作法中，當環境確定後，障礙物與目 標物的磁場會立即確定，環境中的各個位置不會因為機械手臂的移動而產生改 變，換句話說，當目標物與障礙物確認後，手臂的斥力與引力會根據移動到區域 內周圍磁場而定，機械手臂的移動路徑也隨之確定。如前所述此種區域規劃可能 會產生 local minima 等問題，為了解決此問題，我們希望能針對影像中所能看到 的環境，進行全域規劃，本論文建立了兩個安全指標，此安全指標用於 Potential field 中斥力的部分，使 X、Y、Z 三個方向產生不同大小的斥力，來調整手臂路 徑。這些指數是根據環境的狀況與障礙物在空間中的分布，將環境劃分成數個區 域，並將各個區域都定義出他們安全程度，使機械手臂可以根據這些區域所獲得 的安全值來進行規劃。傳統上 Potential field 只針對手臂與障礙物之間關係產生 斥力，而我們建立安全指標表示障礙物在空間分佈情況，進行手臂全域的路徑規 劃，能避免因為區域規劃所產生 local minima 的問題。

我們想在機械手臂的工作區域與攝影機所能觀察到範圍內，將環境劃分成安 全、不確定與危險三種區域，使機械手臂能經由安全的區域到達目標點。首先，

我們將目標點設為原點，建立 XY、YZ 和 XZ 平面，分別由機械手臂工作範圍 最上緣、最右邊緣與最靠近身體的邊界將障礙物投影至三個平面。其次，分析障 礙物在此三個平面上分佈的位置，如圖 4.4(b)，我們先找出各障礙物投影後的邊 界，由上至下記錄邊緣的各數，將所遇到奇數條的邊界記錄為正緣，偶數條邊緣 記錄為負緣，記錄下每個障礙物所對應的一組正緣與負緣，相同的，由左至右，

記錄下障礙物左右的正緣與負緣，最後找出其交集的地方為危險的區域，而在這 些區域外的為安全的區域，如此，將各個區塊分成白色斜線區域為安全、深色為

350 400 450 500 550 600 650 700 向的安全區域的大小做比較，我們將此安全指數應用於我們所建立的 Potential field 上(詳見下節說明)，判斷 Potential field 所產生之斥力方向朝向與此六個方向 的安全指數相乘，(4.1)式中P 為手部的位置，_a P 為障礙物的位置，_o P 為區域的_r 位置，當判斷式大於零表示斥力方向朝向安全區域，安全指數較大，相反的，斥 力指向危險區域時，則會產生較小的斥力，如此一來，可以將手臂引導致較安全 區域，避免碰撞的發生。另外，除了偵測到危險的區域，還有一些區域是深度攝

Y

T r ( )   l R (

_l²

 r

_i²

) 

(4.2) 如圖 4.5，我們以機械手臂下手臂的長度為圓柱的直徑，在手臂與目標物間 建立一個同心圓柱，藉由判斷同心圓柱與障礙物的交集來計算各個障礙物的權

T r ( )   l R (

_l²

 r

_i²

) 

(4.2) 如圖 4.5，我們以機械手臂下手臂的長度為圓柱的直徑，在手臂與目標物間 建立一個同心圓柱，藉由判斷同心圓柱與障礙物的交集來計算各個障礙物的權

在文檔中 擬人機械手臂基於影像之適應性抓取設計 (頁 43-0)