bài tập lớn robot.docx

Câu 1 : Cho Robot có cấu hình như hình vẽ ;a
1
=0,5m ; a
2
=0,2m ;a
3
=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

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 a
i
α
i
d
i
θ
i
1 a
1
0 0 θ
1
2 a
2
0 0 θ
2
3 a
3
0 0 θ
3
Kí hiệu :
i-1
A
i
=













−
−
1000
00
***
***
ii
iiiiiii
iiiiiii
CS
SaSCCCS
CaSSCSC
αα
θθθαθθ
θαθαθθ
Ta được :
0
A
1
=













−
1000
0100
*0
*0
1111
1111
θθθ
θθθ
SaCS
CaSC
1
A
2
=













−
1000
0100
*0
*0
2222
2222
θθθ
θθθ
SaCS
CaSC
2
A
3
=













−
1000
0100
*0
*0
3333
3333
θθθ
θθθ
SaCS
CaSC
Ma trận T biểu diễn tay robot :
T =
0
A
1
*
1
A
2
*
2
A
3
=













−
1000
0100
*0
*0
1111
1111
θθθ
θθθ
SaCS
CaSC
*













−
1000
0100
*0
*0
2222
2222
θθθ
θθθ
SaCS
CaSC
*












−

1000
0100
*0
*0
3333
3333
θθθ
θθθ
SaCS
CaSC
=












+
+−
1000
0100
**0
**0
111221212

111221212
θθθθ
θθθθ
SaSaCS
CaCaSC
*
2
A
3
=












++
++−
1000
0100
***0
***0
111221233123123
111221233123123

θθθθθ
θθθθθ
SaSaSaCS
CaCaCaSC
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 đó :
a
1
= 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 :
T =












++
++−
1000
0100

30°*5.045°*2.090°*1.0090°90°
30°*5.045°*2.090°*1.0090°90°
SSSCS
CCCSC
T =


















+
+
−
1000
0100
20
227

001
20
2235
010
Vị trí tay Robot : P=[
20
2235 +
,
20
227 +
,0 ,1]
T

Bài 2:
Bài làm
Robot θ-r
Giả thiết toàn bộ khối lượng thanh 1 (m
1
) tập trung vào điểm 1 và khối
lượng thanh 2 (m
2
) 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: r
1
= 0,25 m
x
m1
y
0
r
r1
m2
x1
x1
y2
y1
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.
. 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
i i
i i
d L L
M (F )
dt p p
 
∂ ∂
= −

 ÷
∂ ∂
 
(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
2 2
i i
i i
vθ
K m J
2 2
= +
&
2
1
1 1
v
K m
2
=
2 2 2
1 1 1
= +
& &
v x y
1 1 1 1
x r cosθ & y r sin θ= =

1 1 1
θ θ θ θ⇒ = − × = ×
& &
& &
1
x r sin & y r cos
2 2 2
1 1
θv r⇒ =
&
2 2
1
1 1
θ
2
r
K m⇒ =
&
2 2
2 2 2
v x y
= +
& &
2
2
θ θ θ θ
θ
θ θ θ
2
2

x r cos x r sin rcos

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

2
θ
2
m
K r r⇒ = +
&
&
( )
2 2 2 2 2
1 2 1 1 2
1 1
θ θ
2 2
K K K m r m r r⇒ = + = + +
& &
&
Thế năng của Robot
1 2 1 1 2
θ θP P P m gr sin m gr sin= + = +
( )
( )
2 2 2 2 2
1 1 2 1 1 2
1
θ θ θ θ
2
L K P
1
= m r m r r m gr sin m gr sin
2

= −
+ + − +
& &
&
Ta có :
2 2 2
1 1 2
θ θ
θ
L
m r m r
∂
= +
∂
& &
&
2 2
1 1 2 2
θ 2 θ θ
θ
d L
m r m rr m r
dt
∂
 
⇒ = + +
 ÷
∂
 

&& & &&
&
&
(2)
1 1 2
θ θ
θ
L
m gr cos m gr cos
∂
= − −
∂
(3)
2
L
m r
r
∂
=
∂
&
&
2
d L
m r
dt r
∂
 
⇒ =
 ÷

∂
 
&&
&
(4)
2
2 2
θ θ
L
m r m gsin
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
1 1 1 2 2 1 1 2
2
2 2 2 2
θ 2 θ θ
2θ θ
M m r m r m rr gcos m r m r
F m r m r m gsin

= + + + +



= − +


&& &
&
&
&&
Khi Robot ở cuối hành trình có :
r = r
max
= 0.5m
θ = θ
max
= π/6
Theo bài ra :
0 025
=
&
r , m / s
θ π 60
=
&
/ rad / s
r
1
= 0,25m
m
1

= m
2
= 1Kg
Do ở trên ta đã giả sử các khớp của Robot chuyển động đều nên
2
θ 0rad / s
=
&&
2
0r m / s=
&&
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à:
1
2
6 5
4 99
=


=

M . Nm
F . N
c.Thiết kế bộ điều khiển phản hồi độc lập cho từng khớp:
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 d

i
Theo đề bài: tỉ số truyền cho cả hai khớp (i) là 30 nên
có thể xem  d
i
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 d
i
, 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:
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:
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à R
a
(p), R
ω
(p) và R
p
(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ố K
a
, K
ω
, K
p
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:
R
p
(p) = K
Rp
R
ω
(p) = K
Rω
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:
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 T
Ra
= 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:
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á độ T
qđ
= 0.1 s
Để đá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ó:
Vậy tham số của bộ điều khiển có thể được thiết kế như sau:
R
p
(p) =4000 ; R
ω
(p) = 100
T
Ra
đượ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)
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 :
Hình 2 : Đồ thị đáp ứng góc quay
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)

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
Hình 6 : Đồ thị sai lệch khớp tịnh tiến r