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
ai
Cθ i
ai
Sθ i
0
1
Ket-noi.com diễn đàn công nghệ, giáo dục
Cθ1 −Sθ 1
Sθ 1 Cθ1
0
0
0
0
0
1
A1 =
A2 =
Cθ 2 −Sθ 2
Sθ 2 Cθ 2
0
0
0
0
Cθ 3 −Sθ 3
Sθ 3 Cθ3
0
0
0
0
2
A3 =
0 a 1
Cθ1
0 a 1
Sθ 1
1
0
0
1
0 a 2
Cθ 2
0 a 2 Sθ 2
1
0
0
1
0 a3
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
Ket-noi.com diễn đàn công nghệ, giáo dục
Cθ1 −Sθ 1
Sθ 1 Cθ1
0
0
0
0
=
0 a 3
Cθ 3
0 a 3 Sθ 3
1
0
0
1
Cθ12 −Sθ 12
Sθ 12 Cθ 12
0
0
0
0
Cθ 123 −Sθ 123
Sθ 123 Cθ 123
0
0
0
0
=
Cθ 2 −Sθ 2
Sθ 2 Cθ 2
0
0
0
0
0 a 2
Cθ 2
0 a 2 Sθ 2
1
0
0
1
*
Cθ3 −Sθ 3
Sθ 3 Cθ3
0
0
0
0
=
0 a 1
Cθ1
0 a 1
Sθ 1
1
0
0
1
0 a2
Cθ 12 a 1
Cθ1
0 a 2 Sθ 12 a 1
Sθ 1
1
0
0
1
0
0
1
0
*
*2A3
a3
Cθ 123 a 2
Cθ 12 a1
Cθ 1
a 3 Sθ 123 a 2
Sθ 12 a1 Sθ 1
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.
Ket-noi.com diễn đàn công nghệ, giáo dục
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 :
T=
T=
C 90° −S 90°
S 90° C 90°
0
0
0
0
0 0. 1C 90° 0 . 2C 45° 0 .5C 30°
0 0 . 1S 90° 0. 2 S 45° 0 .5S 30°
1
0
0
1
5√ 3 2√ 2
20
7 2 √ 2
0
20
1
0
0
1
0 −1 0
1
0
0
0
0
0
Vị trí tay Robot :
Bài 2:
Bài làm
P=[
5 √ 32 √ 2
20
,
7 2 √ 2
20
,0 ,1]T
Ket-noi.com diễn đàn công nghệ, giáo dục
Robot θ-r
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:
θ t θ 0 f θ , t
Với: Giá trị góc ban đầu θ0 = 0
Tốc độ quay của khớp quay:
Vậy góc θ sau 10s là :
θ
= π/60 rad/s
Ket-noi.com diễn đàn công nghệ, giáo dục
θ 10 0
π
π
.10
60
6
- Tính r:
r t r 1 f r , t
Với: r1 = 0,25 m
r
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à :
r 10 0,25 0,025.100.5 m
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
0
x1
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
Ket-noi.com diễn đàn công nghệ, giáo dụ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
&2
vθi2
K mi
Ji i
2
2
v12
K1 m1
2
v12 x&12 y&12
x1 r1 cosθ
x&1 r1 sin θ θ&
& y 1 r1 sin θ
&
y&1 r1cosθ θ&
v12 r12θ&2
r12θ&2
K1 m1
2
v 2 x&22 y&22
x 2 r cos θ
y 2 r sin θ
v 2 r&2 r 2θ&2
& θ
x&2 r sin θ θ& rcos
y&2 rcosθ θ& r&sin θ
Ket-noi.com diễn đàn công nghệ, giáo dục
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 m 2r 2 θ&
&
θ
d L
2&
2&
&
&
&
& m1r1 θ 2m2rr&θ m2r θ
dt θ
(2)
L
m1gr1cosθ m2gr cos θ
θ
(3)
L
m2r&
r&
d L
m2&
r&
dt r&
(4)
Ket-noi.com diễn đàn công nghệ, giáo dục
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
M1 m1r12 m2r 2 &
θ& 2m2rr&θ& gcos θ m1r1 m2r
r& 2θ
m2r &2 m2gsin
θ
F2 m2&
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 / 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 :
J
Đi K . H ii 0 q i Bi .
q i K gi . M 0 i d i
K gi . K Ui . U Đi
2
gi
với :
d i K
2
gi
n
∑
j 1 ≠ i
H ij q j ∆ H ii q i
Cq
G i
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
1
K gi rất nhỏ
i
Ket-noi.com diễn đàn công nghệ, giáo dục
có thể xem
2
K gi ≈ 0
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:
1
q i K gi K Ui U Đi− Bi q Đi −M 0 i
JΣ
i
trong đó :
1
K gi làtỉ số truyềncủa khớp
i
i
K Ui
K Mi
Ri
U Đi
là điện áp phần ứng động cơ truyền động khớp
Bi B Đi
q Đi
K ei K mi K ei K mi
bỏ qua ma sát nhớt động cơ
Ri
Ri
là góc quay động cơ truyền động.
J Σ J Đi K 2gi . H ii 0
i
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:
Ket-noi.com diễn đàn công nghệ, giáo dục
Hàm truyền của một khớp như sau:
W kh p
qi
K gi . K Đi
U Đi p T Đi . p 1
với:
K Đi
1
hệ số suất điệnđộng
K ei
T Đi
J Σ i Ri
hằng số thời gian động cơ
K mi K ei
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).
Ket-noi.com diễn đàn công nghệ, giáo dục
- 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ω
Ra p K Ra
1 T Ra . p
p
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:
W DT p
qi
U Đi
1 K D K Ra K a 1
K Đi
T Ra
TD p
TD
1 K D K Ra K a
1 K D K Ra K a
Ket-noi.com diễn đàn công nghệ, giáo dục
Hàm truyền mạch thẳng có dạng:
WT p
K gi K Rp K Rω K Ra 1 T Ra p
p
2
W DT p
Hàm truyền mạch phản hồi có dạng:
W f p K p
Kω p
K Rp
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:
Wk p
qi
W T p
qi 1 W T p . W f p
1
Kp
1
Hàm truyền theo tín hiệu nhiễu tải có dạng:
Kp
qi
K Đ K Rp K q K Rω K Ra K gi 1 T Ra p
Wc p
M p
1 K Đ K Ra K a
Kω
1
p
p2
K Rp K q K gi
K Đ K Rp K q K Rω K Ra K gi
Xét phương trình đặc tính của hệ thống:
1
1 K Đ K Ra K a
Kω
p
p2 0
K Rp K q K gi
K Đ K Rp K q K Rω K Ra K gi
Có dạng chuẩn bậc hai:
1 K Đ K Ra K a
Kω
p
p2
K Rp K p K gi
K Đ K Rp K p K Rω K Ra K gi
Ket-noi.com diễn đàn công nghệ, giáo dục
1
2ξ
1
p 2 p2 0
ωn
ωn
Trong đó: ξ là hệ số suy giảm
ωn
là tần số dao động
Đồng nhất hệ số ta được:
Kω
2ξ
1
K Rp K p K gi ωn
1 K Đ K Ra K a
1
2 2
K Đ K Rp K p K Rω K Ra K gi ωn
Tiêu chuẩn thiết kế :
Chỉ tiêu quá trình quá độ σ ≤ 20
Thời gian quá độ
Tqđ = 0.1 s
ξ 0,5
Để đáp ứng những tiêu chuẩn như trên, ta chọn :
ωn
Chọn các hệ số phản hồi như sau :
K p 1 ; K ω 1 ; K a 1
Và :
Từ (1) suy ra:
K e 2.3,14 . K m
;
K Đ
1
Ke
4
80
ξ . T qđ
Ket-noi.com diễn đàn công nghệ, giáo dục
K Rp
ωn
Kω
.
2 ξ K p K gi
80.1
1
2 .0,5.1 .
50
4000
Thay số vào (2) ta có:
1 K
K Ra K a
1
2
K Đ K Rp K p K Rω K Ra K gi ω n
↔
↔
Đ
1
1
. K .1
2.3,14 .0,1 Ra
1
1
1 802
.4000 .1 . K Rω . K Ra .
2.3,14 .0,1
50
1 1,5 9. K Ra
1
127,2. K Rω . K Ra 80 2
Chọn
K Rω 100
ta có:
1 1,59. K Ra
1
2
127,2.100 . K Ra 80
→ K Ra 2,52
Vậy tham số của bộ điều khiển có thể được thiết kế như sau:
Rp(p) =4000 ;
Ra p 2,52.
TRa được chọn bằng TĐ
T Đi
J Σ i Ri
K mi K ei
Rω(p) = 100
1 T Ra . p
p
Ket-noi.com diễn đàn công nghệ, giáo dục
- Đối với khớp quay θ :
2
2
2
J Σ 1 J Đ K g . m 1 . r 1 0,0005
2
g
2
1 1
JĐ K . m r
J R
T Đ1 Σ 1 1
K m1 Ke 1
K m1 . K e 1
Với
1
50
.R
2
.1 . 0,25 5,25.10
0,0005
1
50
−4
2
.1 . 0,25
2.3,14 . 0,1
2
2
.0,3
2,51.10−3
K e1 2.3,14 . K m 1
Suy ra bộ điều khiển Ra p
cho khớp θ như sau:
1 2.51 .10−3 . p
Ra p 2,52.
θ
p
- Đối với khớp tịnh tiến r:
2
2
J Σ 2 J Đ K g . m 2 m t 0,0005
2
g
1
50
J Đ K . m2 mt . R
J R
T Đ2 Σ 2 2
K m2 K e 2
K m2 . Ke 2
Suy ra bộ điều khiển Ra p
−4
. 1 0,2 9,8.10
0,0005
1
50
1 4,68. 10−3 . p
p 2,51.
r
p
d.Mô phỏng bằng MATLAB/SIMULINK
. 1 0,2 .0,3
2.3,14 .0,1
cho khớp � như sau:
Ra
2
2
4,68.10−3
Ket-noi.com diễn đàn công nghệ, giáo dục
+ Đối với khớp quay :
θ đặt
0
= 25
;
t mô phỏng
= 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 θ
- Xem thêm -