ĐỀ BÀI:
Hình 1: Robot Scalar 2DOF
Cho robot Scalar 2DOF có thông số các khớp như sau:
l1 =0.5 ; lg1 = 0.25;
l2 =0.5 ; lg2 =0.25 ;
m1 =25 ; m2 =25 ;
J1 = 5; J2 = 5;
θ1 = ;
θ2
=;
K1 = 21;
K2 = 30;
T1 = T2 = 0.05;
BÀI TẬP SỐ 1:
Mỗi khớp được điều khiển bằng hệ driver có hàm truyền:
qi
=
ui
K
Ts 1
Khớp 1 có K=21, T=0,05; khớp 2 có K=21, T=0,05
1.Thiết kế bộ điều khiển vị trí cho mỗi khớp biết |Ui| ≤ 10, độ quá điều
chỉnh không lớn hơn 5%, thời gian quá độ nhỏ nhất.
Ta coi thời gian quá độ là thời gian hệ thống lần đầu đạt đến giá trị sai số
± 2% so với giá trị đặt.
Mô hình điều khiển:
Hàm truyền đối tượng:
GR =
K
1
.
Ts 1
s =
K
s Ts 1
sử dụng phương pháp tối ưu module để tìm bộ điều khiển tối ưu cho hệ
thống.
Đối tượng có hàm truyền như trên nên ta sẽ sử dụng phương pháp bù hằng số
thời gian lớn nhất của đối tượng để tìm bộ điều khiển.
Bộ điều khiển có dạng:
G PI =
K p 1 T I s
TIs
K p 1 T I s
TIs
G PI =
Với: T I = 4T;
Kp =
TI
2. K . T ∑ =
1
2. K . T
*Khớp 1 có K=21, T=0.05 => T I =0.2; K p =0.48
=> G PI =
0.48
1 0.2 s
0.2 s
Lắp thông số vào bộ điều khiển ta được đồ thị:
1.4
1.2
1
0.8
0.6
0.4
0.2
0
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Hệ thống có độ quá điều chỉnh 5.34% và thời gian quá độ là 0,55, vẫn chưa
đáp ứng được yêu cầu. Nhưng so với mục tiêu độ quá điều chỉnh 5% thì 5,34% là
một con số khá sát. Do đó ta có thể chỉnh lại tham số bộ điều khiển một chút để hệ
thống đạt yêu cầu.
Giảm Kp giúp độ quá điều chỉnh giảm, thời gian quá độ ít thay đổi
Giảm Ki cũng giúp độ quá điều chỉnh giảm nhưng thời gian quá độ sẽ tăng.
Ta sẽ chọn tham số vừa đủ để độ quá điều chỉnh đạt 5% mà thời gian quá độ
vẫn đủ nhỏ.
1
Chọn bộ điều khiển G PI =
0.45
1 0.18 s
0.18 s
Ta được đồ thị:
1.4
1.2
1
0.8
0.6
0.4
0.2
0
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.6
0.7
0.8
0.9
1
Độ quá điều chỉnh 4,66%, thời gian quá độ 0,5s.
*Khớp 2 có K=30, T=0,05 => T_I=0.2; K_p=1/3.
1
1 0.2 s
3
;
0.2 s
=> G_PI=
1.4
1.2
1
0.8
0.6
0.4
0.2
0
0
0.1
0.2
0.3
0.4
0.5
Độ quá điều chỉnh 6,36%, thời gian quá độ 0,585s, vẫn chưa đạt yêu cầu
Chọn lại tham số bộ điều khiển
G PI =
0.3
1 0.1 s
0.1 s
1.4
1.2
1
0.8
0.6
0.4
0.2
0
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Độ quá điều chỉnh 3,66%; thời gian quá độ 0.58s.
2.Chọn 1 quỹ đạo thẳng và 1 quỹ đạo tròn, tính quỹ đạo cho từng khớp.
Giả sử điểm cuối của robot ở vị trí có tọa độ ( x 0 , y 0 ), ta sẽ đi xác
định góc quay của 2 khớp robot là q 1 và q 2 .
Ta có:
x0
= l 1 .cos q 1 + l 2 .cos( q 1 + q 2 )
y 0 = l 1 .sin q 1 + l 2 .sin( q 1 + q 2 )
(1)
(2)
Bình phương 2 vế của (1) và (2), sau đó cộng với nhau ta được:
x 20
+
=> q 2
y 20 = l 21 + l 22 +2. l 1. l 2 .cos q 2
= arccos
x 20 y 20 −l 21−l 22
2.l 1. l 2
Từ (1) và (2) ta lại có:
x0
- l 1 .cos q 1 = l 2 .cos( q 1 + q 2 )
(3)
y 0 - l 1 .sin q 1 =
l 2 .sin( q 1 + q 2 )
(4)
Bình phương 2 vế của (3) và (4), sau đó cộng với nhau ta được:
2
x0
+
2
2
2
y 0 + l 1 - 2. x 0 . l 1 .cos q 1 - 2. y 0 . l 1 .sin q 1 = l 2
=> x 0 .cos q 1 +
=>
x0
√ x 20 y 20
2
y0
cos q 1 +
√ x 20 y 20
x0
Đặt sinα = √ x 2 y 2 =>cosα =
0
0
2
2
sin q 1 =
√x
2
2
2
2
x 0 y 0 l 1 −l 2
2. l 1. √ x 0 y 0
2
2
y0
2
0
2
y0
2
x 0 y 0 l 1 −l 2
=>sin( q 1 α) =
2. l 1. √ x 0 y 0
2
2
=> q 1 = arcsin
x 20 y 20 l 12−l 22
2. l 1
y 0 .sin q 1 =
2
2
2
2
x 0 y 0 l 1 −l 2
2. l 1. √ x 0 y 0
2
2
x0
- arcsin √ x 2 y 2 .
0
0
Như vậy, khi ta biết được vị trí tọa độ của robot, ta sẽ tính được 2 góc quay
của 2 khớp robot.
2.1.Robot đi theo một quỹ đạo thẳng từ (0,0.5) đến (0.5,0) trong khoảng
thời gian 10s
Robot sẽ chuyển động theo 1 quỹ đạo có phương trình y = 0.5-x (0
≤ x ≤ 0.5 )
Thay y = 0.5-x; l 1 =0.5; l 2 =0.5 vào công thức q 1 , q 2 đã tìm ở trên ta
được.
2
q 1 = arcsin
2 x − x 0.25
2
1. √ 2 x − x 0.25
x
–arcsin √ 2 x 2− x 0.25
x
2
=arcsin √ 2 x − x 0.25 -arcsin √ 2 x 2− x 0.25
q 2 = arccos
2
2 x − x−0.25
0.5
.
Mặt khác, robot chuyển động từ vị trí x có giá trị 0 đến 1 trong 10s, nếu coi
t
robot chuyển động đều và liên tục thì x= 10
2
q 1 = arcsin
q2
= arccos
2 t −1 0 t 25
2
10. √ 2 t −1 0 t 25
2 t 2−10 t−25
50
-
q 1 , q 2 ta được:
, thay vào
t
arcsin √ 2 t −10 t 25
.
2
.
Lệnh vẽ đồ thị q 1 , q 2 trong khoảng thời gian (0,10) thực hiện trên
matlab.
>>ezplot('asin((2*t^2-10*t+25)/(10*(2*t^2-10*t+25)^0.5))-asin(t/(2*t^210*t+25)^0.5)',[0,10]) (q1)
>>ezplot('acos((2*t^2-10*t-25)/50)',[0,10])(q2)
Ta thu được kết quả:
asin((2 t 210 t+25)/(10 (2 t 210 t+25)0.5))asin(t/(2 t 210 t+25)0.5)
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
0
1
2
3
4
5
t
6
7
8
9
10
q 1 (rad)
acos((2 t 210 t25)/50)
2.5
2
1.5
1
0.5
0
0
1
2
3
4
5
t
6
7
8
9
10
q 2 (rad)
2.2.Robot chuyển động theo một quỹ đạo tròn từ (0,0.5) đến (0.5,0) trong
thời gian 10s, tâm đường tròn ở gốc O(0,0):
2
Robot sẽ chuyển động theo 1 quỹ đạo tròn có phương trình x +
y
2
=
0.25 với 0 ≤ x ≤ 0.5
2
Thay x +
y
2
= 0.25; l 1 =0.5; l 2 =0,5 vào công thức q 1 , q 2 đã
tìm ta được:
q 1 = arcsin(0.5)-arcsin(2x) = arcsin(0.5)-arcsin(0,2t)
q 2 = arccos(-0.25)
Ta có đồ thị quỹ đạo góc khớp theo thời gian:
asin(0.5)asin(0.2 t)
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
0
1
2
3
4
5
t
6
7
8
9
10
6
7
8
9
10
q 1 (rad)
acos(0.25)
3
2.5
2
1.5
1
0.5
0
1
2
3
4
5
x
q 2 (rad)
3.Mô phỏng Matlab-Simulink, so sánh quỹ đạo với quỹ đạo thực tế:
3.1.Quỹ đạo thẳng:
Tại thời điểm ban đầu t=0 thì: q 1 = arcsin(0.5)-arcsin(0); q 2 = arccos (0,5)
Sơ đồ khối mô phỏng trên Simulink:
Trong đó Clock dùng để tạo thời gian, ở đây là 10 (s)
Cụ thể 2 khối tính góc đặt của 2 khớp:
* q 1 = arcsin
q2
= arccos
2 t 2 −10 t 25
10. √ 2 t 2−10 t 25
2 t 2−10 t−25
50
.
t
- arcsin √ 2 t 2−10 t 25 .
Kết quả mô phỏng:
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
1.2
0
1
2
3
4
5
6
7
8
9
10
8
9
10
q 1 đặt và q 1 thực
2.5
2
1.5
1
0.5
0
0.5
0
1
q 2 đặt và q 2 thực
2
3
4
5
6
7
*Nhận xét: Sau một khoảng thời gian thì rất nhanh chóng hệ thống đạt đến
giá trị sát với giá trị xác lập, ở trên đồ thị có thể quan sát được 2 đường gần như là
trùng nhau. Nhưng khoảng 0.5s cuối hệ không ổn định cho lắm.
Để đánh giá 1 cách chính xác hơn, ta sẽ quan sát sai lệch tĩnh của hệ thống:
0.1
0.08
0.06
0.04
0.02
0
0.02
0.04
0.06
0.08
0.1
0
1
2
3
4
5
6
7
8
9
10
6
7
8
9
10
Sai lệch góc khớp 1
Sai lệch khá nhỏ, chỉ khoảng từ 0 đến 0.009
0.25
0.2
0.15
0.1
0.05
0
0.05
0.1
0.15
0.2
0
1
2
3
4
5
Sai lệch góc khớp 2
Sai lệch khá nhỏ trong thời gian từ 0 đến 9s, chỉ khoảng từ 0 đến 0.02, nhưng
từ 9s đến 10s lại khá lớn.
3.1.Quỹ đạo tròn:
Tại thời điểm ban đầut=0 thì: q 1 = arcsin(0.5); q 2 = arccos (-0,25)
Sơ đồ khối mô phỏng trên Simulink:
*Khối tính q 1 = arcsin(0.5)-arcsin(0,2t)
Khối tính q 2 = arcos(-0.25)
Kết quả mô phỏng:
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
1.2
0
1
2
3
4
5
q 1 đặt và
6
7
q 1 thực
8
9
10
3
2.5
2
1.5
1
0.5
0
1
2
3
4
5
6
7
8
9
10
8
9
10
q 2 đặt và q 2 thực
0.15
0.1
0.05
0
0.05
0.1
0
1
2
3
4
5
6
Sai lệch góc quay khớp 1
7
1
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
0
1
2
3
4
5
6
7
8
9
Sai lệch góc quay khớp 2
BÀI TẬP SỐ 2:
a) Xây dựng mô hình robot
Giả thiết khớp 1 sinh ra momen M1 tác dụng giữa bệ và thanh nối 1; khớp 2
sinh ra momen M2 tác dụng giữa thanh nối 1 và 2; trọng lực có hướng theo trục y
Ký hiệu động năng và thế năng của thanh nối i là Ki và Pi (i=1,2)
Ta có:
Động năng thanh nối 1:
. 2
. 2
1
1
2
K1 m1l g 1 1 J1 1
2
2
10
Thế năng thanh nối 1:
P1 m1 glg1 sin 1
Tọa độ tâm khối thanh nối 2:
x2 l1cos1 lg 2cos(1 + 2 )
y2 l1 sin 1 l g 2sin(1 + 2 )
Tốc độ tâm khối thanh nối 2:
.
.
.
.
x
l
sin
l
sin(
+
)(
1
2)
1
1 1
g2
1
2
2
.
.
.
.
y 2 l1cos1 1 lg 2cos(1 + 2 )( 1 2 )
→Bình phương tốc độ dài tâm thanh nối 2:
. 2
. 2
. 2
.
.
. 2
.
.
v x 2 y 2 l 1 l ( 1 2 ) 2l1l g 2cos 2 ( 1 1 2 )
2
2
2
1
2
g2
2
Động năng thanh nối 2:
.
.
1
1
2
K 2 m2v2 J 2 ( 1 2 ) 2
2
2
. 2
.
.
. 2
. .
.
.
1
1
2
2
2
K 2 m2 [l1 1 lg 2 ( 1 2 ) 2l1l g 2cos 2 ( 1 1 2 )] J 2 ( 1 2 ) 2
2
2
Thế năng thanh nối 2:
P2 m2 g[l1 sin 1 lg 2 sin(1 2 )]
Hàm lagrange của robot:
L ( K1 K 2 ) ( L1 L2 )
. 2
. 2
1
1
2
m1l g 1 1 J1 1
2
2
2
.
.
.
. 2
.
.
1
2
2
2
m2 [l1 1 lg 2 ( 1 2 ) 2l1l g 2cos 2 ( 1 1 2 )]
2
.
.
1
J 2 ( 1 2 ) 2 m1 glg1 sin 1 m2 g[l1 sin 1 lg 2 sin(1 2 )]
2
Mômen của khớp 1:
M1
d L
L
( . )(
)
dt
1
1
[m1l g 1 J1 m2 (l l
2
[m2 (l
2
1
2
g2
2
g2
..
2l1l g 2cos 2 ) J 2 ]1
..
.
.
.
l1l g 2cos 2 ) J 2 ] 2 m2l1l g 2 sin 2 (21 2 22 )
m1 gl g1cos1 m2 g[l1 sin 1 l g 2cos(1 2 )]
Mômen của khớp 2:
M2
d L
L
( . )(
)
dt
2
2
[m2 (l
2
g2
..
l1l g 2cos 2 ) J 2 ]1 (m l
m2 gl g 2cos(1 2 )
Đặt
M
M 1 q 1
M 2
2
,
Ta có mô hình toán học robot:
..
.
M H ( q ) q V ( q, q ) G ( q )
Trong đó:
2
2 g2
..
.
J 2 ] 2 m2l1l g 2 sin 2 12
H11 H12
H
H 21 H 22
: ma trận quán tính
Với:
H11 m1l g21 J1 m2 (l12 lg22 2l1lg 2cos 2 ) J 2
2
H12 H 21 m2 (lg 2 l1l g 2cos 2 ) J 2
2
H 22 m2lg 2 J 2
.
.
.
2
m
l
l
sin
(
2
)
2
1
g
2
2
2
1
2
V
.
2
m2l1l g 2 sin 2 1
: thành phần momen nhớt và
hướng tâm
g
G 1
g 2
: thành phần momen trọng lực
Với:
g1 m1 gl g 1cos1 m2 g[(l1cos1 l g 2cos(1 2 )]
g 2 m2 glg 2cos(1 2 )
Thực hiện mô phỏng trên matlab:
Mô hình trên matlab:
Trong đó:
- Hàm tính G:
function y = G(q)
% thong so ro bot
m1 = 25; m2 = 25;
l1 = 0.5; l2 = 0.5;
g = 9.8;
J1 = 5; J2 = 5;
lg1= l1/2; lg2 = l2/2;
% ma tran G
g1 = m1*g*lg1*cos(q(1))+m2*g*[l1*cos(q(1))+lg2*cos(q(1)+q(2))];
g2 = m2*g*lg2*cos(q(1)+q(2));
y = [g1;g2];
- Hàm tính V:
function y = V(q,dq)
% thong so robot
m1 = 25; m2 = 25;
l1 = 0.5; l2 = 0.5;
g = 9.8;
J1 = 5; J2 = 5;
lg1= l1/2; lg2 = l2/2;
- Xem thêm -