HỌC VIỆN KĨ THUẬT QUÂN SỰ

KHOA HÀNG KHÔNG VŨ TRỤ

BÀI TẬP LỚN

ĐỘNG LỰC HỌC HỆ NHIỀU VẬT

Họ và tên: Nguyễn Huy Hoàng

Cấp bậc: Trung sĩ
Chức vụ: Học viên
Đơn vị: Lớp Cơ điện tử - c252 - d2

Hà Nội, năm 2019

1. Động học
1.1 Mô hình động học.............................................................................................................................2
1.1.1 Thiết lập hệ tọa độ.........................................................................................................................2
1.1.2 Bảng thông số động học (D-H).......................................................................................................2
1.2 Thiết lập các phương trình động học.............................................................................................2
1.2.1 Các ma trận chuyển.......................................................................................................................2
1.2 Thiết lập các phương trình động học.............................................................................................2
1.2.1 Các ma trận chuyển.......................................................................................................................2
1.3 Bài toán động học thuận...............................................................................................................3
1.3.1 Thuật toán......................................................................................................................................3
1.3.2 Chương trình matlab......................................................................................................................7
1.3.3 Kết quả............................................................................................................................................7
1.4 Bài toán động học ngược...............................................................................................................7
1.4.1 Thuật toán......................................................................................................................................7
1.4.2 Chương trình matlab......................................................................................................................8
1.4.3 Kết quả............................................................................................................................................8

2. Động học
2.1 Mô hình động học...........................................................................................................................10
2.2 Tính các thông số động lực học.......................................................................................................10
2.3 Phương pháp Newton - Euler.........................................................................................................10
2.3.1 Tách khâu, biểu diễn lực ...............................................................................................................10
2.3.2 Viết phương trình cho các khâu....................................................................................................11
2.3.3 Hệ phương trình N-E dạng chưa tối giản......................................................................................12
2.3.4 Hệ phương trình vi phân dạng tối giản.........................................................................................18
2.4 Phương pháp Lagrange II................................................................................................................19
2.4.1 Tính động năng.............................................................................................................................19
2.4.2 Tính các lực suy rộng.....................................................................................................................21
2.4.3 Phương trình vi phân chuyển động...............................................................................................21
2.5 Bài toán động học thuận.................................................................................................................21
2.5.1 Thuật toán....................................................................................................................................21
2.5.2 Chương trình matlab....................................................................................................................21
2.5.3 Kết quả..........................................................................................................................................21
2.6 Bài toán động học thuận.................................................................................................................21
2.6.1 Thuật toán....................................................................................................................................21
2.6.2 Chương trình matlab....................................................................................................................21
2.6.3 Kết quả..........................................................................................................................................21
Đề 31:
Cho mô hình cánh tay robot như hình vẽ. Động cơ dẫn động của robot đặt vào các khớp
O, O’. Gọi điểm tác động cuối là E. Các khâu làm bằng thép hộp đặc có mặt cắt không
đổi, coi là tuyệt đối cứng.
- Thiết lập các hệ tọa độ, lập bảng thông số động học, tính các ma trận chuyển động.
- Viết các học phương trình động, mô phỏng robot gắp vật từ điểm M(300,200,200) đến
vị trí N(350,400,550) với vận tốc đi từ M đến N là 0,1 m/s. Vẽ các đồ thị vị trí, vận tốc,
gia tốc của các khớp O, O’ và điểm E.
- Viết các phương trình động lực học. Tính toán mô men của các động cơ đặt tại khớp O,
O’ sao cho cánh tay nâng được vật có khối lượng 15 kg di chuyển trên đoạn MN. Vẽ các
đồ thị vị trí, vận tốc, gia tốc của các khóp O, O’ và điểm E. Vẽ đồ thị momen của các
khớp O, O’.
Cho các thông số: OA = 835mm, BE = 600mm. Tiết diện mặt cắt các khâu: OA:
60x60mm; BE: 40x40mm.

1
 1. Động học
1.1. Mô hình động học
1.1.1. Thiết lập hệ tọa độ

1.1.2. Bảng thông số động học (D-H)

Từ sơ đồ trên với các hệ tọa độ được thiết lập như hình vẽ ta có được bảng
thông số D-H như sau

Khâu θi di ai αi
0-1 q1 0 0 0
1-2 0 q2 0 π/2
2-3 q3 d3 0 π/2
3-4 0 q4 0 0

1.2. Thiết lập các phương trình động học

1.2.1. Các ma trận chuyển
Từ bảng thông số D-H ta lập được các ma trận chuyển:

cos(q 1 )  sin(q 1 ) 0 0
 sin(q ) cos(q ) 0 0 
H 0-1  1 1

 0 0 1 0
 
 0 0 0 1
1 0 0 0
0 0 1 0 
H1-2 
0 1 0 q2 
 
0 0 0 1
2
  cos(q 3 ) 0 sin(q 3 ) 0
 sin(q ) 0  cos(q 3 ) 0 

H 2-3 3

 0 1 0 d3 
 
 0 0 0 1
1 0 0 0 
0 1 0 0 
H 3-4   
 0 0 1 q4 
 
0 0 0 1 
cos(q 1 ) 0 sin(q 1 ) 0
 sin(q ) 0  cos(q ) 0 
D0-2   1 1 
 0 1 0 q2 
 
 0 0 0 1
 cos(q 1 ).cos(q 3 ) sin(q 1 ) cos(q 1 ).sin(q 3 ) d3.sin( q1 ) 
 sin(q ).cos(q )  cos(q ) sin(q ).sin(q ) d .cos(q ) 
D0-3  1 3 1 1 3 3 1 

 sin(q 3 ) 0  cos( q3 ) q2 
 
 0 0 0 1 

 cos(q 1 ).cos(q 3 ) sin(q 1 ) cos(q 1 ).sin(q 3 ) d 3 .sin(q1 )  q 4 .cos(q 1 ).sin(q 3 ) 

 sin(q ).cos(q )  cos(q ) sin(q ).sin(q )  d .cos(q )  q .sin(q ).sin(q ) 
D0-4  1 3 1 1 3 3 1 4 1 3 

 sin(q 3 ) 0  cos( q3 ) q2  q4 .cos( q3 ) 

 
 0 0 0 1 
Ta xác định được tọa độ vị trí điểm E có dạng
 xE(0)  d 3 .sin(q1 )  q 4 .cos(q 1 ).sin(q 3 )
 (0)
 yE   d 3 .cos(q1 )  q 4 .sin(q 1 ).sin(q 3 )
 z E(0)  q2  q4 .cos(q3 )

1.3. Bài toán động học thuận
1.3.1. Thuật toán(*)
Đối với hệ 4 bậc tự do này ta sử dụng ma trận Denavit-Hartenberg để phân tích
động học. Thiết lập các tọa độ thuần nhất từ đó tìm ra phương trình chuyển động của
khâu thao tác.
* Xác định phương trình chuyển động của khâu thao tác :
Ta có ma trận chuyển hệ tọa độ từ hệ qui chiếu (HQC) (Oxyz)0 sang hệ qui chiếu
(Oxyz)4
D0-4= H0-1* H1-2* H2-3* H3-4
Trong đó : H0-1 là ma trận chuyển hệ tọa độ từ HQC (Oxyz)0 sang HQC (Oxyz)1
H1-2 là ma trận chuyển hệ tọa độ từ HQC (Oxyz)1 sang HQC (Oxyz)2
H2-3 là ma trận chuyển hệ tọa độ từ HQC (Oxyz)2 sang HQC (Oxyz)3
H3-4 là ma trận chuyển hệ tọa độ từ HQC (Oxyz)3 sang HQC (Oxyz)4
Từ ma tận D0-4 ta xác định được phương trình chuyển động của khâu thao tác.
* Xác định vận tốc góc của các khâu :
Từ các ma trận Denavit-Hartenberg ta suy ra ma trận cosin chỉ hướng tương ứng của các
khâu.
A_01 : ma trận cosin chỉ hướng khâu 1
3
 A_02 : ma trận cosin chỉ hướng khâu 2
A_03 : ma trận cosin chỉ hướng khâu 3
A_04 : ma trận cosin chỉ hướng khâu 4
Từ các ma trận cosin chỉ hướng ta tính được toán tử sóng của các vecto vận tốc góc
của các khâu.
•
 1 = A T . A 01
ω 01

Tương tự ῶ2 ῶ3 ῶ4

Từ đó ta có thông số động học các khớp :

* Khớp O:
Tọa độ của khớp O có dạng:
0
rO   0 
 0 
Jacobi tịnh tiến :
0 0 0 0 
drO 
J TO   0 0 0 0 
dq 
0 0 0 0 
Vận tốc của khớp O:
0
 rO  rO
vO   q  J T O.q  0 
t q
0 
Gia tốc của khớp O:
0
vO
aO   J T O* q  J T O.q  0 
t
0 
Toán tử sóng :
0 q1 0 
 =  q1
 = A .A
ῶ_O = ω T
0 0 
01 
01
 0 0 0 
Vận tốc góc :
ωO   0 0 q1 
T

Jacobi quay :
0 0 0 0
 0 
ωO 0 0 0
J RO  
q 0 0 0 0
 
1 0 0 0

* Khớp O’:
Tọa độ của khớp O’:

4
  d3 .sin(q1 ) 
rO '   d3 .cos(q1 ) 
 q2 
Jacobi tịnh tiến :
 d3 .cos(q1 ) 0 0 0 
d rO ' 
JT O '    d3 .sin( q1 ) 0 0 0 
dq
 0 1 0 0 

Vận tốc của khớp O’:

 d3 .q1.cos q1 ) 
 rO  rO
vO   q  J T O.q   d3 .q1.sin( q1 ) 
t q
 q 2 
Gia tốc của khớp O’:
 d3 .(q1.cos(q1 )  q1.sin( q1 )) 
vO
aO   JT O .q  J T O .q   d 3 .(q1.cos( q1 )  q1.sin(q1 )) 
t
 q2 
Toán tử sóng :
 0 q1 q 3 .cos(q1 ) 
 q 3 .sin(q1 ) 
ῶ_O’ = 
  A .A
 =
03
T

03
q1 0
 q 3 .cos(q1 ) q 3 .sin(q1 ) 0 
Vận tốc góc :
 q 3 .sin(q1 ) 
ωO   q 3 .cos(q1 ) 
 q1 
Jacobi quay :
0 0 sin(q1 ) 0
 0  cos(q1 ) 0 
ωO ' 0
J RO '  
q 0 0 0 0
 
1 0 0 0

* Khớp E:
Tọa độ của điểm E có dạng :

 d3 .sin(q1 )  q4 .cos(q1 ).sin(q 3 ) 

rE   d3 .cos(q1 )  q4 .sin(q1 ).sin(q 3 ) 
 q2  q4 .cos(q3 ) 
Jacobi tịnh tiến :
 d3 .cos(q1 )  q4 .sin(q1 ).sin(q3 ) 0 q4 .cos( q1 ).cos(q3 ) cos( q1 ).sin(q3 ) 
 rE 
JT E    d3 .sin(q1 )  q4 .cos(q1 ).sin(q3 ) 0 q4 .cos(q3 ).sin(q1 ) sin(q1 ).sin(q3 ) 
q
 0 1 q4 .sin(q3 )  cos( q3 ) 

5
 Vận tốc khâu thao tác :
 rE  rE
vE   q  J T E.q
t q
 q1.( d3 .cos(q1 )  q4 .sin( q1 ).sin( q3 ))  q 4 .cos(q1 ).sin( q3 )  q 3.q 4 .cos(q1 ).cos(q3 ) 
  q1.(d3 .sin(q1 )  q4 .cos( q1 ).sin( q3 ))  q 4 .sin(q1 ).sin( q3 )  q 3.q 4 .sin(q1 ).cos(q3 ) 
 q 2  q 4 .cos(q3 )  q 3 .q4 .sin(q3 ) 
Gia tốc của khớp E:
dvE
aE   J T E.q   J T E.q
dt
 q1 .(d 3 c1  q 4 s1s3 )  q 1 (d3 s1  q4c1s3 )  q4 .c1s3  q 4 s1s3  q3 .q4c1c3  q 3 .q4c3 s1 
 q 1 .(d 3 s1  q 4 c1s3 )  q 1 (d3c1  q4 s1s3 )  q 4c1s3  q4 .s1s3  q 3 .q4c3c1  q 3 .q4 s1c3 
 q 2  q 4 .c3  q 3 .q 4 .s3 
Trong đó :
ci  cos( qi )
si  sin( q1 )

Ma trận cosin chỉ hướng khâu thao tác :

 cos(q1 ).cos(q3 ) sin( q1 ) cos(q1 ).sin( q3 ) 
A _ 0 E   sin( q1 ).cos( q3 )  cos( q1 ) sin( q1 ).sin( q3 ) 
 sin(q3 ) 0 1 
Toán tử sóng :
 0 q1 q 3 .cos(q1 ) 
 =  q1
 E  AT04 .A 0 q 3 .sin(q1 ) 
04 
 q 3 .cos(q1 ) q 3 .sin(q1 ) 0 
Vận tốc góc khâu thao tác :

 q 3 .sin(q1 ) 
ωE    q 3 .cos(q1 ) 
 q1 
Jacobi quay :
0 0 sin(q1 ) 0
 0  cos(q1 ) 0 
ωE  0
JRE  
q 0 0 0 0
 
1 0 0 0

1.3.2. Chương trình matlab(kèm theo file.m)

1.3.3. Kết quả(*)
1.4. Bài toán động học ngược
1.4.1. Thuật toán
Theo bài ta có tọa độ các điểm : M(300,200,200), N(350,400,550)
Vận tốc điểm E : v_rE = 100 mm/s
Với OA = 835 mm, BE = 600mm
Độ dài đoạn MN là:

6
MN  ( xM  xN ) 2  (y M  yN ) 2  (z M  z N ) 2
 (300  350) 2  (200  400) 2  (200  550) 2
 400(mm)
Điểm cuối di chuyển với vận tốc đều nên thời gian di chuyển từ M đến N là :
MN 400
t   4( s)
v _ rE 100
Lập phương trình chính tắc của đoạn MN :
x  xM  ( xN  xM )*0, 25t  300  12,5t
y  yM  ( y N  yM )*0, 25t  200  50t
z  zM  ( z N  zM ) *0, 25t  200  87,5t
Từ tọa độ điểm E ta có :
 d3 .sin(q1 )  q4 .cos(q1 ).sin(q 3 ) 
rE   d3 .cos(q1 )  q4 .sin(q1 ).sin(q 3 ) 
 q2  q4 .cos(q3 ) 
Bài toán ta có 4 khớp tự do, tuy nhiên phương trình chuyển động điểm E có 4 biến
thế nên ta giả sử giá trị q3=pi/2. Từ đó ta giải hệ phương trình
xE  d3 .sin(q1 )  q4 .cos(q1 )
yE   d3 .cos(q1 )  q4 .sin(q1 )
z E  q2
Suy ra :

xE d3
q1  arccos( )  arcsin( ) ; q4  xE 2  yE 2  d32
x y
2
E
2
E x  yE2
2
E

q2  z E
Điểm E thuộc đường thẳng MN nên ta có :
12.5t  300 50
q1  arccos( )  arcsin( );
2656.25t 2  130000 2656.25t 2  130000
q4  2656.25t 2  127500
q2  87.5t  200
1.4.2. Chương trình matlab (đính kèm file.m)
1.4.3. Kết quả
Đồ thị các giá trị biến khớp lần lượt là q1,q2,q4

7
 0

radian
-1
-2

0 0.5 1 1.5 2 2.5 3 3.5 4

Thoi gian
600
mm

400

200
0 0.5 1 1.5 2 2.5 3 3.5 4
Thoi gian
500
mm

-500
0 0.5 1 1.5 2 2.5 3 3.5 4
Thoi gian

Đồ thị quỹ đạo của các điểm O,O’,E

Quy dao cac diem

Diem O
Diem O2
Diem E
600

500

400
Truc z

300

200

100

0
200
150 400
100 300
50 200
0 100
Truc y 0 Truc x

Vận tốc các điểm O, O’, E

8
 Do thi van toc cac diem
100
Diem O
90 Diem O2
Diem E
80

70

60
v(mm/s)

50

40

30

20

10

0
0 0.5 1 1.5 2 2.5 3 3.5 4
t(s)
Gia tốc các điểm O,O’,E:
Gia toc diem cac diem
12
Diem O
Diem O2
10 Diem E

8
v(mm/s 2 )

0
0 0.5 1 1.5 2 2.5 3 3.5 4
t(s)

2. Động lực học

9
2.1. Mô hình động lực học

2.2. Tính các thông số động lực học

Khâu Tọa độ khối Khối Ma trận momen quán tính

tâm địa lượng I (kg.mm2)
phương
xc yc zc Ixx Iyy Izz Ixy Iyz Izx
1 0 0 lC1 29.5 2035378 2035378 28367 0 0 0
2 0 0 0 0.7 1166 903 829 0 0 0
3 0 0 0 0.6 477 383 333 0 0 0
4 0 0 -lC4 7.5 226477 226477 2004 0 0 0

2.3. Phương pháp Newton – Euler

2.3.1. Tách các khâu, biểu diễn lực

Khâu 1 Khâu 2

10
 Khâu 3

Khâu 4

2.3.2. Viết phương trình cho các khâu

Khâu 1:
 0   X 0  X 21 
f1  f  f   0    Y0  Y21 
a
1
c
1

 m1.g   Z 0 

 0   Y0 .lC1  Y21.(q 2  lC1 ) 

M1  M1a  M1c   0     X 0 .lC1  X 21.(q 2  lC1 ) 
 1   0 
Khâu 2 :
 0   X 12  X 32 
f 2  f 2a  f 2c   0    Y Y 
  12 32 
 m2 .g  F1   0 

11
  
0  0 
 
M 2  M a2  M c2   0    0 
 
 0   d3 
  X 32 . 
 2

Khâu 3:
 0   X 23 
f 3  f  f   0   Y23  Y43 
a
3
c
3

 m3 .g    Z 43 

 0 
0  
  d
M 3  M 3  M 3   2    X 23 . 
a c 3

 2
 0   
 0 

Khâu 4 :
 0   F2 
f4  f4a  f4c   0    Y34 
 m4 .g   Z 34 

0  0 
M 4  M 4  M 4   0    Z 34 .(q 4  lC 4 ) 
a c   
 0   Y34 .(q 4  lC 4 ) 
2.3.3. Hệ phương trình N-E dạng chưa tối giản
Từ kết quả động học ta có:
Các ma trận chuyển trong hệ tọa độ toàn cục là:
 cos(q 1 )  sin(q 1 ) 0 0
 sin(q ) cos(q ) 0 0 
D0-1  1 1

 0 0 1 0
 
 0 0 0 1

cos(q 1 ) 0 sin(q 1 ) 0
 sin(q ) 0  cos(q 1 ) 0 
D0-2  1

 0 1 0 q2 
 
 0 0 0 1

12
  cos(q 1 )*cos(q 3 ) sin(q 1 ) cos(q 1 )*sin(q 3 ) d3 *sin( q1 ) 
 sin(q ) *cos(q )  cos(q ) sin(q ) *sin(q ) d *cos(q ) 
D0-3  1 3 1 1 3 3 1 

 sin(q 3 ) 0  cos(q3 ) q2 
 
 0 0 0 1 

 cos(q 1 )*cos(q 3 ) sin(q 1 ) cos(q 1 )*sin(q 3 ) d 3*sin(q1 )  q 4 *cos(q 1 )*sin(q 3 ) 

 sin(q ) *cos(q )  cos(q ) sin(q )*sin(q )  d *cos(q )  q *sin(q )*sin(q ) 
D0-4  1 3 1 1 3 3 1 4 1 3 

 sin(q 3 ) 0  cos(q3 ) q2  q4 *cos( q3 ) 

 
 0 0 0 1 
Tọa độ khối tâm của các khâu gắn với hệ quy chiếu gắn liền với khâu có dạng:
0 0 0  0 
       
uC1   0  uC2  0  uC3  0  uC4   0 

 1  l  
 1  0  
 1  0  
 1   l 
  C1       C4
       
1 1  1   1 
Tọa độ trọng tâm các khâu trong hệ quy chiếu cố định tính theo công thức sau :
rCi  uCi 
 1   D0i .  1 
   
Các tọa độ khối tâm trong hệ tọa độ R0
0 0   d3 .sin(q1 ) 
     
rC1  uC1   0  rC 2  uC 2   0  rC3  uC3   d3 .cos(q1 ) 
 1   D01.  1   l   1   D02 .  1    q   1   D03 .  1    q2 
    C1     2    
     
1 1  1 

 d3 .sin(q1 )  lC 4 .cos(q1 ).sin(q 3 )  q 4 .cos(q1 ).sin(q 3 ) 

 
rC 4  uC 4   q 4 .sin(q1 ).sin(q 3 )  lC 4 .sin(q1 ).sin(q 3 )  d3.cos(q1 ) 
1  D .
04   
   1   q 2  lC 4 .cos(q 3 )  q 4 .cos(q 3 ) 
 
 1 
Ma trận Jacobi tịnh tiến và đạo hàm Jacobi tịnh tiến của trọng tâm các khâu có
dạng :
0 0 0 0 0 0 0 0 
r1  J
JT 1    0 0 0 0  J T 1  T 1  0 0 0 0 
q q
 0 0 0 0  0 0 0 0 

0 0 0 0  0 0 0 0 
r2  J T 2 
JT 2   0 0 0 0  J T 2   0 0 0 0 
q q
0 1 0 0  0 0 0 0 

 d 3 .cos(q1 ) 0 0 0   d3 .q 1.sin(q1 ) 0 0 0 

r3   JT 3 
JT 3    d3 .sin(q1 ) 0 0 0  JT 3    d 3 .q1.cos(q1 ) 0 0 0 
q q
 0 1 0 0   0 0 0 0 

13
  d3 .cos(q1 )  lC 4 .sin(q1 ).sin(q 3 )  q 4 .sin(q1 ).sin(q 3 ) 0 (q 4  lC 4 ).cos(q1 ).cos(q3 ) cos(q1 ).sin(q3 ) 
r4 
JT 4    d3 .sin(q1 )  lC 4 .cos(q1 ).sin(q 3 )  q 4 .cos(q1 ).sin(q 3 ) 0 (q 4  lC 4 ).cos(q3 ).sin(q1 ) sin(q1 ).sin(q 3 ) 
q
 0 1 (q 4  lC 4 ).sin(q 3 )  cos(q 3 ) 
    

 J11 J12 J13 J14 
J      
J T 4  T 4   J 21 J 22 J 23 J 24 
q      
 J 31 J 32 J 33 J 34 
 
Trong đó :

J11  dq3 .(lC 4 .cos(q 3 ).sin(q1 )  q 4 .cos(q 3 ).sin(q1 )  dq1.( d3 .sin(q1 )  lC 4 .cos(q1 ).sin(q3 ))  dq4 .sin(q1 ).sin(q3 )

J 21  dq1.(d3 .cos(q1 )  lC 4 .sin(q1 ).sin(q 3 )  q 4 .sin(q1 ).sin(q 3 ))  dq3.(lC 4 .cos(q1 ).cos(q3 )  q 4 .cos(q1 ).cos(q 3 )  dq4 .cos(q1

J13  dq4 .cos(q1 ).cos(q 3 )  dq1.cos(q 3 ).sin(q1 ).(lC 4  q 4 )  dq3 .cos(q1 ).sin(q3 ).(lC 4  q 4 )

J 23  dq4 .sin(q1 ).cos(q 3 )  dq1.sin(q1 ).cos(q 3 ).(lC 4  q 4 )  dq3.sin(q1 ).sin(q 3 ).(lC 4  q 4 )

J 33  dq4 .sin(q 3 )  dq3 .cos(q 3 ).(lC 4  q 4 )

J14  dq3 .cos(q1 ).cos(q 3 )  dq1.sin(q1 ).sin(q3 )

J 24  dq1.cos(q1 ).sin(q 3 )  dq3 .sin(q1 ).cos(q 3 )

J 34  dq3 .sin(q3 )
   
J 31  J12  J 22  J 32  0
Toán tử sóng vecto vận tốc góc của khâu :
ῶi = ῶi0  A 0i .A 0iT
Từ đó ta xác định được vận tốc góc các khâu
0 0  q 3 .sin(q1 )   q 3 .sin(q1 ) 
ω1   0  ω 2   0  ω3   q 3 .cos(q1 )  ω 4    q 3 .cos(q1 ) 
 q1   q1   q1   q1 
Từ vận tốc góc các khâu ta xác định được ma trận Jacobi quay:
0 0 0 0  0 0 0 0 
ω1 J  J R1  0 0 0 0 
J R1   0 0 0 0  R1  
 q 1 0 0 0   q 0 0 0 0 
   

0 0 0 0  0 0 0 0
ω 2 J R 2
J R2   0 0 0 0  J R 2    0 0 0 0 
 q 1 0 0 0   q 0 0 0 0
 

0 0 sin(q1 ) 0  0 0 q1.cos(q1 ) 0 
ω3  J R 3
J R3   0 0  cos(q1 ) 0  J R 3   0 0 q1.sin(q1 ) 0 
q  q 0 0
1 0 0 0   0 0 

0 0 sin(q1 ) 0   0 0 q1.cos(q1 ) 0 
ω 4  J  J R 4   0 0 q .sin(q ) 0 
J R4   0 0  cos(q1 ) 0  R4  1 1 
q
  q 0 0
1 0 0 0   0 0 

14
Phương trình Newton – Euler của robot có dạng :
a c
M (q, t) q k (q, q , t)  Q  Q

Trong đó :
 m1.J T 1 
 m .J   f1a   f1c 
 2 T2  f  f 
 m3 .J T 3   2a   2c 
   f3a   f3c 
 m4 .BT 4     
  a
 f4a  c
 f4c 
M (q, t)   I C1. J R1  Q  Q 
 M1a   M1c 
     
 I C 2 .J R 2  M 2 a   M 2c 
  M  M 
 I C 3 .J R 3   3a   3c 
  M 4 a   M 4c 
 I .J 
 C 4 R4 
   
 m1. J T 1 . q1 
 
   
 m2 . J T 2 . q 2 
 
   
 m3 . J T 3 . q 3 
 
 

 m4 . J T 4 . q 4 
k (q, q, t)   
  

 I . J .q  ω  .I .ω 
 C1 R1 1 1 C1 1 
   
IC 2 . J R 2 . q 2  ω 2 .I C 2 .ω 2 
 
   
 IC 3 . J R3 . q3  ω  3 .I C 3 .ω3 
 
  

IC 4 . J R 4 . q 4  ω 4 .I C 4 .ω 4 

Ma trận tenxo quán tính của từng khối tâm đối với hệ tọa độ cục bộ tương ứng :
 1 I xx1 0 0   2 I xx 2 0 0 
   
1
I C1   0 1
I yy1 0  2
IC 2  0 2
I yy 2 0 
 0 0 1
I zz1   0 0 2
I zz 2 
 
 3 I xx 3 0 0   4 I xx 4 0 0 
   
3
IC 3  0 3
I yy 3 0  4
IC 4  0 4
I yy 4 0 
 0 0 3
I zz 3   0 0 4
I zz 4 
 
Ma trận tenxo quán tính của từng khối tâm đối với hệ tọa độ toàn cục được tính :
0
I Ci  A 0i .i I Ci .AT0i  mi .A 0i .(rCi .rCiT ).AT0i
Thế các biểu thức ta tính được :
Ma trận khối lượng M có dạng :

15
  M 11 M 12 M 13 M 14 
M M 22 M 23 M 24 
M   21
 ... ... ... ... 
 
 M 241 M 242 M 243 M 244 
Với :
M 62  m2 ; ; M 71  m3 .d3 .cos(q1 ); M 81  m3 .d3 .sin(q1 )
M 92  m3
M 101  m4 .(d3 .cos(q1 )  lC 4 .sin(q1 ).sin(q 3 )  q 4 .sin(q1 ).sin(q 3 ))
M 111  m4 .(d3 .sin(q1 )  lC 4 .cos(q1 ).sin(q 3 )  q 4 .cos(q1 ).sin(q 3 ))
M 103  m4 .((q 4  lC 4 ).cos(q1 ).cos(q 3 ))
M 104  m4 .(cos(q1 ).sin(q 3 ))

M 113  m4 .((q 4  lC 4 ).cos(q 3 ).sin(q1 ))

M 114  m4 .(sin(q1 ).sin(q 3 ))
M 122  m4
M 123   m4 .((q 4  lC 4 ).sin(q 3 )); M 124  m4 .( cos(q 3 ))
M 151  I zz1 ; M 181  I zz 2
M 211  I zz 3 ; M 193  I xx 3 .sin(q1 )
M 203   I yy 3 .cos(q1 ); M 223  I xx 4 .sin(q1 )
M 233   I yy 4 .cos(q1 ); M 241  I zz 4
Các phần tử M ij khác có giá trị bằng 0.
 k11 

k 
Ma trận : k (q, q , t)   21 
 ... 
 
 k241 
Trong đó :

16
k71  d3 .dq12 .m3 .sin(q1 )
k81  d 3 .dq12 .m3 .cos(q1 )
k101  dq3 .m4 .(dq4 .c1 .c3  (dq1.c3 .s1  dq3 .c1 .s 3 .).(lC 4  q 4 ))
 dq1.m4 .(dq1.(d3 .s1  lC 4 .c1.s3  q 4 .c1.s3 )  dq3 .(lC 4 .c3 .s1 )  q 4 .c3 .s1  dq4 .s1.s3 )
 dq4 .m4 .(dq3 .c1.c3  dq1.s1.s3 ))
   
k111  q1 .m4 .(q1 .(d3 .c1  lC 4 .s1.s3  q 4 .s1.s3  q3 .(lC 4 .c1.c3  q 4 .c1.c3  q4 .c1.s3 ) 
      
q3 .m4 .(q4 .c3 .s1  (q1 .c1.c3  q3 .s1.s3 ).(lC 4  q 4 ))  q4 .m4 .(q1 .c1.s3  q3 .c3 .s1 ))
    
k121  q3 .m4 .(q4 .s3  q3 .c3 .(lC 4  q 4 )  q3 .q4 .m4 .s3
     
k191  I yy1. q3 .c1.(q1 .c12  c3 .s1.(q1 .c3 .s1  q3 .c1.s3 )  s1.s3 .(q3 .c1.c3  q1 .s1.s3 ))
      
 I zz1. q1 .c3 .(q3 .c1.c3  q1 .s1.s3 )  s3 .(q1 .c3 .s1  q3 .c1.s3 ))  I xx3 .q3 . q1. c1 
      
I xx1 .q3 .s1.(c1.s3 .(q3 .c1.c3  q1. s1.s3  c1.c3 .(q1. c3 .s1  q3 .c1.s1  q3 .c1.s3 )  q1. c1.s1 )
     
k201  I xx1. q3 .s1 .(q1 .s12  c3 .c1 .(q1 .c3 .c1  q3 .s1 .s3 )  c1.s3 .( q1 .c1.s 3  q3 .s1.c3 ))

      
 I zz1. q1 .c3 .(q1 .c1.s3  q3 .s1.c3 )  s3 .(q1 .c1.c3  q3 .s1 .s3 ))  I yy 3 .q3 . q1. s1
     
 I yy1 .q3 .c1 .(c3 .s1.(q1 .c1.c3  q3 . s1.s3  q1 .c1.s1  s1.s3 .(q1 .c1.s3  q3 .c3 .s1 ))
     
k211  I xx1. q3 . s1.(q3 .c1 .c32  q3 .c1 .s32 )  I yy1. q3 .c1 .(q3 . s1.c32  q3 .s1 .s32 )
     
k221  I yy1. q3 .c1.(q1 .c12  c3 .s1.(q1 .c3 .s1  q3 .c1.s3 )  s1.s3 .(q3 .c1.c3  q1 .s1.s3 ))
      
 I zz1. q1 .c3 .(q3 .c1.c3  q1 .s1.s3 )  s3 .(q1 .c3 .s1  q3 .c1.s3 ))  I xx3 .q3 . q1. c1 
      
I xx1 .q3 .s1.(c1.s3 .(q3 .c1.c3  q1. s1.s3  c1.c3 .( q1. c3 .s1  q3 .c1.s1  q3 .c1.s3 )  q1. c1.s1 )
     
k231  I xx1. q3 .s1 .(q1 .s12  c3 .c1 .(q1 .c3 .c1  q3 .s1 .s3 )  c1.s3 .( q1 .c1.s 3  q3 .s1.c3 ))
      
 I zz1. q1 .c3 .(q1 .c1.s3  q3 .s1.c3 )  s3 .(q1 .c1.c3  q3 .s1 .s3 ))  I yy 3 .q3 . q1. s1
     
 I yy1 .q3 .c1 .(c3 .s1.(q1 .c1.c3  q3 . s1.s3  q1 .c1.s1  s1.s3 .(q1 .c1.s3  q3 .c3 .s1 ))
     
k241  I xx1. q3 . s1.(q3 .c1 .c32  q3 .c1 .s32 )  I yy1. q3 .c1 .(q3 . s1.c32  q3 .s1 .s32 )
Các phần tử kij khác có giá trị bằng 0.
a c
Lực vectơ suy rộng Q và Q

17
  X 0  X 21 
 0   Y0  Y21 
   
 0   Z0 
 m1.g   
 X 12  X 32 
 
 0   Y12  Y32 
 0   
   0 
  m2 .g  F1   X 23 
   
0  Y23  Y43 
 
 0   Y23  Y43 
  m .g   
 3    Z 43 
 0   
 F2 
 
 0   Y34 
 m4 .g   
a
Q 
c
Q  Z 34 

 0   Y .l  Y .(q  l ) 
 0   0 C1 21 2 C1 
    X 0 .lC1  X 21.(q 2  lC1 ) 
 1   0 
   
0  0 
 
 0   
 0 
 
 0   d3 
    X 32 . 
0 2
   
 2   0 
 0   d3 
   X 23 . 
 0   2 
   0 
0  
 
 Z 43 .(q4  lC 4 ) 
 0 
 Y43 .(q4  lC 4 ) 
 

2.3.4. Hệ phương trình vi phân dạng tối giản

Ta tiến hành khử các phản lực liên kết lý tưởng để đưa phương trình vi phân về
dạng tối giản bằng cách nhân với chuyển vị ma trận Jacobi tổng hợp:
T
J (q, t )  [J TT 1 J TT 2 J TT 3 J TT 4 J TR1 J TR 2 J TR 3 J TR 4 ]
Ta được:
 m11 m12 m13 m14   q1   k11   11 
m m24   q2   k21   21 
 21 m22 m23
.  
 m31 m32 m33 m34   q3   k31   31 
       
 m41 m42 m43 m44   q4   k41   41 

18
m11  I zz1  I zz 2  I zz 3  I zz 4  d32 .m3  d32 .m4  lC 4 2 .m4 / 2
 m4 .q 4 2 / 2  lC 4 .m4 .q 4  lC 4 2 .m4 .cos(2 q 3 ) / 2  lC 4 .m4 .q 4 .cos(2 q 3 )
m13   d3 .m4 .c3 .(lC 4  q 4 ); m13  d3 .m4 .s3
m22  m2  m3  m4 ; m23   m4 .s3 .(lC 4  q 4 ); m24  m4 .c3
m31  d3 .m4 .c3 .(lC 4  q 4 ); m32   m4 .s3 .(lC 4  q 4 )
2 2 2 2
m33  I yy 3  I yy 4  lC 4 2 .m4  m4 .q 4 2  I xx 3 .s1  I xx 4 .s1  I yy 3 .s1  I yy 4 .s1  2.lC 4 .m4 .q 4
m41  d3 .m4 .s3 ; m42  m4 .c3 ; m44  m4

k11  Ixx1*dq3^2*sin(2*q1) - Iyy1*dq3^2*sin(2*q1)

- dq1*dq4*lc4*m4 + dq1*dq4*m4*q4 + dq1*dq4*lc4*m4*cos(2*q3)
- dq1*dq4*m4*q4*cos(2*q3) + d3*dq3^2*lc4*m4*sin(q3)
- d3*dq3^2*m4*q4*sin(q3) + dq1*dq3*lc4^2*m4*sin(2*q3)
+ dq1*dq3*m4*q4^2*sin(2*q3) + 2*d3*dq3*dq4*m4*cos(q3)
- 2*dq1*dq3*lc4*m4*q4*sin(2*q3)
k21  dq3*m4*(dq4*sin(q3) - dq3*cos(q3)*(lc4 - q4)) + dq3*dq4*m4*sin(q3)
k31  2*dq3*dq4*m4*q4 - 2*dq3*dq4*lc4*m4 - (dq1^2*lc4^2*m4*sin(2*q3))/2
- (dq1^2*m4*q4^2*sin(2*q3))/2 - Ixx1*dq1*dq3*sin(2*q1)
+ (Ixx3*dq1*dq3*sin(2*q1))/2 + (Ixx4*dq1*dq3*sin(2*q1))/2
+ Iyy1*dq1*dq3*sin(2*q1) - (Iyy3*dq1*dq3*sin(2*q1))/2
- (Iyy4*dq1*dq3*sin(2*q1))/2 + dq1^2*lc4*m4*q4*sin(2*q3)
k41  m4*(dq1^2*sin(q3)^2 + dq3^2)*(lc4 - q4)

 11  0
 21  - g*m2 - g*m3 - g*m4
 31  g*m4*sin(q3)*(lc4 - q4)
 41  g*m4*cos(q3)
2.4. Phương pháp Lagrange II
2.4.1. Tính động năng
- Thay các Ma trận Jacobi tịnh tiến và Jacobi quay vào biểu thức:

4
M   (mi .JTTi .J Ti  J TRi .A 0i .0 I i .AT0i .J Ri )
i1

Ma trận tenxo quán tính có dạng :

 I xx1 0 0   I xx 2 0 0 
I C1   0 I yy1 0  IC 2   0 I yy 2 0 
 0 0 I zz1   0 0 I zz 2 

 I xx 3 0 0   I xx 4 0 0 
IC 3   0 I yy 3 0  IC 4   0 I yy 4 0 
 0 0 I zz 3   0 0 I zz 4 
19
  Ma trận khối lượng suy rộng M của Robot là:

 m11 m12 m13 m14 

m m22 m23 m24 
M   21
 m31 m32 m33 m34 
 
 m41 m42 m43 m44 

Trong đó :
 2  2
m11  I xx 3 / 2  I xx 4 / 2  I zz1  I zz 3 / 2  I zz 4 / 2  m3 . q3  m4 . q3
 (lC 4 2 .m4 ) / 2  (m4 .q 4 2 ) / 2  ( I xx 3 .cos(2.q 3 )) / 2  ( I xx 4 .cos(2.q 3 )) / 2
 ( I zz 3 .cos(2.q 3 )) / 2  ( I zz 4 .cos(2.q 3 )) / 2  lC 4 .m4 .q 4  lC 4 2 .m4 .q 4
(lC 4 2 .m4 .cos(2.q 3 )) / 2  (q 4 2 .m4 .cos(2.q 3 )) / 2  lC 4 2 .m4 .cos(2.q 3 )
m31  d 3 .m4 .c3 .(lC 4  q 4 )
m41  d 3 .m4 .s3 ; m22  m2  m3  m4 ; m32   m4 .s3 .(lC 4  q 4 )
m42  m4 .c3 ; m13  d 3 .m4 .c3 .(lC 4  q 4 )
m23  m4 .s3 .(lC 4  q 4 )
m33  m4 .lC 4 2  2.m4 .lC 4 .q 4  m4 .q 4 2  I yy 3  I yy 4
m14  d3 .m4 .s3 ; m24  m4 .c3 ; m44  m4

- Động năng của hệ:

1
T  .q T .M.q
2
  2
 q1 .( q1 .( I xx 3 / 2  I xx 4 / 2  I yy 2  I zz1  I zz 3 / 2  I zz 4 / 2  d 32 .m3
 d32 .m4  lC 4 2 .m4 / 2  m4 .q 4 2 / 2  I xx 3 .cos(2 q 3 ) / 2  I xx 4 .cos(2 q 3 ) / 2
 I xx 3 .cos(2 q 3 ) / 2  I xx 4 .cos(2 q 3 ) / 2  lC 4 .m4 .q 4  lC 4 2.m4 cos(2 q 3 ) / 2
m4 .q 4 2 .cos(2 q 3 ) / 2  lC 4 .m4 .q 4 .cos(2 q 3 ))) / (2 dq1 ) 
(d3 .m4 .conj (dq3 ).c3 .(lC 4  q 4 ) / 2)  ( m4 .dq4 2  dq4 .conj (dq2 ).c3
 d3 .dq4 .conj (dq1 ).s3 )) / 2  dq2 .((m4 .conj (dq4 ).c3 / 2  (dq2 2 .(m2  m3  m4 ) / (2dq2 )
(m4 .conj ( dq3 ).s3 .(lC 4  q 4 ) / 2)  dq3 .(( m4 .conj (dq2 ).s3 .(lC 4  q 4 )) / 2
dq32 .(m4 .lC 4 2  2.lC 4 .m4 .q 4  m4 .q 4 2  I yy 3  I yy 4 )) / (2dq3 )
(d3 .m4 .conj (dq1 ).c3 .(lC 4  q 4 ) / 2)

Thế năng của hệ :

  m1 glC1  m2 gq2  m3 gq2  m4 g (q2  (q  lC 4 ) sin(q3 ))

Công ảo của các lực không thế

A   1q1  F2q2   3q3  F4 q4

2.4.2. Tính các lực suy rộng

20
 Các lực suy rộng ứng với các lực không thế :
Q1np   1 ; Q2np  F2 ; Q3np   3 ; Q4np  F4

2.4.3. Phương trình vi phân chuyển động

Thay các giá trị vào phương trình Lagrange loại II :
d T T 
( )( )  Qinp
dt q i qi qi

Ta nhận được hệ phương trình vi phân chuyển động của robot như sau
I xx 3 I xx 4 I I l 2 m q2m
 1  q1 (   I yy 2  I zz1  zz 3  zz 4  d32 m3  d 32 m4  C 4 4  4 4
2 2 2 2 2 2
I cos(2q3 ) I xx 4 cos(2q3 ) I zz 3 cos(2q3 ) I zz 4 cos(2q3 )
 xx 3     lC 4 m4 q4
2 2 2 2
l 2 m cos(2q3 ) m4 q42 cos(2q3 )
 C4 4   lC 4 m4 q4 cos(2q3 ))  q 3 ( q1m4 sin(2q3 )lC2 4
2 2
2qm  4 sin(2q3 )lC 4 q4  d3 q 3 sin(q3 )lC 4  q1m4 sin(2q3 )q42  d 3q 3m4 sin(q3 ) q4
 I xx 3q1 sin(2q3 )  I xx 4 q 1 sin(2q3 )  I zz 3q 1 sin(2q3 )  I zz 4 q 1 sin(2q3 )  d 3q4m4 cos( q3 ))
 q 4 m4 (2q 1q4  2q1lC 4  2q 1lC 4 cos(q3 ) 2  2q 1q4 cos(q3 ) 2  d3q 3 cos(q3 ))

F2  q2 m2  q2 m3  q2 m4  g  m2  m3  m4   2q 3q 4 m4 s3  q 32lC 4 m4c3  q 32m4q4c3

 3  g  m2  m3  m4   q(m4lC 4 2  2m4lC 4 q4  m4 q42  I yy 3  I yy 4 )

I xx 3 q12 sin(2q3 ) I xx 4 q12 sin(2q3 )
q 4 m4 (2q 3 q4  2q 3lC 4  q 2 s3  d 3q1c3 )  
2 2
2 2
I q sin(2q3 ) I zz 4 q1 sin(2q3 ) q l m sin(2q3 ) q12m4q42 sin(2q3 )
2 2
 zz 3 1   q 2 q 4 m4 s3  1 C 4 4 
2 2 2 2
q 3m4 (q 2c3  d 3q1s3 )(lC 4  q4 )  q1 lC 4 m4q4 sin(2q3 )  d 3q 1q 4 m4c3  q 2q 3lC 4m4c3
2

q 2 q 3 m4 q4 c3  d 3q1q 3lC 4 m4 s3  d 3 q1q 3m4q4 s3

F2  q4 m4  m4 (q12 q4  q 32lC 4  q12lC 4  q 32q4  q 2 q 3s3  q 12lC 4c32  q12q4c32  d 3q1q 3c3 )
 gm4 (q 2 s3  d3q1c3 )
2.5. Bài toán động học thuận
2.5.1. Thuật toán
2.5.2. Chương trình matlab (Kèm theo file.m)
2.5.3. Kết quả
2.6. Bài toán động học ngược
2.6.1. Thuật toán
2.6.2. Chương trình matlab (Kèm theo file.m)

21