Đ án C đi n tồ ơ ệ ử
Đ án C đi n tồ ơ ệ ử
1. GI I THI UỚ Ệ
Ra đ i cách đây n a th k , robot công nghi p đã có nh ng phát tri nờ ử ế ỷ ệ ữ ể
v t b c. Nhi u n c trên th gi i s m áp d ng m nh m k thu t robotượ ậ ề ướ ế ớ ớ ụ ạ ẽ ỹ ậ
vào s n xu t và nó đã đem l i nh ng hi u qu to l n v kinh t và kả ấ ạ ữ ệ ả ớ ề ế ỹ
thu t, nâng cao năng su t lao đ ng, tăng ch t l ng và kh năng c nh tranhậ ấ ộ ấ ượ ả ạ
c a s n ph m, c i thi n đi u ki n làm vi c c a công nhân...ủ ả ẩ ả ệ ề ệ ệ ủ
Đ i v i n c ta, k thu t robot v n còn là v n đ khá m i m , nh tố ớ ướ ỹ ậ ẫ ấ ề ớ ẻ ấ
là vi c nghiên c u thi t k , ch t o robot. N i dung nghiên c u nh m chệ ứ ế ế ế ạ ộ ứ ằ ế
t o m t robot sáu b c t do, ki u robot PUMA, s d ng card đi u khi nạ ộ ậ ự ể ử ụ ề ể
LAB-PC+ đ đi u khi n các đ ng c b c d n đ ng các kh p. S n ph mể ề ể ộ ơ ướ ẫ ộ ớ ả ẩ
đ c s d ng trong nghiên c u và gi ng d y môn robot công nghi p t iượ ử ụ ứ ả ạ ệ ạ
tr ng Đ i h c Bách khoa Hà N i.ườ ạ ọ ộ
2. S Đ Đ NG VÀ H T A Đ C A ROBOT Ơ Ồ Ộ Ệ Ọ Ộ Ủ PUMA
Robot PUMA là robot có 6 b c t do, c u hình RRRRRR.ậ ự ấ
S đ đ ng và h t a đ g n trên các khâu c a robot nh sau:ơ ồ ộ ệ ọ ộ ắ ủ ư
Hình 1: H t a đ g n trên các khâu c a robot.ệ ọ ộ ắ ủ
Đ án C đi n tồ ơ ệ ử
B NG THÔNG S DENAVIT - HARTENBERG (DH)Ả Ố
Khâu θiαiaidi
1θ1*
2
π
−
0 d1
2θ2*0 a2d2
3θ3*
2
π
a30
4θ4*
2
π
−
0 D4
5θ5*
2
π
0 0
6θ6*0 0 d6
+ Ch n aọ1 = 00 mm; a2 = 0 mm;
a3max = 0mm.
+ Gi i h n chuy n đ ng cu các khâu:ớ ạ ể ộ ả
- Khâu 1: -900 ≤ θ1 ≤ +900
- Khâu 2: -1200 ≤ θ2 ≤ +1200
- Khâu 3: 0 ≤ a3 ≤ 150 mm
- Khâu 4: -900 ≤ θ4 ≤ +900
- Khâu 5:
- Khâu 6:
Đ án C đi n tồ ơ ệ ử
3. PH NG TRÌNH Đ NG H C ROBOTƯƠ Ộ Ọ
+ Đ mô t m i quan h v h ng và v trí c a h t a đ g n trênể ả ố ệ ề ướ ị ủ ệ ọ ộ ắ
hai khâu li n k nhau (Khâu th i và khâu i-1) ta dùng các ma tr n Aề ề ứ ậ i ; đ cượ
bi u di n b i các phép bi n đ i:ể ễ ở ế ổ
Ai = Rot(z, θ0). Trans(a,0,0). Trans(0,0,d). Rot(x, α)
Hay
Ai =
−
−
1000
cossin0
sinsincoscoscossin
cossinsincossincos
d
a
a
αα
θαθαθθ
θαθαθθ
Qui c vi t t t các hàm l ng giác nh sau:ướ ế ắ ượ ư
Ci = cosθi; Si = sinθi; Cij = cos(θi+θj); Sij = sin(θi+θj);.......
Ta có:
A1 =
1 1
1 1
1
0 0
0 0
0 1 0
0 0 0 1
C S
S C
d
−
� �
� �
� �
� �
−
� �
� �
; A2 =
2 2 2 2
2 2 1 2
2
0
0
0 0 1
0 0 0 1
C S a C
S C a S
d
−
� �
� �
� �
� �
� �
� �
;
A3 =
3 3 3 3
3 3 3 3
0
0
0 1 0 0
0 0 0 1
C S a C
S C a S
� �
� �
−
� �
� �
� �
� �
; A4 =
4 4
4 4
4
0 0
0 0
0 1 1
0 0 0 1
C S
S C
d
−
� �
� �
� �
� �
−
� �
� �
;
A5 =
5 5
5 5
0 0
0 0
0 1 0 0
0 0 0 1
C S
S C
� �
� �
−
� �
� �
� �
� �
; A6 =
6 6
6 6
6
0 0
0 0
0 0 1
0 0 0 1
C S
S C
d
−
� �
� �
� �
� �
� �
� �
;
Tích các ma tr n Aậi đ c g i là ma tr n T: ượ ọ ậ
5T6 = A6; 4T6 = A5. A6 ;3T6 = A4.A5.A6;
2T6= A3.A4.A5.A6 ; 1T6 = A2.A3.A4.A5.A6 ;
T6 = A1.A2.A3.A4.A5.A6 ;
Ta còn có ma tr n tr ng thái cu i:ậ ạ ố
TE =
Đ án C đi n tồ ơ ệ ử
Ma tr n TậE mô t h ng và v trí c a h to đ g n trên khâu ch pả ướ ị ủ ệ ạ ộ ắ ấ
hành cu i đ i v i h t a đ g c. Trong đó: ố ố ớ ệ ọ ộ ố
, s, anr r
r
là các véct ch ph ngơ ỉ ươ
c a h t a đ g n trên khâu ch p hành cu i, ủ ệ ọ ộ ắ ấ ố
p
r
là véct đi m ch v trí c aơ ể ỉ ị ủ
g c h t a đ g n trên khâu ch p hành cu i.ố ệ ọ ộ ắ ấ ố
a-vector có h ng ti p c n (approach) đ i tácướ ế ậ ố
s-vector có h ng đ ng tr t (sliding) đóng m bàn k pướ ườ ượ ở ẹ
ho c ph ng n m b t(occupation) kí hi u là ặ ươ ắ ắ ệ o
n- vector pháp tuy n (normal)ế
************Ma trân ***********************
5T6 = A6
Môt h ng và v trí c a khâu th 6 so v i khâu th 5:ả ướ ị ủ ứ ớ ứ
5T6 =A6 =
6 6
6 6
6
0 0
0 0
0 0 1
0 0 0 1
C S
S C
d
−
� �
� �
� �
� �
� �
� �
;
4T6 = A5. A6 =
5 6 5 6 5 5 6
5 6 5 6 5 5 6
6 6
. .
. .
1 0
0 0 0 1
C C C S S S d
S C S S C C d
S C
−
� �
� �
− −
� �
� �
� �
� �
3T6 = A4.A5.A6=
2T6 = A3.A4.A5.A6
[1,1]=
[1,2]=
[1,3]=
[1,4]=
[2,1]=
[2,2]=
[2,3]=
[2,4]=
[3,1]=
[3,2]=
[3,3]=
Đ án C đi n tồ ơ ệ ử
[3,4]=
1T6 = A2.A3.A4.A5.A6
[1,1]=
(CH A CH NH S A XONG)Ư Ỉ Ử
Trên c s đó, ta có h ph ng trình đ ng h c c a Robot PUMA nhơ ở ệ ươ ộ ọ ủ ư
sau:
nx = C1C24 - S1S24 = C124
ny = S1C24 + C1S24 = S124
nz = 0
ox = - C1S24 - S1C24 = - S124
oy = - S1S24 + C1C24 = C124
oz = 0
ax = 0
ay = 0
az = -1
px = a2C12 + a1C1.
px = a2S12 + a1S1.
pz = d3 + d4.
4. PH NG TRÌNH Đ NG H C NG C ROBOT PUMAƯƠ Ộ Ọ ƯỢ
Trong th c t , th ng ta bi t tr c ự ế ườ ế ướ v tríị và h ngướ mà khâu ch pấ
hành cu i c a robot c n đ t đ n. Đi u ta c n bi t là giá tr c a các bi nố ủ ầ ạ ế ề ầ ế ị ủ ế
kh p (góc quay) t i m i th i đi m đó. Gi i h ph ng trình (1), khi bi tớ ạ ỗ ờ ể ả ệ ươ ế
tr c h ng và v trí c a h t a đ g n trên khâu ch p hành cu i, ta s xácướ ướ ị ủ ệ ọ ộ ắ ấ ố ẽ
đ nh đ c t p nghi m (ị ượ ệ ệ θ1, θ2, θ3, θ4,θ5, θ6) là giá tr c a các bi n kh p. Cácị ủ ế ớ
ph ng trình xác đ nh giá tr các bi n kh p thông qua các véct ươ ị ị ế ớ ơ
p ,a ,o , rrr
r
n
đ c g i là h ph ng trình đ ng h c ng c c a robot. ượ ọ ệ ươ ộ ọ ượ ủ
Tham kh o bài gi ng c a ả ả ủ GS.TSKH Nguy n Văn Khangễ trong bài gi i vả ề
vi c gi i m t bài toán đ ng h c ng c b ng ph ng pháp s .ệ ả ộ ộ ọ ượ ằ ươ ố
G i ọtrong đó là t a đ suy r ng, ọ ộ ộ Trong đó
là v trí c a khâu thao tác. Gi a ị ủ ữ và liên h v i nhau b ngệ ở ằ
ph ng trình sau: ươ . Bài toán đ ng h c ng c đ c phát bi u nhộ ọ ượ ượ ể ư
sau: bi t ếtìm .T c là: ứ
Khi đó x y ra 3 tr ng h p:ả ườ ợ
+ m=n g i là Robot có c u trúc đ ng h c cân b ng (chu n). Ph ngọ ấ ộ ọ ằ ẩ ươ
trình có th có nghi m duy nh tể ệ ấ tùy thu c vào c u trúc c a h .ộ ấ ủ ệ
(1)