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

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... Thiết kế bộ điều khiển PD bù trọng trường- Mô hình toán học robot: , Trong đó các ma trận H, V, G

x

J2

1

l1

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: l1 = 0,5 m

Chiều dài thanh nối 2: l2 = 0,4 m

Khối lượng thanh nối 1: m1 = 1 Kg

Khối lượng thanh nối 2: m2 = 1 Kg

Momen quán tính khớp 1 quay quanh tâm khối: J1 = 0,5 Kgm2

Momen quán tính khớp 2 quay quanh tâm khối: J2 = 0,5 Kgm2

Khoảng cách từ khớp 1 đến tâm thanh nối 1: lg1 = 0,25 m Khoảng cách từ khớp 2 đến tâm thanh nối 2: lg2 = 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 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:

1

2

Thế năng thanh nối 1:

Tọa độ tâm khối thanh nối 2:

os os( + ) sin sin( + )

g g

x l c l c





 Tốc độ tâm khối thanh nối 2:

2

sin sin( + )( )

os cos( + )( )

g

g

     

     







→Bình phương tốc độ dài tâm thanh nối 2:

v x  y l  l    l l c    

Động năng thanh nối 2:

2

K  m l  l    l l c      J   Thế năng thanh nối 2:

2 2 [ sin1 1 g2sin( 1 2)]

Hàm lagrange của robot:

1

2

2 2

1

2

1

2

g

Mômen của khớp 1:

1 1

( ) ( )

M



 1

.

Mômen của khớp 2:

2 2

( ) ( )

M





.

os( )

g

m gl c

Đặt

1 2

M M

M

 

 

 

,

1 2



 

 

 

Ta có mô hình toán học robot:

( ) ( , ) ( )

Trong đó:

H H

H

H H

: ma trận quán tính

Với:

1

2

2

g













2

2

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:

g

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 H1( )q

Hàm tính H1( )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:

( ) ( , ) ( )

Trong đó các ma trận H, V, G được xác định theo trên

- Thuật toán điều khiển:

.

( )

Trong đó: sai lệch góc qay: e q d  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



 

 

 

0

0

/ 6

q



 

 

 

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

và

3.50

0 3.5

D

Kết quả mô phỏng:

Góc quay khớp 1,2:

150

0 15

P

 

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