Đề 14Câu 1. Cho Robot có cấu hình như hình vẽ : d2 = 0,2 m ;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 = 45o ; θ2 = 450 ; d3 = 0,1 m .
Trang 1Mục lục
Trang 2Đề 14
Câu 1 Cho Robot có cấu hình như hình vẽ : d2 = 0,2 m ;
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 = 45o ; θ2 = 450 ; d3 = 0,1 m
a
d
2
3
Hình 1 : Cơ cấu Robot.
Câu 2 Cho Robot r - θ có r1 = 0,5 m ; m1 = m2 = 2 Kg Khớp tịnh tiến dịch chuyển từ
vị trí ban đầu r1 đến vị trí cuối ứng với rmax = 1, m Khớp quay quay với tốc độ
θ = 18 độ/s từ góc ban đầu là 0o đến góc 900
a Hãy xác định momen ở 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
b Thiết kế bộ điều khiển phản hồi PID cho từng khớp
c Mô phỏng hệ thống
Trang 3y
z
d
d
O
2
3
z3
z2
z1
x 1 x 2
x 3
O1
O3
O2
x
Câu 1:
a Xây dựng hệ tọa độ cho các thanh nối.
Xây dựng hệ toạ độ cho các thanh nối theo phương pháp Denavit_Hartenberg: Chọn khung tọa độ gốc (O) và các khung toạ độ thanh nối như hình vẽ
Hình 1.2 : Các khung tọa độ thanh nối của Robot.
Trong đó:
+ Gốc khung toạ độ thanh i đặt ở trục i+1 (cuối thanh i)
+ Trục zi trùng trục khớp i+1
+ Trục xi trùng phương ai nhưng đi từ i đến i+1
+ Khi hai trục cắt nhau ta chọn x, y sao cho khi quay từ zi – 1 thì đến trùng với zi
b Ma trận T biểu diễn hệ tọa độ tay Robot
Các khung tọa độ các thanh nối của Robot đã được thiết kế ở hình 1.2 Từ đó,
ta xác định được các tham số theo biểu diễn D – H trong bảng sau :
Trang 4
Bảng 1.1 : Bảng D – H của Robot.
Ma trận biểu diễn quan hệ giữa hai khung tọa độ i-1 và i
1
i
i
A
−
( 1.2 )
Trong đó :
Ta sử dụng ma trận ( 1.2 ) và bảng 1.1 thì ta xác định được các ma trận thành
phần
1
i
i
A
−
biểu diễn quan hệ giữa hai khung tọa độ của hai khớp i-1, i là :
= ; =
= ;
Ma trận T biểu diễn hệ trục tọa độ tay Robot :
3 1. 2. 3
T = A A A
=
=
= ; ( 1.3 )
Trong đó :
0
1
2
T3
Trang 5c Ý nghĩa của ma trận T3
• Ma trận T3 có 4 hàng 4 cột
• Biểu diễn một khung tọa độ tay Robot trong khung tọa chuẩn
• Biểu diễn vị trí và hướng của tay Robot
• 3 cột đầu tiên biểu diễn hướng của tay Robot, cột thứ tư biểu diễn vị trí của tay Robot
+ Cột thứ nhất biểu diễn vectơ pháp tuyến n ( normal )
+ Cột thứ hai biểu diễn vectơ có hướng mà theo đó ngón tay của bàn tay nắm vào nhau khi cần nắm đối tượng, gọi là vectơ o (occupation)
+ Cột thứ ba biểu diễn vectơ có hướng mà theo đó bàn tay tiếp cận với đối tượng, gọi là vectơ a ( approach )
d Vị trí của tay Robot trong hệ tọa độ gốc
Khi ; ; d2 = 0,2 m ; d3 = 0,1 m
Vị trí tay Robot được xác định bởi ma trận T3 như sau:
= ; ( 1.3 )
=
=
Vậy vị trí tay robot trong hệ tọa độ gốc là: [-0,091 0,191 0,071 1]T
T3
Trang 62 r
r1
m2
m1
y2
y1
y
1
2 r
r1
m2
m1
y2
y1 y
câu 2
a Momen ở khớp quay và lực tổng hợp ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động
Hình 2.1 : Cấu trúc Robot có hai thanh nối.
Dạng tổng quát của phương trình động lực học:
θ +
=
=
=
−
=
∂
∂
−
∂
∂
=
∑
∑
=
=
2
J 2
v m K
P P
K K
P K L
q
L q
L dt
d ) F ( M
2 i i
2 i i i
2
1
i i
2
1
i i
i
i i
Trong đó :
• Mi và Fi lần lượt là momen của khớp thứ i và lực tác dụng lên thanh nối thứ i
x
Trang 7• q là biến của khớp quay hoặc khớp tịnh tiến.
• L là một hàm của Lagrange
• Ki và Pi lần lược là động năng và thế năng của thanh nối thứ i
• mi và lần lượt là khối lượng và vận tốc dài của thanh nối thứ i
• Ji và lần lượt là momen quán tính của thanh nối thứ i đối với trục qua tâm khối của thanh nối thứ i và khớp quay của thanh nối thứ i
Ta lần lượt xác định các giá trị Ki và Pi:
Xác định động năng và thế năng của thanh nối thứ 1
Động năng của thanh nối thứ nhất là :
K1 = m1 + J1
Vì J1 = 0 nên ta có : K1 = m1
Theo hình 2.1, ta có :
2 1
2 1
2
v = +
=
&
&
&
&
Do đó:
2 2 2
1 1
2 2 1
1 1
(2.1) 2
r
θ θ
=
=
&
&
Thế năng của thanh nối thứ nhất là :
P1 = m1gr1 sinθ ( 2.2 )
Xác định động năng và thế năng của thanh nối thứ 2
Động năng của thanh nối thứ 2 là :
K2 = m2 + J2
Vì J1 = 0 nên ta có : K2 = m2 Tương tự thanh nối thứ 1:
Trang 82 2
2 2
2
v = +
⇒
&
& &
&
&
Do đó:
( )
2 2 2 2 2
2 2 2
2 2
2.3 2
θ θ
= +
+
=
&
&
&
&
Thế năng của thanh nối thứ 2 là :
P2 = m2gr sinθ (2.4)
Từ (2.1) và (2.3), ta có tổng động năng của hai thanh nối là :
K = K1 + K2 =
) r r m 2
1 r
m 2
2 2
2 1
1 θ + + θ
( 2.5 )
Từ (2.2) và (2.4), ta có tổng thế năng của hai thanh nối là :
P = P1 + P2 = m1gr1 sinθ + m2gr sinθ ( 2.6 )
Do vậy, từ (2.5) và (2.6), ta có :
θ
− θ
− θ + +
θ
=
−
= m r r ) m gr sin m grsin
2
1 r
m 2
1 P K
2 2
2 1
Xác định momen ở khớp quay của Robot θ - r
Theo phương trình động lực học tổng quát :
i i
i i
q
L q
L dt
d ) F ( M
∂
∂
−
∂
∂
=
Đối với khớp quay q1 = θ, ta có :
Trang 9∂θ
∂
−
θ
∂
∂
dt
d
Với :
θ
− θ
− θ + +
θ
=
−
= m r r ) m gr sin m grsin
2
1 r
m 2
1 P K
2 2
2 1
θ
− θ
−
= θ
∂
∂
θ +
θ +
θ
=
θ
∂
∂
=>
θ +
θ
= θ
∂
∂
cos gr m cos gr m L
r m 2 r
m r
m
L dt d
r m r
m L
2 1
1
2
2 2
2 1 1
2 2
2 1 1
Do đó :
θ +
θ +
θ +
θ +
θ
=
cos g ) m r m ( r m 2 ) r m r m (
cos gr m cos gr m r m 2 r
m r
m
M
2 1 1 2
2 2
2 1 1
2 1
1 2
2 2
2 1 1 1
Xác định tốc độ chuyển động của khớp tịnh tiến
Theo đầu bài, ta có :
= 18o = ( rad/s ) dθ = dt
Mà = 90o = ( rad/s )
Thời gian chuyển động của khớp quay là :
Tốc độ chuyển động của khớp tịnh tiến là :
Theo đề bài, ta đã biết ở cuối hành trình chuyển động :
rmax = 1.1 m
(rad/s)
Do khớp tịnh tiến chuyển động với tốc độ không đổi
Trang 10r=0
Do khớp quay chuyển động với tốc độ không đổi
Thay số vào M1 , ta thu được momen ở khớp quay Robot θ – r :
M1 = 0 + 2*2*1*0.1* + (2*0.5+2*1)*9.81*
= 0,125 ( N.m )
Xác định lực tổng ở khớp tịnh tiến của Robot θ - r
Theo phương trình động lực học tổng quát :
i i
i i
q
L q
L dt
d ) F ( M
∂
∂
−
∂
∂
=
Đối với khớp tịnh tiến q 2 = r, ta có :
r
L r
L dt
d
F2
∂
∂
−
∂
∂
=
Với :
θ
− θ
− θ + +
θ
=
−
= m r r ) m gr sin m grsin
2
1 r
m 2
1 P K
2 2
2 1
Ta có:
θ
− θ
=
∂
∂
=
∂
∂
=>
=
∂
∂
sin g m r
m
r m dt
d
r m
2
2 2
2 2
r L
r L r
L
Trang 11=> F =m r−m rθ2 +m2gsinθ
2 2
Thay số vào F2 , ta thu được lực tổng ở khớp tịnh tiến khi Robot ở cuối hành trình chuyển động :
F2 = = 19,4 (N)
Vậy momen ở 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 là:
M = 0,125 N.m
F = 19,4 N