BẢNG THÔNG SỐ KÍCH THƯỚC

θ1 [−π ; π ]
Phạm vi chuyển động θ2 −π π
[ ; ]
3 2
(rad)
θ3 −3 π 3 π
[ ; ]
4 4
θ4 −3 π 3 π
[ ; ]
4 4
L1 250
Chiều dài các khâu (mm) L2 450
L3 540
L4 310

BÀI TOÁN ĐỘNG HỌC

Bài toán động học thuận

 Hình: Đặt các hệ trục lên robot

Áp dụng phương pháp Denavit – Hatenberg, ta lập được bảng sau

# L θ α d Var
1
0 θ1 π /2 L1 θ1
2 L2 θ2 0 0 θ2
3 L3 θ3 0 0 θ3
4 L4 θ4 0 0 θ4
Bảng: Xác định các thông số sử dụng phương pháp Denavit –
Hatenberg

Ta có ma trận chuyển vị:

cos ( θ ) −sin ( θ )∗cos ( α ) sin (θ )∗sin ( α ) L∗cos ( θ )

+ T cv=
[
sin (θ ) cos ( θ )∗cos ( α ) −cos ( θ )∗sin ( α ) L∗sin ( θ )
0 sin ( α )
0 0
cos ( α ) d
0 1
]
Từ bảng D-H trên ta có:
 cos ( θ 1) 0 sin ( θ1 ) 0

+ T 01=
[
sin ( θ1 ) 0 −cos ( θ 1) 0
0 1
0 0
0 L1
0 1
]
cos θ2 −sin θ2 0 L2 cos θ 2

+ T 12=
[
sin θ2 cos θ2 0 L2 sinθ 2
0 sin θ2
0 0
1 0
0 1
]
cos θ3 −sin θ3 0 L3 cos θ3

+ T 23=
[
sin θ3 cos θ3 0 L3 sin θ3
0 sin θ3
0 0
1 0
0 1
]
cos θ 4 −sin θ4 0 L4 cos θ 4

+ T 34=
[
sinθ 4 cos θ4 0 L 4 sinθ 4
0 sin θ 4
0 0
1 0
0 1
]
Để tìm tọa độ điểm cuối cùng, ta nhân 4 ma trận trên bằng phần mềm matlab
ta được:

T 04=T 01*T 12*T 23*T 34

Suy ra: ¿

Bài toán động học nghịch

Đặt: cos θ 1= c1 ; sin θ1= s1 ; cos θ 2= c2 ; sin θ2= s2 ; cos ¿ +θ3 ¿= c23 ; sin ¿+θ3 ¿=
s23 ;cos ¿ +θ2 +θ3 ¿=c123 ; sin ¿+θ2 +θ3 ¿=s123

Ta có: T 04=T 01*T 12*T 23*T 34

<=> 01T −1∗T 04 =01T −1∗T 01*T 12*T 23*T 34

<=> 01T −1∗T 04 =T 12*T 23*T 34

r 11 r 12 r 13 n1 r 11 r 12 r 13 m1
<=>
[
r 21 r 22 r 23 n2
r 31 r 32 r 33 n3
0 0 0 1
=
][
r 21 r 22 r 23 m2
r 31 r 32 r 33 m3
0 0 0 1
]
 r 11 r 12 r 13 Px
Với: FK =
[
r 21 r 22 r 23 Py
r 31 r 32 r 33 Pz
0 0 0 1
]
r 11 r 12 r 13 n1
=> T 10FK = T 10*FK =inv (T ¿¿ 01)¿ *FK =
[
r 21 r 22 r 23 n2
r 31 r 32 r 33 n3
0 0 0 1
]
r 11 r 12 r 13 m1
Ta có: T234=T 12*T 23*T 34 =
[
r 21 r 22 r 23 m2
r 31 r 32 r 33 m3
0 0 0 1
]
n 1=P y∗s 1+ P x∗c 1 m1=l 3∗c 23+ l2∗c 2+l 4∗c 234
Ta được:
{n 2=P z – l1
n 3=Py∗c 1−Px∗s 1 {
; m 2=l3∗s 23+l 2∗s 2+l 4∗s 234
m3=0

Ta có: n3 = m3

s1 y
<=> Py*c1 - Px*s1 = 0 <=> =
c1 x

y
<=> tan(θ1) = x

<=> θ1 = atan2(y,x)

Ta có: {nn 1=m 1

2=m 2

<=> {Px∗cz1+– l1=l3∗s

Py∗s 1=l 3∗c 23+l 2∗c 2+l 4∗c 234
23+l2∗s 2+l 4∗s 234

<=> {Px∗cz1+– l1−l

Py∗s 1−l 4∗c 234=l3∗c 23+l2∗c 2
4∗s 234=l 3∗s 23+l 2∗s 2
(*)

Đặt : {nx=Pny=z
x∗c 1+ Py∗s 1−l4∗c 234
– l1−l 4∗s 234

Từ (*) ta được: {nx=l 3∗c 23+l 2∗c 2

ny=l 3∗s 23+l 2∗s 2
 nx 2=( l3∗c 23+l2∗c 2 )2
<=> {
ny 2=( l3∗s 23+l 2∗s 2 )
2

nx 2=l3 2∗c 232+ l22∗c 22 +2∗l 3∗l2∗c 23∗c 2

<=> {
ny 2=l3 2∗s 232+ l22∗s 22 +2∗l3∗l2∗s 23∗s 2

=> nx 2 + ny 2 = l 32∗¿ ¿)+ l 22∗(c 2 ¿ ¿ 2+ s 22 )¿ + 2*l3*l2*(c 23∗c 2+ s 23∗s 2 )

<=> nx 2 + ny 2 = l 32 + l 22+ 2*l3*l2* c3

nx 2+ ny 2−l3 2−l22
<=> c3 =
2∗l 3∗l 2

Ta có: θ2 +θ3 +θ 4= 0

cos ( θ2 +θ3 +θ 4 ) =1

{
=> sin ( θ2 +θ3 +θ4 )=0
θ4 =−θ2−θ 3

Từ đó ta tính được: {nx=Px∗cny=z1+ Py∗s

–l 1
1−l 4

2
Ta có: sin(θ3) = √ 1−cos ( θ3 )

sin ( θ3 )
=> tan (θ3) =
cos ( θ3 )

=>θ3 = atan2(s3,c3)

Ta có : {nx=l 3∗c 23+l 2∗c 2

ny=l 3∗s 23+l 2∗s 2

nx=l 3∗( c 2∗c 3−s 2∗s 3 ) +l2∗c 2

<=> {
ny =l3∗( s 2∗c 3+c 2∗s 3 ) +l 2∗s 2

nx=c 2∗( l 3∗c 3+l 2 )−l 3∗s 2∗s 3

<=> { ny=s 2∗( l3∗c 3+ l2 ) +l 3∗c 2∗s 3

nx+l 3∗s 2∗s 3

<=>
s 2={ c 2=
l 3∗c 3+l 2
ny−l3∗c 2∗s 3
l3∗c 3+l 2
Ta thay s2 lên trên

Ta được:
c 2=
nx+ l3∗( nyl3∗c
−l3∗c 2∗s 3
3+l2 )
∗s 3

l3∗cos ( t 3 ) +l2

nx∗( l 3∗c 3+l 2 )+ l 3∗s 3∗( ny −l3∗c 2∗s 3 )

=> c2 =
( l 3∗c 3+l 2 )2

nx∗( l 3∗c 3+l 2 )+ l3∗s 3∗ny −l32∗c 2∗s 32

=> c2 =
( l3∗c 3+l 2 )2

nx∗( l 3∗c 3+l 2 )+ l3∗s 3∗ny l32∗c 2∗s 32

=> c2 = −
( l 3∗c 3+l 2 )2 ( l3∗c 3+l2 )2

l 32∗s 32 nx∗( l 3∗c 3+l 2 )+ l3∗s 3∗ny

=> c2*[1 + 2 ] =
( l3∗c 3+ l2 ) ( l 3∗c 3+l 2 )2

( l3∗c 3+ l2 )2 +l3 2∗s 32 nx∗( l 3∗c 3+l 2 )+ l3∗s 3∗ny

=> c2*[ 2 ]=
( l 3∗c 3+l 2 ) ( l 3∗c 3+l 2 )2

nx∗( l 3∗c 3+l 2 )+ l3∗s 3∗ny

=> c2 =
( l3∗c 3+l2 )2 +l 32∗s 32

Tương tự ta thay c2 xuống phương trình dưới để tìm s2

Ta được:
s 2=
ny−l 3∗ ( nx+l3∗c
l3∗s 2∗s 3
3+ l2 )
∗s 3

l3∗c 3+l 2

ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗( nx +l3∗s 2∗s 3 )

=> s 2=
( l3∗c 3+l2 )2

ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗nx−l 32∗s 2∗s 3 2

=> s 2=
( l3∗c 3+l2 )2

ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗nx l 32∗s 2∗s 32

=> s2 = −
(l 3∗c 3+l 2 )2 ( l 3∗c 3+l 2 )2

l 32∗s 32 ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗nx

=> s2*[1 + 2 ] =
( l3∗c 3+ l2 ) (l 3∗c 3+l 2 )2
 ( l3∗c 3+ l2 )2 +l3 2∗s 32 ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗nx
=> s2*[ 2 ]=
( l 3∗c 3+l 2 ) (l 3∗c 3+l 2 )2

ny∗( l 3∗c 3+l 2 )−l 3∗s 3∗nx

=> s2 =
( l 3∗c 3+l 2 )2+l 32∗s 32

s2 ny∗ (l 3∗c 3+l 2 )−l 3∗s 3∗nx A

Vậy, t2 = c 2 =
nx∗ (l 3∗c 3+ l2 ) +l 3∗s 3∗ny
=B

<=> t2 = atan2(A,B)

Động học vận tốc:

Ta có: ¿

θ̇1

[]
VP x
θ̇
Vector vận tốc chung cho robot: [ V P ] = [J] . 2
y
θ̇3
VP z
θ̇4
θ̇1

[]
VP
θ̇2 x

=> = [ V P ] . [ J −1]
θ̇3 y

VP
θ̇4 z

∂ Px ∂ Px ∂ Px ∂ Px

Với Ma trận Jacobi: J =

+∂ Px
∂ θ1
∂ Py
∂ θ1
∂ Pz
∂ θ1
[ ∂ θ2
∂ Py
∂ θ2
∂ Pz
∂ θ2
∂θ 3
∂ Py
∂ θ3
∂ Pz
∂ θ3
∂ θ4
∂Py
∂θ 4
∂ Pz
∂ θ4
]
=−sin(θ 1)∗¿
∂θ1

+∂ Px
=−cos (θ1)∗¿
∂ θ2

+∂ Px
=−cos (θ1)∗¿
∂ θ3

+∂ Px
=−cos ( θ 1 )∗l 4∗sin ( θ 2+θ 3+θ 4 )
∂ θ4
+∂P y
=cos(θ 1)∗¿
∂θ 1

+∂P y
=−sin(θ1 )∗¿
∂θ 2

+∂ P y
=−sin(θ1 )∗¿
∂θ 3

+∂P y
=−sin ( θ1 )∗l 4∗sin ( θ2 +θ3 +θ4 )
∂θ 4

+ ∂ Pz
=0
∂ θ1

+∂ Pz
=l 3∗cos(θ 2+θ ¿ ¿3)+l 2∗cos (θ2 )+ l 4∗cos(θ 2+ θ3 +θ4 )¿
∂ θ2
+∂ Pz
=l 3∗cos(θ 2+θ ¿ ¿3)+l 4∗cos (θ2 +θ3 +θ 4 )¿
∂ θ3

+∂ Pz
=l 4∗cos ( θ2 +θ3 +θ4 )
∂ θ4

Ta dùng matlab tính được: [ J −1] = inv([J])

PHƯƠNG TRÌNH ĐỘNG LỰC HỌC

Tính động năng

Toạ độ điểm P: ¿

Khâu 1:
¿

+ Ẋ 1 =−sin(θ1 )∗¿

+Y˙ 1=cos (θ1 )∗¿

+ Ż1 =0

 V 12= X˙12 + Y˙12 + Z˙12

 V 12=¿ ¿

1 2 1
 K 1= m 1 V 1 = m1 ¿ ¿
2 2

Khâu 2:
¿

+ X˙ 2 =−sin(θ 1)∗¿

+Y˙ 2=cos(θ1 )∗¿

+
Ż2 =l3∗c os(θ 2+θ ¿ ¿ 3)∗( θ˙2+ θ˙3)+l 2∗cos (θ2 )∗θ˙2 +l 4∗cos (θ2 +θ3 +θ 4 )∗( θ̇2+ θ̇3 + θ̇4 ) ¿

 V 22= X˙22 + Y˙22 + Z˙22

 V 22=2∗¿ ¿

1 2 1
 K 2= m2 V 2 = m2 ¿ ¿
2 2

- Khâu3:¿

+ X˙ 3 =−sin(θ 1)∗¿

+Y˙ 3=cos(θ1 )∗¿

+
Ż3 =l3∗c os(θ 2+ θ ¿ ¿ 3)∗( θ˙2+ θ̇3)+l 2∗cos (θ2 )∗θ˙2+l 4∗cos(θ2 +θ3 +θ 4 )∗( θ̇2+ θ̇3 + θ̇4 )¿

 V 32= X˙32+ Y˙32 + Z˙32

V 32=2∗¿ ¿
 1 2 1
 K 3= m 3 V 3 = m 3 ¿ ¿
2 2

- Khâu4¿

+ X˙ 4=−sin(θ1 )∗¿

+Y˙ 4 =cos (θ1)∗¿

+
Ż 4=l 3∗c os (θ2 +θ ¿ ¿ 3)∗( θ̇2 + θ̇3 )+l 2∗cos( θ2)∗θ̇2 +l 4∗cos (θ2 +θ3 +θ4 )∗( θ̇2 + θ˙3 + θ˙4 )¿

 V 4 2= X˙4 2+ Y˙42 + Z˙4 2

V 4 2=2∗¿ ¿

1 2 1
 K 4 = m4 V 4 = m4 ¿ ¿
2 2

Tổng động năng của hệ là:

K = K1+ K2+ K3+ K4
1
K = ∗(m2 +m3 +m4 )¿ ¿
2

Tính thế năng

- Khâu 1: U 1=0
- Khâu 2: U 2=m2 g h2=m2 g (l1+l 2∗cos (θ1 ))
- Khâu 3: U 3=m3 g h3=m3 g(l 1+l 2∗cos(θ 1)+l3∗cos (θ2 ))

Trong đó: h là chiều cao từ mặt đất đến khâu 2

- Khâu 4:U 4 =m4 g h4 =m4 g(l1+l 2∗cos(θ 1)+l3∗cos (θ2 )+l 4∗cos(θ 4 ))

Tổng thế năng của hệ là:

U = U 1 +U 2 +U 3 +U 4
 U =g∗¿
 U =g∗¿

Thiết lập phương trình vi phân chuyển động

d δL δL
( ) − =τ
dt δ q̇ i δ qi i

- Hàm Lagrange của hệ thống:

1
L(θ , θ̇) = K(θ , θ̇) – U(θ , θ̇) = ∗(m2 +m3 +m4 )¿ ¿
2

δL(θ , θ̇)
=¿
δ θ̇i