Bài tập dài điều khiển robot công nghiệp part 2 potx

y
x
J2
1
l1
lg1
lg2
2
m2
J1
m1
l2
z
Hình 1: Cơ cấu Robot
Bài số 2
Cho một cơ cấu robot 2DOF như hình vẽ
Sử dụng matlab- Simulink
1. Xây dựng mô hình toán học robot
2. Thiết kế bộ điều khiển PID bù trọng trường. Chạy mô phỏng
Số liệu của robot:
Chiều dài thanh nối 1: l
1
= 0,5 m
Chiều dài thanh nối 2: l
2
= 0,4 m
Khối lượng thanh nối 1: m
1
= 1 Kg
Khối lượng thanh nối 2: m
2

= 1 Kg
Momen quán tính khớp 1 quay quanh tâm khối: J
1
= 0,5 Kgm
2
Momen quán tính khớp 2 quay quanh tâm khối: J
2
= 0,5 Kgm
2
Khoảng cách từ khớp 1 đến tâm thanh nối 1: l
g1
= 0,25 m
Khoảng cách từ khớp 2 đến tâm thanh nối 2: l
g2
= 0,2 m
Góc ban đầu của các thanh nối: θ
1
= 0 rad ; θ
2
= Л/6 rad
Góc đặt của các thanh nối: θ
1
= Л/3 rad ; θ
2
= Л/2 rad
1. 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 M

2
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à K
i
và P
i
(i=1,2)
Ta có:
Động năng thanh nối 1:
1
2 2
. .
2
1 1
1 1 1
1 1
2 2
g
K m l J
θ θ
= +
Thế năng thanh nối 1:
1 1 1 1
sin
g
P m gl
θ
=
Tọa độ tâm khối thanh nối 2:

2 1 1 2 1 2
2 1 1 2 1 2
os os( + )
sin sin( + )
g
g
x l c l c
y l l
θ θ θ
θ θ θ
= +



= +


Tốc độ tâm khối thanh nối 2:
. . . .
2 1 1 2
1 1 2 1 2
. . . .
1 1 2
1 1 2 1 2
2
sin sin( + )( )
os cos( + )( )
g
g
x l l

y l c l
θ θ θ θ θ θ
θ θ θ θ θ θ

= − − +



= + +

→Bình phương tốc độ dài tâm thanh nối 2:
2 2 2 2
. . . . . . . .
2 2 2 2
2 1 1 2 1 1 2
2 1 2 1 2 22
( ) 2 os ( )
g g
v x y l l l l c
θ θ θ θ θ θ θ
= + = + + + +
Động năng thanh nối 2:
. .
2 2
2
2 2 2 2
1
1 1
( )
2 2

K m v J
θ θ
= + +
2 2
. . . . . . . .
2 2 2 2
1 1 2 1 1 2 2
2 2 1 2 1 2 2 2 1
1 1
[ ( ) 2 os ( )] ( )
2 2
g g
K m l l l l c J
θ θ θ θ θ θ θ θ θ
= + + + + + +
Thế năng thanh nối 2:
2 2 1 1 2 1 2
[ sin sin( )]
g
P m g l l
θ θ θ
= + +
Hàm lagrange của robot:
1 2 1 2
( ) ( )L K K L L= + − +
1
2 2
. .
2
1 1

1 1
2 2
. . . . . .
2 2 2
1 1 2 1 1 2
2 1 2 1 2 2
. .
2
2
2 1 1 1 2 1 1 2 1 21
1 1
2 2
1
[ ( ) 2 os ( )]
2
1
( ) sin [ sin sin( )]
2
g
g g
g g
m l J
m l l l l c
J m gl m g l l
θ θ
θ θ θ θ θ θ θ
θ θ θ θ θ θ
= + +
+ + + + + +
+ + − − + +

Mômen của khớp 1:
1
.
1
1
( ) ( )
d L L
M
dt
θ
θ
∂ ∂
= −
∂
∂
1

2 2 2
1 1 2 1 2 1 2 2 2 1
.
. .
2 2
2 2 1 2 2 2 2 2 1 2 2 1 2 2
1 1 1 2 1 1 2 1 2
[ ( 2 os ) ]
[ ( os ) ] sin (2 )
os [ sin os( )]
g
g g
g g g

g g
m l J m l l l l c J
m l l l c J m l l
m gl c m g l l c
θ θ
θ θ θ θ θ θ
θ θ θ θ
= + + + + +
+ + + − +
+ + + +
Mômen của khớp 2:
2
.
2
2
( ) ( )
d L L
M
dt
θ
θ
∂ ∂
= −
∂
∂
.

2 2 2
2 2 1 2 2 2 1 2 2 2 2 2 1 2 2 1
2 2 1 2

[ ( os ) ] ( ] sin
os( )
g g g g
g
m l l l c J m l J m l l
m gl c
θ θ θ θ θ
θ θ
= + + + + +
+ +
Đặt
1
2
M
M
M
 
=
 
 
,
1
2
q
θ
θ
 
=
 
 

Ta có mô hình toán học robot:
.
( ) ( , ) ( )M H q q V q q G q= + +
Trong đó:
11 12
21 22
H H
H
H H
 
=
 
 
: ma trận quán tính
Với:
1
2 2 2
11 1 1 2 1 2 1 2 2 2
2
12 21 2 2 1 2 2 2
2
22 2 2 2
( 2 os )
( os )
g
g g
g g
g
H m l J m l l l l c J
H H m l l l c J

H m l J
θ
θ

= + + + + +


= = + +


= +


.
. .
2
2 1 2 2 2 1 2
.
2
2 1 2 2 1
sin ( 2 )
sin
g
g
m l l
V
m l l
θ θ θ θ
θ θ
 

− +
 
=
 
 
 
: thành phần momen nhớt và hướng
tâm
1
2
g
G
g
 
=
 
 
: thành phần momen trọng lực
Với:
1 1 1 1 2 1 1 2 1 2
2 2 2 1 2
os [( os os( )]
os( )
g g
g
g m gl c m g l c l c
g m gl c
θ θ θ θ
θ θ
= + + +




= +


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 = 1; m2 = 1;
l1 = 0.5; l2 = 0.4;
g = 9.8;
J1 = 0.5; J2 = 0.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 = 1; m2 = 1;
l1 = 0.5; l2 = 0.4;
g = 9.8;
J1 = 0.5; J2 = 0.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
1
( )H q
−
Hàm tính
1
( )H q
−
function y = H(q,M)
% thong so robot
m1 = 1; m2 = 1;
l1 = 0.5; l2 = 0.4;
g = 9.8;
J1 = 0.5; J2 = 0.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)
2. 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:
.
( )
dk P D
M K e K q G q= − +
Trong đó: sai lệch góc qay:
d
e q q= −
Tham số bộ điều khiển
1
1
0
0
P
K
K
K
 
=
 
 
Tao khối:

2
2
0
0
D
K
K
K
 
=
 
 
Tạo khối:
Mô hình bộ điều khiển trên matlab:
/3
/ 2
d
q
π
π
 
=
 
 
0
0
/6
q

π
 
=
 
 
Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn:
150
0 15
P
K
 
=
 
 
và
3.50
0 3.5
D
K
 
=
 
 
Kết quả mô phỏng:
Góc quay khớp 1,2:
Sai lệch góc quay khớp 1,2