Mô hình hệ trục 2 D của robot

Bảng DH

θi αi di ai
1 0 0 d 0
2 0 0 0 l1
3 θ3 0 0 l2
4 θ4 0 0 l3

Dựa vào bảng DH ta có

[ ] [ ]
1 0 0 0 1 0 0 l1
0 0 1 0 0 1 0 1 0 0
T 1=T(z,d)= và T 2=T(x,l 1)=
0 0 1 d 0 0 1 0
0 0 0 1 0 0 0 1

[ ][ ][ ]
cos θ3 −sin θ3 0 0 1 0 0 l 2 cos θ3 −sin θ3 0 l 2 cos θ 3
sin θ3 cos θ3 0 0 0 1 0 0 = sin θ3 cos θ3 0 l 2 sinθ 3
T 3=R(z,θ3 ).T(x, l 2)=
2
.
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
 [ ][ ][ ]
cos θ4 −sin θ 4 0 0 1 0 0 l 3 cos θ4 −sin θ 4 0 l 3 cos θ 4
sin θ4 cos θ 4 0 0 0 1 0 0 = sin θ4 cos θ 4 0 l 3 sinθ 4
T 4=R(z,θ3 ).T(x, l 2)=
3
.
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

[ ][ ][ ][ ]
1 0 0 0 1 0 0 l1 cos θ3 −sin θ3 0 l 2 cos θ 3 …
…
0 1 0 0 0 1 0 0 sin θ3 cos θ3 0 l 2 sinθ 3
T 04=T 01. T 12. T 23. T 34= . . .…
0 0 1 d 0 0 1 0 0 0 1 0 …
0 0 0 1 0 0 0 1 0 0 0 1 …

[ ][ ]
cos θ3 −sin θ3 0 l 1 +l 2 cos θ3 cos θ4 −sin θ 4 0 l 3 cos θ 4
sin θ3 cos θ3 0 l 2 sin θ3 sin θ4 cos θ 4 0 l 3 sinθ 4
= .
0 0 1 d 0 0 1 0
0 0 0 1 0 0 0 1

=¿
Tính toán động học nghịch
Ta có

P x=l 1 +l 3 cos (θ ¿ ¿ 3+θ 4 )+l 2 cos θ3 ¿

P y =l 3 sin(θ ¿ ¿ 3+θ 4 )+l 2 sin θ3 ¿

P z=d

( P x-l 1)2 + P y 2 =l 32.cos((θ ¿ ¿ 3+θ 4 )¿ 2+2l 2. l 3.cos(¿ ¿).cos(θ3 ¿+ l 22 .cosθ3 2 +l 32.sin((θ ¿ ¿ 3+θ 4 )¿ 2 +

2l 2. l 3.sin(θ3 +θ 4 ¿.sinθ3 +l 22 .sinθ3 2

= l 32 + l 22+2l 2. l 3.cosθ 4
2 2 2 2
( P x −l 1) + P y −l 3 −l 4
=> cosθ 4=
2.l 2 .l 3

√
=> sinθ 4= - 1−cos θ 42

Vậy θ 4=arctan2(sinθ 4 . cos θ 4)

Khi đó ta viết lại dưới dạng
P x-l 1=l 2 cos θ3+l 3.( cos θ 3. cos θ 4-sinθ3 .sinθ 4)

P y =l 2 sin θ 3+l 3.( sin θ 4. cos θ 3+sinθ3 .cosθ 4)

{
=>
Px −l 1=( l 2+l 3 cos θ 4 ) .cos θ 3−l 2 .sin θ3 . sin θ4
P y =( l 2 +l 3 cos θ4 ) . sin θ3 −l3 . cos θ3 . sin θ4

Giải hệ phương trình tuyến tính ta được

∆= |
l 2 +l 3 cos θ4 −l 3 sin θ4
l 3 sin θ 4 |
=l 2 + 2. l 2 l 3 . cos θ 4+l 32=( P x −l 1 ¿2+ P y 2
l 2+l 3 cos θ 4 2

∆ 1=
|
P x −l 1 −l 3 sin θ4
Py l 2+ l 3 cos θ 4 |
=( P x −l 1) l 2+( P x −l 1) l 3cosθ 4+ P y l 3 sin θ 4

∆ 2=
| l 2 +l 3 cos θ4
l3 cos θ 4
Px −l 1
Py |= P y ¿+l 3 cos θ4 )-( P x-l 1) l 3 sinθ 4

∆1
cos θ 3= =¿ ¿ ¿
∆
∆2
sinθ3 = =P y ¿ ¿ ¿
∆
=>θ3 =arctan2(sinθ3 ,cosθ3 )

3 Ma trận Jacobi
Ta có:

P x=l 1 +l 3 cos (θ ¿ ¿ 3+θ 4 )+l 2 cos θ3 ¿

P y =l 3 sin(θ ¿ ¿ 3+θ 4 )+l 2 sin θ3 ¿

P z=d

[ ]
∂ P x /∂ d ∂ P x /∂θ3 ∂ P x /∂ θ 4
J= ∂ P y /∂ d ∂ P y /∂ θ3 ∂ P x /∂ θ 4
∂ P z /∂ d ∂ P z /∂ θ3 ∂ P x /∂ θ 4
 =
[0 −l 3 sin (θ ¿ ¿ 3+θ 4)−l 2 sin θ3 ¿−l3 sin(θ ¿ ¿ 3+θ4 ) ¿ 0 ∂ P y /∂ d l 3 cos (θ ¿ ¿ 3+θ 4)¿ 1 ∂ P z /∂ d 0 ]

Từ đó ta có được vận tốc dài :

[ ] []
Vx ḋ
V y =[J]. θ̇3 =
Vz θ̇4
[0 −l 3 sin (θ ¿ ¿ 3+θ 4)−l 2 sin θ3 ¿−l3 sin(θ ¿ ¿ 3+θ4 ) ¿ 0 ∂ P y /∂ d l 3 cos (θ ¿ ¿ 3+θ 4)¿ 1 ∂ P z /∂ d 0 ]

[]
ḋ
. θ̇3
θ̇4

Vận tốc các khớp

[] [ ]
ḋ Vx
θ̇3 =¿ J-1. V y
θ̇4 Vz

Không gian hoạt động

Code:
%% khong gian hoat dong
hold off
l1=200.72; l2=250; l3=200;
for tt3=-5*pi/9:pi/18:5*pi/9
for tt4=-2*pi/3:pi/18:2*pi/3
for d=0:5:220
Px=l1+l3*cos(tt3+tt4)+l2*cos(tt3);
Py=13*sin(tt3+tt4)+l2*sin(tt3);
Pz=d;
plot3(Px,Py,Pz,'*');
hold on
end
end
end
Đồ thị mô phỏng
Tính toán động lực học
Động năng và thế năng của các khâu tay máy
Khâu 1
Do không di chuyển nên khâu 1 có:
+ Động năng bằng 0

+ Thế năng bằng P1=m1.g. h1 =-m 1.g. sin θ1

Khâu 2: có chuyển động tịnh tiến thông qua cơ cấu trục vít

{ {
x2 =l 2 . cos . θ2 ẋ2 =−l 2 . sin θ2 θ̇2
y 2=l 2 . sin .θ 2 => ẏ 2=l 2 .cos θ2 θ˙2
z 2=−θ 1 z˙2 =−θ̇1

Ta có

v 22=( x ¿¿˙ 2)¿2. ( y ¿¿˙ 2)¿ 2. (z ¿¿˙ 2) ¿2=(- l 2 . sin . θ2 . θ˙2)2 +( l 2 .sin θ 2 θ̇2 )2 + θ˙1 2=l 2 . sin .θ2 . θ̇22+θ̇12

Thế năng khâu 2: P2=m 2.g. h1 =-m2.g. sin θ1

1 1
Động năng khâu 2: K 2= . m2. v 22= . m 2.¿ ¿2(θ2 )2+θ̇12)
2 2
Khâu 03: khâu quay
{ x 3=l 2 . cos θ2 +l2 . cos .(θ ¿ ¿ 2+ θ3) ¿ y 3=−l2 . cos .θ 2+ l2 . sin(θ ¿ ¿ 2+θ 3) ¿ z3 =0
¿
Ta có v3 2=( x˙3)2.( ẏ 3)2.( z˙3)2=(−l 2 .sin . θ2−l 2 . sin(θ ¿ ¿ 2+θ3 )¿)2+(
l 2 . sin θ2 θ̇2 +l 2 . cos (θ ¿ ¿ 2+θ3 ) (θ ¿ ¿ 2 ¿˙+ θ̇3 )¿ ¿ ¿ 2

Thế năng khâu 3 bằng 0

Động năng khâu 3:
1 1 1
K3 = . m 3. v3 2+ .j3.w32= m 3. (−l 2 .sin . θ2−l 2 . sin(θ ¿ ¿ 2+θ3 )¿)2+(
2 2 2
1
l 2 . sin θ2 θ̇2 +l 2 . cos (θ ¿ ¿ 2+θ3 ) (θ ¿ ¿ 2 ¿˙+ θ̇3 )¿ ¿ ¿ 2 + .j3. θ 32
2
Khâu 4: khâu quay

{ x 4 =l 2 . cos θ2 +l3 . cos .(θ ¿ ¿ 2+θ3 )¿ y 4=−l2 . sinθ 2+l3 . sin (θ ¿ ¿ 2+θ 3)¿ z 4=0
¿
Ta có

v 42=( x˙4 )2.( y˙4 )2.( z˙4 )2=(-l 2.sinθ2 θ̇2 - l 3 . s∈(θ ¿ ¿ 2+ θ3) ¿. ( θ˙2 + θ˙3)2)2+(
l 2 . cos θ 2 θ˙2 . cos(θ ¿ ¿ 2+ θ3) (θ ¿ ¿ 2 ¿˙+ θ̇3)¿ ¿ ¿)2
Thế năng khâu 4 bằng 0
Động năng khâu 4:
1 1 1
K4 = . m 4 . v 42+ .j4.w42= m4 . =(-l 2.sinθ2 θ̇2 - l 3 .sin (θ ¿ ¿ 2+θ3 )¿ . ( θ˙2 + θ˙3)2)2+(
2 2 2
1
l 2 . cos θ 2 θ˙2 . cos(θ ¿ ¿ 2+ θ3) (θ ¿ ¿ 2 ¿˙+ θ̇3)¿ ¿ ¿)2+ .j4. θ 42
2
Tổng thế năng

P= P1+ P2+ P3+ P4 =−m 1.g.sin θ1 -m2.g. sin θ1

Tổng động năng

1 1 1 1 1
K= K 1+ K 2+ K 3+ K 4 = . m 2. v 22= . m2.¿ ¿2(θ2 )2+θ̇12)+ . m3. v3 2+ .j3.w32= m3. (
2 2 2 2 2
1
−l 2 .sin . θ2−l 2 . sin(θ ¿ ¿ 2+θ3 )¿)2+(l 2 . sin θ2 θ̇2 +l 2 . cos (θ ¿ ¿ 2+θ3 ) (θ ¿ ¿ 2 ¿˙+ θ̇3 )¿ ¿ ¿ 2 + .j3. θ 32 +(-l 2
2
1
.sinθ2 θ̇2 - l 3 .sin (θ ¿ ¿ 2+θ3 )¿ . ( θ˙2 + θ˙3)2)2+(l 2 . cos θ 2 θ˙2 . cos(θ ¿ ¿ 2+ θ3) (θ ¿ ¿ 2 ¿˙+ θ̇3)¿ ¿ ¿)2+ .j4. θ 42
2
Ta có hàm lagrange
L=K-P