Z 軸座標值估測

由於 X、Y、Z 都需要 Z(深度)的估測值來做計算，所以一開始我們先將上面 得到的所有對應點代入(3-12)得到深度的估測值(Z^)。從圖 3.5 及表 3.2 可以發 現，Z^跟實際距離(Z_a)的關係呈現出一個二元一次直線的關係，因此，只要找出 兩者之間的關係就能從Z^得到實際的 Z 座標。

p

l

p

r

表 3.1 不同距離時目標點在影像平面上的座標值

Za(mm)center_lxcenter_lycenter_rxcenter_ryZa(mm) center_lxcenter_lycenter_rxcenter_ry

280.000 493 150 157 150 868.000 389 208 277 208

離增加，誤差也跟著增加，但是結合公式值及本論文所提出的轉換關係尋找方法 之後，Z 座標的估測精確度跟未修正前之結果相比，可以說是有相當顯著的提升。

表 3.2 Z 座標未校正前的估測值 Za (mm) ^_Z Z_a (mm) _Z^

280.000 265.119 868.000 -29.458 308.000 250.679 896.000 -45.823 336.000 236.290 924.000 -63.447 364.000 221.331 952.000 -77.582 392.000 207.908 980.000 -87.480 420.000 193.430 1008.000 -103.102 448.000 178.969 1036.000 -119.731 476.000 164.963 1064.000 -131.427 504.000 150.561 1092.000 -143.648 532.000 137.469 1120.000 -156.430 560.000 122.999 1148.000 -169.815 588.000 108.795 1176.000 -183.844 616.000 95.171 1204.000 -198.567 644.000 80.267 1232.000 -222.066 672.000 66.331 1260.000 -238.762 700.000 53.792 1288.000 -256.361 728.000 37.492 1316.000 -265.523 756.000 22.731 1344.000 -274.938 784.000 10.059 1372.000 -284.619 812.000 -3.466 1400.000 -304.823 840.000 -21.706

Z

Z

表 3.3 Z 座標校正後的結果及誤差值

Za (mm) pose_Z pose_Z_error Za (mm) pose_Z pose_Z_error 280.000 280.289 0.289 868.000 860.635 -7.365 308.000 308.737 0.737 896.000 892.876 -3.124 336.000 337.084 1.084 924.000 927.598 3.598 364.000 366.556 2.556 952.000 955.444 3.444 392.000 393.001 1.001 980.000 974.945 -5.055 420.000 421.523 1.523 1008.000 1005.721 -2.279 448.000 450.013 2.013 1036.000 1038.482 2.482 476.000 477.606 1.606 1064.000 1061.524 -2.476 504.000 505.979 1.979 1092.000 1085.600 -6.400 532.000 531.772 -0.228 1120.000 1110.784 -9.216 560.000 560.280 0.280 1148.000 1137.152 -10.848 588.000 588.264 0.264 1176.000 1164.792 -11.208 616.000 615.104 -0.896 1204.000 1193.796 -10.204 644.000 644.466 0.466 1232.000 1240.091 8.091 672.000 671.921 -0.079 1260.000 1272.986 12.986 700.000 696.624 -3.376 1288.000 1307.658 19.658 728.000 728.738 0.738 1316.000 1325.706 9.706 756.000 757.817 1.817 1344.000 1344.256 0.256 784.000 782.783 -1.217 1372.000 1363.329 -8.671 812.000 809.428 -2.572 1400.000 1403.132 3.132

840.000 845.362 5.362

圖 3.6 校正後 Z 座標誤差值

3.2.2. X 軸座標值 軸座標值 軸座標值估測 軸座標值 估測 估測 估測

在 X 座標方面，一樣要先把對應點代入(3-5)式中，求出各個距離的^X ，在 本論文中，攝影機座標系的原點是訂在左右攝影機的中間，加上在本次校正實驗 中是針對 X 座標等於 0 時，所以對於左攝影機來說，實際的 X 座標值Xl_a是 60，

對於右攝影機，實際的 X 座標值Xr_a則是-60，由此可得到左右攝影機的估測值 跟實際 X 座標值之間的誤差。

從左攝影機跟右攝影機的^X 值統計圖表可以看出來，隨著距離增加，X 值 的誤差逐漸增加，但是從圖 3.7 及圖 3.8 可以看出來誤差大致上是呈線性的增 加，所以在這邊我們需要找出來，隨著距離的增加我們所要減掉的估測誤差，也 就是誤差方程式。

^{X = x}_λ⁰

(

^λ−Z

)

為估測出來的 X 座標， 將 pose_Z 跟算出來的 X 座標誤差 x_error 的所有數值，算出誤差方程式，(3-14)及(3-15)是由 MATLAB 所求出來的 誤差方程式。

_ 1.1557 0.41236 * _

xl error= + pose Z (3-14)

_ 4.3684 0.40812 * _

xr error= + pose Z (3-15)

Xl

Xl

表 3.4 Xl 座標未校正前的估測值

Za(mm) Xl Xla Xl_error Za(mm) Xl Xla Xl_error 280.000 176.071 60 116.071 868.000 416.786 60 356.786 308.000 187.843 60 127.843 896.000 430.000 60 370.000 336.000 199.004 60 139.004 924.000 444.231 60 384.231 364.000 211.737 60 151.737 952.000 455.050 60 395.050 392.000 222.645 60 162.645 980.000 463.030 60 403.030 420.000 235.221 60 175.221 1008.000 475.000 60 415.000 448.000 246.226 60 186.226 1036.000 489.032 60 429.032 476.000 258.000 60 198.000 1064.000 498.462 60 438.462 504.000 269.841 60 209.841 1092.000 508.315 60 448.315 532.000 280.667 60 220.667 1120.000 518.621 60 458.621 560.000 291.930 60 231.930 1148.000 529.412 60 469.412 588.000 303.313 60 243.313 1176.000 540.723 60 480.723 616.000 314.615 60 254.615 1204.000 552.593 60 492.593 644.000 326.980 60 266.980 1232.000 572.308 60 512.308 672.000 339.021 60 279.021 1260.000 585.789 60 525.789 700.000 349.565 60 289.565 1288.000 600.000 60 540.000 728.000 362.727 60 302.727 1316.000 608.219 60 548.219 756.000 375.118 60 315.118 1344.000 615.000 60 555.000 784.000 385.366 60 325.366 1372.000 623.662 60 563.662 812.000 396.303 60 336.303 1400.000 640.000 60 580.000

840.000 410.526 60 350.526

Xr

Xr

表 3.5 Xr 座標未校正前的估測值

表 3.6 X 座標校正後的結果及誤差值

Za (mm) pose_X pose_X_error Za (mm) pose_X pose_X_error 280.000 -0.991 -0.991 868.000 0.822 0.822 308.000 -0.861 -0.861 896.000 0.734 0.734 336.000 -1.324 -1.324 924.000 0.637 0.637 364.000 -0.679 -0.679 952.000 -0.034 -0.034 392.000 -0.634 -0.634 980.000 -0.101 -0.101 420.000 0.224 0.224 1008.000 -0.831 -0.831 448.000 -0.492 -0.492 1036.000 -0.320 -0.320 476.000 -0.071 -0.071 1064.000 -0.401 -0.401 504.000 0.091 0.091 1092.000 -0.485 -0.485 532.000 0.296 0.296 1120.000 -0.573 -0.573 560.000 -0.186 -0.186 1148.000 -0.666 -0.666 588.000 -0.333 -0.333 1176.000 -0.764 -0.764 616.000 -0.091 -0.091 1204.000 -0.867 -0.867 644.000 0.172 0.172 1232.000 -0.264 -0.264 672.000 0.895 0.895 1260.000 -0.362 -0.362 700.000 1.254 1.254 1288.000 -0.465 -0.465

728.000 1.172 1.172 1316.000 0.302 0.302

756.000 1.570 1.570 1344.000 -0.574 -0.574

784.000 1.520 1.520 1372.000 0.212 0.212

812.000 1.465 1.465 1400.000 0.117 0.117

840.000 0.864 0.864

我們希望在影像伺服的過程中所觀測到目標物的 X 座標應該是一致的，不

Yl 

Yl 

圖 3.10 Yl 座標估測值 表 3.8 Yr 座標未校正前的估測值

Za (mm) Yr Yr^a Yr_error Za (mm) Yr Yr^a Yr_error 280.000 55.151 27 28.151 868.000 222.614 27 195.614 308.000 63.534 27 36.534 896.000 231.842 27 204.842 336.000 71.574 27 44.574 924.000 241.779 27 214.779 364.000 80.259 27 53.259 952.000 250.043 27 223.043 392.000 88.222 27 61.222 980.000 256.233 27 229.233 420.000 96.419 27 69.419 1008.000 264.132 27 237.132 448.000 104.885 27 77.885 1036.000 273.826 27 246.826 476.000 112.818 27 85.818 1064.000 279.767 27 252.767 504.000 121.771 27 94.771 1092.000 285.976 27 258.976 532.000 129.720 27 102.720 1120.000 293.843 27 266.843 560.000 137.795 27 110.795 1148.000 300.675 27 273.675 588.000 145.882 27 118.882 1176.000 309.274 27 282.274 616.000 153.829 27 126.829 1204.000 316.823 27 289.823 644.000 162.520 27 135.520 1232.000 330.403 27 303.403 672.000 170.043 27 143.043 1260.000 339.005 27 312.005 700.000 176.955 27 149.955 1288.000 348.071 27 321.071 728.000 184.848 27 157.848 1316.000 354.424 27 327.424 756.000 192.940 27 165.940 1344.000 359.297 27 332.297 784.000 200.082 27 173.082 1372.000 365.987 27 338.987 812.000 207.703 27 180.703 1400.000 376.492 27 349.492 840.000 217.716 27 190.716

Yr

Yr

圖 3.11 Yr 座標估測值

從Y^的統計圖表可看出，大致上跟距離成正比，誤差隨距離逐漸增加，所以 我們要求出其誤差方程式，針對不同距離減去對應的誤差值，得到正確的 Y 座 標。

 ^y0

( )

Y λ Z

= λ − 為估測出來的 Y 座標，接著算出 pose_Z 跟 Y 座標誤差 y_error 之間的誤差方程式，(3-18)及(3-19)是透過 MATLAB 所求出來的誤差方程式。

_ 50.722 0.28601* _

yl error= − + pose Z (3-18)

_ 49.993 0.28507 * _

yr error= − + pose Z (3-19) 將估測值減去對應的誤差值，就可以得到左右攝影機對應的Y座標值。

pose_y=est_y−y error_ (3-20)

(3-20)式所代表的意義是，隨著距離的不同，在y座標產生的誤差量也不

同，由誤差方程式我們可以算出在不同距離時產生的誤差量，得到此誤差量後，

將當時y座標的估測值減掉誤差就能得到正確的y座標。

分別將左影像跟右影像的結果代入得到pose_yl跟pose_yr，將兩個值相加取 平均，就能得到目標點的Y座標值

= +

度是在哪個位置，也就是手臂在抓取時所要移動到的垂直位置，所以 Y 座標必

圖 3.12 校正後 Y 座標誤差值

這個設計的目的是在於得到目標物相對於機器人本體的角度變化，因為在更 進一步的抓取規劃上，目標物不見得是直立於桌面，且正面朝著攝影機，他可能 有其他角度的變化，例如:左右傾斜，前後傾斜，正面向左偏轉幾度等情況。若 是我們能夠知道這些狀態，就可以調整我們抓取的姿態，以最適合的方式抓取目 標物[12]。

為了驗證這個三軸角度計算方法的正確性，我們任意選取幾個角度，對每一 筆選取的角度做十次估測取其平均值，觀察其實際估測結果所呈現出的誤差情 況，如表 3.10 所示，當各軸旋轉的角度加大時，雖然是對單一軸旋轉，其他軸 也會被算出轉動的角度，整體來說，各軸在旋轉 30 度之內時，誤差大致上在 10 度之內，但是隨著角度上升，誤差就更大，所以目前來說，這個角度估測方法適 用在小角度變化時的計算，若是出現大角度的旋轉時，需要再輔以其他的演算 法，以便能算出更精確的角度變化。

φ

θ ϕ

圖 3.13 目標物的三軸向量與世界座標的旋轉關係

表 3.10 三軸旋轉角實際測試結果 Assigned angle Experiment angle error

X Y Z X Y Z X Y Z

0 0 0 -1.752 -1.771 0.204 1.752 1.771 0.204

-20.000 0.000 0.000 -19.365 -2.159 1.449 0.635 2.159 1.449 30.000 0.000 0.000 30.743 -16.348 -7.143 0.743 16.348 7.143 0.000 20.000 0.000 1.817 21.084 1.007 1.817 1.084 1.007 0.000 -30.000 0.000 7.481 -29.488 -2.536 7.481 0.512 2.536 0.000 0.000 30.000 0.946 -1.273 33.264 0.946 1.273 3.264 0.000 0.000 -20.000 8.475 -9.083 -21.417 8.475 9.083 1.417 0.000 0.000 -40.000 7.929 -10.085 -38.624 7.929 10.085 1.376

第 第 第

第四 四 四章 四 章 章移動式機械臂之 章 移動式機械臂之 移動式機械臂之影像伺服 移動式機械臂之 影像伺服 影像伺服 影像伺服控制 控制 控制 控制設計 設計 設計 設計

經由第二章跟第三章所介紹的方法之後，我們可以得到目標物在空間中的三 維座標。本章節的目的在說明我們如何使用這個估測出來的座標當作控制器的輸 入，導引機器人向前移動並抓取目標物。首先在 4.1 節跟 4.2 節之中，我們分別 介紹機械手臂跟全向式移動平台的控制設計。在 4.3 節中，將會介紹移動式機械 臂整體的系統架構，說明移動式抓取的方法跟設計理念。

4.1 手臂控制 手臂控制 手臂控制 手臂控制

整體的控制透過工業電腦(IPC)，而利用 CANbus 作為傳輸介面，控制各馬 達的運作。手臂的馬達控制器是由 Maxon 公司所生產的 EPOS Positioning

Controller，我們可以用用其內建的指令集，針對我們控制上的需要去撰寫程式。

在本論文中，主要用到的是位置控制，當我們經由逆運動學得到各軸旋轉的角度 後，將角度轉為馬達轉動所需要的資料型態，將這些資訊送給馬達，就可以讓手 臂轉到我們所指定的座標位置，而在手臂從起點移動到終點的過程中，我們可以 藉由讀回編碼器(Encorder)的數值，判斷各軸馬達是否已經轉到我們給定的轉動 量，以便確認手臂是否已經移動至終點。

4.1.1. 硬體構造 硬體構造 硬體構造 硬體構造

本論文所使用的機械手臂是由實驗室所自行設計的雙臂構造。為一具有 6 自由度的機械手臂，其中在肩膀及手肘關節的部分使用的是 Harmonic driver，這 種設計可以讓機械手臂在運作時動作更加的平順。如圖 4.1 所示，單邊肩長 198mm，肩膀到手軸長 285mm，手軸到手腕長 287mm，手掌長度約為 200mm。

4.1.2. 座標轉換 座標轉換 座標轉換 座標轉換

本論文中所使用的實驗平台由數個部分組合起來，包括立體攝影機、機器人 頭部、機械手臂，移動平台，為了整合機器人的各個部分需要知道每個座標間的 轉換關係，以利於整體的控制設計。機器人的座標原點定在雙臂肩關節的連線，

圖 4.1 機械手臂設計圖

跟頭部旋轉軸的交叉點，其中攝影機是主要的資訊來源，所以必須知道攝影機座 標到手臂及平台間的座標轉換。整個移動式機械臂之間的座標關係，如圖 4.2 所示，其中的符號定義如下，C：立體視覺攝影機、B：機械手臂本體中心、E：

機械手臂的手腕點、G：機械手臂的夾爪末端、P：全向式移動平台、O：欲抓取 的目標物體，而^AH_B矩陣代表的意義是從 A 座標系轉換到 B 座標系之轉換矩陣。

我們得知了機器人各個部位的座標轉換關係之後，就能夠針對控制的需求，將命 令轉換到適當的座標系，例如:當機器人位於機械手臂的工作範圍之外時，我們 就必須將攝影機所觀測到的座標轉換到移動平台的座標系，給出適當的命令，導 引平台移動到手臂的工作範圍之內。

在圖 4.3 中所表達的是機器人本體的座標系(ver.new0)跟右機械手臂座標系 (ver.0)之間的座標轉換關係，我們希望在手臂控制方面能夠先轉換到一個統一的 座標系，之後再根據逆運動學的需要，將座標系轉換到左手或是右手，以符合手 臂逆運動學所需的座標系。機器人本體的座標系(ver.new0)跟右機械手臂座標系 (ver.0)之間的座標轉換關係之中，在 X 軸跟 Z 軸的座標原點及方向是一致的，不

C

camera head pose head y pose y

pose head z pose z

(^{0, 0, 0})

pwx end pose head x

head ver pwy end pose head y

pwz end pose head z

       

θ16

θ16

圖 4.4 機器人頭部跟目標物的座標關係

θ

15

θ

15

_ _

pose head z

由(4-4)可以得到 X 方向的距離，也就是將機器人移動到目標物正前方的左

1 1

n



E

L



θ

π θ2−

圖 4.8 空間中向量及平面的夾角關係

4.1.4. 從假定腕部點 從假定腕部點 從假定腕部點 從假定腕部點逼近 逼近 逼近實際腕部 逼近 實際腕部 實際腕部 實際腕部點的方法 點的方法 點的方法 點的方法

手臂逆向運動學的推導分為兩個階段，第一階段是肩關節到手腕的部分，第 二階段則是利用前四軸的結果，推導出手掌的逆向運動學。讓手掌也就是夾爪開 口保持水平的結果，使手掌能夠順利抓起目標物，但是預抓取位置(pre-grasp point) 並不是我們當初所給的那點，因為本質上逆運動學的輸入還是手腕的位置，順向 運動學所算回來的夾爪位置並不是我們所給定抓取目標物時，所需達到的預抓取 位置(pre-grasp point)，所以為了讓逆向運動學的結果能夠是我們所給定的

pre-grasp point，我們需要得到一個最適合的手腕位置當作逆運動學的輸入，讓對 應的夾爪位置是我們所給定的點。為了達到這個結果我們設計了一個逼近抓取點 估測法。

由圖 4.9 我們可以看出，當我們給定抓取點 b(pw_end)時，減掉手掌長度約 20cm，會得到手腕位置 c(pw_rwst)，但是將 c 代入逆向運動學之後，得到的抓取 點位置是 a(end_point)，跟我們所給定的抓取位置不同，於是根據 a、b、c 三點 間的關係，我們假設對應 b 的腕部點是在 c’的位置，劃出一個平行四邊形的區 域。但是將 c’代入逆運動學之後，得到的位置不一定是 b，於是我們繼續在 c 跟

圖 4.9 給定抓取點 b，假定手腕點 c，對應 c 的抓取點 a，對應 b 的手腕點 c’，

四點之間的相對關係圖。

以下是逼近抓取點方法的運算步驟：

a. 運用平行四邊形原理求出參考點c rwst bar '( _ ) '

ba=c c

b. 將c'帶入逆向運動學，得到current end b_ ( ')做為跟給定抓取點 _ ( )

pw end b 比較的依據

c. 算出目前手臂的終端點(current_end)跟給定要到達的終端點(pw_end)兩者

間的距離(dis_end)，根據這個結果來判斷逆向運動學的結果是否合乎我們

的要求

d. 根據腕部點跟終端點的變化趨勢，可以看出符合給定點的對應腕部點是沿 著cc'



來做變化，所以基本上就是搜尋這條線上的腕部點中，算出來最接 近給定抓取點的，即是我們需要的結果

e. 持續步驟c跟d，直到dis_end小於某個值(在這邊是設定為5mm)，或是b' 已經到達 b 的邊界。此時的b'所對應的逆向運動學結果，就是我們所需要 的最終結果。如圖 4.10所示。

1 1 1

'( 1) (

_i

,

_i

,

_i

) b i + = x

₊

y

₊

z

₊

'( ) ( ,

_{i i}

, )

_i

b i = x y z

( , , ) b = x y z

圖 4.10 逼近抓取點的示意圖，隨著 b’漸漸接近 b，我們可以得到對應 b 的腕部 點 c’

目前來說，這個逼近抓取點的方法能夠讓機械手臂真正的達到我們的給定抓

目前來說，這個逼近抓取點的方法能夠讓機械手臂真正的達到我們的給定抓

在文檔中 基於立體視覺之移動式機械臂影像伺服設計 (頁 35-0)