BÀI tập lớn kỹ THUẬT ROBOT

Ket-noi.com diễn đàn công nghệ, giáo dục

Câu 1 : Cho Robot có cấu hình như hình vẽ ;a1=0,5m ; a2=0,2m ;a3=0,1m.
a.

Xây dựng hệ tọa độ cho các thanh nối .

b.

Xác định ma trận T biểu diễn hệ tọa độ tay Robot.

c.

Giải thích ý nghĩa của ma trận T

d.
Xác định vị trí của tay Robot trong hệ tọa độ gốc khi
θ1=30° ;θ2=15° ;θ3=45°

Bài làm :
a)

Xây dựng hệ tọa độ cho các thanh nối


Ket-noi.com diễn đàn công nghệ, giáo dục

Hệ tọa độ cho các thanh nối được xây dựng được xây dựng theo phương pháp biểu
diễn Danevit-Hartenberg (D-H). Khung tọa độ thanh nối thứ i được xây dựng theo
nguyên tắc
+ Gốc khung tọa độ thanh i đặt trùng với chân pháp tuyến chung của trục i và i+1,

nằm trên trục khớp i+1
i

+ Trục x đặt theo phương pháp tuyến chung của trục i và i+1 theo hướng đi từ
trục i đến i+1

b) Xác định ma trận T biểu diễn tay robot.

Bảng D – H:

Thanh

ai

αi

di

θi

1

a1

0

0

θ1

2

a2

0

0

θ2

3

a3

0

0

θ3

Kí hiệu : i-1Ai =

Ta được :

Cθ i
 Sθ
 i
 0

 0

− Sθ i * C α i
C θ i * Cα i
Sα i
0

Sθ i * Sα i
− C θ i * Sθ i
Cα i
0

a i * Cθ i 
ai * Sθ i 
0 

1 


Ket-noi.com diễn đàn công nghệ, giáo dục

0

1

2

A1 =

A2 =

A3 =

Cθ 1
 Sθ
 1
 0

 0

− Sθ 1
Cθ 1
0
0

Cθ 2
 Sθ
 2
 0

 0

Cθ 3
 Sθ
 3
 0

 0

− Sθ 2
Cθ 2

0
0

− Sθ 3
Cθ 3
0
0

0 a1 * Cθ 1 
0 a1 * Sθ 1 
1
0 

0
1 
0 a 2 * Cθ 2 
0 a 2 * Sθ 2 

1
0

0
1


0 a 3 * Cθ 3 
0 a 3 * Sθ 3 
1
0 


0
1


Ma trận T biểu diễn tay robot :

T = 0A1 * 1A2*2A3

=

Cθ 1
 Sθ
 1
 0

 0

− Sθ 1
Cθ 1
0
0

0 a1 * Cθ 1 
0 a1 * Sθ 1 
1
0 

0
1 

*

Cθ 2
 Sθ
 2
 0

 0

− Sθ 2
Cθ 2
0
0

0 a 2 * Cθ 2 
0 a 2 * Sθ 2 

1
0

0
1


*

Cθ 3
 Sθ
 3
 0


 0

− Sθ 3
Cθ 3
0
0

0 a 3 * Cθ 3 
0 a 3 * Sθ 3 
1
0 

0
1



Ket-noi.com diễn đàn công nghệ, giáo dục

=

=

Cθ 12
 Sθ
 12
 0

 0

− Sθ 12
Cθ12
0
0

Cθ 123
 Sθ
 123
 0

 0

− Sθ 123
Cθ 123
0
0

0 a 2 * Cθ 12 + a1 * Cθ 1 
0 a 2 * Sθ 12 + a1 * Sθ 1 

1
0

0
1


*2A3

0 a 3 * Cθ 123 + a 2 * Cθ 12 + a1 * Cθ 1 
0 a 3 * Sθ 123 + a 2 * Sθ 12 + a1 * Sθ 1 

1
0

0
1


Trong đó θ12= θ1+ θ2 ; θ123= θ1+ θ2+ θ3
c Ý nghĩa của ma trận T

Ma trận T biểu diễn khung tọa độ tay robot so với khung tọa độc gốc thông qua
các biến khớp θ , d .
Do đó khi biết các biến khớp ,ma trận T cho phép xác định vị trí và định hướng
tay robot.
3 vecto đầu biểu diễn hướng , vecto thứ 4 biểu diễn vị trí tay robot.
d) Xác định vị trí tay robot trong hệ tọa độ gốc .
Trong đó :
a1 = 0.5

θ1=30°

θ12 =45°

a = 0.2

θ2=15°

θ123=90°

a = 0.1

θ3=45°

Thay giá trị vào ta được :


Ket-noi.com diễn đàn công nghệ, giáo dục

T=

T=

C 90°
 S 90°

 0

 0

− S 90°
C 90°
0
0


0 − 1


1 0

0 0

0 0

5 3 + 2 2

20

7+2 2 
0

20

1
0

0
1

0

Vị trí tay Robot :

Bài 2:
Bài làm

Robot θ-r

0 0.1 * C 90° + 0.2 * C 45° + 0.5 * C 30°
0 0.1 * S 90° + 0.2 * S 45° + 0.5 * S 30°
1
0
0
1

P=[

5 3+2 2
20

,

7+2 2
20

,0 ,1]T









Ket-noi.com diễn đàn công nghệ, giáo dục

Giả thiết toàn bộ khối lượng thanh 1 (m1) tập trung vào điểm 1 và khối

lượng thanh 2 (m2) tập trung vào điểm 2 (như hình vẽ).
a) Xác định góc θ và r của Robot ở cuối hành trình chuyển động với thời gian

chuyển động là 10s và các thông số khác cho trong đề bài.
Ta có:
-

Tính góc quay:

Với: Giá trị góc ban đầu θ0 = 0
Tốc độ quay của khớp quay: = π/60 rad/s
Vậy góc θ sau 10s là :

-

Tính r:

Với: r1 = 0,25 m


Ket-noi.com diễn đàn công nghệ, giáo dục

Tốc độ chuyển động của khớp tịnh tiến: = 0,025 m/s
Vậy giá trị r sau 10s là :

b) Xác định mô men ở khớp quay và lực tổng ở khớp tịnh tiến khi Robot ở

cuối hành trình chuyển động.
y

r
r1
y2
y1

m2
m1
x1

0

x1

x

. Sơ đồ phân tích động lực học của Robot r - θ
Dạng tổng quát của phương trình động lực học
M i (Fi ) =

d  ∂L  ∂L

÷−
dt  ∂pi  ∂pi

(1)
Trong đó : L = K – P
K = tổng động năng của hệ
P = tổng thế năng của hệ
Động năng của Robot
K = mi

&2
vθi2
+ Ji i
2
2


Ket-noi.com diễn đàn công nghệ, giáo dục
K1 = m1

v12
2

v12 = x&12 + y&12

x1 = r1 cosθ

& y 1 = r1 sin θ

⇒ x&1 = −r1 sin θ ×θ&

&

y&1 = r1cosθ ×θ&

⇒ v12 = r12θ&2
⇒ K1 = m1

r12θ&2

2

v 2 = x&22 + y&22

& θ
 x& = −r sin θ ×θ&+ rcos
⇒  2
&
 y&2 = rcosθ ×θ + r&sin θ

 x 2 = r cos θ

 y 2 = r sin θ
⇒ v 2 = r&2 + r 2θ&2
⇒ K2 =

m2 2 2 &2
r& + r θ
2

(

⇒ K = K1 + K 2 =

)
(

1
1
m1r12θ&2 + m2 r&2 + r 2θ&2

2
2

)

Thế năng của Robot
P = P1 + P2 = m1gr1 sin θ + m2gr sin θ


Ket-noi.com diễn đàn công nghệ, giáo dục

L = K −P
1
1
= m1r12θ&2 + m2 r&2 + r 2θ&2 − ( m1gr1 sin θ + m2gr sin θ )
2
2

(

)

Ta có :
∂L
= m1r12θ&2 + m 2r 2 θ&
&
∂θ

⇒

d  ∂L 
= m1r12&
θ&+ 2m2rr&θ&+ m2r 2&
θ&

÷
dt  ∂θ&

(2)
∂L
= −m1gr1cosθ − m2gr cos θ
∂θ

(3)
∂L
= m2r&
∂r&

⇒

d  ∂L 
r&
 ÷ = m2&
dt  ∂r&

(4)
∂L
= m2rθ&2 − m2gsin θ
∂r

(5)
Thay (2) & (3), (4) & (5) vào (1) ta được phương trình động lực học của
Robot

(

)

2
2 &
&

θ&+ gcos θ ( m1r1 + m2r )
M1 = m1r1 + m2r θ + 2m2rr&

r&− 2θ
m2r &2 + m2gsin
θ

F2 = m2&

Khi Robot ở cuối hành trình có :


Ket-noi.com diễn đàn công nghệ, giáo dục

r = rmax = 0.5m
θ = θmax = π/6
Theo bài ra :
r&= 0, 025m / s

θ&= π / 60rad / s

r1 = 0,25m
m1 = m2 = 1Kg
Do ở trên ta đã giả sử các khớp của Robot chuyển động đều nên
&
θ&= 0rad / s2
&
r&= 0m / s 2

Mô men ở khớp quay và lực tổng ở khớp tịnh tiến khi robot ở cuối hành trình
là:
M1 = 6.5Nm

F2 = 4.99N

c.Thiết kế bộ điều khiển phản hồi độc lập cho từng khớp:


Ket-noi.com diễn đàn công nghệ, giáo dục

Phương trình động lực học Robot và cơ cấu chấp hành, viết cho khớp i :

với :

Phương trình động lực học Robot và cơ cấu chấp hành khớp i gồm hai thành phần:
-

Thành phần tuyến tính và chỉ chứa các tham số,hằng số của khớp i
Thành phần phi tuyến ràng buộc di

Theo đề bài: tỉ số truyền cho cả hai khớp (i) là 30 nên

có thể xem

 di rất bé có thể bỏ qua hoặc xem là nhiễu.

Nếu bỏ qua thành phần nhiễu tải di, Robot được xem như hệ n khớp độc lập nhau.
Ta có thể viết lại phương trình (2.1) như sau:


Ket-noi.com diễn đàn công nghệ, giáo dục

trong đó :
i

là điện áp phần ứng động cơ truyền động khớp

là góc quay động cơ truyền động.
là mô-men quán tính tổng
Suy ra sơ đồ cấu trúc động cơ - khớp Robot, trong trường hợp xem các khớp là độc
lập, như sau:

Hàm truyền của một khớp như sau:

với:

Sơ đồ hệ thống điều khiển phản hồi cho khớp có dạng như sơ đồ sau:

Ket-noi.com diễn đàn công nghệ, giáo dục

Hệ thống điều khiển gồm 3 mạch vòng điều chỉnh gia tốc, tốc độ, vị trí tương ứng
với 3 bộ điều khiển là Ra(p), Rω(p) và Rp(p).
Mạch vòng trong cùng điều chỉnh gia tốc là khâu tỷ lệ - tích phân (PI) để
nhận được sai lệch tĩnh bằng 0
- Các bộ điều khiển vòng ngoài có cấu trúc tỷ lệ (P)
-

Các hệ số Ka, Kω, Kp tương ứng là các hệ số phản hồi gia tốc, tốc độ và vị trí khớp.
Cấu trúc các bộ điều khiển có dạng:
Rp(p) = KRp
Rω(p) = KRω

Biến đổi sơ đồ điều khiển thành sơ đồ như sau:


Ket-noi.com diễn đàn công nghệ, giáo dục

Từ sơ đồ trên ta có hàm truyền đối tượng điều khiển của hệ thống:

Hàm truyền mạch thẳng có dạng:

Hàm truyền mạch phản hồi có dạng:

Bằng cách chọn TRa = TĐ sẽ khử được hằng số thời gian lớn của hệ thống. Hàm
truyền kín của hệ thống được viết như sau:

Hàm truyền theo tín hiệu nhiễu tải có dạng:

Xét phương trình đặc tính của hệ thống:


Ket-noi.com diễn đàn công nghệ, giáo dục

Có dạng chuẩn bậc hai:

Trong đó: là hệ số suy giảm
là tần số dao động
Đồng nhất hệ số ta được:

Tiêu chuẩn thiết kế :

Chỉ tiêu quá trình quá độ
Thời gian quá độ

Để đáp ứng những tiêu chuẩn như trên, ta chọn :

Chọn các hệ số phản hồi như sau :

Và :
Từ (1) suy ra:

Thay số vào (2) ta có:

Chọn ta có:

;

Tqđ = 0.1 s

Ket-noi.com diễn đàn công nghệ, giáo dục

Vậy tham số của bộ điều khiển có thể được thiết kế như sau:
Rp(p) =4000 ;

TRa được chọn bằng TĐ

-

Đối với khớp quay :

Với
Suy ra bộ điều khiển cho khớp như sau:

-

Đối với khớp tịnh tiến r:

Suy ra bộ điều khiển cho khớp � như sau:

d.Mô phỏng bằng MATLAB/SIMULINK
+ Đối với khớp quay : =

;

= 0.5 (s)

Rω(p) = 100

Ket-noi.com diễn đàn công nghệ, giáo dục

Hình 1: Sơ đồ khối hệ thống điều khiển phản hồi khớp quay
Kết quả mô phỏng :


Ket-noi.com diễn đàn công nghệ, giáo dục

Hình 2 : Đồ thị đáp ứng góc quay


Ket-noi.com diễn đàn công nghệ, giáo dục

Hình 3 : Đồ thị sai lệch khớp quay
+ Đối với khớp tịnh tiến r :

= 0.4 m

;

= 0.5 (s)


Ket-noi.com diễn đàn công nghệ, giáo dục

Hình 4 : Sơ đồ khối hệ thống điều khiển phàn hồi khớp tịnh tiến r
Kết quả mô phỏng :

Hình 5: Đồ thị đáp ứng khớp tịnh tiến r


Ket-noi.com diễn đàn công nghệ, giáo dục

Hình 6 : Đồ thị sai lệch khớp tịnh tiến r