tính toan thiết kế robot

Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

Semester project
ROBOT CƠNG NGHIỆP

1.Tính số bậc tự do:
k

f = λ (n-k) +

∑f

i

+ fc + fp

i= 0

Với :
+ f : số bậc tự do của cơ cấu
+ fi : số bậc tự do chuyển động cho phép của khớp i
+ k : số khớp của cơ hệ
+ n : số khâu động của cơ hệ
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

1


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

+ λ : số bậc tự do của không gian cơ cấu thực hiện chuyển động
+ fc : số rằng buộc thừa
+ fp : số bậc tự do thừa

Thay số vào ta có:
f = 6(3-3) + 3 + 0 +0 = 3

Vậy cơ cấu đã cho có 3 bậc tự do.
2.Xây dựng hệ toạ độ khảo sát:

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

2


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

3.Lập bảng Denavit-Hartenbeg(DH):
Với cách thiết lập hệ toạ độ ở mỗi khâu của cơ cấu, có thể thành lập được ma trận
liên hệ giữa 2 hệ toạ độ liên tiếp. Hệ toạ độ thứ I có thể nhận được bằng cách biến
đổi :
+ Hệ toạ độ thứ i-1 dịch chuyển theo trục zi-1 một khoảng di.
+ Tiếp theo, quay hệ trục toạ độ i-1 mới quanh trục zi-1 một góc θi-1 để chuyển trục
xi-1 đến trục xi .
+ Tiếp tục thực hiện phép tịnh tiến hệ trục thu được ở bước trên dọc theo trục xi để
gốc toạ độ Oi-1 chuyển đến Oi.
+ Tiếp tục quay hệ trục toạ độ mới thu được quanh trục xi một góc αi-1 để đưa hệ
trục toạ độ i-1 trùng hệ trục toạ độ i.
Bảng Denavit – Hartenberg (DH)
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

3


Trường đại học bách khoa Hà Nội
ứng dụng
Khớp thứ i
1
2
3

di
d1
0
0

Bộ mơn cơ học

θi

θ1

θ2
θ3

ai
0
a2 = const
a3 = const

αi
90°

0
90°

4.Tính các ma trận truyền DH
Ma trận truyền DH có dạng:

cosθi

 sin θ
i
i −1
Ai = 
 0

 0

− sin θi cos αi
cosθi cosαi
sin α i
0

sin θi sin αi
−cosθi sinαi
cosαi
0

a1 cosθi 
a1 sin θi 
di 

1 

Theo bảng DH ta có

cos0 − sin 0 cos 0 sin 0sin 0 a1 cos0  1
sin 0 cos0cos0 −cos0sin0 a sin 0  0
1
0

=
A1 = 
 0
sin 0
cos0
d1  0

 
0
0
1  0
 0

SVTH

0
1
0
0

0 a1 
0 0 
1 d1 

0 1

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

4


Trường đại học bách khoa Hà Nội
ứng dụng
cos θ 2
 sin θ
2
1
A2 = 
 0

 0

Bộ môn cơ học

− sin θ 2 cos 0 sin θ 2 sin 0 a 2 cos θ2   cos θ2
cos θ 2 cos 0 − cos θ2 sin 0 a 2 sin θ2   sin θ2
=
sin 0
cos 0
0   0
 
0
0
1
  0

− sin θ2
cos θ2

0
0

0 a2 cos θ2 
0 a2 sin θ2 
1
0 

0
1 

Đặt c2 = cos θ2 ; s2 = sin θ2
Ta được

c 2
s
1
A2 =  2
0

0

−s2
c2
0
0

cos θ3
 sin θ
3

2
A3 = 
 0

 0
cos θ3
 sin θ
3
=
 0

 0

0 a 2 c2 
0 a 2 s2 
1
0 

0
1 

− sin θ3 cos θ3
cos θ3 cos θ3
sin 0
0
− sin θ3
cos θ3
0
0

sin θ3 sin 0 a 3 cos θ3 
− cos θ3 sin 0 a 3 sin θ3 
cos 0
0 

0
1 

0 a 3 cos θ3 
0 a 3 sin θ3 
1
0 

0
1 

Đặt c3 = cosθ3 ; s3 = sin θ3

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

5


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

Ta được

 c3
s
3
A4 =  3
0

0

−s 3

0 a 3c3 
0 a 3s3 
1
0 

0 1 

c3
0
0

Vậy ta có

−s 3
c3
0

0

 c3
s
3
2
T3 = A3 = 
0

0

1

T3

T3

SVTH

−s 2
c2
0
0

c2
s
2
2
= A2 . T3 = 
0


0

 c 2 c 3 - s 2s 3
s c + s c
2 3
3 2
= 

0

0

0

0 a 3 c3 
0 a 3 s3 
1
0 

0
1 

- s 3c 2 - s 2 c 3
- s 2s 3 + c 2 c 3
0
0

0 a2 c2  c3
0 a2 s2   s3

.
1
0  0
 
0
1  0

−s 3
c3
0
0

0 a 3 c3 
0 a 3 s3 
1
0 

0
1 

a 3c 2c 3 - a 3s 2 s3 + a 2c 2 
0 a 3s 2c 3 + a 3c 2 s3 + a 2 s2 

1
0

0
1

0

= A1 .1T3 =

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

6


Trường đại học bách khoa Hà Nội
ứng dụng
1
0
=
0

0

0 0 a1   c 2c3 - s2 s3
1 0 0  s 2c3 + s3c2
.
0
0 1 d1  
 
0
0 0 1 

 c2 c3 - s2 s3
s c + s c

2 3
3 2
= 

0

0


- s3 c2 - s2 c3
- s2 s3 + c2 c3
0
0

- s3 c2 - s2 c3
- s2 s3 + c2 c3
0
0

Bộ môn cơ học

0 a3 c2 c3 - a3 s2 s3 + a2 c2 
0 a3 s2 c3 + a3 c2 s3 + a2 s2 

1
0

0
1


0 a3 c2 c3 - a3 s2 s3 + a2 c2 + a1 
0
a3 s2 c3 + a3 c2 s3 + a2 s2 

1
d1

0
1


5.Thiết lập hệ phương trình đơng học robot
Vị trí của khâu tác động cuối với khâu cố định bởi ma trận biến đổi thuần nhất 4x4
sau:
0

T3 =

với u =

;v=

;w=

;p=

Hệ phương trình động học của robot là:
ux =

c 2 c 3 - s 2s 3

uy =

s 2c3 + s 3c 2 ;

uz =

SVTH

;

;

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

7


Trường đại học bách khoa Hà Nội
ứng dụng
vx =

- s 3c 2 - s 2 c 3

vy =

- s 2 s3 + c2 c3

Bộ môn cơ học

;

;

vz = 0 ;
wx = 0 ;
wy = 0 ;
wz = 1 ;
px = a 3c 2c3 - a 3s2 s3 + a 2 c2 + a1 ;
py = a 3s 2 c3 + a 3c2 s3 + a 2 s2 ;
pz =

d1

6.Bài toán động học thuận
a.Gán quy luật chuyển động cho các khâu của robot
+ q1 = d1 = sint
+ q2 = θ2 = 2t
+ q3 = θ3 = 3t
hay ta có ma trận:

sin t 


q =  2t 
 3t 
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

8


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

b.Phương pháp xác định vị trí của điểm tác động cuối và hướng của khâu thao tác
theo quy luật đã cho
Thay các giá trị q1 , q2, q3 vào hệ phương trình động học của robot ta có:
+Vị trí của điểm tác động cuối được xác định bằng toạ độ điểm tác động
cuối E theo phương x,y,z của hệ toạ độ gốc tương ứng là px, py, pz, thể hiện trong
ma trận :
p = p(q) =

Với p =

=

a 3c2 c3 - a 3s2 s3 + a 2 c2 + a1 
 a s c +a c s +a s 
 3 2 3 3 2 3 2 2 



d1
a 3cosq 2 cosq3 - a 3 sin q2 sinq3 + a2 cosq2 + a1 
 a sinq cosq + a cosq sinq + a sinq 
3
2
3
3
2
3
2
2




q1

a 3cos2tcos3t- a 3 sin 2tsin3t+ a2 cos2t + a1 


ta có p(t) =  a 3sin2tcos3t+ a3 cos2tsin3t+ a2 sin2t 


sint

tại mỗi thời điểm t điểm cuối E ở 1 vị trí xác định
+Hướng của khâu tác : được xác định bằng toạ độ của các véctơ đơn vị u,
v, w của hệ toạ độ gắn với khâu tác động cuối trong hệ toạ độ gốc, thể hiện trong
ma trận cosin chỉ hướng:

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

9


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

C = C(q) =

 c 2 c3 - s 2 s 3

= s 2 c3 + s 3 c 2


0

- s3c2 - s2 c3
- s 2s3 + c2 c3
0

 cosq 2 cosq 3 - sinq 2 sin q 3

= sinq 2 cosq 3 + sin q 3cosq 2


0

0
0 
1 
- sin q 3cosq 2 - sinq 2cosq 3
- sinq 2 sin q 3 + cosq 2cosq 3
0

0
0 
1 

Thay q1 = sint, q2 = 2t, q3 = 3t ta có:

 cos2tcos3t- sin2t sin 3t - sin 3tcos2t- sin2tcos3t 0 


C = C(t) = sin2tcos3t+ sin 3tcos2t - sin2t sin 3t+ cos2tcos3t 0 

0
0
1 

c. Ứng dụng matlab tính tốn và vẽ quỹ đạo chuyển động của điểm tác động cuối E
Dùng lập trình Matlab ta có với a1 = a2 = a3 = 10 (cm)
>>t=linspace(0,3*pi,300);
>> x=10.*(cos(2*t)).*(cos(3*t))-10.*(sin(2*t)).*(sin(3*t))+10.*(cos(2*t))+10;
>> y=10.*(sin(2*t)).*(cos(3*t))+10.*(cos(2*t)).*(sin(3*t))+10.*(sin(2*t));

>> z=sin(t);
>> plot3(x,y,z)
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

10


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

Ta được quỹ đạo điểm cuối như hình vẽ

Quỹ đạo im E
7. Bi toỏn ng hc ngc
Bài toán ngợc là bài toán có ý nghĩa rất quan trọng trong
thực tế. Khi biết quy luật chuyển động của khâu thao tác và ta
phải tìm các giá trị của biến khớp. Việc xác định các giá trị của
biến khớp cho phép ta ®iỊu khiĨn robot theo ®óng q ®¹o ®·
cho.

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

11


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ hc

Trong bài toán này, ta biết trớc 3 tham số là x, y, z và dựa
vào 3 phơng trình xác định vị trí, ta xác định đợc quy luật
của d1, θ2,θ3.

Cho quỹ đạo khâu thao tác E chuyển động

 x = 5sin(t )

 y = 5cos(t )
z = 5


6

5.5

5

4.5

4
5
5
0
0
-5

-5

Nhiệm vụ của chúng ta bây giờ là tìm d1, θ2,θ3

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

12


Trường đại học bách khoa Hà Nội
ứng dụng
 c 2 c 3 - s 2 s3
s c + s c
2 3
3 2
Ta có 0T3 = 

0


0

uEx
u
Ey
= 
uEz

 0

- s3 c2 - s2 c3
- s2 s3 + c2 c3
0
0

vEx

w Ex

vEy
vEz

w Ey
w Ez

0

0

Bộ môn cơ học

0 a3 c2 c3 - a3 s2 s3 + a2 c2 + a1 
0
a3 s2 c3 + a3 c2 s3 + a2 s2 

1
d1

0
1


p Ex 
p Ey 
(*)
p Ez 

1 

Từ (*) ta có
pEx = a3c2c3 - a3s2s3 + a2c2 +a1 = a3c23 + a2c2 +a1 = 5 sin(t) (1)
pEy = a3s2c3 + a3c2s3 + a2s2

= a3s23 + a2s2

= 5 cos(t) (2)

pEz = d1 = 5
với c23 = cos(θ2+θ3)
s23 = sin(θ2+θ3)

Từ (1) và (2) ta có
(pEx - a1)2 = (a3c23 + a2c2 )2 = a32. c232 + 2a2a3c2c23 + (a2c2)2

(1’)

pEy 2 = (a3s23)2 + 2a3s23a2s2 + (a2s2 )2

(2’)

từ (1’) và (2’) ta suy ra
(pEx - a1)2 + pEy 2 = a32 + a22 + 2a2a3c3
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

13


Trường đại học bách khoa Hà Nội
ứng dụng

Hay c3 =

( p Ex − a1 )

2

Bộ môn cơ học

+ p Ey 2 −a 32 + a 22
2a 2a 3

 ( p Ex − a1 ) 2 + pEy 2 −a3 2 + a2 2
Vậy θ3 = arcosc3 = arcos 
2a 2a 3






Thay vào (2) ta được

s2 =

p Ey a 2 ± ( p Eya 2 )2 − (sin 2 θ 3 − p Ey 2 )(a 32cos 2θ 3 − sin 2 θ 3 − a 22)
(a 32 cos 2θ 3 − sin 2 θ 3 − a 2 2 )

 p a ± ( p a ) 2 − (sin 2 θ − p 2 )(a 2 cos2θ − sin2 θ − a 2 ) 
Ey 2
Ey 2
3
Ey
3
3
3
2


θ2 = arcsin s2 = arcsin 
2
2
2
2
(a 3 cos θ3 − sin θ3 − a 2 )




chọn a1 = a2 = a3 = 10 ta có
t=linspace(0,3*pi,300);
>> p1 = 5*sin(t);
>> p2 = 5*cos(t);
>> p3 = 5;
SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

14


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

>> x = acos((((p1)-10).^2+(p2).^2-10.^2+ 10.^2)/(2.*10.*10));

>>y=asin(((p2)*10-sqrt(((p2*10).^2)-((sin(x)).^2-(p2).^2).*((10*cos(x)).^2(sin(x)).^2)-10.^2))/((10*cos(x)).^2-(sin(x)).^2-10.^2));
>>z = p3;

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

15


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

>>plot3(x,y,z)

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

16


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

Đồ thị của các khớp khi biết khâu thao tác E
8. Không gian làm việc của robot
Không gian làm việc của cánh tay robots là không gian được giới hạn bởi mặt trụ
như hình vẽ có chiều cao bằng với giới hạn di chuyển của khâu 1 và có đường kính
bằng tổng chiều dài của khâu 2 và khâu 3. Và vùng làm việc của robot bị giới hạn
một phần do bị cản bởi khâu 1 nên góc quay Ѳ2 sẽ không quay được hết 360o

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

17


Trường đại học bách khoa Hà Nội
ứng dụng

Bộ môn cơ học

9. Trình bày giải thuật và chương trình tính tốn động lực học của robot

SVTH

: Nguyễn Quang Nam

GVHD : PGS.TS. Phan Bùi Khôi

18