Tin s V Minh Hng

http://matlabs.wordpress.com

SO SNH 2 PHNG PHP NG LC HC NEWTON V LAGRANGE

V d v m phng trn Matlab
1. t vn
Xy dng phng trnh ng lc hc cho i tng l mt bc quan trng khi thit k h
thng iu khin. S chnh xc ca phng trnh ng lc c ngha v cng quan trng, nh
hng ti kt qu phn tch v thit k iu khin. Do vy, ngi ta thng dng hai cch khc
nhau xy dng phng trnh ng lc hc v so snh.
Ti liu tham kho ny s trnh by mt v d n gin v hai phng php ng lc hc
ph bin nht hin nay l: ng lc hc Lagrange v ng lc hc Newton. C cu mt bc t
do c rng buc (gn tng t nh c cu song song) c phn tch v m phng trn Matlab
gip cho ngi c hiu c nhng bc c bn nht trong vic xy dng phng trnh
ng lc hc.
Mt iu lu l cc c cu h (nh robot chui) th ngi ta quan tm n ng lc hc
thun trong khi cc c cu kn (nh robot song song) th l ng lc hc ngc. Vi phng
php ng lc hc Newton, chng ta c th xy dng phng trnh ng lc hc cho i tng
mt cch r rng. Sau s s dng phng php ng lc hc Lagrange chng minh rng
hai kt qu ging nhau.
C cu mt bc t do c rng buc
C cu mt bc t do c rng buc c v nh trn hnh 1. OA l khu ch ng,
c iu khin bi ng c im O. Trong khi , AB l khu b ng, vi khp b ng ti
im A. im cui ca khu AB c kt ni vi mt ci xe chy trn mt ng ray khng ma
st mt bc t do. Khong cch t im O ti mt phng xe chy l L. Nh vy, c cu ny c
mt bc t do: M men quay quanh im O s iu khin ci xe ti im B. Cc k hiu trn
hnh 1 c din t nh sau:
di ca khu OA l L1 v ca khu AB l L2.
1

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Trng lc ca khu OA l W1 ti im G1v ca khu AB l W2 ti im G2

V tr, vn tc v gia tc ca xe l x, x& , &&x
Gc quay ca khu ch ng OA l q1
H trc ta v cc vc t phn lc ti im A v im B c ch ra trn hnh.
O

F1y

q1

L1

G1

W1

F1x

F1x
A

F1y

F2 y

G2

q2

L2

& &&
B x, x, x

W2

F2x

F2x

F2 y

Hnh 1 C cu mt bc t do c rng buc

2. Phng php ng lc hc Newton

Phng trnh lc v m men
Phng trnh m men ca khu OA quay quanh im O c tnh nh sau:

L1W1 cos (q1 )

2

- L1 F1 y cos (q1 ) - L1 F1x sin (q1 ) + t = I oq&&1

(1)

Phng trnh lc v m men ca khu AB quay quanh trng tm G2 c tnh nh sau:

F
F

= F1x + F2 x = m2 &&
x2

= F1 y + F2 y + W2 = m2 &&
y2

Din n C in t, Haptic v Thc t o (Din n CHAT)

(2)

Tin s V Minh Hng

http://matlabs.wordpress.com

2g

L2 F1 y sin (q 2 )
2

L2 F1x cos (q 2 )
2

L2 F2 x cos (q 2 )
2

L2 F2 y sin (q 2 )
2

= I 2 gq&&2

(3)

x2 , y2 l ta im G2.
Phng trnh lc ca xe l:

= - F2 x = mx&&

(4)

Cc phng trnh rng buc

C 4 phng trnh rng buc c la chn nh sau:
L1 sin (q1 ) + L2 cos (q 2 ) = L

(5a)

- L1 cos (q1 ) + L2 sin (q 2 ) = x

(5b)

L1 sin (q1 ) +

L2
cos (q 2 ) = y2
2

(5c)

L2
sin (q 2 ) = x2
2

(5d)

- L1 cos (q1 ) +

Tnh ton vn tc v gia tc ca cc khu

Vn tc v gia tc x& , &&x ca xe (gi tr mong mun) c cho trc, tnh vn tc v gia tc ca
cc khu. Vn ny s c gii quyt da vo vic o hm 4 phng trnh rng buc.
Ly o hm cp 1 v cp 2 ca phng trnh 5a i vi thi gian, c kt qu nh sau;
L1 sin (q1 ) + L2 cos (q 2 ) = L
L q& cos (q ) - L q& sin (q ) = 0
1 1

2 21

Lq&&1 cos (q1 ) - L2q&&21 sin (q 2 ) - L1q&12 sin (q1 ) - L2q&22 cos (q 2 ) = 0

Tng t, ly o hm cp 1 v cp 2 ca phng trnh 5b i vi thi gian, thu c kt qu;

Din n C in t, Haptic v Thc t o (Din n CHAT)

(6)

Tin s V Minh Hng

http://matlabs.wordpress.com
- L1 cos (q1 ) + L2 sin (q 2 ) = x
L q& sin (q ) + L q& cos (q ) = x&
1 1

2 21

(7)

L1q&&1 sin (q1 ) + L2q&&21 cos (q 2 ) + L q& cos (q1 ) - L q& sin (q 2 ) = &&
x
2
1 1

2
2 2

Tng t, ly o hm cp 1 v cp 2 ca phng trnh 5c i vi thi gian, thu c kt qu;

L2
cos (q 2 ) = y2
2
L
L1q&1 cos (q1 ) - 2 q&21 sin (q 2 ) = y& 2
2
L
L
Lq&&1 cos (q1 ) - 2 q&&21 sin (q 2 ) - L1q&12 sin (q1 ) - 2 q&22 cos (q 2 ) = &&
y2
2
2
L1 sin (q1 ) +

(8)

Tng t, ly o hm cp 1 v cp 2 ca phng trnh 5d i vi thi gian, thu c kt qu;

L2
sin (q 2 ) = x2
2
L
L1q&1 sin (q1 ) + 2 q&21 cos (q 2 ) = x&2
2
L2 &&
L
&&
L1q1 sin (q1 ) + q 21 cos (q 2 ) + L1q&12 cos (q1 ) - 2 q&22 sin (q 2 ) = &&
x2
2
2
- L1 cos (q1 ) +

(9)

Tnh vn tc gc ca cc khu nh sau:

T hai phng trnh sau:
L1q&1 cos (q1 ) - L2q&21 sin (q 2 ) = 0
L1q&1 sin (q1 ) + L2q&21 cos (q 2 ) = x&

(10)

Dn ti vn tc gc ca cc khu l:
q&1 L1 cos (q1 )
=
q&2 L1 sin (q1 )

-1

- L2 sin (q 2 ) 0

L2 cos (q 2 ) x&

(11)

Tnh gia tc gc ca cc khu nh sau:

T phng trnh:
L cos (q1 )

L1 sin (q1 )

- L2 sin (q 2 ) q&&1 L1q&12 sin (q1 ) + L2q&22 cos (q 2 )

=
L2 cos (q 2 ) q&&21 &&
x - L1q&12 cos (q1 ) + L2q&22 sin (q 2 )

Din n C in t, Haptic v Thc t o (Din n CHAT)

(12)

Tin s V Minh Hng

http://matlabs.wordpress.com

Dn ti gia tc gc ca khu l:
q&&1 L cos (q1 )
=
&&
q 21 L1 sin (q1 )

- L2 sin (q 2 )

L2 cos (q 2 )

-1

L1q&12 sin (q1 ) + L2q&22 cos (q 2 )

&&
x - L1q&12 cos (q1 ) + L2q&22 sin (q 2 )

(13)

Tnh vn tc tuyn tnh ca khu AB:

L
&

L q sin q + 2 q& cos (q 2 )

x&2 1 1 ( 1 ) 2 21

& =
y2 L q& cos (q ) - L2 q& sin (q )
1
21
2
1 1
2

(14)

Tnh gia tc tuyn tnh ca khu AB:

L
L
&&

L q sin q + 2 q&& cos (q 2 ) + L1q&12 cos (q1 ) - 2 q&22 sin (q 2 )

x2 1 1 ( 1 ) 2 21
&&
2

&& =
y2 Lq&& cos (q ) - L2 q&& sin (q ) - L q& 2 sin (q ) - L2 q& 2 cos (q )
1
21
2
1 1
1
2
2
1
2
2

(15)

Tnh ng hc ngc tm gc quay ca cc khu

T phng trnh rng buc 5a v 5b, ta c;
L1 sin (q1 ) + L2 cos (q 2 ) = L

(16)

- L1 cos (q1 ) + L2 sin (q 2 ) = x

Ly tng bnh phng 2 v ca phng trnh ny thu c;

( L sin (q ) + L cos (q ) ) + ( - L cos (q ) + L sin (q )) = ( L ) + ( x )

2 L L ( sin (q ) cos (q ) - cos (q ) sin (q ) ) = L + x - L - L
2

1 2

2
1

2
2

(17)

Dn n;
sin (q1 - q 2 ) =

L2 + x 2 - L12 - L22
2 L1 L2

(18)

t
K=

L2 + x 2 - L12 - L22
2 L1 L2

V vy;
5

Din n C in t, Haptic v Thc t o (Din n CHAT)

(19)

Tin s V Minh Hng

http://matlabs.wordpress.com
q1 - q 2 = sin -1 ( K )

(20)

q1
L1

a2

a3

y a1

L
A

q2

L2

Hnh 2 Gc quay ca c cu mt bc t do c rng buc

Cc gc quay ca c cu c biu din trn hnh 2 v c tnh nh sau:
x 2 + L2 + L2 - L2
1
2
a 2 = cos -1
2 L x 2 + L2
1

q1 =

(21a)

x
a1 = tan -1
L

(21b)

a 3 = a 2 - a1

(21c)

p
- a 3 and q 2 = q1 - sin -1 ( K )
2

(21d)

Tm phng trnh ng lc hc bng phng php Newton

T cc phng trnh 1, 2, 3, 4 dn ti:
- L1 sin (q1 )

1
0

- L2 cos (q 2 ) / 2

- L1 cos (q1 )
0
1
L2 sin (q 2 ) / 2
0

1 F1x I oq&&1 - L1W1 cos (q1 ) / 2

x2
1
0
0 F1 y m2 &&

(22)

&&
0
1
0
F = m y - W2

2x 2 2

L2 cos (q 2 ) / 2 - L2 sin (q 2 ) / 2
0 F2 y I 2 gq&&2

-1
0
0 t mx&&

t
6

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

- L1 sin (q1 )

1
A= 0

- L2 cos (q 2 ) / 2

http://matlabs.wordpress.com
- L1 cos (q1 )
0
1
L2 sin (q 2 ) / 2
0

1
0
0
0
1
0

L2 cos (q 2 ) / 2 - L2 sin (q 2 ) / 2
0

-1
0
0
0

I oq&&1 - L1W1 cos (q1 ) / 2

x2
m2 &&

&&
B = m2 y2 - W2

I 2 gq&&2

mx&&

Do vy, cc phng trnh lc v m men c tnh nh sau:

F1 x

F1 y
F = A-1 B
2x
F2 y

t

(23)

F1 , F2 , t l phn lc ca khu OA, AB v m men ca khu OA quay quanh gc O.

y l nhng phng trnh ng lc hc ca c cu trn hnh 1. Trong phng trnh ca m
men l quan trng nht v n lin quan n m men quay ch ng (gi tr m men cn thit
iu khin ci xe chy vi tc v gia tc mong mun). Phng trnh ng lc hc ny s
c so snh trn m phng Matlab vi phng trnh xy dng bng phng php Lagrange.
3. ng lc hc Lagrange
Phng php ng lc hc Lagrange lin quan n tnh ton v nng lng (ng nng v th
nng) v s bin i ca chng.
Tnh th nng
Th nng ca khu OA c tnh nh sau:
7

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com
1
1
V1 = - W1 L1 sin (q1 ) = - m1 gL1 sin (q1 )
2
2

(24)

, m1 l khi lng ca khu OA.

Th nng ca khu AB c tnh nh sau:
1
1
V2 = W2 L2 sin (q 2 ) = m2 gL2 sin (q 2 )
2
2

(25)

, m2 l khi lng ca khu AB.

Tng th nng ca c cu c tnh l (th nng ca xe bng khng):
1
1
V = V1 + V2 = - m1 gL1 sin (q1 ) + m2 gL2 sin (q 2 )
2
2

(26)

Tnh ng nng
ng nng ca khu OA c tnh nh sau:
T1 =

1 &2
I10q1
2

(27)

, I10 l m men qun tnh ca khu OA quanh gc O.

ng nng ca khu AB c tnh nh sau:
T2 =

1
1
1
I 2 gq&22 + m2 x&22 + m2 y& 22
2
2
2

(28)

, I 2 g l m men qun tnh ca khu AB quay quanh G2 .

ng nng ca xe c tnh nh sau:
T3 =

1 2
mx&
2

(29)

, m l khi lng ca xe
8

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Tng ng nng ca c cu trn hnh 1 l:

T = T1 + T2 + T3 =

1 &2 1
1
1
1
I10q1 + I 2 gq&22 + m2 x&22 + m2 y& 22 + mx& 2
2
2
2
2
2

(30)

Phng trnh Lagrange c nh ngha nh sau

L = T -V

(31)

Phng trnh ng lc hc Lagrange cho cc c cu c rng buc (hoc l c cu song song)

c tnh bi phng trnh sau:
d L

dt q& j

4
L
G
+ li i = Q j
q j i =1 q j

(32)

, q j , j = 1, 2,..., 5 ta suy rng c tnh nh sau:

q1 = q1
q2 = q 2
q3 = x2
q4 = y2
q5 = x

v Gi , i = 1, 2,3, 4 l cc phng trnh rng buc, trong khi li , i = 1, 2,3, 4 l nhn t Lagrange.
Lc suy rng, Q j , j = 1, 2,3, 4,5 c tnh nh sau (b qua ma st ca cc khp):
Q1 = t
Q2 = 0
Q3 = 0
Q4 = 0
Q5 = 0

Ly o hm phng trnh Lagrange theo cc ta suy rng, thu c:

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com
fq 1 =

L
= fq 1 q1 , q 2 , x2 , y2 , x, q&1 , q&2 , x&2 , y& 2 , x&
q&1

fq 2 =

L
= fq 2 q1 , q 2 , x2 , y2 , x, q&1 , q&2 , x&2 , y& 2 , x&
q&2

f x2 =

L
= f x 2 q1 , q 2 , x2 , y2 , x, q&1 , q&2 , x&2 , y& 2 , x&
x&2

f y2 =

L
= f y 2 q1 , q 2 , x2 , y2 , x, q&1 , q&2 , x&2 , y& 2 , x&
y& 2

fx =

L
= f x q1 , q 2 , x2 , y2 , x, q&1 , q&2 , x&2 , y& 2 , x&
x&

)
)

Tip tc ly o hm theo thi gian i vi cc phng trnh (33), dn ti:

f
f
d L dfq 1 fq 1 & fq 1 & fq 1
=
q1 +
q2 +
x&2 + q 1 y& 2 + q 1 x& +
& =
dt q1 dt
q1
q 2
x2
y2
x
+

fq 1 && fq 1 && fq 1
f
f
&&
q1 +
q2 +
x2 + q 1 &&
y2 + q 1 &&
x
x&2
y& 2
x&
q&1
q&2

d L

dt q&2

dfq 2 fq 2 & fq 2 & fq 2

f
f
=
q1 +
q2 +
x&2 + q 2 y& 2 + q 2 x& +
=
dt

y
x
1
2
2
2

f
f
f
f
f
+ q 2 q&&1 + q 2 q&&2 + q 2 &&
x2 + q 2 &&
y2 + q 2 &&
x
x&2
y& 2
x&
q&1
q&2

d L

dt x&2

df x 2 f x 2 & f x 2 & f x 2
f
f
=
q1 +
q2 +
x&2 + x 2 y& 2 + x 2 x& +
=
dt

y
x
1
2
2
2

f
f
f
f
f
+ x 2 q&&1 + x 2 q&&2 + x 2 &&
x2 + x 2 &&
y2 + x 2 &&
x
&
&
&
&
x2
y2
x&
q1
q 2

f y 2
f y 2
d L df y 2 f y 2 & f y 2 & f y 2
=
q1 +
q2 +
x&2 +
y& 2 +
x& +

=
&
dt y2
dt
q1
q 2
x2
y2
x
f y 2 && f y 2 && f y 2
f y 2
f y 2
&&
&&
&&
+
q1 +
q2 +
x2 +
y2 +
x
&
&
&
&

y
x&
q
q
1

f
f
d L df x f x & f x & f x
=
=
q1 +
q2 +
x&2 + x y& 2 + x x& +
dt x& dt
q1
q 2
x2
y2
x
+

f x && f x && f x
f
f
&&
q1 +
q2 +
x2 + x &&
y2 + x &&
x
&
&
&
&

y
x&
q1
q 2
2
2

10

Din n C in t, Haptic v Thc t o (Din n CHAT)

(33)

Tin s V Minh Hng

http://matlabs.wordpress.com

Tm cc nhn t Lagrange
Cc nhn t Lagrange li tng ng vi cc phng trnh rng buc Gi . o hm nhng phng
trnh rng buc ny theo cc ta suy rng, dn ti:
G1 G1 G1 G1 G1
,
,
,
,
q1 q 2 x2 y2
x
G 2 G 2 G 2 G 2 G 2
,
,
,
,
q1 q 2 x2
y2
x
G3 G3 G3 G3 G3
,
,
,
,
q1 q 2 x2
y2
x
G 4 G 4 G 4 G 4 G 4
,
,
,
,
q1 q 2 x2
y2
x

Pht trin phng trnh (32) cho cc ta suy rng q 2 , x2 , y2 , x thu c cc phng trnh
sau:
l1

G1
G
G
G
d L
+ l2 2 + l3 3 + l4 4 = -
q 2
q 2
q 2
q 2
dt q&2

L
+ q + Q2
2

(34a)

l1

G1
G
G
G
d L
+ l2 2 + l3 3 + l4 4 = -
x2
x2
x2
x2
dt x&2

L
+ Q3
+
x2

(34b)

l1

G1
G
G
G
d L L
+ l2 2 + l3 3 + l4 4 = -
+ Q4
+
y2
y2
y 2
y2
dt y& 2 y2

(34c)

l1

G
G1
G
G
d L L
+ l2 2 + l3 3 + l4 4 = - +
+ Q5
x
x
x
x
dt x& x

(34d)

Nh tnh ton trn, tt c cc gi tr v v tr, vn tc, gia tc c tm ra. Do vy nhn t

Lagrange c th c tnh t cc phng trnh tuyn tnh (34) trn nh sau:
l1

l
l = 2 = A -1 B
l3

l4

11

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

,
G1
q
2
G1

x
A= 2
G1

y2
G
1
x

G 2
q 2

G 3
q 2

G 2
x2

G3
x2

G 2
y2

G3
y2

G 2
x

G3
x

d L L

G 4
- Q2
- & +
dt q 2 q 2

q 2

G 4
- d L + L - Q

3
dt x&2 x2

x2

, B=

G 4
d L L

- Q4
-
+
y2
&
dt y2 y2

G 4

- d L + L - Q5
x
dt x& x

Tm phng trnh ng lc hc bng phng php Lagrange

Pht trin phng trnh (32) cho ta suy rng q1 dn ti;
t=

G
G
G
d L L G1
+ l1
+ l2 2 + l3 3 + l4 4
& dt q1 q1 q1
q1
q1
q1

(35)

, t l m men ca khu OA quanh gc O

y l phng trnh ng lc hc ca c cu trn hnh 1 s dng phng php Lagrange.
Phng trnh ny s c so snh vi phng trnh thu c bng phng php Newton trn
phn mm matlab.
4. M phng trn phn mm Matlab so snh hai kt qu
Cc tham s cho vic m phng l:
L1 (mm)
L2(mm)
L(mm)
m1(g)
m2(g)
m(g)
Io(kgm2)
I2g(kgm2)
g(m/s2)

200
300
350
200
250
600
0.03
0.04
9.81
12

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Qu o ca xe ( x, x& , &&x ) c cho trc trn hnh 3.

Hnh 3 Qu o ca xe
Kt qu ca phng php ng lc hc Newton

Hnh 4 Phn lc ca cc khp A v B

13

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Hnh 5 M men ca khp ch ng O i vi phng php Newton

Kt qu ca phng php ng lc hc Lagrange

Hnh 6 M men ch ng ca khp O i vi phng php Lagrange

14

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Hnh 7 Cc thnh phn ca m men khp O

Kt lun
Kt qu m phng trn phn mm Matlab cho thy, cc m men ch ng ca khu OA quanh
gc O ging nhau i vi c hai phng php ng lc hc Newton v Lagrange. Nh vy
phng trnh ng lc hc ca c cu trn hnh 1 c th c s dng phn tch v thit k h
thng iu khin.

15

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

Chng trnh trn phn mm Matlab

A. Phng php ng lc hc Newton
% Newton method
clear all;
% Parameters of 1-DOF dynamics with constraints
L1 = 0.20; % unit: meter
L2 = 0.30;
L = 0.35;
m1 = 0.20;
m2 = 0.25;
m = 0.60;
I0 = 0.03;
I2g = 0.04;
g = 9.81;
%% Trajectory for simulation
x1 = 0;
x2 = 0.1;
% Trajectory planning
t = 1; dt = 0.001; tf = 0:dt:t;
x0 = [x1
x2]';
xd0 = [0
0]';
xdd0 = [0
0]';
[ax, adx, addx] = inter_2point(x0, xd0, xdd0, 0, t, dt);
%% Calculate position, velocity and acceleration
n = length(ax);
for i=1:n
% position
x = ax(i);
K1 = (L^2 + x^2 - L1^2 - L2^2)/(2*L1*L2);
K2 = (L1^2 + x^2 + L^2 - L2^2)/(2*L1*(x^2 + L^2)^0.5);
anfa2 = acos(K2);
anfa1 = atan(x/L);
anfa3 = anfa2 - anfa1;
th1(i) = pi/2 - anfa3;
th2(i) = th1(i) - asin(K1);
% velocity
dx = adx(i);
A1 = [L1*cos(th1(i))
-L2*sin(th2(i));
L1*sin(th1(i))
L2*cos(th2(i))];
B1 = [0;
dx];
if det(A1) == 0
dth = zeros(2,1);
else
dth = inv(A1)*B1;
end
dth1(i) = dth(1);
dth2(i) = dth(2);
% acceleration
ddx = addx(i);
A2 = [L1*cos(th1(i))
-L2*sin(th2(i));
L1*sin(th1(i))
L2*cos(th2(i))];
B2 = [L1*dth1(i)^2*sin(th1(i)) + L2*dth2(i)^2*cos(th2(i));
ddx - L1*dth1(i)^2*cos(th1(i)) + L2*dth2(i)^2*sin(th2(i))];
if det(A2) == 0
ddth = zeros(2,1);
else
ddth = inv(A2)*B2;
end
ddth1(i) = ddth(1);
ddth2(i) = ddth(2);
dx2(i) = [L1*sin(th1(i))
dy2(i) = [L1*cos(th1(i))

0.5*L2*cos(th2(i))]*dth;
-0.5*L2*sin(th2(i))]*dth;

ddx2(i) = L1*sin(th1(i))*ddth1(i) + L1*cos(th1(i))*dth1(i)^2 + 0.5*L2*cos(th2(i))*ddth2(i) 0.5*L2*sin(th2(i))*dth2(i)^2;

16

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

http://matlabs.wordpress.com

ddy2(i) = L1*cos(th1(i))*ddth1(i) - L1*sin(th1(i))*dth1(i)^2 - 0.5*L2*sin(th2(i))*ddth2(i) 0.5*L2*cos(th2(i))*dth2(i)^2;

A3 = [-L1*sin(th1(i))
-L1*cos(th1(i))
1
0
0
1
-0.5*L2*cos(th2(i))
0.5*L2*sin(th2(i))
0
0
W1 = m1*g;
W2 = m2*g;
B3 = [I0*ddth1(i) - 0.5*L1*W1*cos(th1(i));
m2*ddx2(i);
m2*ddy2(i) - W2;
I2g*ddth2(i);
m*ddx];

0
1
0
0.5*L2*cos(th2(i))
-1

0
0
1
-0.5*L2*sin(th2(i))
0

if det(A3) == 0
s = zeros(5,1);
else
s = inv(A3)*B3;
end
f1x(i) = s(1);
f1y(i) = s(2);
f2x(i) = s(3);
f2y(i) = s(4);
tou1n(i) = s(5); % torque
end

figure(1)
plot(tf,tou1n)
xlabel('time(sec)')
ylabel('torque of joint 1 (Nm)')
legend('Newton method')

B. Phng php ng lc hc Lagrange

% Lagrange method
clear all;
% 1-DOF dynamics with constraints
L1 = 0.20;
L2 = 0.30;
l = 0.35;
m1 = 0.20;
m2 = 0.25;
m = 0.60;
I0 = 0.03;
I2 = 0.04;
g = 9.81;
%% Trajectory for simulation
x1 = 0;
x2 = 0.1;
% Trajectory planning
t = 1; dt = 0.001; tf = 0:dt:t;
x0 = [x1
x2]';
xd0 = [0
0]';
xdd0 = [0
0]';
[ax, adx, addx] = inter_2point(x0, xd0, xdd0, 0, t, dt);
%% Calculate position, velocity and acceleration
n = length(ax);
for i=1:n
% position
x = ax(i);
K1 = (l^2 + x^2 - L1^2 - L2^2)/(2*L1*L2);
K2 = (L1^2 + x^2 + l^2 - L2^2)/(2*L1*(x^2 + l^2)^0.5);
anfa2 = acos(K2);
anfa1 = atan(x/l);
anfa3 = anfa2 - anfa1;
th1(i) = pi/2 - anfa3;
th2(i) = th1(i) - asin(K1);

17

Din n C in t, Haptic v Thc t o (Din n CHAT)

1;
0;
0;
0;
0];

Tin s V Minh Hng

% velocity
xd = adx(i);
A1 = [L1*cos(th1(i))
L1*sin(th1(i))
B1 = [0;
xd];
if det(A1) == 0
dth = zeros(2,1);
else
dth = inv(A1)*B1;
end

http://matlabs.wordpress.com

-L2*sin(th2(i));
L2*cos(th2(i))];

dth1(i) = dth(1);
dth2(i) = dth(2);
% acceleration
xdd = addx(i);
A2 = [L1*cos(th1(i))
-L2*sin(th2(i));
L1*sin(th1(i))
L2*cos(th2(i))];
B2 = [L1*dth1(i)^2*sin(th1(i)) + L2*dth2(i)^2*cos(th2(i));
xdd - L1*dth1(i)^2*cos(th1(i)) + L2*dth2(i)^2*sin(th2(i))];
if det(A2) == 0
ddth = zeros(2,1);
else
ddth = inv(A2)*B2;
end
ddth1(i) = ddth(1);
ddth2(i) = ddth(2);
dx2(i) = [L1*sin(th1(i))
dy2(i) = [L1*cos(th1(i))

0.5*L2*cos(th2(i))]*dth;
-0.5*L2*sin(th2(i))]*dth;

ddx2(i) = L1*sin(th1(i))*ddth1(i) + L1*cos(th1(i))*dth1(i)^2 + 0.5*L2*cos(th2(i))*ddth2(i) 0.5*L2*sin(th2(i))*dth2(i)^2;

ddy2(i) = L1*cos(th1(i))*ddth1(i) - L1*sin(th1(i))*dth1(i)^2 - 0.5*L2*sin(th2(i))*ddth2(i) 0.5*L2*cos(th2(i))*dth2(i)^2;

x = ax(i); xd = adx(i); xdd = addx(i);

t1 = th1(i); t1d = dth1(i); t1dd = ddth1(i);
t2 = th2(i); t2d = dth2(i); t2dd = ddth2(i);
x2d = dx2(i); x2dd = ddx2(i);
y2d = dy2(i); y2dd = ddy2(i);
tou_in(i) = I0*t1dd;
tou_g(i) = -1/2*m1*g*L1*cos(t1);
Fc1(i) = (-1/L2/sin(t2)*(-I2*t2dd+1/2*m2*g*L2*sin(t2))+1/2*cos(t2)/sin(t2)*m2*x2dd1/2*m2*y2dd+cos(t2)/sin(t2)*m*xdd)*L1*cos(t1)+m*xdd*L1*sin(t1)+m2*y2dd*L1*cos(t1)+m2*x2dd*L1*sin(t1);
tou1t(i) = I0*t1dd-1/2*m1*g*L1*cos(t1)+(-1/L2/sin(t2)*(I2*t2dd+1/2*m2*g*L2*sin(t2))+1/2*cos(t2)/sin(t2)*m2*x2dd1/2*m2*y2dd+cos(t2)/sin(t2)*m*xdd)*L1*cos(t1)+m*xdd*L1*sin(t1)+m2*y2dd*L1*cos(t1)+m2*x2dd*L1*sin(t1);
end

figure(2)
plot(tf,tou1t)
xlabel('time(sec)')
ylabel('torque of joint 1 (Nm)')
legend('Lagrange method')

C. Thit k qu o ca xe
% This file is to generate trajectory of vehicle
function [theta, velocity, acc]=inter_2point(th, vel, acc, t0, t1, step_t )

t=t0: step_t: t1;

T1=[1,
0,
0,

t0,
1,
0,

t0^2,
t0^3,
t0^4,
t0^5;
2*t0, 3*t0^2,
4*t0^3,
5*t0^4;
2,
6*t0, 12*t0^2, 20*t0^3;

18

Din n C in t, Haptic v Thc t o (Din n CHAT)

Tin s V Minh Hng

1,
0,
0,

t1,
1,
0,

t1^2,
2*t1,
2,

t1^3,
3*t1^2,
6*t1,

http://matlabs.wordpress.com
t1^4,
4*t1^3,
12*t1^2,

t1^5;
5*t1^4;
20*t1^3];

traj_1=[th(1), vel(1), acc(1), th(2), vel(2), acc(2)]';

a=inv(T1)*traj_1;

theta=a(1)+a(2).*t+a(3).*t.^2+a(4).*t.^3+a(5).*t.^4+a(6).*t.^5;
velocity=a(2)+2.*a(3).*t+3.*a(4).*t.^2+4.*a(5).*t.^3+5.*a(6).*t.^4;
acc=2.*a(3)+6.*a(4).*t+12.*a(5).*t.^2+20.*a(6).*t.^3;

Khi c bt k vn g cn trao i, ng ngn ngi lin h vi chng ti!

Din n C in t, Haptic v Thc t o (Din n CHAT)
Website: http://matlabs.wordpress.com/
Email: vmhung2010@gmail.com

19

Din n C in t, Haptic v Thc t o (Din n CHAT)