Transcript 下載/瀏覽

遠端五軸氣壓機械手臂控制之設計與
製作
南台科技大學電機工程系
指導教授：許毅然
組員：吳炯男、許晉杰、卓耀加
機械手臂整體系統架構
 控制部分
 影像伺服部分
 網路監控部分
無線PDA部分
控制部分
機構-五軸機械手臂
控制器-工業電腦
自製的驅動器
手臂座標系初始化
θ3
L31
L30
L2
Z
θ2
L1
θ1
h0
X
φ0
Y
第1軸與第2軸關節點座標
L2
θ2
X0＝0
Y0＝0
(X1 , Y1 , Z1)
Z0＝h0＝22.4cm
L1
θ1
(X0 , Y0 , Z0)
X1＝L1COSθ1COSψ
Y1＝L1COSθ1SINψ
h0
Z1＝L1SINθ1＋h0
第3軸關節點座標
X2＝X1＋L2COS (θ1＋θ2) COSψ
(X2 , Y2 , Z2)
L2
θ2
(X1 , Y1 , Z1)
Y2＝Y1 ＋L2COS (θ1＋θ2) SINψ
Z2＝ Z1 ＋L2SIN (θ1＋θ2)
夾頭與CCD中心點座標
θ1＋θ2＋θ3＝0
L31
L30
(X2 , Y2 , Z2)
夾頭中心點位置
X30＝X2＋L30XCOSψ－L30YSINψ
Y30＝Y2＋ L30XSINψ＋L30YCOSψ
Z30＝Z2－h1
h1
CCD中心點位置
X31＝ X2＋L31COSψ－ L31YSINψ
Y31＝ Y2＋ L31SINψ＋ L31YCOSψ
手臂臂長量測
Y
X
L1
L2
L30
L31
機械手臂控制器

LM628是一顆專為直流或直流無刷伺服馬達所設計的位置控制處理器，且
可運用在提供正交增量位置回授信號之伺服機器。
機械手臂控制器
手臂驅動電路
機械手臂驅動卡
影像擷取卡與CCD攝影機
使用UPMOST UPG301BT影像擷取卡
使用黑白CCD攝影機
CCD攝影機選用
4mm CCD
Y
8mm CCD
6mm CCD
X
CCD影像識別法則
Y
X
CCD影像識別法則
Y
A
B
X
1
2
3
4
CCD座標轉換到卡氏座標
Y
ψ0
Y’
（X31 , Y31）
αC
（XC , YC）
（XA , YA）
ψ0
X
X’
CCD與手臂座標系轉換
CCD影像格式：240 X 320 (像素)
（0 , 0）
當θ1＝
400
θ2＝
00
時
CCD可視範圍：20 X 27（CM）
(120 , 160)
rC
αC
X’
27CM
Y’
20CM
(XCP , YCP)
(XA , YA)
（240 , 320）
XC＝（XCP－120）/ 240
YC＝（YCP－160）/ 320
1
αC＝ tan （YC / XC）
rC＝
XC 2  YC 2
XA＝X31＋rC COS（αC＋ψ0）
YA＝Y31＋rC SIN（αC＋ψ0）
夾頭位置的決定

X夾 ＝ XA ＝〔 L1COSθ1 ＋ L2COS（θ1＋θ2）＋ L31 〕COSψ

Y夾 ＝ YA ＝〔 L1COSθ1 ＋ L2COS (θ1＋θ2) 〕 SINψ

Z夾 ＝ 0 ＝Z2 － h1 ＝ L1SINθ1 ＋ L2SIN（ θ1＋θ2 ）＋ h0 － h1
XA、YA在機械手臂可移動範圍內，可以找到兩組ψ、θ1、θ2 。
令θ1 ＞ 0 則可得到唯一解。
夾頭夾角的決定
Y
（XM1 , YM1）
（XM2 , YM2）
（XA , YA）
β
（XM3 , YM3）
（XM4 , YM4）
X
夾頭夾取物體最長邊。
由M1～M4點可以
決定最長邊夾角β。
夾頭夾角
θ4＝β－ψA
三層式架構
第一層
第二層
第三層
VB之動態連
結檔
Net_class . cls
Server端
Client端
機械手臂
無線PDA部分
RS-232
Server端
機械手臂
PDA
Server & Client端操作監控介面
PDA端操作監控介面
敬請指教
END