第四章 模擬與實驗結果
4.2 視覺伺服模擬與實驗
本節的模擬與實驗為驗證第二章所提出的控制器為有效之設計。其中，視覺 伺服控制器為(2.32)式，整個 IBVS 伺服架構如圖 4.3 所示。
4.2.1 模擬結果
在第一個模擬中，預定到達的影像特徵點位置(Desired feature position)為將 目標影像放置在機器人本體前方 1 公尺處取得影像特徵點。假設世界座標系為機 器人初始狀態時的機器人座標系，則機器人初始位置座標為(0, 0, 0)，目標物體 座標為(1, 0.05, 5)m，機器人手臂長度參數和實體相同(不考慮攝影機架設誤差
θ x
)，影像取樣頻率設定為理想的 30 frames/sec，即取樣週期為 0.033sec，並假設 每次都能成功取到目標特徵點，距離估測採用 3.4 節所介紹的方法，假設兩個特 徵點和鏡頭的距離相等。模擬的結果如圖 4.4(a)~(d)所示。控制器有效地在影像平面上的特徵點移到 預定到達的位置(圖 4.4(a))，且機器人也同時向目標物體靠近(圖 4.4(c))。
在第二個模擬中，所有的模擬條件都和前一次模擬相同，唯有特徵點和鏡頭 的距離採用真正空間中的距離資訊，而非利用 3.4 節中的方法估測出的資訊，同
Feature extraction +
- Feature error
Actual image feature
Robot velocity
Robot
Camera
e KJ u = − 1
⎥ ⎥
⎥ ⎥
⎦
⎤
⎢ ⎢
⎢ ⎢
⎣
⎡
=
d d d d
d
v u v u f
2 2 1 1
f f
e = d − [ v B w B w 1 w 2 ] T
[ u v u v ] T
f = 1 1 2 2
Desired image feature
圖 4.3 IBVS 伺服架構
時兩特徵點和鏡頭的距離也不再視為相等。其模擬結果如圖 4.5(a)~(d)所示，其 中，影像特徵點軌跡和前次模擬稍微不同，但是由圖 4.4(d)與圖 4.5(d)比較可發 現，手臂兩軸的角度響應則比前次模擬更好，第二個模擬中角度響應的 overshoot 和 undershoot 幅度都比第一次模擬小。
由以上兩個模擬中，我們知道不將兩特徵點與鏡頭的距離視為相等時，機械 臂的角度響應會比視為相等時更好，但是因為在擷取影像特徵點時，尋找角點所 做的 SSD Correlation 運算需要大量的運算時間，若利用 3.4 節所介紹的距離估測 方法，我們需要額外找出目標影像的另外兩個角點(右上、左下)，並利用左上和 右上角點計算出的目標距離作為左上角距離，利用左下和右下角點計算出的目標
-0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15
feature trajacoty
u (m)
v (m) Desired feature(top-left)
Desired feature(bottom-right)
0 10 20
robots linear velocity
VB(m/s)
0 10 20
robots angular velocity
WB(rad/sec)
0 0.5 1
shoulder anglular velocity
W1(rad/sec)
0 50 100 150 200 250 300 350 400 450 500
-10 0 10
elbow anglular velocity
sample
W2(rad/sec)
(a) (b)
0 1 2 3 4 5
-0.5 0 0.5 1 1.5
robot trajactory
Z (m)
X (m)
Target position
Robot initial position
Robot final position
0 0.5 1
robot angle
ThetaB (rad)
-1 -0.5 0 0.5
shoulder angle
Theta1 (rad)
0 50 100 150 200 250 300 350 400 450 500
0 0.5 1 1.5
elbow angle
sample Theta2 (rad)
(c) (d)
圖 4.4 視覺伺服模擬結果 1：(a) 影像特徵點移動軌跡；(b) 機器人本體角速度、
手臂兩軸之角速度；(c) 機器人移動軌跡；(d) 機器人本體朝向角、手臂兩 軸角度
距離作為右下角距離，經過實際測試，這樣做會使得影像處理部分的 Frame rate 降低到 5 frames/sec 以下。因此，為了維持影像特徵點擷取的速度，之後的實驗 中還是採用將兩個特徵點與攝影機的距離視為相等的做法。
另外，由圖 4.4(d)與圖 4.5(d)中，可發現兩次模擬的手臂兩軸角度都有很大 的 overshoot 和 undershoot，這似乎是控制器的特性，雖然可以藉由降低 Gain matrix 中的數值來消除，但是這麼做當特徵點靠近預定位置(Desired feature position)時所產生的速度命令會變得太小，無法控制機器人靠近目標。
-0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15
feature trajacoty
Desired feature(top-left)
Desired feature(bottom-right)
v (m)
u (m)
0 10 20
robots linear velocity
0 10 20
robots angular velocity
0 0.5 1
shoulder anglular velocity
0 50 100 150 200 250 300 350 400 450 500
-10 0 10
elbow anglular velocity
sample VB(m/s)WB(rad/sec)W1(rad/sec)W2(rad/sec)
(a) (b)
0 1 2 3 4 5
-0.5 0 0.5 1 1.5
robot trajactory
X (m)
Z (m)
Target position
Robot final position
Robot initial position
0 0.5 1
robot angle
-0.5 0 0.5 1
shoulder angle
0 50 100 150 200 250 300 350 400 450 500
-0.5 0 0.5
elbow angle
sample ThetaB (rad)Theta1 (rad)Theta2 (rad)
(c) (d)
圖 4.5 視覺伺服模擬結果 2：(a) 影像特徵點移動軌跡；(b) 機器人本體角速度、
手臂兩軸之角速度；(c) 機器人移動軌跡；(d) 機器人本體朝向角、手臂兩軸角度
4.2.2 實驗結果
實驗時，機器人起始位置為(0, 0, 0)m，目標物體位置約在(0.32, 0.37, 1.26)m，如圖 4.6。圖 4.7(a)~(d)為實驗結果，由圖(a)的特徵點軌跡圖中，藍色與 紅色軌跡分別是左上與右下角點的運動軌跡，可看出特徵點確實往預定特徵點位 置(方形點)靠近。在本實驗的程式中，有設定目前特徵點與預定特徵點位置誤差 小於某個臨界值內便使機器人停止，因此最後軌跡沒有完全到達預定特徵點。由 圖 4.7(a)中可發現特徵點軌跡有跳動的現象，推測可能的原因有兩個，第一是因 為機器人的前後兩個萬向輪(Caster)與地面並非完全貼齊，因此在降速與加速 時，將會使機器人稍微往前傾或往後傾，造成攝影機搖晃；第二個原因可能是不 穩定的光源或不完美的二值化過程，造成目標影像的角落不平整，使得有多組不 同的角點都能被辨識為特徵點，造成特徵點軌跡的跳動。
在此實驗中，機器人成功地由初始位置往目標物體移動，並在攝影機如我們 所預期的在距離目標約 30 公分處停止(圖 4.7(b))，因為實驗中所使用的預定特徵 點是將目標放在攝影機前 30 公分處時所取得的特徵點。圖 4.7(d)由上而下為機 械臂兩軸的角度與攝影機傾斜角，由圖中可看出攝影機大致上會保持水平。
圖 4.6 機器人初始位置