bài tập lớn robot mô phỏng đại học bách khoa BKA

ĐỀ 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;
=;
=;
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: =
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:
= .=
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:
=
=
Với: = 4T;
==
*Khớp 1 có K=21, T=0.05 =>=0.2; =0.48


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

1

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ỏ.
Chọn 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

1

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.
=> 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

0.6

Độ 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
=


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 độ (,), ta sẽ đi xác định góc
quay của 2 khớp robot là và .
Ta có:
=.cos + .cos(+)

(1)

=.sin + .sin(+)

(2)

Bình phương 2 vế của (1) và (2), sau đó cộng với nhau ta được:
+ = + +2..cos
=> = arccos
Từ (1) và (2) ta lại có:
-.cos = .cos(+)

(3)

-.sin = .sin(+)

(4)

Bình phương 2 vế của (3) và (4), sau đó cộng với nhau ta được:
+ + - .cos -..sin =
=>.cos + .sin =

=>cos + sin =


Đặt sinα = =>cosα =
=>sin(α) =
=> = arcsin

- arcsin.

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)
Thay y = 0.5-x; =0.5; =0.5 vào công thức đã tìm ở trên ta được.
= arcsin–arcsin

=arcsin-arcsin
= arccos .
Mặt khác, robot chuyển động từ vị trí x có giá trị 0 đến 1 trong 10s, nếu coi
robot chuyển động đều và liên tục thì x= , thay vào ta được:
= arcsin

- arcsin.

= arccos .
Lệnh vẽ đồ thị , 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 2-10 t+25)/(10 (2 t 2-10 t+25)0.5))-asin(t/(2 t 2-10 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

6

7

8

9

10

(rad)

acos((2 t 2-10 t-25)/50)
2.5

2

1.5

1

0.5

0
0

1

2

3

4

5
t

(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):
Robot sẽ chuyển động theo 1 quỹ đạo tròn có phương trình + = 0.25 với


Thay + = 0.25; =0.5; =0,5 vào công thức đã tìm ta được:
= arcsin(0.5)-arcsin(2x) = arcsin(0.5)-arcsin(0,2t)
= 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

(rad)
acos(-0.25)
3

2.5

2

1.5

1

0.5

0

1

2

3

4

5
x

(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ì: = arcsin(0.5)-arcsin(0); = 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:
* = arcsin

= arccos .

- arcsin.


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

6

7

8

9

10

đặt và thực

2.5

2

1.5

1

0.5

0

-0.5

0

đặt và thực

1

2

3

4

5


*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ì: = arcsin(0.5); = arccos (-0,25)
Sơ đồ khối mô phỏng trên Simulink:

*Khối tính = arcsin(0.5)-arcsin(0,2t)


Khối tính = 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

đặt và thực

6

7

8

9

10

3

2.5

2

1.5

1

0.5

0

1

2

3

4

5

6

7

8

9

10

6

7

8

9

10

đặt và thực
0.15

0.1

0.05

0

-0.05

-0.1

0

1

2

3

4

5

Sai lệch góc quay khớp 1


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

10

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 M 1 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


Thế năng thanh nối 1:

P1 = m1 gl g1 sin θ1
Tọa độ tâm khối thanh nối 2:

 x2 = l1cosθ1 + 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 = l1cosθ1 θ 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 (2θ1 θ 2 + θ 22 )

+ m1 gl g1cosθ1 + 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
 g2 
: thành phần momen trọng lực

Với:


 g1 = m1 gl g 1cosθ1 + m2 g[(l1cosθ1 + 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;


% ma tran V
V1 = -m2*l1*lg2*sin(q(2))*[dq(2)^2+2*dq(1)*dq(2)];
V2 = m2*l1*lg2*sin(q(2))*dq(1)^2;
y = [V1;V2];
- Khối

H −1 (q )

Hàm tính

H −1 (q )

function y = H(q,M)
% 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;
% bien khop
theta1=q(1);
theta2=q(2);
% ma tran H(q)

H11=m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(theta2))+J2;
H12=m2*(lg2^2+l1*lg2*cos(theta2))+J2;
H21=H12;
H22=m2*lg2^2+J2;
H = [H11 H12;H21 H22];
% ma tran nghich dao H^(-1)(q)
Hinv = inv(H);
% Gia toc khop


y = Hinv * M;
Tạo khối robot (Create Subsystem)

c) Thiết kế bộ điều khiển PD bù trọng trường
- Mô hình toán học robot:
..

.

M = H ( q ) q + V ( q, q ) + G ( q )
Trong đó các ma trận H, V, G được xác định theo trên
- Thuật toán điều khiển:
.

M dk = K P e − K D q + G ( q )
Trong đó: sai lệch góc qay:

Tham số bộ điều khiển

e = qd − q

K 0 
KP =  1

 0 K1 

K 0 
KD =  2

0 K 2 

Mô hình bộ điều khiển trên matlab:

Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn:


150 
KP = 

 0 15 

và

3.50 
KD = 

 0 3.5

Kết quả mô phỏng (không chạy được).!!!!!