Bai Tap Lon Robot-thay Tien

7/30/2019 Bai Tap Lon Robot-thay Tien

1/22

4/30/2013

| VAN TRONG

BCH KHOA H NI BI TP LN ROBOT CNG NGHIP


2/22

BI TP LN ROBOT CNG NGHIP

TRNG VN TRNG Page 1

TRNG I HC BCH KHOA H NI

============

B MN TNG HA

4

BI TON 1. Cho robot c c cu nh hnh v

a. Xy dng h ta cho cc thanh ni.b. Xc nh ma trn T biu din h ta tay robot.c. Gii thch ngha ca ma trn T.d. Xc nh v tr ca ta Robot trong h ta gc khi 1=300 , d2 = 0,2m ,

d3 = 0,1m.

Hnh 1.1 k hiu minh ha ca c cu Robot

BI TON 2. Cho Robot r c r1 = 0,5 m ; m1 = m2 = 1Kg . Khp tnh tin chuynng vi tc r = 0,1 m/s t r1n rmax = 1 m. Robot quay tgc ban u 0

0n gc600

a.Hy xc nh m mem khp quay v lc tng hp khp tnh tin khi Robot cui hnh trnh chuyn ng.

b. Thit k biu khin m mem tnh ton cho Robotc. M phng h thng


3/22


TRNG VN TRNG Page 2

BI TON 1

a.Xy dng h ta cho cc thanh niTa c s cnh tay ca Robot nh sau:

Hnh 1.1. k hiu minh ha ca c cu Robot

Ts cnh tay ta gn cc h trc ta nh sau:

V tr gc ta :- Khu s 0 thn Robot l khu cnh c gc ta O c t ti khp 1- Khung ta 1 c gc O1c gn vo khp th 2- Khung ta 2 c gc O2c gn vo khp th 3- Khung ta 3 c gc O3c gn vo khu tc ng cui Chiu ca cc khung ta :- Khung ta s 0 c trc Z0 trng vi trc ca khp 1, trc X0c chn

vung gc vi mt giy- Khung ta s 1 c trc Z1 trng vi trc ca khp s 2 v trc X1 chn

vung gc vi mt giy- Khung ta s 2 c trc Z2 trng vi trc ca khp s 3 v trc X2c

chn vung gc vi mt giy- Khung ta s 3 c trc Z3 trng vi phng ca trc Z2 v trc X3 vung

gc vi mt giy- Trc Y0, Y1, Y2, Y3c xc nh theo quay tc bn tay phiT ta xy dng c h trc ta nh sau:


4/22


TRNG VN TRNG Page 3

Hnh 1.2. vtr v hng ca cc khung ta

b.Xc nh ma trn T biu din h ta tay RobotThnh 1.2 lp c bng D-H cc tham s ca khu nh sau:

KT i di i aiK1 1 0 0 0K2 0 d2 - 90

0 0K3 0 d3 0 0

V tr ca hai khu lin knhau c m t bi mt ma trn bin i ng nht i-i-1Ai .Ma trn

0A1 m t quan h gia khu 1 v khu 0 (khu cnh gn vi thnRobot ) , Ma trn 1A2 m t quan h gia khu 2 v khu 1, Ma trn

2A3 m t quanh gia khu 3 v khu 2. Cu hnh ca khu mtt bt k no ca Robot c th thuc bng cch nhn mt s thch hp cc ma trn i-1Ai li vi nhau

Dng tng qut ca ma trn i-1Ainh sau:

1 (1.2)0

0 0 0 1

i i i i i i i

i i i i i i ii

i

i i i

C S C S S a C

S C C C S a S A

S C d


5/22


TRNG VN TRNG Page 4

p dng (1.2) tnh ton cc ma trn i-1Ai vi tham s cho trong bng D-H nhsau:

1 1

1 10

1

1

2

2

2

3

3

0 0

0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 0 1 0

0 1 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1

0 0 0 1

C S

S CA

Ad

A

d

T thu c ma trn T nh sau:0 1 2

1 2 3

1 1

1 1

2 3

1 1 3 1

1 1 3 1

2

0 0 1 0 0 0 1 0 0 0

0 0 0 0 1 0 0 1 0 0

0 0 1 0 0 1 0 0 0 1

0 0 0 1 0 0 0 1 0 0 0 1

0

0

0 1 0

0 0 0 1

T A A A

C S

S C

d d

C S d S

S C d C

d

c.Gii thch ngha ca ma trn TMa trn T c dng tng qut nh sau:

0 0 0 1

X X X X

y y y y

z z z z

n o a p

n o a pT

n o a p

Trong :- Vc tn , o, a xc nh hng ca khung ta tay so vi khung ta c

nh ,- Vc tp biu din v tr ca khung ta tay so vi khung ta gc


6/22


TRNG VN TRNG Page 5

Qua ma trn T ngi ta c th phn tch s hot ng v lp trnh iu khincho Robot.

Ma trn T c ngha rt ln trong bi ton ng hc thun v bi ton nghc ngc :

- ng hc thun : khi bit gi tr ca bin khp thay i theo thi gian th vv v hng ca tay Robot shon ton xc nh ti mi thi im

- ng hc ngc : khi bit v tr v hng ca im tc ng cui ta honton c thxc nh c gi tr ca cc bin khp t vic gii hphngtrnh ng hc T trn.

d.Xc nh vtr Robot tr ong h ta gc khi1=300, d2= 0,2[m] , d3= 0,1 [m]Khi robot quay mt gc 1 = 30

0 v chuyn ng tnh tin cc on d2= 0,2m v

d3 = 0,1 m th ta c ma trn T c gi trnh sau:0 0 0

0 0 0

30 0 30 0,1. 30

30 0 30 0,1. 30

0 1 0 0, 2

0 0 0 1

3 1 10

2 2 20

1 3 30

2 2 201

0 1 05

0 0 0 1

C S S

S C CT

Vy v tr ca gc ta gn ln tay c vtr nh sau:

1

20

3

20

1

5

1

p


7/22


TRNG VN TRNG Page 6

BI TON 2

a.Xc nh m mem khp quay vlc tngkhp tnh tin kh i Robotcui hnhtrnh chuyn ng

Ta c m hnh ca Robot rnh sau :

Hnh 2.1. m hnh robot -r xy dng c cc phng trnh ng lc hc ca Robot -r n gin

chng ta gi thuyt:- Khi lng m1 ca xilanh tp trung ti im cui ca xilanh tc l im A- Khi lng m2 ca pittong tp trung bn tay Robot tc l tp trung ti im

B- M mem qun tnh cc khp Ji = 0Trnh t xy dng phng trnh ng lc hc nh saU:

Xc nh hm lagrangePhng trnh lagrange ca mt c cu c xc nh nh sau:L = KPTrong :

- K : Tng ng nng ca h thng- P : Tng thnng ca h thng

Vi khp quay ta c phng trnh ng lc hc nh sau:

r1

r

X1 X

2

Y

Y2

Y1

A

B

X


8/22


TRNG VN TRNG Page 7

Vi khp tnh tin ta c phng trnh ng lc hc nh sau:

Xc nh ng nngca h thngng nng ca h thng : K = K1 + K2Trong :

K1ng nng ca khp 1K2ng nng ca khp 2

Theo hnh 2.1 ta c:- ng nng ca khp 1:

V tr ca im A trong h ta nh sau:

1 1

1 1

. os

.sin

X r c

Y r

o hm hai v theo thi gian thu c thnh phn vn tc theo cc trc:

1 1

1 1

. sin .

. os .

X r

Y r c

Vn tc im A:

ng nng ca khp 1:

- ng nng ca khp 2:

Vtr im B trong h ta nh sau:

Vn tc ca im B theo cc trc nh sau:

Vn tc ca B :

ng nng ca khp 2: (

)


9/22


TRNG VN TRNG Page 8

Xc nh thnng ca h thng- Thnng ca khp 1:

- Thnng ca khp 2:

Hm lagrangeTcc tnh ton trn ta c hm lagrange nh sau:

( )

Tnh M men khp quay (khp 1)M men qun tnh c tnh theo cng thc :

Ta c :

V

T m men khp quay nh sau:

Tnh lc khp tnh tin (khp 2Lc tc ng ln khp tnh tin c tnh nh sau:


10/22


TRNG VN TRNG Page 9

Trong :

Vy lc cn tm l :

Tm ton vi tham s c th

Robot -r c hai bc t do gm khp 1 l khp quay, khp th hai l khp tnhtin. Gi thuyt chuyn ng ca cc khp ny l chuyn ng u. ng thi thi

gian khp quay quay ht quo yu cu bng thi gian m khp tnh tin thchin ht hnh trnh lm vic ca mnh.Thi gian c cu tnh tin chuyn ng t r1 = 0,5 [m] ti v tr rmax = 1 [m]

vi vn tc r = 0,1 [m/s] l:

max 1 1 0, 5 5 [ ]0,1

r rt s

r

Mt khc, c cu Robot li quay t 00n 600 vy cui hnh trnh th khp quay mt gc = 600 = /3 [rad] v thi gian khp quay quay ng bng thi giankhp tnh tin thc hin ht hnh trnh ca mnh nn ta c vn tc gc ca khp

quay tnh nh sau:

Tip theo ta c khp quay v khp tnh tin chuyn ng u vi vn tc khngi nn:

Mt khc cui hnh trnh chuyn ng th :r = rmax = 1 [m] v = /3

thay vo cc cng thc xy dng trn c

- M mem ca khp quay:


11/22


TRNG VN TRNG Page 10

- Lc tng ca khtnh tin:

b.Thit kbiu khin M mem tnh ton cho Robot

Phng trnh ng lc hc ca c cu Robot -r trnh by trn c vit linh sau:

2

1 1 2 2 1 1 2

2

2 2 2

( . ) 2 ( )M m r m r m rr m r m r gc

F m r m r m gS

Hay vit di dng ma trn:

(2-1)Trong :M

MF

- m men hoc lc ca khp

2 2

1 1 2

2

0

0

m r m r H

m

- ma trn m men hoc lc t l vi gia tc

22

2

2m rrVm r

- ma trn m men v lc nht

1 1 2

2

( )m r m r gC G

m gS

-ma trn m men v lc ca trng lc

Nguyn l thit k biu khin m men tnh ton l :kh cc thnh phn phituyn ca phng trnh ng lc hc ca Robot v phn ly c tnh ng hc ca ccthanh ni.Kt qu nhn c mt h thng tuyn tnh t d dng s dng cc

phng php thit kkinh in ca h thng tuyn tnh.


12/22



T tng ta c cu trc biu khin :

Hnh 2.2. cu trc biu khinlm c iu chn phng trnh m t biu khin m men tnh ton

nh sau:

(2-2)T (2-1) v (2-2) ta c:

(2-3)Thit k lut iu khin PD cho Robot ta c:

(2-4)T (2-3) v (2-4) c :

Laplace ha hai v ca phng trnh thu c phng trnh sau:

2 0 (2 5)d pp K p K

Mong mun h thng c p ng nhkhu giao ng bc 2 c dng nhsau :

2 22 0 (2 6)n np p

Trong :

-h s suy gim

4n

qdt

-tn sgiao ng

RK 2

U

K 1

Tuyn tnh phi tuyn


13/22



Cn bng phng trnh (2-5) v (2-6) thu c:

2

2

4 82 2 .

4

di n

qd qd

pi n

qd

Kt t

Kt

Hnh 2.3.s cu trc h thng iu khin m men tnh ton

Chn cht lng iu khin c h s suy gim 0,7 v thi gian qu tqd =

1s thu c tham s biu khin ca khp i nh sau:

2

88

1

432,65

0,7.1

di

pi

K

K

Trong bi ny biu khin thit k cho hai khp l ging nhau nn cc thngs biu khin ca hai khp ging nhau

Suy ra cc h s biu khin:8 0

0 8

32, 65 0

0 32, 65

d

p

K

K


14/22



c.M phng h thng Thit k quo chuyn ng cho hai khp dng 212

m bo tay Robot di chuyn t vtr ban u A(x0 , y0 ) n v tr cui cngl B(xc , yc) trong khong thi gian tc (s) ta c th tnh ton quo nh sau:

Hnh 2.4. quo 2-1-2

Khi n Robot di chuyn t A ti B xy ra 3 qu trnh :- u tin l qu trnh khi ng c dng bc 2- Qu trnh chuyn ng u c dng l ng thng bc 1- Qu trnh hm c dng ng cong bc 2Ta gi thuyt 0

0

0c

q

q

. mt khc ta c :


15/22



0

211 1 1 1 1 1 1 0

1

2

1 0 1 1

2

01

1

; .2 2

. . . . 0

1.

2

2 4

c cm m

mc c

m

c c c

t q qt q

q qq q t q t q t t q q

t t

q q q t

t t q qt

q

T th trn chng ta thy 12

ctt . Do :

20

1

1

(2 7)2 4c c ct t q qt

q

Biu thc (2-7) c ngha khi v ch khi :

2

0

1

0

1 12

4

4.(2 8)

c c

c

cp

c

t q q

q

q qq q

t

Gii hn ca gia tc ph thuc vo bn c kh ca Robot . Nh vy quothit knh sau:

2

1 0 1

11 2 0 1 1

22 3 1 1

10 : ( ) .

2

: ( ) ( )2

1: ( ) ( )2

c c

t q t q q t

tt t q t q q t t

t t q t q q t t t

Trong :

- Chn1

q nm trong khong theo biu thc (2-8).

- Bit1

q , tc, qc, q0 ta tnh c t1 theo (2-7).


16/22



Kt qu m phngM men v lc ca 2 khp Sai lch ca bin khp

Gc quay khp 1 dch chuyn r khp 2

Hnh 2.5. cc tn hiu thu c tm phng

Nhn xt: Gc quay ca cc bin khp l v r bm st quo 2-1-2 cho trcvi sai s nh v cc khp thc hin ti ng im dng cui ca mnh

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2

0

2

4

6

8

10

12

mo men khop 1

luc khop 2

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-0.12

-0.1

-0.08

-0.06

-0.04

-0.02

0

0.02

khop 2 quay

khop 1 tinh tien

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50.5

0.6

0.7

0.8

0.9

1

1.1

1.2


17/22



Chng trnh m phng Chng trnh chnh%======================================

% CHUONG TRINH CHINH%=======================================

% khai bao gia tri bo dieu khienKp = 32.65Kd = 8% goi ham RobotThetaR[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd)% ========================================% do thi tin hieu thu duoc%=========================================% ve mo men va luc cua robot tren cung do thiplot(At1,AM1,'r') % mo mem khop quay (khop 1)

text (1,11,'mo men khop 1')hold onplot(At1,AM2) % luc cua khop tinh tien (khop 2)text(4,11,'luc khop 2')grid% ve sai so goc quay va tinh tien tren cung do thiplot(At1,AeTheta,'r') % mo mem khop quay (khop 1)text (2.5,-0.11,' khop 2 quay')hold onplot(At1,Aer) % luc cua khop tinh tien (khop 2)text(2,-0.036,'khop 1 tinh tien')grid% ve goc quay khop 1plot(At1,Aq1)grid% do dich chuyen cua khop 2 tinh tien

plot(At1,Aq2)grid

Xy dng cc hm+ hm RobotThetaR

function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =

RobotThetaR(Kp,Kd)

tc = 5; %Dat thoi gian di chuyen cua tay Robot.m1 = 1 ; %Khoi luong thanh 1m2 = 1 ; %Khoi luong thanh 2r1 = 0.5; %Chieu dai thanh noi 1%===Vi tri cua tay ban dau` va` cuoi ==S0 = [0.5 0]; % r1=0,5 mSc = [0 1]; % rmax =1 m

%Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop


18/22



q0 = DHN(S0);qc = DHN(Sc);%Khac phuc truong hop chia 0if (qc(2)==q0(2));

qc(2) = qc(2)-0.0001;endif (qc(1)==q0(1));

qc(1) = qc(1)-0.0001;end

%Tinh toan gia toc hai khop tinh tien va quay

ddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quayddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien

%Xac dinh cac khoang thoi gian chuyen dong tang toc, deu va giam toc cho

%cac khopt11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toct21 = tc - t11; %t21 - t11/2 se la thoi gian chuyen dong deu, tc-t21 se la

%thoi gian giam toc ve 0. Tuc la thoi gian tang va giamtoc%deu bang t11/2

t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang toct22 = tc - t12;

Tk = 0.01; %Sau Tk(s) ta se tinh toan cac tham so cua robot

%Dieu kien ban dau (So kien)q = q0; dq = [0;0];X0 = [q0(1);dq(1);q0(2);dq(2)]; %So kien bien trang thai X

file1 = fopen('RobotThetaR.txt','w');

i = 0; %Bien dung de demfor t = 0:0.001:tc;i = i+1;At1(i)=t; %Lay thoi gian de ve do thi%Tinh toan gia tri dat cho cac khop trong tung khoang thoi gian chuyen%dong[qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1

la van%toc cua khop 1, ddq1 la gia toc

khop 1[qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t);

qd = [qd1; qd2];dqd = [dqd1; dqd2];G11 = (m1*r1 + m2*X0(3))*9.81*cos(X0(1));G12 = m2*9.81*sin(X0(1));G1 = [G11;G12];v1 = 2*m2*X0(3)*X0(4)*X0(2);v2 = -m2*X0(4)*X0(1)*X0(1);V1 = [v1;v2];H11 = m1*r1^2 + m2*X0(3)^2; %r = x21H12 = 0;H21 = 0;H22 = m2;


19/22



H = [H11, H12; H21, H22];

%Tinh momen can thiet de thuc hien chuyen dong

[M,fe] = ControllerPD(qd,dqd,q,dq,Kp,Kd,G1,V1,H);

%Lay thong so qui dao thuc robot chuyen dong duoc qua M[q, dq] = RobotModel(M,X0,Tk);

%Lay cac thong so de ve do thi%------------------------------------------------------qdd1(i)= qd(1);qdd2(i) = qd(2);Aq1(i) = q(1); %Goc quay khop quayAq2(i) = q(2); %rAdq1(i) = dq(1);%Toc do khop quayAdq2(i) = dq(2);%Toc do khop tinh tienAM1(i) = M(1); %Mo men khop quayAM2(i) = M(2); %Luc truyen dong cho khop tinh tien

AeTheta(i) = qd(1) - q(1); %Sai lech goc quayAer(i) = qd(2) - q(2); %Sai lech chuyen dong tinh tien%--------------------------------------------------------

X0 = [q(1); dq(1); q(2); dq(2)]; %Dat lai so kien cho lan tinh sau

%q(1)-> Theta; q(2)->r% plot(At1,q(1))%Luu du lieu vao file

fprintf(file1,'%2.4f%2.4f%2.4f%2.4f%2.4f%3.4f%3.4f\n',t,qd(1),q(1),qd(2),q

(2),M(1),M(2));end

fclose(file1);

+ hm RobotModel :

function[q,dq] = RobotModel(M,X0,Tk)%-------------------------------------------M01 = M(1); %Mo men dieu khien cho khop quayF02 = M(2); %Luc dieu khien cho khop tinh tien

x11 = X0(1); %Goc Theta1x12 = X0(2); %Toc do goc khop quayx21 = X0(3); %Gia tri rx22 = X0(4); %Toc do khop tinh tien

%-------------------------------------------%Cac thong so cua Robotm1 = 1 ; %Khoi luong thanh 1m2 = 1 ; %Khoi luong thanh 2r1 = 0.5; %Chieu dai thanh noi 1%-------------------------------------------C1 = cos(x11);S1 = sin(x11);%---------------------------------------------%Ma tran quan tinh


20/22



H11 = m1*r1^2 + m2*x21^2; %r = x21H12 = 0;H21 = 0;H22 = m2;H = [H11, H12; H21, H22];

%Momen nhot va momen huong tamv1 = 2*m2*x21*x22*x12;v2 = -m2*x22*x12*x12;V = [v1;v2];

%Momen trong lucG1 = (m1*r1 + m2*x21)*9.81*C1;G2 = m2*9.81*S1;G = [G1;G2];

%Nghich dao ma tran HHinv = inv(H);

%Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguocdX = -Hinv * (V+G) + Hinv*[M01;F02];

%Phuong trinh trang thaix11p = x12; %Toc do khop quayx21p = x22; %Toc do khop tinh tienx12p = dX(1); %Gia toc khop quayx22p = dX(2); %Gia toc khop tinh tien

%Tinh gan dung phuong trinh vi phanx11 = x11 + Tk * x11p;x21 = x21 + Tk * x21p;x12 = x12 + Tk * x12p;

x22 = x22 + Tk * x22p;

X = [x11;x12;x21;x22];

%Tinh vi tri tay Robots = [x21; 0];q = [x11; x21];dq = [x12; x22];

+ hmquiDaoKhopThetaR:

function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t)

if tt1) & (tt2)&(t


21/22



endend

+ hm DHNtnh ton ng hc ngc

function[q] = DHN(S)

%Px = S(1); Py = S(2)%Theta = q(1); r = q(2)

Px = S(1);Py = S(2);

Theta = atan2(Py,Px);r = Px*cos(Theta) + Py*sin(Theta);

q = [Theta; r];

+ hm ControllerPD

function[M,errorTheta, errordTheta] =

ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G,V,H)

%Cai dat tham so bo dieu khien vi tri phan hoi PDkp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd;

%Tinh sai lech goc quay va dao hamerrorTheta = sd - theta;errordTheta = dsd - dtheta;

%Momen dieu khienKp = [kp1,0; 0,kp2];Kd = [kd1,0; 0,kd2];

%M = Kp * errorTheta - Kd * dtheta + G;U = dsd+Kp*errorTheta+Kd*errordTheta;M=H*U+V+G;


22/22



Ti liu tham kho:

1. TS. Nguyn Mnh Tin. iu Khin Robot Cng Nghip . Nh xut bn KhoaHc v K Thut. H Ni 2006.

2. TS. Nguyn Mnh Tin . Bi Ging Robot Cng Nghip .3. GS.TSKH . Nguyn Thin Phc . Robot Cng Nghip . Nh xut bn Khoa

Hc v K Thut. H Ni 2006.

Documents

Bai Tap Lon Robot-thay Tien