Bài tập lớn kĩ thuật robot 1

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
+ Trục x i đặ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

C i
 S
Kí hiệu : i-1Ai =  i
 0

 0

Ta được :

 S i * C i
C i * C i
S i

0

S i * S i
 C i * S i
C i
0

ai * C i 
ai * S i 
0 

1 


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

C 1
 S
0
A1 =  1
 0

 0
C 2
 S
1
A2 =  2
 0

 0

C 3
 S
2
A3 =  3
 0

 0

 S 1
C 1
0
0

0 a1 * C 1 
0 a1 * S 1 
1
0 

0
1 

 S 2
C 2
0
0

 S 3
C 3
0

0

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

C 12
 S
=  12
 0

 0

 S 1
C 1
0
0

0 a1 * C 1  C 2
0 a1 * S 1   S 2
*
1
0   0
 
0
1   0

 S 12
C 12
0
0

 S 2
C 2
0
0

0 a 2 * C 2  C 3
0 a 2 * S 2   S 3
*
  0
1
0
 
0
1
  0

0 a 2 * C 12  a1 * C 1 
0 a 2 * S 12  a1 * S 1  2
* A3

1
0

0
1


 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 123
 S
=  123
 0

 0

 S 123
C 123
0
0

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 :

C 90°
 S 90°

T= 
 0

 0

 S 90°
C 90°
0
0

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









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


0  1


T = 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

P=[

5 32 2 72 2
,
,0 ,1]T
20
20


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:
𝜃(𝑡) = 𝜃0 + 𝑓(𝜃̇, 𝑡)
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à :
𝜃(10) = 0 +

𝜋
𝜋
. 10 =
60
6

- Tính r:

𝑟(𝑡) = 𝑟1 + 𝑓(𝑟̇ , 𝑡)
Với: r1 = 0,25 m
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à :
𝑟(10) = 0,25 + 0,025.10 = 0.5( 𝑚)
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.


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

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
Mi (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

vi2
θ2
 Ji i
2
2

K1  m1

v12
2

v12  x12  y12

x1  r1 cosθ
 x1  r1 sin θ  θ

& y1  r1 sin θ
&

y1  r1cosθ  θ


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

 v12  r12θ 2

r12θ2
 K1  m1
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 θ

L  K P
1
1
= m1r12θ 2  m2 r 2  r 2θ 2  m1gr1 sin θ  m2gr sin θ 
2
2





Ta có :
L
 m1r12θ2  m2r 2θ
θ



d  L 
 m1r12θ  2m2rrθ  m2r 2θ


dt  θ 

(2)
L
 m1gr1cosθ  m2gr cos θ
θ

(3)
L
 m2r
r


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



d  L 
 m2 r

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

M1  m1r1  m2r θ  2m2rrθ  gcos θ m1r1  m2r 

2

F2  m2r  2m2rθ  m2gsin θ

Khi Robot ở cuối hành trình 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 / s2

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 :
2
𝐾𝑔𝑖 . 𝐾𝑈𝑖 . 𝑈Đ𝑖 = (𝐽Đ𝑖 + 𝐾𝑔𝑖
. 𝐻𝑖𝑖0 )𝑞𝑖̈ + 𝐵𝑖 . 𝑞̇ 𝑖 + 𝐾𝑔𝑖 . 𝑀0𝑖 + 𝑑𝑖

với :
𝑛
2
𝑑𝑖 = 𝐾𝑔𝑖
[ ∑ 𝐻𝑖𝑗 𝑞̈ 𝑗 + ∆𝐻𝑖𝑖 𝑞̈ 𝑖 + (𝐶𝑞̅̇ )𝑖 + 𝐺𝑖 ]
𝑗=1(≠𝑖)

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
𝐾𝑔𝑖 =

1
𝑖

rất nhỏ

2
có thể xem 𝐾𝑔𝑖
≈ 0  di rất bé có thể bỏ qua hoặc xem là nhiễu.


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

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:
𝑞̈ 𝑖 = 𝐾𝑔𝑖 (𝐾𝑈𝑖 𝑈Đ𝑖 − 𝐵𝑖 𝑞̇ Đ𝑖 − 𝑀0𝑖 )

1
𝐽Σ𝑖

trong đó :
𝐾𝑔𝑖 =
𝐾𝑈𝑖 =

1

𝑖

là tỉ số truyền của khớp i

𝐾𝑀𝑖
𝑅𝑖

𝑈Đ𝑖 là điện áp phần ứng động cơ truyền động khớp
𝐵𝑖 = 𝐵Đ𝑖 +

𝐾𝑒𝑖 𝐾𝑚𝑖 𝐾𝑒𝑖 𝐾𝑚𝑖
=
(bỏ qua ma sát nhớt động cơ)
𝑅𝑖
𝑅𝑖

𝑞̇ Đ𝑖 là góc quay động cơ truyền động.
2
𝐽Σ𝑖 = 𝐽Đ𝑖 + 𝐾𝑔𝑖
. 𝐻𝑖𝑖0 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:
𝑊𝑘ℎ (𝑝) =

𝐾𝑔𝑖 . 𝐾Đ𝑖
𝑞𝑖
=

𝑈Đ𝑖 𝑝(𝑇Đ𝑖 . 𝑝 + 1)


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

với:

𝐾Đ𝑖 =

1
𝐾𝑒𝑖

𝑇Đ𝑖 =

hệ số suất điện động
𝐽Σ𝑖 𝑅𝑖
𝐾𝑚𝑖 𝐾𝑒𝑖

hằng số thời gian động cơ

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

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ω
𝑅𝑎 (𝑝) = 𝐾𝑅𝑎

1 + 𝑇𝑅𝑎 . 𝑝
𝑝


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

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

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

𝑞𝑖
=
𝑈Đ𝑖

𝐾Đ𝑖
(1 + 𝐾𝐷 𝐾𝑅𝑎 𝐾𝑎 ) [1 +

𝑇
(1+𝐾𝐷 𝐾𝑅𝑎 𝐾𝑎 𝑅𝑎 )𝑇𝐷 𝑝
𝑇𝐷

(1+𝐾𝐷 𝐾𝑅𝑎 𝐾𝑎 )

]

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

𝑊𝑇 (𝑝) =

𝐾𝑔𝑖 𝐾𝑅𝑝 𝐾𝑅𝜔 𝐾𝑅𝑎 (1 + 𝑇𝑅𝑎 𝑝)
𝑊𝐷𝑇 (𝑝)
𝑝2

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:
𝑞𝑖
𝑊𝑇 (𝑝)
𝑊𝑘 (𝑝) = ∗ =
=
𝑞𝑖 1 + 𝑊𝑇 (𝑝). 𝑊𝑓 (𝑝) 1 +

1⁄
𝐾𝑝
𝐾𝜔
𝐾𝑅𝑝 𝐾𝑝 𝐾𝑔𝑖

𝑝+

(1+𝐾Đ 𝐾𝑅𝑎 𝐾𝑎 )
𝐾Đ 𝐾𝑅𝑝 𝐾𝑝 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖

𝑝2


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

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

𝑊𝑐 (𝑝) =

𝑞𝑖
=
𝑀𝑛𝑖 (𝑝) 1 +

𝐾Đ 𝐾𝑅𝑝 𝐾𝑞 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖 (1+𝑇𝑅𝑎 𝑝)
𝐾𝜔
𝐾𝑅𝑝 𝐾𝑞 𝐾𝑔𝑖

𝑝+

(1+𝐾Đ 𝐾𝑅𝑎 𝐾𝑎 )
𝐾Đ 𝐾𝑅𝑝 𝐾𝑞 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖

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

(1 + 𝐾Đ 𝐾𝑅𝑎 𝐾𝑎 )
𝐾𝜔
𝑝+
𝑝2 = 0

𝐾𝑅𝑝 𝐾𝑞 𝐾𝑔𝑖
𝐾Đ 𝐾𝑅𝑝 𝐾𝑞 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖

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

2𝜉
1
𝑝 + 2 𝑝2 = 0
𝜔𝑛
𝜔𝑛

Trong đó: 𝜉 là hệ số suy giảm
𝜔𝑛 là tần số dao động
Đồng nhất hệ số ta được:
𝐾𝜔
2𝜉
=
𝐾𝑅𝑝 𝐾𝑝 𝐾𝑔𝑖
𝜔𝑛
(1 + 𝐾Đ 𝐾𝑅𝑎 𝐾𝑎 )
1
= 2
𝜔𝑛
{𝐾Đ 𝐾𝑅𝑝 𝐾𝑝 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖

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

(1)

(2)

Chỉ tiêu quá trình quá độ

𝜎% ≤ 20%

Thời gian quá độ

Tqđ = 0.1 s

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

𝜉 = 0,5

𝑝2


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

𝜔𝑛 =

4
= 80
𝜉. 𝑇𝑞đ

Chọn các hệ số phản hồi như sau :
𝐾𝑝 = 1 ; 𝐾𝜔 = 1
Và :

𝐾𝑒 = 2.3,14. 𝐾𝑚

; 𝐾𝑎 = 1
;

𝐾Đ =

1
𝐾𝑒

Từ (1) suy ra:
𝐾𝑅𝑝 =

𝜔𝑛 𝐾𝜔
80.1
.
=
2𝜉 𝐾𝑝 𝐾𝑔𝑖 2 .0,5 .1 .

1

= 4000

50

Thay số vào (2) ta có:
(1 + 𝐾Đ 𝐾𝑅𝑎 𝐾𝑎 )
1
= 2
𝐾Đ 𝐾𝑅𝑝 𝐾𝑝 𝐾𝑅𝜔 𝐾𝑅𝑎 𝐾𝑔𝑖
𝜔𝑛

1

(1 + 2.3,14.0,1 . 𝐾𝑅𝑎 . 1)

↔

1
2.3,14.0,1

↔

. 4000 .1 . 𝐾𝑅𝜔 . 𝐾𝑅𝑎 .

1

=

50

1 + 1,59. 𝐾𝑅𝑎
1
= 2
127,2. 𝐾𝑅𝜔 . 𝐾𝑅𝑎 80

Chọn 𝐾𝑅𝜔 = 100 ta có:
1 + 1,59. 𝐾𝑅𝑎
1
= 2
127,2.100 . 𝐾𝑅𝑎 80
→ 𝐾𝑅𝑎 = 2,52

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

Rω(p) = 100

1
802


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

𝑅𝑎 (𝑝) = 2,52.

1 + 𝑇𝑅𝑎 . 𝑝
𝑝

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

𝐽Σ𝑖 𝑅𝑖
𝐾𝑚𝑖 𝐾𝑒𝑖

- Đối với khớp quay 𝜃:
𝐽Σ1 = 𝐽Đ +

𝑇Đ1 =

𝐾𝑔2 . 𝑚1 . 𝑟12

1 2

= 0,0005 + ( ) . 1 . 0,252 = 5,25. 10−4
50

𝐾𝑔2 . 𝑚1 𝑟12 ). 𝑅

(𝐽Đ +
𝐽Σ1 𝑅1
=
𝐾𝑚1 𝐾𝑒1
𝐾𝑚1 . 𝐾𝑒1
= 2,51. 10−3

1 2

=

(0,0005 + (50) . 1 . 0,252 ) . 0,3
2.3,14. 0,12

𝐾𝑒1 = 2.3,14. 𝐾𝑚1

Với

Suy ra bộ điều khiển 𝑅𝑎 (𝑝) cho khớp 𝜃 như sau:
1 + 2.51. 10−3 . 𝑝
𝑅𝑎 (𝑝)𝜃 = 2,52.
𝑝
- Đối với khớp tịnh tiến r:
𝐽Σ2 = 𝐽Đ +

𝑇Đ2 =

𝐾𝑔2 . (𝑚2

1 2
+ 𝑚𝑡 ) = 0,0005 + ( ) . (1 + 0,2) = 9,8. 10−4
50

𝐾𝑔2 . (𝑚2

1 2

+ 𝑚𝑡 )). 𝑅
(𝐽Đ +
𝐽Σ2 𝑅2
=
=
𝐾𝑚2 𝐾𝑒2
𝐾𝑚2 . 𝐾𝑒2
= 4,68. 10−3

(0,0005 + (50) . (1 + 0,2) ) . 0,3

Suy ra bộ điều khiển 𝑅𝑎 (𝑝) cho khớp 𝑟 như sau:
𝑅𝑎 (𝑝)𝑟 = 2,51.

1 + 4,68. 10−3 . 𝑝
𝑝

2.3,14.0,12

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

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

𝜃đặ𝑡 = 250

;

𝑡𝑚ô 𝑝ℎỏ𝑛𝑔 = 0.5 (s)

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