RANSAC

1

b

b b b

x

p p ω y

 

= =  

  

(2-8)

因為 homography 具有對應旋轉及尺度變化的特性，所以即使目標物在不同 距離或是不同角度，我們都可以準確的框出目標物在目前影像平面上的位置。如 圖 2.8 所示，左邊是資料庫影像，首先藉由所擷取到的特徵點，求得資料庫影像 跟目前影像平面之間的轉換矩陣，接著透過轉換矩陣H，我們可以得到目標物 在目前影像平面(image plane)中的位置。

2.2.2. RANSAC

從 2.2.1 節中得知，要算出平面轉換矩陣，需要四個對應點，雖然從前面的 特徵點擷取及最近鄰居演算法（Nearest neighborhood algorithm）運算過後，可以 得到一群對應點，但是要算出更精確的轉換矩陣，就需要更準確的特徵對應點，

所在這邊我們採用 RANSAC(Random sample consensus)[21]此種隨機取樣方式。

RANSAC 演算法，其主要精神在於從一群分布不均勻或是參雜雜訊的資料中，

隨機挑選數個樣本，算出參考的模型，之後再把其他資料逐次拿進來運算，留住 符合模型的大多數，排除少數不符合者，以符合大眾資料的模型為最後結果。舉 例來說[22]，如圖 2.9 所示，運用此演算法能夠從一群分布不均勻的點集合之中 找出一條直線，符合大多數的點，而少數不符合的點則被剔除在外。

RANSAC 演算法，為一個條件中止式的隨機演算法，當算出來的結果合乎 我們所設定的閥值時，表示此結果已經合乎大部分的特徵對應關係了。

其演算步驟簡述如下:

1. 隨機挑選 4 對以上在第一階段比對成功的對應點，求出目前影像跟資料 庫影像之間的平面轉換矩陣 H。

2. 將所有對應點使用步驟 1 求得的平面轉換矩陣進行位置轉換，將資料庫 影像的對應點代入轉換矩陣得到另一組配對點，之後計算這些經過轉換 的配對點座標與原本目前影像中的配對點座標之間的距離。

3. 統計步驟 2 中所有配對點間的距離，其配對點的距離若小於某個自訂的 臨界距離D_τ，則表示此組配對點符合這個平面轉換矩陣，然後統計這 個平面轉換矩陣的正確配對數量。

重複上述步驟 N 次之後，若某次的正確配對點數量最多，且大於某個自訂 的臨界數量S_τ時，則採用此次的平面轉換矩陣，再將這些配對點拿去求得最終 的平面轉換矩陣。

在本論文中，參雜雜訊的資料樣本就是第一次特徵比對的結果，如圖 2.10 所示，可以看出當中存在在錯誤比對的特徵點，而模型就是轉換矩陣(homography matrix)，將 RANSAC 作為計算轉換矩陣的演算法，如此一來就可以得到對應正 確比例最高的一群特徵點，用 homography 來框出在目前影像平面上的目標物 時，位置也能更加準確，如圖 2.11，剔除錯誤比對特徵點之後，再將剩餘比對 成功的特徵點拿來算轉換矩陣，就可以準確的框出目標物的位置，在分別對左右 攝影機影像目標物的四個角點取平均後，就能得到中心點，在平面轉換矩陣計算 的相當準確的情形下，目標物的範圍可以清楚的被框出來，所以用這個範圍的平 均位置來當作參考點是比較穩定的結果。將這對中心點代入三維座標估測公式，

如此一來就能夠得到穩定的控制參考點，將會對整個移動式抓取的控制上有很大 的助益。

圖 2.9 左圖為一群分布不均勻的點集合，需要從這裡面決定出一條直線。右圖 為經過 RANSAC 計算後，得到符合條件的直線，不符合的點則被剔除在外。[22]

實際的運算結果如圖 2.12 及圖 2.13 所示，我們將立體攝影機所讀取到的左 右兩個影像，分別跟資料庫影像做前述的處理，框出目標物後，分別求得目標物 在左右攝影機影像的中心點，接著將這對中心點代入第三章將會提到的三維座標 估測公式，就可以知道目標物在世界座標中的位置，做為導引機器人往前抓取目 標物的控制命令輸入。

100 200 300 400 500 600

50

100

150 200

圖 2.10 用 SURF 做特徵比對的結果

50

100

150 200

圖 2.12 實際左影像跟資料庫影像做轉換矩陣計算的結果

圖 2.13 實際右影像跟資料庫影像做轉換矩陣計算的結果

2.3 定義 定義 定義 定義中心點 中心點 中心點 中心點

當我們算出 homography 之後，我們可以得到目標物跟目前影像之間的平面 轉換關係。接著我們可以算出目標物在左右影像平面上，四個角點的位置，將這 四個角點連接起來框出目標物在目前影像平面的位置。如圖 2.14 所示，我們將 目標物在左攝影機影像平面的四個角點取平均，定為左中心點(left center)，目標 物在右攝影機影像平面的四個角點取平均，定為右中心點(right center)，將這組 中心點當作控制用的參考點，以這點的三維座標表示目標物的位置，藉此導引機 器人完成抓取任務。

圖 2.14 目標物在影像平面的區域及中心點位置的定義

第 第 第

第三 三 三章 三 章 章目標物姿態估測 章 目標物姿態估測 目標物姿態估測 目標物姿態估測

本章節的主要目的在說明如何將第二章所算出的特徵對應點，轉換成在空間 中實際的三維座標，當作控制器輸入的重要資訊來源。

3.1 立體視覺原理 立體視覺原理 立體視覺原理 立體視覺原理

要如何從空間中一組成對的影像中找出互相匹配的特徵配對，也就是在世界 座標中的某個點分別在左右眼影像中成像的位置是立體視覺的形成很重要的一 個關鍵點。這組對應點稱為(Corresponding points)，而對應點在影像中的位置差 稱為視差(Disparity)。圖 3.1 中，世界座標中的一點w=( , , )x y z 分別投影到左影 像平面中的p u v_l( , )_{l l} 以及右影像平面中的 p u v_r( ,_{r r})，兩個攝影機之間的鏡心距離 設為B(baseline)，透鏡的焦距為λ，兩個成像點的橫軸誤差為d = −u_l u_r，最後 可由這組對應點求回世界座標[23]。

圖 3.2為圖 3.1投影到XZ平面的示意圖，由其中的幾何關係可以得到，如

何從對應點p u v_l( , )_{l l} 和 p u v_r( ,_{r r})推導回世界座標w=( , , )x y z 的公式。

( , )

l l l

p u v

( , )

r r r

p u v

( , , ) w= x y z

λλλλ

1

28 mm

28 mm

圖 3.3 校正版圖示

視窗中的目標點)的座標值跟上一個距離相比幾乎沒有變動時，表示到達攝影機 辨識物體的極限距離。把這過程中記錄下來的所有座標對，用來求出二維座標到 三維座標之間的轉換公式。

3.2.1. 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.10 所示，當各軸旋轉的角度加大時，雖然是對單一軸旋轉，其他軸 也會被算出轉動的角度，整體來說，各軸在旋轉 30 度之內時，誤差大致上在 10 度之內，但是隨著角度上升，誤差就更大，所以目前來說，這個角度估測方法適 用在小角度變化時的計算，若是出現大角度的旋轉時，需要再輔以其他的演算

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