Bai Tap Lon Robot-Thay Tien

4/30/2013
| VAN TRONG
BCH KHOA H NI
BI TP LN ROBOT CNG NGHIP


TRNG VN TRNG Page 1

TRNG I HC BCH KHOA H NI
============
B MN T NG 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
=30
0
, d
2
= 0,2m ,
d
3
= 0,1m.

Hnh 1.1 k hiu minh ha ca c cu Robot
BI TON 2. Cho Robot r c r
1
= 0,5 m ; m
1
= m
2
= 1Kg . Khp tnh tin chuyn
ng vi tc r = 0,1 m/s t r
1
n r
max
= 1 m. Robot quay t gc ban u 0
0
n gc
60
0

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 b iu khin m mem tnh ton cho Robot
c. M phng h thng


TRNG VN TRNG Page 2

BI TON 1

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

Hnh 1.1. k hiu minh ha ca c cu Robot

T s cnh tay ta gn cc h trc ta nh sau:
- V tr gc ta :
- Khu s 0 thn Robot l khu c nh c gc ta O c t ti khp 1
- Khung ta 1 c gc O
1
c gn vo khp th 2
- Khung ta 2 c gc O
2
c gn vo khp th 3
- Khung ta 3 c gc O
3
c gn vo khu tc ng cui
- Chiu ca cc khung ta :
- Khung ta s 0 c trc Z
0
trng vi trc ca khp 1, trc X
0
c chn
vung gc vi mt giy
1
trng vi trc ca khp s 2 v trc X
1
chn
vung gc vi mt giy
2
trng vi trc ca khp s 3 v trc X
2
c
chn vung gc vi mt giy
3
trng vi phng ca trc Z
2
v trc X
3
vung
gc vi mt giy
- Trc Y
0
, Y
1
, Y
2
, Y
3
c xc nh theo quay tc bn tay phi
T ta xy dng c h trc ta nh sau:

TRNG VN TRNG Page 3

Hnh 1.2. v tr v hng ca cc khung ta

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

KT
i
d
i
i
a
i
K1
1
0 0 0
K2 0 d
2
- 90
0
0
K3 0 d
3
0 0

V tr ca hai khu lin k nhau c m t bi mt ma trn bin i ng nht
i-
i-1
A
i
.Ma trn
0
A
1
m t quan h gia khu 1 v khu 0 (khu c nh gn vi thn
Robot ) , Ma trn
1
A
2
m t quan h gia khu 2 v khu 1, Ma trn
2
A
3
m t quan
h gia khu 3 v khu 2. Cu hnh ca khu mtt bt k no ca Robot c th thu
c bng cch nhn mt s thch hp cc ma trn
i-1
A
i
li vi nhau
Dng tng qut ca ma trn
i-1
A
i
nh sau:

1
(1.2)
0
0 0 0 1
i i i i i i i
i i i i i i i i
i
i i i
C S C S S a C
S C C C S a S
A
S C d
u u o u o u
u u o u o u
o o
(
(
(
=
(
(


TRNG VN TRNG Page 4

p dng (1.2) tnh ton cc ma trn
i-1
A
i
vi tham s cho trong bng D-H nh
sau:
1 1
1 1 0
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 C
A
A
d
A
d
u u
u u
(
(
(
=
(
(

(
(
(
=
(
(

(
(
(
=
(
(

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

(
(
(
=
(
(

c. Gii thch ngha ca ma trn T
Ma 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 p
T
n o a p
(
(
(
=
(
(

Trong :
- Vc t n , o, a xc nh hng ca khung ta tay so vi khung ta c
nh ,
- Vc t p biu din v tr ca khung ta tay so vi khung ta gc

TRNG VN TRNG Page 5

- Qua ma trn T ngi ta c th phn tch s hot ng v lp trnh iu khin
cho Robot.
- Ma trn T c ngha rt ln trong bi ton ng hc thun v bi ton ng
hc ngc :
- ng hc thun : khi bit gi tr ca bin khp thay i theo thi gian th v
v v hng ca tay Robot s hon ton xc nh ti mi thi im
- ng hc ngc : khi bit v tr v hng ca im tc ng cui ta hon
ton c th xc nh c gi tr ca cc bin khp t vic gii h phng
trnh ng hc T trn.

d. Xc nh v tr Robot trong h ta gc khi
1
=30
0
, d
2
=0,2[m] , d
3
=0,1 [m]
Khi robot quay mt gc
1
= 30
0
v chuyn ng tnh tin cc on d
2
= 0,2m v
d
3
= 0,1 m th ta c ma trn T c gi tr nh 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 1
0
2 2 20
1 3 3
0
2 2 20
1
0 1 0
5
0 0 0 1
C S S
S C C
T
(
(
(
=
(
(

(
(
(
(
(
=
(
(
(
(
(

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

1
20
3
20
1
5
1
p
(
(
(
(
(
=
(
(
(
(


TRNG VN TRNG Page 6

BI TON 2

a. Xc nh m mem khp quay v lc tng khp tnh tin khi Robot cui hnh
trnh chuyn ng
Ta c m hnh ca Robot r nh 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 m
1
ca xilanh tp trung ti im cui ca xilanh tc l im A
- Khi lng m
2
ca pittong tp trung bn tay Robot tc l tp trung ti im
B
- M mem qun tnh cc khp J
i
= 0
Trnh t xy dng phng trnh ng lc hc nh saU:
Xc nh hm lagrange
Phng trnh lagrange ca mt c cu c xc nh nh sau:
L = K P
Trong :
- K : Tng ng nng ca h thng
- P : Tng th nng ca h thng
Vi khp quay ta c phng trnh ng lc hc nh sau:

r
1

r
X
1
X
2

Y
Y
2

Y
1

A
B

X

TRNG VN TRNG Page 7

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

- Xc nh ng nng ca h thng
ng nng ca h thng : K = K
1
+ K
2

Trong :
K
1
ng nng ca khp 1
K
2
ng 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
u
u
=
=

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

Vn tc im A:

ng nng ca khp 1:

- ng nng ca khp 2:
V tr 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:
)


TRNG VN TRNG Page 8

- Xc nh th nng ca h thng
- Th nng ca khp 1:

- Th nng ca khp 2:

- Hm lagrange
T cc 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 :

T m men khp quay nh sau:

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


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 tnh
tin. Gi thuyt chuyn ng ca cc khp ny l chuyn ng u. ng thi thi
gian khp quay quay ht qu o yu cu bng thi gian m khp tnh tin thc
hin ht hnh trnh lm vic ca mnh.
Thi gian c cu tnh tin chuyn ng t r
1
= 0,5 [m] ti v tr r
max
= 1 [m]
vi vn tc r = 0,1 [m/s] l:

max 1
1 0, 5
5[ ]
0,1
r r
t s
r

= = =

Mt khc, c cu Robot li quay t 0
0
n 60
0
vy cui hnh trnh th khp
quay mt gc = 60
0
= /3 [rad] v thi gian khp quay quay ng bng thi gian
khp 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 khng
i nn:

Mt khc cui hnh trnh chuyn ng th :
r = r
max
= 1 [m] v = /3
thay vo cc cng thc xy dng trn c
- M mem ca khp quay:

)
(
)
(
( )


TRNG VN TRNG Page 10

- Lc tng ca kh tnh tin:

b. Thit k b iu khin M mem tnh ton cho Robot
Phng trnh ng lc hc ca c cu Robot -r trnh by trn c vit li
nh sau:

2
1 1 2 2 1 1 2
2
2 2 2
( . ) 2 ( ) M m r m r m rr mr m r gc
F m r m r m gS
u u u
u u
= + + + +
= +

Hay vit di dng ma trn:
()
) () (2-1)
Trong :

M
M
F
(
=
(

- m men hoc lc ca khp

2 2
1 1 2
2
0
0
mr m r
H
m
( +
=
(

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

2
2
2
2m rr
V
m r
u
u
(
=
(

- ma trn m men v lc nht

1 1 2
2
( ) mr m r gC
G
m gS
u
u
+ (
=
(

-ma trn m men v lc ca trng lc
Nguyn l thit k b iu khin m men tnh ton l :kh cc thnh phn phi
tuyn ca phng trnh ng lc hc ca Robot v phn ly c tnh ng hc ca cc
thanh ni.Kt qu nhn c mt h thng tuyn tnh t d dng s dng cc
phng php thit k kinh in ca h thng tuyn tnh.



T tng ta c cu trc b iu khin :

Hnh 2.2. cu trc b iu khin
lm c iu chn phng trnh m t b iu 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 p
p K p K + + =
Mong mun h thng c p ng nh khu giao ng bc 2 c dng nh
sau :

2 2
2 0 (2 6)
n n
p p e e + + =
Trong :
-h s suy gim

4
n
qd
t
e
= -tn s giao ng

R K 2
1

U
K 1

Tuyn tnh phi tuyn


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

2
2
4 8
2 2 .
4
di n
qd qd
pi n
qd
K
t t
K
t
e
= = =
| |
= =
|
|
\ .

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 t
qd
=
1s thu c tham s b iu khin ca khp i nh sau:

2
8
8
1
4
32, 65
0, 7.1
di
pi
K
K
= =
| |
= =
|
\ .

Trong bi ny b iu khin thit k cho hai khp l ging nhau nn cc thng
s b iu khin ca hai khp ging nhau
Suy ra cc h s b iu khin:

8 0
0 8
32, 65 0
0 32, 65
d
p
K
K
(
=
(

(
=
(



c. M phng h thng
- Thit k qu o chuyn ng cho hai khp dng 2 1 2
m bo tay Robot di chuyn t v tr ban u A(x
0
, y
0
) n v tr cui cng
l B(x
c
, y
c
) trong khong thi gian t
c
(s) ta c th tnh ton qu o nh sau:

Hnh 2.4. qu o 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 2
Ta gi thuyt
0
0
0
c
q
q
=
. mt khc ta c :


0
2
1
1 1 1 1 1 1 1 0
1
2
1 0 1 1
2
0
1
1
; .
2 2
. . . . 0
1
.
2
2 4
c c
m m
m
c c
m
c c c
t q q
t q
q q
q q t q t q t t q q
t t
q q q t
t t q q
t
q
+
= =

= = + =
= +
=

T th trn chng ta thy
1
2
c
t
t < . Do :

2
0
1
1
(2 7)
2 4
c c c
t t q q
t
q
=
Biu thc (2-7) c ngha khi v ch khi :

2
0
1
0
1 1 2
4
4.
(2 8)
c c
c
cp
c
t q q
q
q q
q q
t
< s

Gii hn ca gia tc ph thuc vo bn c kh ca Robot . Nh vy qu o
thit k nh sau:

2
1 0 1
1
1 2 0 1 1
2
2 3 1 1
1
0 : ( ) .
2
: ( ) ( )
2
1
: ( ) ( )
2
c c
t q t q q t
t
t t q t q q t t
t t q t q q t t t
= +
= +

Trong :
- Chn
1
q nm trong khong theo biu thc (2-8).
- Bit
1
q , tc, qc, q0 ta tnh c t1 theo (2-7).


- Kt qu m phng
M 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 t m phng
Nhn xt: Gc quay ca cc bin khp l v r bm st qu o 2-1-2 cho trc
vi 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 5
0
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 5
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2


- Chng trnh m phng

Chng trnh chnh
%======================================
% CHUONG TRINH CHINH
%=======================================

% khai bao gia tri bo dieu khien
Kp = 32.65
Kd = 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 thi
plot(At1,AM1,'r') % mo mem khop quay (khop 1)
text (1,11,'mo men khop 1')
hold on
plot(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 thi
plot(At1,AeTheta,'r') % mo mem khop quay (khop 1)
text (2.5,-0.11,' khop 2 quay')
hold on
plot(At1,Aer) % luc cua khop tinh tien (khop 2)
text(2,-0.036,'khop 1 tinh tien')
grid
% ve goc quay khop 1
plot(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 1
r1 = 0.5; %Chieu dai thanh noi 1
%===Vi tri cua tay ban dau` va` cuoi ==
S0 = [0.5 0]; % r1=0,5 m
Sc = [0 1]; % rmax =1 m

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


q0 = DHN(S0);
qc = DHN(Sc);
%Khac phuc truong hop chia 0
if (qc(2)==q0(2));
qc(2) = qc(2)-0.0001;
end
if (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 quay
ddq2 = 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 khop
t11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toc
t21 = 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 giam
toc
%deu bang t11/2
t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang toc
t22 = 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 dem
for 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 = x21
H12 = 0;
H21 = 0;
H22 = m2;


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 quay
Aq2(i) = q(2); %r
Adq1(i) = dq(1);%Toc do khop quay
Adq2(i) = dq(2);%Toc do khop tinh tien
AM1(i) = M(1); %Mo men khop quay
AM2(i) = M(2); %Luc truyen dong cho khop tinh tien
AeTheta(i) = qd(1) - q(1); %Sai lech goc quay
Aer(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 quay
F02 = M(2); %Luc dieu khien cho khop tinh tien

x11 = X0(1); %Goc Theta1
x12 = X0(2); %Toc do goc khop quay
x21 = X0(3); %Gia tri r
x22 = X0(4); %Toc do khop tinh tien
%-------------------------------------------
%Cac thong so cua Robot
r1 = 0.5; %Chieu dai thanh noi 1
%-------------------------------------------
C1 = cos(x11);
S1 = sin(x11);
%---------------------------------------------
%Ma tran quan tinh


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

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

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

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

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

%Phuong trinh trang thai
x11p = x12; %Toc do khop quay
x21p = x22; %Toc do khop tinh tien
x12p = dX(1); %Gia toc khop quay
x22p = dX(2); %Gia toc khop tinh tien

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

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

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

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

if t<=t1 %Tang toc
q = q0 + ddq*t^2/2;
dq = ddq * t;
elseif((t>t1) & (t<=t2)) %Chuyen dong deu
q = q0 + ddq*t1*(t-t1/2);
dq = ddq * t1;
elseif((t>t2)&(t<=tc)) %Giam toc do ve khong
q = qc - ddq*(t-tc)^2/2;
dq = ddq*t1 - ddq*(t-t2);


end
end

+hm DHN tnh 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 PD
kp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd;

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

%Momen dieu khien
Kp = [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;



Ti liu tham kho:

1. TS. Nguyn Mnh Tin . iu Khin Robot Cng Nghip . Nh xut bn Khoa
Hc 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.

Bai Tap Lon Robot-Thay Tien

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Bai Tap Lon Robot-Thay Tien

Uploaded by

Copyright:

Available Formats

4/30/2013

You might also like