tính toán động học và mô phỏng robot motoman

MỤC LỤC
Trang 
2 
3 
 
4 
4 
4 
6 
 
7 
7 
10 
14 
 
21 
21 
21 
21 
25 
26 
 
32 
32 
34 
35 
35 
37 
39 

Yêu cầu của đồ án

Lời nói đầu
Chương I: Tổng quan về Robot Motoman

1. Lịch sử phát triển sơ lược  
2. Robot Motoman 
3. Các thông số kỹ thuật của Robot Motoman 
 
Chương II: Các bài toán động học cho Robot
1. Bài toán động học thuận  
2. Bài toán động học ngược 
3. Ma trận Jacobien cho Robot 
Chương III: Bài toán động lực học Robot
1. Hàm Lagrange và các vấn đề về động lực học Robot 
2. Tính toán các giá trị động lực học cho Robot Motoman 
a. Động năng 
b. Thế năng 
c. Xây dựng phương trình động lực học Robot 
 
Chương V: Xây dựng các thuật toán điều khiển và mô phỏng
1. Thiết kế bộ điều khiển theo phương pháp PD bù trọng trường  
2. Thiết kế quỹ đạo bậc ba cho Robot  
3. Mô phỏng ba khớp đầu Robot 
a. Phương pháp điều khiển PD bù trọng trường trên Simulink 
b. Phương pháp điều khiển PD bù trọng trường có kèm thiết kế quỹ đạo 
c. Xây dựng mô phỏng bằng Toolbox SimMechanics  
 
Tài liệu tham khảo

 
44 

 
1


YÊU CẦU ĐỐI VỚI ĐỒ ÁN
- Tìm hiểu các thông tin về Robot Motoman. 
- Tính toán mô hình động học Robot gồm động học thuận cho các khớp và động 
học ngược cho ba khớp đầu 
- Giải bài toán động lực học cho ba khớp đầu Robot 
- Giải bài toán động học vận tốc (tính toán ma trận Jacoby) 
- Xây dựng luật điều khiển và thiết kế quỹ đạo cho Robot 
- Mô phỏng kết quả tính toán được cho ba khớp đầu bằng phần mềm Matlab 

2


LỜI NÓI ĐẦU
Ngày nay trong sự nghiệp công nghiệp hoá, hiện đại hoá đất nước vấn đề tự động hoá 
sản  xuất  có  vai  trò  đặc  biệt  quan  trọng.Vì  vậy  trong  những  năm  gần  đây  các  lĩnh  vực 
thuộc tự động hóa ngày càng được quan tâm nghiên cứu và phát triển, trong đó không thể 
không nói đến những ứng dụng quan trọng của kỹ thuật Robot trong công nghiệp. 
Mục  tiêu  ứng  dụng  kỹ  thuật  Robot  trong  công  nghiệp  nhằm  nâng  cao  năng  suất  dây 
chuyền công nghệ, nâng cao chất lượng và khả năng cạnh tranh của sản phầm, đồng thời 
cải thiện điều kiện lao đông. Sự cạnh tranh hàng hóa đặt ra một vấn đề thời sự là làm sao 
để hệ thống tự động hóa sản xuất phải có tính linh hoạt nhắm đáp ứng với sự biến động 
thường xuyên của thị trường hàng hóa. Robot công nghiệp là bộ phận cấu thành không 
thể thiếu trong hệ thống sản xuất tự động linh hoạt đó. 
Ở nước ta, từ những năm 1990 trở lại đây, Robot và kỹ thuật Robot đã được ứng dụng 
vào  sản  xuất  khá  rộng  rãi.  Trong  những  Robot  được  ứng  dụng  vào  sản  xuất  thì  Robot 
Motoman là một Robot đóng góp phần đáng kể trong lĩnh vực tự động hóa công nghiệp ở 

nước  ta.  Đây  là  một  Robot  được  ứng  dụng  khá  đa  dạng  trong  nhiều  lĩnh  vực  :  hàn  tự 
động, các dây chuyền sản xuất công nghiệp tự động, phun sơn... 
Nhận  thấy  tầm  quan  trọng  của  kỹ  thuật  Robot  nói  chung  và  ứng  dụng  của  Robot 
Motoman  nói  riêng,  với  kiến  thức  học  hỏi  dược  trong  quá  trình  học  tập  tại  bộ  môn  tự 
động hóa và sự hướng dẫn nhiệt tình của cô  giáo T.S Nguyễn Phạm Thục Anh nhóm 
sinh viên chúng em đã chọn đề tài đồ án chuyên nghành là “ROBOT MOTOMAN” với 
mục  đích  tìm  hiểu,  xây  dựng,  mô  phỏng  mô  hình  điều  khiển  robot  nhằm  ứng  dụng  lý 
thuyết vào thực tế. Dù được sự hướng dẫn tận tình của cô giáo nhưng do quá trình tích 
lũy kiến thức và kinh nghiệm còn hạn chế nên đồ án không thể tránh khỏi những thiếu 
sót. Chúng em mong các thầy cô góp ý để đồ án của chúng em được hoàn thiện hơn. 
Chúng em xin chân thành cảm ơn! 
Sinh viên thực hiện 
  Lương Đình Ngọc 
  Trần  Thành  Kiên 

3


CHƯƠNG I: TỔNG QUAN VỀ ROBOT MOTOMAN
1. Lịch sử phát triển sơ lược
Kể  từ  khi  Robot  đầu  tiên  được  chế  tạo  với  sản  phẩm  đầu  tiên  có  tên  gọi  người  máy 
công nghiệp là verstran của công ty Mỹ vào năm 1960 các sản phẩm liên quan đến tay 
máy  công  nghiệp  bắt  đầu  được  quan  tâm  nghiên  và  nhờ  đó  kỹ  thuật  Robot  bắt  đầu  có 
những bước phát triển đầu tiên. 
Cùng theo bản quyền của Mỹ các nước trên thế giới đă chạy đua sản xuất robot công 
nghiệp,các công ty về Robot được thành lập để nghiên cứu và phát triển ,ứng dụng Robot 
trong tự động hoá công nghiệp. 
Yaskawa Motoman là một công ty chuyên cung cấp các giải pháp tự động hóa sáng tạo 
cho hầu như tất cả các ngành công nghiệp và ứng dụng robot như: hàn, lắp ráp, sơn, pha 
chế,  cắt  gọt  vật  liệu…  với  hơn  175  mô  hình  robot  khác  biệt  và  hơn  40  giải  pháp  công 

nghệ để giải quyết các công việc một cách hoàn chỉnh như các hệ thống vận hành, thiết bị 
an toàn. 
Yaskawa Motoman là công ty đi đầu trong lĩnh vực phát triển các Robot phục vụ cho 
các công nghệ gia công và tự động, Robot Motoman chính là sản phẩm nghiên cứu sản 
xuất thành công của hãng. 
 

2. Robot Motoman
a. Giới thiệu chung
Robot Motoman là Robot tác động nhanh, linh hoạt, nhỏ gọn và đáng tin cậy. Đây là 
một loại Robot hoạt động tốt với nhiều cài đặt. Nó cũng cung cấp rất nhiều ứng dụng, với 
hiệu suất sử dụng cao, đảm bảo những yêu cầu về chất lượng, thời gian hoàn vốn ngắn. 
Motoman được thiết kế cứng và thẳng, điều này dẫn đến độ ồn làm việc thấp, thời gian 
bảo trì lâu. Ngoài ra nó còn được thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất hoạt động cao 
ngay cả trong những vị trí khó. 
b. Các ứng dụng
     Robot Motoman được ứng dụng rộng rãi vào các dây chuyền sản xuất tự động, hiện 
nay các lĩnh vực phổ biến nhất là: 
- Các quá trình hàn và nhiệt luyện. 
- Công nghệ gia công lắp ráp. 
- Phun sơn,vận chuyển hàng hoá. 
... 

4


c. Phân loại sản phẩm.
Với dòng sản phẩm ban đầu,qua nhiều năm phát triển đã có nhiều mẫu Motoman được 
sản xuất dựa trên cùng một mô hình động học hệ thống nhưng có cải tiến và khác nhau về 
cơ cấu cơ điện tử và giới hạn làm việc các khâu. 

Các  dòng  sản  phẩm  thuộc  Motoman  trên  thị  trường  hiện  nay  phổ  biến  là  các  mẫu 
series Motoman HP , Motoman UP, Motoman SV. 
 

 
Motoman UP

Motoman HP20

Motoman SV3

5


3. Thông số kỹ thuật
- Số bậc tự do: 6
- Kiểu khớp: quay
Mô hình Robot:

 

Không gian làm việc của Robot:

 
6


CHƯƠNG II: CÁC BÀI TOÁN ĐỘNG HỌC ROBOT
A. BÀI TOÁN ĐỘNG HỌC THUẬN ROBOT MOTOMAN
Áp dụng phương pháp Denavit – Hartenberg cho mô hình toán học của Robot Motoman, 

ta đặt các hệ trục  như sau:  
 

 
 
Từ mô hình của Robot, ta xác định được bảng thông số D–H như sau : 
i 
1 
2 
3 
4 
5 
6 

o

θ i ( ) 
θ1 
θ2+90 
θ3 
θ4 
θ5 
θ6 

 
αi(o) 
900 
00 
900 
-900 

900 
00 

ai(mm) 
a1  
a2  
a3  
0 
0 
0 

di(mm) 
d1 
0 
d3 
0 
d5 
d6 

 
7


Ma trận biến đổi thuần nhất   : 
−
−

=

0

0

 
0

0

1

Dựa vào bảng thông số D-H, thay các giá trị vào ma trận ta được:  

c1
s
A1  1
0

0

0 s1 a1c1 
0  c1 a1s1      
1 0
d1 

0 0
1 

 c3
s
A3  3
0


0

0 s3
0  c3
1 0
0 0

a 3 c3 
a 3s3 
d3 

1 

 c5
s
A5   5
0

0

0 s5
0  c5
1 0
0 0

0
0     
d5 


1

    

 s2
c
A2  2
 0

 0

 

   

 

 

 

 c2
s2
0
0

0  a 2s 2 
0 a 2c2   
1
0 


0
1 

c 4
  A  s4
4
0

0

0  s4
0 c4
1 0
0
0

 c6

  A s6
6
0

0

 s6
c6
0
0

0
0   
0

1

 

 

0 0
0 0   
1 d6 

0 1

   
Nhân các ma trận 

, 

, 

, 

, 

, 

 ta được ma trận biểu diễn vị trí và hướng của 

khâu tác động cuối của Robot như sau: 
 
=
       

=

 
. 

. 

. 

. 

. 

 

 
=

 
0

0

0

1

 
8


n x  c1[s 23 ( c 4c5c 6  s 4s 6 )  c 23s5s 6 ]  s1 (s 4c5c 6  c 4s 6 )  

n y  s1[s23 (c4c5c6  s4s6 )  c23s5s6 ]  c1 (s4c5c6  c4s6 )  
n z  c 23 (c 4c5c6  s 4s 6 )  s32c5c6  
 

o x  c1[s 23 (c 4 c5c 6  s 4 c 6 )  c 23s 5s 6 ]  s1 (s 4 c5s 6  c 4s 6 )  

oy  c1[c23 (c4c5c6  s4s6 )  s23s5s6 ]  c1 (s1c5s6  c4c6 )  
o z  c 23 (c 4c5s 6  s 4c 6 )  s 23s5s 6

 

 

a x  c1 ( s 23c 4s5  c 23c5 )  s1s 4s5  

a y  s1 (c23c4s5  s23s5 )  c1s4s5  
a z  c 23c 4 s 5  s 23c 5

 

 

p x  c1[ s 23 (c 4s5d 6  s 4d 5 )  c 23c5d 6  (s 2  c 2 )a 3s 3  a 2s 2 )  s1 (s 4s5 d 6  c 4 d 5  d 3 )  a1c1  

 

py  s1[c23 (c4s5d6  s4d5 )  s23c5d6  (c2  s2 )a 3s3  a 2s2 )  c1 (s4s5d6  c4d5  d3 )  a1s1  

 

p z  c 23 (c 4s5d 6  s 4d 5 )  s 23c5d 6  (c 2  s 2 )a 3s3  a 2s 2  d1  

 
Giao diện tính toán động học thuận thiết kế trên giao diện GUIDE Matlab:

 
Giao diện tính toán động học thuận 
9


B. BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT MOTOMAN
 
Bài toán với dữ kiện ban đầu là vị trí của khâu tác động cuối, nhiệm vụ đưa ra là tìm 
giá trị của các biến khớp để đảm bảo là khi chuyển động ứng với các biến khớp đó thì 
khâu tác động cuối của Robot sẽ chuyển động chính xác đến vị trí này. 
Trong  bài  toán  động  học  thuận,  vị  trí  và  hướng  tay  máy  được  xác  định  từ  các  biến 
khớp (góc quay ở khớp quay đã biết ) 
Để điều khiển Robot  di chuyển theo các vị trí mong muốn của tay máy trong không 
gian,  cần  xác  định  các  giá  trị  biến  khớp  tương  ứng  với  vị  trí  và  hướng  của  tay  Robot 
mong muốn. Đây là nội dung của bài toán động học ngược 
Do với robot motoman có 6 khớp quay tự do nhưng việc xác định vị trí trong không 

gian là bài toán của ba khớp đầu,còn 3 khớp cuối là khâu tác động tuỳ từng nhiệm vụ kỹ 
thuật nên trong đồ án này chúng em sẽ chỉ tính toán động học ngược và động lực học của 
ba khớp đầu để ứng dụng điều khiển vị trí của robot. 
Ta có phương trình động học thuận của Robot có dạng: 
 

(

=

 

).

(

).

(

).

(

).

(

=

).

(

)  

 

(*) 

 
0

0

0

1

Ma trận   đã biết, tức là vị trí và hướng của khung tọa độ tay Robot đã biết, cần xác 
định giá trị các biến khớp. Nhân hai vế của phương trinh (*) với ma trận nghịch đảo của 
 là ma trận ( )  nhận được phương trình sau: 
 

 

 

(

 

) .

=

=

  

 

         (1) 

Từ đó ta có: 

=

0
0

0
−
0

0
1
0
0

0
0
0
1

 
0

0

0

1

 

10


=

                               

( )
( )
( )
0

( )
( )

( )
0

( )
( )
( )
0

( )
( )
   
( )
1

 

(2) 

 
Trong đó: 
( )=

. +

.   

( ) =   
( )=

. +

.   

Với x, y, z là các thành phần của vecto v 
Ma trận 

 được tính theo phương trình (1): 
−
−

−
−

=

−
−

+
+
0
1

−
0

0

+
+

0

 (3) 

 
Cân  bằng  các  thành  phần  cột  4  của  hai  ma  trận  phương  trình  (2)  và  (3)  ta  nhận  được 
phương trình sau: 
( )=

.

+

.

+

.

  

 

 

 

 

 

 

(4a) 

( )=

.

+

.

+

.

 

 

 

 

 

 

 

(4b) 

 

 

 

 

 

 

(4c) 

 

 

 

 

 

(5a) 

 

 

 

 

 

(5b) 

( ) = 0   

 

 

Ta có: 

−

= 0 

Suy ra:

=

(

,

 

)

Kết hợp phương trình (1),(4a) và (6c) ta được: 
. +
−

.

=
=

.

+
+

.
 

+
 

.

  
 

 
11


Viết lại phương trình (5a) và (5b) như sau: 
. +

. −

−

.

=

=

.

+

+

 

.

 

 
 

 

 

 

 

 

(6a) 

 

 

 

 

 

(6b) 

 
Bình phương hai phương trình (6a) và (6b) và cộng lại ta có phương trình sau: 
(

+

−

) +(

−

) =(

) +(

) +2

(

+

) 

Sử dụng các hàm lượng giác, viết gọn lại phương trình trên như sau: 
= cos[(

+

                  

)−

+

]=

   

−

) − [(

 

 

(7) 

 
Do đó nhận được: 
=

(

+

−

) +(
2

) +(

) ]
 

 
Suy ra: 

=

(

,

)

Tiếp tục nhân ma trận nghịch đảo của 
(

)

=(

) (

−

 sẽ nhận được phương trình sau: 

) (

) (

) .

−
−

=
0

0
0

0

=

 

0
0
 
0
1

 
Cân bằng các phần tử ta có: 
−

+

+

= 0 

Giải phương trình trên, ta được : 
=

2( ,

+

) 

 
12


Ta xác định 

 các phương trình sau: 
+

=

+

=
Vì 

=

−

+

=

 và 
+

+

−

+

+(

=

 

 nên ta có: 
+(

=

 

)

−

+

)+

+

 
 

Suy ra: 
=

=
Vậy:

=

(

)(

+

)−

−

+

(
(

)

+

+
(

(

,

(

−

)

−

)

) +
−

+

+

+
) +

(

 

 

).

Giao diện tính toán động học ngược cho 3 khớp đầu:

Giao diện tính toán động học ngược
13


C. MA TRẬN JACOBIEN ROBOT MOTOMAN
 
1. Xác định ma trận HJ cho 6 khớp
Ta có 
nx6     = c6; 
ny6     = s6; 
nz6     = 0; 
 
ox6     = -s6; 

oy6     = c6; 
oz6     = 0; 
 
ax6     = 0; 
ay6     = 0; 
az6     = 1; 
 
px6     = 0; 
py6     = 0; 
pz6     = d6; 
 
J61=-nx6 *py6 + ny6 *px6 = 0 
J62= -ox6 *py6+oy6 *px6  = 0 
J63= -ax6 *py6 +ay6 *px6 = 0 
J64= nz6 = 0  
J65= oz6  =  0 
J66 = az6 =  1 
nx5     = c5*c6; 
ny5     = c6*s5;  
nz5     = s6;      
 
ox5     = -c5*s6;   
oy5     = -s5*s6;  
oz5     = c6;    
 
14


ax5     = s5;   
ay5     = -c5;  

az5     = 0; 
 
px5     = s5*d6; 
py5     = -c5*d6; 
pz5     = d5; 
 
J51=-nx5 *py5 + ny5 *px5  =-(c5*c6)*(-c5*d6)+( c6*s5)* (s5*d6) 
J52= -ox5 *py5+oy5 *px5 = -(-c5*s6)*( -c5*d6)+( -s5*s6)*( s5*d6) 
J53= -ax5 *py5 +ay5 *px5 = -( s5)*( -c5*d6)+( -c5)*( s5*d6) 
J54= nz5 = s6 
J55= oz5  = c6 
J56 = az5 = 0 
nx4     = c4*c5*c6-s4*s6; 
ny4     = c4*s6 + c5*c6*s4; 
nz4     = -c6*s5;        
        
ox4     = - c6*s4 - c4*c5*s6;  
oy4     = c4*c6 - c5*s4*s6; 
oz4     = s5*s6;     
 
ax4     = c4*s5;  
ay4     = s4*s5;  
az4     = c5;      
       
px4     = c4*s5*d6 - s4*d5; 
py4     = c4*d5 + s4*s5*d6; 
pz4     = c5*d6; 
J41=-nx4 *py4 + ny4 *px4   
= -( c4*c5*c6-s4*s6)*( c4*d5 + s4*s5*d6)+( c4*s6 + c5*c6*s4)*( c4*s5*d6 - s4*d5) 
J42= -ox4 *py4+oy4 *px4  

 = -(- c6*s4 - c4*c5*s6)*( c4*d5 + s4*s5*d6)+( c4*c6 - c5*s4*s6)*( c4*s5*d6 - s4*d5) 

J43= -ax4 *py4 +ay4 *px4  
15


= -( c4*s5)*( c4*d5 + s4*s5*d6)+( s4*s5)*( c4*s5*d6 - s4*d5) 
J44= nz4 = -c6*s5 
J45= oz4  =  s5*s6  
J46 = az4 = c5      
 
nx3     = - c3*(s4*s6 - c4*c5*c6) - c6*s3*s5; 
ny3     = c3*c6*s5 - s3*(s4*s6 - c4*c5*c6); 
nz3     = c4*s6 + c5*c6*s4;      
               
ox3     = s3*s5*s6 - c3*(c6*s4 + c4*c5*s6); 
oy3     = - s3*(c6*s4 + c4*c5*s6) - c3*s5*s6;  
oz3     = c4*c6 - c5*s4*s6; 
 
ax3     = c5*s3 + c3*c4*s5;  
ay3     = c4*s3*s5 - c3*c5;  
az3     = s4*s5;     
                 
 
px3     = c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6; 
py3     = s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6; 
pz3     = d3 + c4*d5 + s4*s5*d6; 
J31=-nx3 *py3 + ny3 *px3   
= -(- c3*(s4*s6 - c4*c5*c6) - c6*s3*s5)*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6) 
+( c3*c6*s5 - s3*(s4*s6 - c4*c5*c6))*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6) 

J32 = -ox3 *py3+oy3 *px23 
 = -( s3*s5*s6 - c3*(c6*s4 + c4*c5*s6))*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6) 

+(- s3*(c6*s4 + c4*c5*s6) - c3*s5*s6)*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6) 
J33 = -ax3 *py3 +ay3 *px3 
 = -( c5*s3 + c3*c4*s5)*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6) 

+( c4*s3*s5 - c3*c5)*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6) 
J34= nz3  
= c4*s6 + c5*c6*s4 
16


J35= oz3 
  = c4*c6 - c5*s4*s6 

J36 = az3  
= s4*s5 
 
n x2 =s 23 (-c 4 c5c6 +s 4s 6 )-c 23s 5c6 ;
n y2 =c23 (c 4 c5 c6 -s 4s 6 )+s 23s5c6 ;
n z2 =s 4 c5c6 +c4s 6 ;
o x2 =s 23 (c 4 c5s 6 +s 4c6 )-c 23s5s 6 ;
o y2 =-c 23 (c 4 c5s 6 +s 4c6 )+s 23s5s 6 ;
oz2 =-s 4c5s 6 +c 4c6 ;
a x2 =-s 23c 4s 5 +c23c5 ;
a y2 =c 23c 4s5 +s 23c5 ;
a z2 =s 4s5 ;
p x2 =-s 23 (c4s5d 6 -s 4d 5 )+c 23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 ;
p y2 =c23 (c 4s5d 6 -s 4 d5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 ;

p z2 =s 4s5d 6 +c 4d 5 +d3 ;

 

J 21   n x2 * p y2     n y2  * p x2  
  s 23 (-c 4 c5 c6 +s 4s 6 )-c 23s5c6  *  c 23 (c 4s 5d 6 -s 4 d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2   
  c 23 (c 4 c5c6 -s 4s 6 )+s 23s5 c6  *  -s 23 (c4s5 d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s 3 -a 2s 2 
J 22       o x2 * p y2  o y2   * p x2
   s 23 (c 4 c5s 6 +s 4 c6 )-c23s 5s 6  *  c 23 (c 4s5 d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2   
  -c 23 (c 4 c5s 6 +s 4 c6 )+s 23s5s 6  * (-s 23 (c 4s5 d 6 -s 4 d 5 )+c 23c5 d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )

J 23       a x2 * p y2    a y2   * p x2
 (-s 23c 4s5 +c23c5 ) *(c23 (c 4s5d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 )  
(c 23c 4s5 +s 23c5 )*(-s 23 (c 4s 5d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )

17


J 24   n z2
 s 4c5c6 +c4s6

 

J 25   oz2
 -s4c5s6 +c4c6

 

J 26     a z2
 s 4 s5

 

n x1  c1[s23 (c4c5c6  s4s6 )  c23s5s6 ]  s1 (s4c5c6  c4s6 )  
n y1  s1[s 23 ( c 4c5c 6  s 4s6 )  c 23s 5s 6 ]  c1 (s 4 c5c6  c 4s 6 )  
n z1  c23 (c4c5c6  s 4s6 )  s32c5c6  
 

o x1  c1[s 23 (c 4c5c6  s 4c6 )  c23s5s6 ]  s1 (s 4c5s6  c 4s6 )  
o y1  c1[  c 23 (c 4 c 5c 6  s 4s 6 )  s 23s 5s 6 ]  c1 (  s1c 5s 6  c 4 c 6 )  

oz1  c23 (c4c5s6  s 4c6 )  s23s5s6

 

 

a x1  c1 ( s 23c 4s 5  c 23c5 )  s1s 4s 5  
a y1  s1 (c 23 c 4 s 5  s 23s 5 )  c1s 4 s 5  

a z1  c 23 c 4 s 5  s 23 c 5

 

 

p x1  c1[ s 23 (c 4s5d 6  s 4d 5 )  c 23c5d 6  (s 2  c2 )a 3s3  a 2s 2 )  s1 (s 4s 5d 6  c 4d 5  d 3 )  a1c1  
p y1  s1[c 23 (c 4s 5d 6  s 4 d 5 )  s 23c5d 6  (c 2  s 2 )a 3s 3  a 2s 2 )  c1 (s 4s5d 6  c 4 d 5  d 3 )  a1s1  

 

 

pz1  c23 (c4s5d6  s4d5 )  s23c5d6  (c2  s 2 )a 3s3  a 2s2  d1  
J11 =-n x1*p y1  + n y1*p x1  
=-(c1[s23 (-c4c5c6 +s 4s6 )-c23s5s6 ]+s1 (s 4c5c6 +c4s6 ))*(c1[-s23 (c4s5d6 -s 4d5 )+c23c5d6 -(s 2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d 6 +c4 d5 +d3 )+a1c1 )
+(s1[s23 (-c4 c5c6 +s4s6 )-c23s5s6 ]-c1 (s4c5c6 +c4s6 ))*(c1[-s 23 (c4s5d 6 -s4d5 )+c23c5d6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d6 +c4 d 5 +d3 )+a1c1 )

 
J12  = -o x1*p y1 +o y1  *p x1
=-(c1[s23 (c 4c5c6 +s4 c6 )-c23s5s6 ]+s1 (-s4 c5s6 +c4s6 ))*(s1[c23 (c4s5 d6 -s4 d5 )+s23c5 d6 +(c2 -s2 )a 3s3 +a 2s2 )-c1 (s4s5d 6 +c4 d5 +d3 )+a1s1 )
+(c1[-c23 (c4c5c6 +s4s6 )+s 23s5s6 ]-c1 (-s1c5s6 +c4 c6 ))*(c1[-s23 (c4s5 d 6 -s4 d5 )+c23c5 d 6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s 4s5d 6 +c4 d5 +d3 )+a1c1 )

 
J13  = -a x1*p y1  +a y1  *p x1
=-(c1 (-s 23c 4s5 +c 23c5 )+s1s 4s5 )*(s1[c 23 (c4s5d 6 -s 4d5 )+s 23c5d 6 +(c 2 -s 2 )a 3s3 +a 2s 2 )-c1 (s 4s5 d 6 +c 4 d5 +d3 )+a1s1 )  
+(s1 (c 23c 4s5 +s 23s5 )-c1s 4s5 )*(c1[-s 23 (c4s5d 6 -s 4d5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )+s1 (s 4s5d 6 +c4d 5 +d 3 )+a1c1 )

18


J14 = n z1
=c23 (c4 c5c6 -s 4s6 )+s32 c5 c6
J15 = o z1
=-c 23 (c 4 c5s6 +s 4 c6 )+s 23s5s6

 
 

J16  = a z1
=c 23c 4 s 5 +s 23c5  

Ta có ma trận Jacoby 
 nx
n
 y
n
J  z
0
0

 0
 nx
n
 y
n
 z
0
0

 0

ox

ax

0

0

oy

ay

0

0

oz
0

az
0

0
nx

0
ox

0

0

ny

oy

0

0

nz

oz

ox

ax

0

0

oy

ay

0

0

oz
0

az
0

0
nx

0
ox

0

0

ny

oy

0

0

nz

oz

0
0 
0 H
. J
ax 
ay

a z 
0   J11
0   J 21
0   J 31

.
a x   J 41
a y   J 51
 
a z   J 61

J12

J13

J14

J15

J 22
J 32

J 23
J 33

J 24
J 34

J 25
J 35

J 42

J 43

J 44

J 45

J 52
J 62

J 53
J 63

J 54
J 64

J 55
J 65

J16 
J 26 
J 36 

J 46 
J 56 

J 66 
 

2. Ma trận Jacoby cho 3 khớp đầu của Robot
Ma trận Jacoby dạng điều khiển đối với 3 khớp đầu 
 

 J11 J12
J b =  J 21 J 22
 J 31 J32
 

                                            

J13 
J 23 
J 33 
 

 
19


Ta có vị trí khâu tác động cuối khớp 3 là: 
p x =cos(q1 )  a1 -a 2cosq 2 -a 3sin(q 2 +q 3 ) +s1d 3

 

p y =sin(q1 )  a1 -a 2sinq 2 -a 3sin(q 2 +q 3 ) -cosq1d 3

 

p z =d1 +a 2 cosq 2 +a 3 .cos(q 2 +q 3 )

 
Nên  
p x


J11 = q =-sinq1 a1 -a 2 cosq 2 -a 3sin(q 2 +q 3 )  +d3cosq1
1


p y
=cosq1 a1 -a 2 sinq 2 -a 3sin(q 2 +q 3 )  +d 3sinq1
J 21 =

q
1


p z
=0
J 31 =
q1

p x

J12 = q =a 2 cosq1.sinq 2 -a 3cosq1cos(q 2 +q 3 )
2


p y
=-a 2sinq1 .cosq 2 -a 3sinq1cos(q 2 +q3 )
J 22 =
q 2



p
J 31 = z =-a 2sinq 2 -a 3sin(q 2 +q 3 )
q 2

p x

J13 = q =-a 3cosq1.cos(q 2 +q3 )
3


p y
=-a 3sinq1cos(q 2 +q3 )
J 23 =
q3


p
J 33 = z =-a 3sin(q 2 +q 3 )
q3


 

 
                    

20


CHƯƠNG III: BÀI TOÁN ĐỘNG LỰC HỌC ROBOT

1. Hàm Lagrange và các vấn đề về động lực học:
Hàm Lagrange định nghĩa sư khác biệt giữa động năng và thế năng của hệ thống: 
L=K–P
Trong đó: 
K : Tổng động năng của hệ thống 
P: Tổng thế năng của hệ thống  
 
Do đó phương trình động lực học được xác định bằng biểu thức:  
Fi =

d L L

dt  q. qi
i

Trong đó:   là biến khớp :  
=  nếu là khớp quay 
= nếu là khớp tịnh tiến 
 biểu diễn lực hoặc Momen 
Động năng khớp thứ i được tính theo công thức: 
Ki =

1
1
mivi2 + Ji i2
2
2

Với Ji là Momen quán tính của khớp thứ i 

2. Tính toán các giá trị
a. Động năng:
Thanh nối thứ nhất
Tọa độ trọng tâm thanh nối thứ nhất 
 
.
 a g1.c1 


-a
.s
.θ
1


g1
1


a g1.s1  0 . 
. 

0
2
2
p c1 = 
a .c .θ
d g1  => p c1 =  g1 1 1             => V c1 =(a g1.θ1 )   



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

 
21


Động năng thanh nối thứ nhất:
2

.
. 2
1 

K1 =  m1 .  a g1 .θ1  +J1 .θ1 
2 



 

 

Thanh nối thứ hai:
Tọa độ trọng tâm của thanh nối thứ 2 trong hệ tọa độ 1 

-ag 2 .s2 


ag 2 .c2 
1

pc 2 
0



1


  
          
Tọa độ trọng tâm của thanh nối thứ 2 trong hệ tọa độ 0 
 

c1
s
0
1
p c2 =A1. p c2   1
0

0

0 s1

0 -c1
1 0
0 0

a1c1  -a g2 .s 2  -a g2 .c1s 2 +a l c1 


 
a1s1  a g2 .c 2  -a g2 .s1s 2 +a1s1 
=

 a c  d
d1  0
g2 2
1





1  1

 1
 

 

.
.
.



-a
(-s
s
θ
+c
s
θ
) - a
s
θ
1
2
1
g2
1
2
1
2
l
1


.
.
.
.



 p c2 = -a g2 (c1s 2 θ1 +s1c 2 θ 2 )+a l c1 θ1 


.
-a g2s 2 θ 2
 



         

        

V

2

2

c2

2
g2 2

2
1

.2

=(a s -2a1a g2 .s 2 +a ).θ 1 +a

.2

2
g2

.θ 2  

 
 
22


Động năng thanh nối thứ 2:

              

.2
.2
.2 
1
2
2
2
2
 K 2 =  m 2 ((a g2s 2 -2a1a g2 .s 2 +a 1 ).θ 1 +a g2 .θ 2 )+J 2 .θ 2 
2


 

 
Thanh nối thứ ba:
Tọa độ trọng tâm thanh nối 3 trong hệ tọa độ 2 
a g3 .c3 


a
.s
g3
3

2
p c3 = 
d

 g3 
1


 

  

 

 

Tọa độ trọng tâm thanh nối 2 trong hệ tọa độ gốc 

-c1c 2 -c1c 2
 -s s2 -c s
2 1
0
2
 p c3 =A1.A 2 p c3 =  1
 c2
-s 2

0
 0

s1
c1
0
0

a1c1 -a1c1s 2  a g3 .c3 


a1s1 -a 2s1s 2  a g3 .s3 
 
d1 +a 2 c 2  d g3 


1

 1

c1 (a1 -a 2s 2 -a g3s 23 )+d g3s1 



s
(a
-a
s
-a
.s
)-d
c
1
1
2
2
g3
23
g3
1

= 

d +c a +a c
 1 2 2 g3 23

1

                               
 
.
.

.
.
X
c3 = θ1 (-a1s1 +a 2 s1s 2 +a g3 .s1 .s 23 +d g3 .c1 )+ θ 2 (-a 2 c1c 2 -a g3 c1c 23 )-a g3c1c 23 .θ 3

. 0
.
.
.
 .
P3 = Y c3 = θ1 (a1c1 -a 2 c1s 2 -a g3 .c1.s 23 +d g3 .s1 )+ θ 2 (-a 2s1c2 -a g3s1c23 )-a g3s1c23 .θ3
.
.
.
 Zc3 = θ 2 (-a 2s 2 -a g3 .s 23 )-a g3s 23 .θ3


23


.

.

.

V 2 c3 = θ 2 1 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 ) 2 +θ 2 2 (a 2 c1c2 +a g3c1c23 ) 2 +(a g3 .c1c 23 .θ3 )2
.

.

.

.

-2θ1 θ 2 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 )(a 2 c1c2 +a g3c1c23 )+2θ 2 θ3 .a g3 .c1c 23 (a 2 c1c2 +a g3c1c23 )
.

.

.

-2θ1 θ3 .a g3 .c1c23 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 )+θ 2 1 (a1c1 +a 2 c1s 2 -a g3c1s 23 +d g3s1 )2
.

.

.

.

+θ 2 2 (a 2s1c2 +a g3s1c23 ) 2 +(a g3 .s1c23 .θ3 )2 -2θ1 θ 2 (a1c1 +a 2 c1s 2 -a g3c1s 23 +d g3s1 )(a 2s1c2 +a g3s1c23 )
.

.

.

.

+2θ 2 θ3 .a g3 .s1c23 (a 2s1c2 +a g3s1c23 )-2θ1 θ3 .a g3 .s1c23 (a1c1 -a1c1s 2 -a g3c1s 23 +d g3s1 )
.
2

.

.

.

+θ 2 (a 2s 2 +a g3s 23 ) 2 +2θ 2 θ3 .a g3c23 (a 2s 2 +a g3s 23 )+(a g3s 23 θ3 ) 2
.

.

.

.

.

 

.

.

=2θ1 θ 2 (d g3a g3c 23 -d g3a 2 c 2 )-2 θ1 θ 3 .d g3a g3c 23 +2θ 2 θ3 .(a 2 a g3c 2 c 23 +a 2s 2 +a g3s 23 +a 2 g3c 2 23 θ 3 ) 2
.

.

+ θ 2 1 (a12 +a 2 2s 2 2 +a 2 g3s 2 23 +d 2 g3 -2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ θ 2 2 (a 2 2 +a 2 g3 +2a2ag3c3)
.

+ θ 2 3 a 2 g3
K3 

. .
. .
.
.
.
1
(m 3 (2θ1 θ 2 (d g3a g3c 23 -d g3a 2 c 2 )-2θ1 θ3 .d g3a g3c 23 +2θ 2 θ3 .(a 2 a g3c 2 c 23 +a 2s 2 +a g3s 23 +a 2 g3c 2 23 θ3 ) 2
2
 

.

.

+ θ 2 1 (a12 +a 2 2s 2 2 +a 2 g3s 2 23 +d 2 g3 -2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ θ 2 2 (a 2 2 +a 2 g3 +2a2ag3c3)
.2

.

+ θ 2 3 a 2 g3 )+J 3 .θ 3 )

Ta có tổng động năng các thanh nối

. 2
. 2
.
1 . 2
1


K=K1 +K 2 +K 3 =  J1.θ1 +J 2 .θ 2 +J 3 .θ 3  + m1.  a g1.θ1 
2


 2
.2
.2
1
2
2
2
2
+ m 2 ((a g2s 2 -2a1a g2 .s 2 +a 1 ).θ 1 +a g2 .θ 2 )
2

2

 

. .
. .
.
.

.
1
2
2
 (m3 (2θ1 θ 2 (d g3a g3c 23 -d g3a 2c 2 )-2θ1 θ 3 .d g3a g3c 23 +2θ 2 θ3 .(a 2a g3c 2c 23 +a 2s 2 +a g3s 23 +a g3c 23 θ 3 )2
2
.
2

2

2

2

2

+ θ 1 (a1 +a 2s 2 +a g3s
.
2

+θ 3 a

2
23

+d

.
2

2
g3

-2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ θ 2 (a 2 2 +a 2g3 +2a2ag3c3)

.2

2
g3

)+J 3 .θ 3 )

 
24


b. Thế năng
Thế năng thanh nối thứ 1:

P1 = m 1 .g(d g1 +L 0 )

 

Thế năng thanh nối thứ 2:

P2 = m 2 .g(d g1 +L 0 +a g2 s 2 )  
Thế năng thanh nối thứ 3:

P3= m3 .g(d g1 +L0 +a g2s2 +a g3s3 )

 

Ta có tổng thế năng các thanh nối:

P=P1 +P2 +P3 =g.d g1 (m1 +m 2 +m3 )+gL 0 (m1 +m 2 +m 3 )+a g2 .g.s 2 (m 2 +m 3 )+a g3 .s3 .m 3g 
 
 

 

25