1

MỤC LỤC

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

Trang 

2 

3 

 

4 

4 

4 

6 

 

7 

7 

10 

14 

 

21 

21 

21 

21 

25 

26 

 

32 

32 

34 

35 

35 

37 

39 

 

44 

 

2

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 

3

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 

4

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á. 

... 

5

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

6

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:

 

7

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  θi(o)  αi(

o)  ai(mm)  di(mm) 

1  θ1  900  a1  d1 

2  θ2+90  00  a2  0 

3  θ3  900  a3  d3 

4  θ4  -900  0  0 

5  θ5  900  0  d5 

6  θ6  00  0  d6 

 

8

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

�� = �

����� −���������� ���������� �������

�����

00

����������

�����

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:  

1 1 1 1

1 1 1 11

1

c 0 s a c

s 0 c a sA

0 1 0 d

0 0 0 1

           2 2 2 2

2 2 2 22

s c 0 a s

c s 0 a cA

0 0 1 0

0 0 0 1

   

3 3 3 3

3 3 3 33

3

c 0 s a c

s 0 c a sA

0 1 0 d

0 0 0 1

        4 4

4 44

c 0 s 0

s 0 c 0A

0 1 0 0

0 0 0 1

   

5 5

5 55

5

c 0 s 0

s 0 c 0A

0 1 0 d

0 0 0 1

         6 6

6 66

6

c s 0 0

s c 0 0A

0 0 1 d

0 0 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

� 

 

9

x 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6n c [s ( c c c s s ) c s s ] s (s c c c s )  

y 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6n s [s ( c c c s s ) c s s ] c (s c c c s )  

z 23 4 5 6 4 6 32 5 6n c (c c c s s ) s c c  

 

x 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6o c [s (c c c s c ) c s s ] s ( s c s c s )  

y 1 23 4 5 6 4 6 23 5 6 1 1 5 6 4 6o c [ c (c c c s s ) s s s ] c ( s c s c c )  

z 23 4 5 6 4 6 23 5 6o c (c c s s c ) s s s  

 

x 1 23 4 5 23 5 1 4 5a c ( s c s c c ) s s s  

y 1 23 4 5 23 5 1 4 5a s (c c s s s ) c s s  

z 23 4 5 23 5a c c s s c  

 

x 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1p c [ s (c s d s d ) c c d (s c )a s a s ) s (s s d c d d ) a c    

y 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1p s [c (c s d s d ) s c d (c s )a s a s ) c (s s d c d d ) a s    

z 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1p c (c s d s d ) s c d (c s )a s a s d  

 

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 

10

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

� 

 

11

                               ��� = �

���(�) ���(�) ���(�) ���(�)

���(�)

���(�)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

−�������� − ������ ������ ������ + ����� + ����

���� �� 0

0 0 1

� (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) 

���(�) = 0                      (4c) 

Ta có: ���� − ���� = 0 

Suy ra: �� = ����� (��, ��)

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

��. � + ��. � = ����. �� + ���. �� + ��. ��             (5a) 

�� − ������ = ����� + ����                (5b) 

 

12

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 00 00 1

� 

 

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

−��������� + ����� + ������ = 0 

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

���� = ����2(��, ���� + ����) 

 

13

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

14

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;    

 

15

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  

16

= -( 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 

17

J35= oz3 

  = c4*c6 - c5*s4*s6 

J36 = az3  

= s4*s5 

 

x2 23 4 5 6 4 6 23 5 6

y2 23 4 5 6 4 6 23 5 6

z2 4 5 6 4 6

x2 23 4 5 6 4 6 23 5 6

y2 23 4 5 6 4 6 23 5 6

z2 4 5 6 4 6

x2 23 4 5 23 5

y2 23 4 5 23 5

z2 4 5

x2

n =s (-c c c +s s )-c s c ;

n =c (c c c -s s )+s s c ;

n =s c c +c s ;

o =s (c c s +s c )-c s s ;

o =-c (c c s +s c )+s s s ;

o =-s c s +c c ;

a =-s c s +c c ;

a =c c s +s c ;

a =s s ;

p 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

y2 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

z2 4 5 6 4 5 3

=-s (c s d -s d )+c c d -(s +c )a s -a s ;

p =c (c s d -s d )+s c d -(c -s )a s +a s ;

p =s s d +c d +d ;  

21 x2 y2 y2  x2

23 4 5 6 4 6 23 5 6 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

23 4 5 6 4 6 23 5 6 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

J n *p    n *p  

s (-c c c +s s )-c s c * c (c s d -s d )+s c d -(c -s )a s +a s

c (c c c -s s )+s s c * -s (c s d -s d )+c c d -(s +c )a s -a s

 

22 x2 y2 y2 x2

23 4 5 6 4 6 23 5 6 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

23 4 5 6 4 6 23 5 6 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

J     o *p o   *p

s (c c s +s c )-c s s * c (c s d -s d )+s c d -(c -s )a s +a s

-c (c c s +s c )+s s s *(-s (c s d -s d )+c c d -(s +c )a s -a s )

 

23 x2 y2 y2 x2

23 4 5 23 5 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

23 4 5 23 5 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

J     a *p   a   *p

(-s c s +c c )*(c (c s d -s d )+s c d -(c -s )a s +a s )

(c c s +s c )*(-s (c s d -s d )+c c d -(s +c )a s -a s )

 

18

24 z2

4 5 6 4 6

J  n

s c c +c s

 

25 z2

4 5 6 4 6

J  o

-s c s +c c

 

26 z2

4 5

J    a

s s

 

x1 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6n c [s ( c c c s s ) c s s ] s (s c c c s )  

y1 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6n s [s ( c c c s s ) c s s ] c (s c c c s )  

z1 23 4 5 6 4 6 32 5 6n c (c c c s s ) s c c  

 

x1 1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6o c [s (c c c s c ) c s s ] s ( s c s c s )  

y1 1 23 4 5 6 4 6 23 5 6 1 1 5 6 4 6o c [ c (c c c s s ) s s s ] c ( s c s c c )  

z1 23 4 5 6 4 6 23 5 6o c (c c s s c ) s s s  

 

x1 1 23 4 5 23 5 1 4 5a c ( s c s c c ) s s s  

y1 1 23 4 5 23 5 1 4 5a s (c c s s s ) c s s  

z1 23 4 5 23 5a c c s s c  

 

x1 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1p c [ s (c s d s d ) c c d (s c )a s a s ) s (s s d c d d ) a c    

y1 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1p s [c (c s d s d ) s c d (c s )a s a s ) c (s s d c d d ) a s    

z1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1p c (c s d s d ) s c d (c s )a s a s d  

11 x1 y1 y1 x1

5 5 5 5 5 5 5 51 23 4 6 4 6 23 6 1 4 6 4 6 1 23 4 6 4 23 6 2 2 3 3 2 2 1 4 6 4 3 1 1

5 5 51 23 4 6 4 6 23 6 1 4 6 4 6 1

J =-n *p  + n *p  

=-(c [s (-c c c +s s )-c s s ]+s (s c c +c s ))*(c [-s (c s d -s d )+c c d -(s +c )a s -a s )+s (s s d +c d +d )+a c )

+(s [s (-c c c +s s )-c s s ]-c (s c c +c s ))*(c [-s 5 5 5 5 523 4 6 4 23 6 2 2 3 3 2 2 1 4 6 4 3 1 1(c s d -s d )+c c d -(s +c )a s -a s )+s (s s d +c d +d )+a c )

 

12 x1 y1 y1 x1

1 23 4 5 6 4 6 23 5 6 1 4 5 6 4 6 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1

1 23 4 5 6 4 6 23 5 6 1 1 5 6 4 6 1

J  = -o *p +o  *p

=-(c [s (c c c +s c )-c s s ]+s (-s c s +c s ))*(s [c (c s d -s d )+s c d +(c -s )a s +a s )-c (s s d +c d +d )+a s )

+(c [-c (c c c +s s )+s s s ]-c (-s c s +c c ))*(c [-s23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1(c s d -s d )+c c d -(s +c )a s -a s )+s (s s d +c d +d )+a c )

 

13 1 y1 y1 x1

1 23 4 5 23 5 1 4 5 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2 1 4 5 6 4 5 3 1 1

1 23 4 5 23 5 1 4 5 1 23 4 5 6 4 5 23 5 6 2 2 3 3 2 2

J  = -a *p  +a  *p

=-(c (-s c s +c c )+s s s )*(s [c (c s d -s d )+s c d +(c -s )a s +a s )-c (s s d +c d +d )+a s )

+(s (c c s +s s )-c s s )*(c [-s (c s d -s d )+c c d -(s +c )a s -a s )

x

1 4 5 6 4 5 3 1 1+s (s s d +c d +d )+a c )

 

19

14 z1

23 4 5 6 4 6 32 5 6

J = n

=c (c c c -s s )+s c c 

15 z1

23 4 5 6 4 6 23 5 6

J = o

=-c (c c s +s c )+s s s 

16 z1

23 4 5 23 5

J  = a

=c c s +s c  

Ta có ma trận Jacoby 

x x

x x

11 12 13 14 15 16x x

21 22 23 24 25 26

31 32 33

x x

o a 0 0 0

o a 0 0 0

o a 0 0 0.

0 0 0 o a

0 0 0 o a

0 0 0 o a

o a 0 0 0

o a 0 0 0

o a 0 0 0.

0 0 0 o a

0 0 0 o a

0 0 0 o a

H

x

y y y

z z z

x

y y y

z z z

x

y y y

z z z

x

y y y

z z z

n

n

nJ J

n

n

n

J J J J J Jn

J J J J J Jn

J J Jn

n

n

n

34 35 36

41 42 43 44 45 46

51 52 53 54 55 56

61 62 63 64 65 66

J J J

J J J J J J

J J J J J J

J J J J J J

 

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 

 

                                            

11 12 13

b 21 22 23

31 32 33

J J J

J = J J J

J J J

  

 

20

Ta có vị trí khâu tác động cuối khớp 3 là: 

 

 

 

Nên  

                    

x11 1 2 2 3 2 3 3 1

1

y

21 1 2 2 3 2 3 3 1

1

z31

1

x12 2 1 2 3 1 2 3

2

y

22 2 1 2 3 1 2 3

2

pJ = =-sinq1 a -a cosq -a sin(q +q ) +d cosq

q

pJ = =cosq1 a -a sinq -a sin(q +q ) +d sinq

q

pJ = =0

q

pJ = =a cosq .sinq -a cosq cos(q +q )

q

pJ = =-a sinq .cosq -a sinq cos(q +q

q

z31 2 2 3 2 3

2

x13 3 1 2 3

3

y

23 3 1 2 3

3

z33 3 2 3

3

)

pJ = =-a sinq -a sin(q +q )

q

pJ = =-a cosq .cos(q +q )

q

pJ = =-a sinq cos(q +q )

q

pJ = =-a sin(q +q )

q

 

 

x 1 1 2 2 3 2 3 1 3

y 1 1 2 2 3 2 3 1 3

z 1 2 2 3 2 3

p =cos(q ) a -a cosq -a sin(q +q ) +s d

p =sin(q ) a -a sinq -a sin(q +q ) -cosq d

p =d +a cosq +a .cos(q +q )

21

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 = i

i

q

L

q

L

dt

d

.

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 = 2

1mivi

2 + 2

1Jii

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 

 

          

g1 1

g1 10c1

g1

a .c

a .sp =

1

d

=>

.

1g1 1

0 ..

1g1 1c1

-a .s .θ

a .c .θp =

0

           =>2 2

c1 g1 1V =(a .θ )   

 

22

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

2 2. .

1 1 g1 1 1 1

1K = m . a .θ +J .θ

2

 

 

Thanh nối thứ hai:

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

          

2 2

2 212

- .

.

0

1

g

g

c

a s

a cp

 

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

 

 

 

         

. . .

1 2 1g2 1 2 1 2 l 1

. . . .

1 2 1g2 1 2 1 2 l 1c2

.

2g2 2

-a (-s s θ +c s θ ) - a s θ

p = -a (c s θ +s c θ )+a c θ

-a s θ

 

        

2 2. .2 2 2 2 2

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

 

 

g2 1 2 l 1g2 21 1 1 1

g2 1 2 1 11 1 1 1 g2 20 1c2 1 c2

1 2 2 1

-a .c s +a c-a .sc 0 s

-a .s s +a ss 0 -c a .cp =A . p =

0 1 0 d 0

0 0 0 1 1 1

g

a c

a s

a c d

23

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

              

2 2 2. . .2 2 2 2

1 2 22 2 g2 2 1 g2 2 1 g2 2

1K = m ((a s -2a a .s +a ).θ +a .θ )+J .θ

2

 

 

Thanh nối thứ ba:

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

      

g3 3

g3 32c3

g3

a .c

a .sp =

d

1

 

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

g3 31 2 1 2 1 1 1 1 1 2

g3 31 2 1 1 1 1 2 1 20 2c3 1 2 c3

2 2 1 2 2 g3

a .c-c c -c c s a c -a c s

a .s-s s2 -c s c a s -a s sp =A .A p =

c -s 0 d +a c d

0 0 0 1 1

 

                               

1 1 2 2 g3 23 g3 1

1 1 2 2 g3 23 g3 1

1 2 2 g3 23

c (a -a s -a s )+d s

s (a -a s -a .s )-d c=

d +c a +a c

1

 

. . . .

c3 1 2 31 1 2 1 2 g3 1 23 g3 1 2 1 2 g3 1 23 g3 1 23

0. . . . .

3 c3 1 2 31 1 2 1 2 g3 1 23 g3 1 2 1 2 g3 1 23 g3 1 23

. . .

c3 2 32 2 g3 23 g3 23

X =θ (-a s +a s s +a .s .s +d .c )+θ (-a c c -a c c )-a c c .θ

P = Y =θ (a c -a c s -a .c .s +d .s )+θ (-a s c -a s c )-a s c .θ

Z =θ (-a s -a .s )-a s .θ

24

. . .2 2 2 2 2 2

1 2 3c3 1 1 2 1 2 g3 1 23 g3 1 2 1 2 g3 1 23 g3 1 23

. . . .

1 2 2 31 1 2 1 2 g3 1 23 g3 1 2 1 2 g3 1 23 g3 1 23 2 1 2 g3 1 23

. .

1 3 g3 1 23 1 1 2 1 2

V =θ (-a s +a s s +a s s +d c ) +θ (a c c +a c c ) +(a .c c .θ )

-2θ θ (-a s +a s s +a s s +d c )(a c c +a c c )+2θ θ .a .c c (a c c +a c c )

-2θ θ .a .c c (-a s +a s s +.2 2

1g3 1 23 g3 1 1 1 2 1 2 g3 1 23 g3 1

. . . .2 2 2

2 3 1 22 1 2 g3 1 23 g3 1 23 1 1 2 1 2 g3 1 23 g3 1 2 1 2 g3 1 23

. . . .

2 3 1 3g3 1 23 2 1 2 g3 1 23 g3 1 23 1 1

a s s +d c )+θ (a c +a c s -a c s +d s )

+θ (a s c +a s c ) +(a .s c .θ ) -2θ θ (a c +a c s -a c s +d s )(a s c +a s c )

+2θ θ .a .s c (a s c +a s c )-2θ θ .a .s c (a c 1 1 2 g3 1 23 g3 1

. . . .2 2 2

2 2 3 32 2 g3 23 g3 23 2 2 g3 23 g3 23

-a c s -a c s +d s )

+θ (a s +a s ) +2θ θ .a c (a s +a s )+(a s θ ) 

. . . . . . .2 2 2

1 2 1 3 2 3 3g3 g3 23 g3 2 2 g3 g3 23 2 g3 2 23 2 2 g3 23 g3 23

. .2 2 2 2 2 2 2 2 2 2

1 21 2 2 g3 23 g3 1 2 2 1 g3 23 2 g3 23 2 g3

.2 2

3 g3

=2θ θ (d a c -d a c )-2θ θ .d a c +2θ θ .(a a c c +a s +a s +a c θ )

+θ (a +a s +a s +d -2a a s -2a a s +2a a s )+θ (a +a +2a2ag3c3)

+θ a

 

. . . . . . .2 2 2

1 2 1 3 2 3 33 g3 g3 23 g3 2 2 g3 g3 23 2 g3 2 23 2 2 g3 23 g3 23

. .2 2 2 2 2 2 2 2 2 2

1 21 2 2 g3 23 g3 1 2 2 1 g3 23 2 g3 23 2 g3

2. .2 2

3 3g3 3

13 (m (2θ θ (d a c -d a c )-2θ θ .d a c +2θ θ .(a a c c +a s +a s +a c θ )

2

+θ (a +a s +a s +d -2a a s -2a a s +2a a s )+θ (a +a +2a2ag3c3)

+θ a )+J .θ

K

)

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

22 2 2. . . .

1 2 3 1 1 2 2 3 3 1 g1 1

2 2. .2 2 2 2

1 22 g2 2 1 g2 2 1 g2

1 1K=K +K +K = J .θ +J .θ +J .θ + m . a .θ

2 2

1+ m ((a s -2a a .s +a ).θ +a .θ )

2

 

. . . . . . .2 2 2

1 2 1 3 2 3 33 g3 g3 23 g3 2 2 g3 g3 23 2 g3 2 23 2 2 g3 23 g3 23

. .2 2 2 2 2 2 2 2 2 2

1 21 2 2 g3 23 g3 1 2 2 1 g3 23 2 g3 23 2 g3

2. .2 2

3 3g3 3

1(m (2θ θ (d a c -d a c )-2θ θ .d a c +2θ θ .(a a c c +a s +a s +a c θ )

2

+θ (a +a s +a s +d -2a a s -2a a s +2a a s )+θ (a +a +2a2ag3c3)

+θ a )+J .θ )

 

25

b. Thế năng

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

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

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

2 2 g1 0 g2 2P = m .g(d +L +a s )  

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

3 g1 0 g2 2 g3 3P3= m .g(d +L +a s +a s ) 

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

1 2 3 g1 1 2 3 0 1 2 3 g2 2 2 3 g3 3 3P=P +P +P =g.d (m +m +m )+gL (m +m +m )+a .g.s (m +m )+a .s .m g 

 

   

26

c. Phương trình động lực học:

Phương trình Lagrange chuẩn: L = K – P 

 22 2 2. . . .

1 1 2 2 3 3 1 g1 1

2 2. . . . . .2 2 2 2

1 2 1 2 1 32 g2 2 1 g2 2 1 g2 3 g3 g3 23 g3 2 2 g3 g3 23

. . .2 2 2

2 3 32 g3 2 23 2 2 g3 23 g3 23

.2

1 1

1 1J .θ +J .θ +J .θ + m . a .θ

2 2

1 1+ m ((a s -2a a .s +a ).θ +a .θ )+ m (2θ θ (d a c -d a c )-2θ θ .d a c

2 2

+2θ θ .(a a c c +a s +a s +a c θ )

+θ (a

.2 2 2 2 2 2 2 2 2

22 2 g3 23 g3 1 2 2 1 g3 23 2 g3 23 2 g3

.2 2

3 g3 g1 1 2 3 0 1 2 3 g2 2 2 3 g3 3 3

+a s +a s +d -2a a s -2a a s +2a a s )+θ (a +a +2a2ag3c3)

+θ a )- g.d (m +m +m )+gL (m +m +m )+a .g.s (m +m )+a .s .m g  

 

Momen tác động lên khớp quay thứ nhất:

2 2 2 21 1 1 g1 1 2 g2 2 1 g2 2 1 3 2 g3 g3 23 g3 2 2

1

2 2 2 2 2 23 3 g3 g3 23 1 2 2 g3 23 g3 1 2 2 1 g3 3 2 g3 23

..2 2

11 1 g1 g2 2 2 2 1

1

L=J .θ +m a .θ +m (a s -2a a s +a )+2m θ (d a c -d a c )

θ

-2m θ d a c +(a +a s +a s +d -2a a s -2a a s +2a a s )

d L=(J +m a )θ +m2(a .2s .c .θ -2a a

dt θ

g2 2 2c θ )+

 

3 2 g3 g3 23 g3 2 2 3 2 g3 g3 23 2 3 g3 2 2 2

2 23 g3 23 3 3 g3 23 3 2 3 2 2 2 2 g3 23 23 2 3

1 2 2 2 1 g3 3 3 2 g3 23 2 3

..2

11 1 g1 3

2m θ (d .a .c -d a c )+2m θ (-d .a .s (θ +θ )+d a s θ )

-2m d c θ +2m d s θ (θ +θ )+2a s c θ +2a .s .c (θ +θ )

-2a a c θ -2a a c θ +2a a c (θ +θ )

=(J +m a )θ +2m

..

3g3 g3 23 g3 2 2 2 3 g3 g3 23

2 2 22 g2 2 2 1 2 g2 2 2 2 2 g3 23 23 1 2 2 2 g3 23 2

(d a c -d a c )θ -2m d a c θ

+(2m a s c -2a m a c +2a s c +2a s c -2a a c +2a a c )θ

 

2.2

2g3 23 23 1 g3 3+ 2 g3 23 3 3 g3 g3 23 3 g3 2 2

3 g3 g3 23 3 g3 g3 23 3 g3 g3 23 2 3

+(2a s c -2a a c 2a a c )θ +(-2m d .a .s +2m d .a .s )θ

+2m d a s (-2m d a s +2m d a s )θ θ

 

1

L=0

θ

 

27

1 .

t 11

.. ..2

1 31 1 g1 2 3 g3 g3 23 3 g3 2 2 3 g3 g3 23

2 2 22 2 g2 2 2 1 2 g2 2 2 2 2 g3 23 23 1 2 2 2 g3 23

2.2

23 g3 23 23 1 g3 3+ 2 g3 23 3 g3

d L LM = ( )-

d θθ

=θ (J +m a )+θ (2m d a c -2m d a c )-θ .2m d a c

+θ (2m a s c -2a m a c +2a s c +2a s c -2a a c +2a a c )

+θ (2a s c -2a a c 2a a c )+θ (2m d .a

2 2 3 g3 g3 23

23 3 g3 g3 23

.s -2m d .a .s )

+θ 2m d a s 

 

Momen tác động lên khớp quay thứ hai:

2 2 22 2 g2 3 1 g3 g3 23 g3 2 2 3 g3 2 2 23 2 2 g3 23 g3 23

2

2 2 22 g3 23 2 g3 3

.. .

2 22 3 1 g3 g3 23 g3 2 2 3 1 g3 g3 23 2 2 2 3 g3 2 2

2

L 1=J .θ + a +2m θ (d a c -d a c )+2θ (a a c c +a s +a s +a c )

θ 2

+(a +a s 2a a c )

d L( )=J .θ +2m θ (d a c -d a c )+2m θ d a s +a s +(θ +θ )+d a s θ

dt θ

2θ

.. .2 2

3 3g3 2 2 23 2 2 g3 23 g3 g3 2 g3 3

. . . . . . . .2

2 2 3 2 2 3 2 33 g3 2 2 23 2 23 2 2 g3 23 g3 23 23

.. .. ..

2 1 32 3 g3 g3 23 g3 2 2 g3 2 2 23 2 2 g3 23

(a a c c +a s +a s +a c )-2a a s θ

+2θ a a (-s c θ -c s (θ +θ ))+a c θ +a c (θ +θ )-a 2c s (θ +θ )

=θ J +θ .2.m (d a c -d a c )+θ 2(a a c c +a c +a s

2 2g3 g3

. .

3 32 g3 3 1 2 3 g3 g3 23 3 g3 2 2 1 m3 g3 g3 23

.2

3 2 g3 2 2 23 g3 2 2 23 2 2 g3 23 g3 23 23

2.2

3 g3 2 2 23 g3 23 g3 23 23

+a c )

-θ .2a a s +θ θ (-2m d a s +2m d a s -θ θ 2 d a s

+2θ θ (-a a s c -a a c s +a c +a c -2a c s )-

2θ (a a c s -a c +2a c s )

 

28

.2

32 g2 2 2 1 g2 2 1 3 1 2 g3 g3 23 g3 2 2 1 g3 g3 23

2

.2

3 2 g3 2 2 23 2 23 2 2 g3 23 g3 23 23

.2 2

1 2 2 2 g3 23 23 1 2 2 1 g3 23 2 g3 23 g2 2 3 2

L=(m a 2s c -a a c )θ -2m θ θ (d a s -d a s )+2θ θ d a s

θ

-2θ θ a a (s c +c s )-a c -a c +2a c s

+θ a 2s c +2a c s -2a a c -2a a c +2a a c +a g(m +m )c

 

2

2 2

.. .. ..2 2

2 1 32 3 g3 g3 23 g3 2 2 g3 2 2 23 2 2 g3 23 g3 g3

2 2 21 2 g2 2 2 1 g2 2 2 2 2 g3 23 23 1 2 2 1 g3 23 2 g3 23

.

3 2 g3 3 1 2 3 g3 g3

d L LM = ( )-

dt θ θ

θ J +θ .2.m (d a c -d a c )+θ 2(a a c c +a c +a s +a c )

-θ (m a 2s c -a a c +a 2s c +2a c s -2a a c -2a a c +2a a c )

-2θ a a s +θ θ (-2m d a

23 3 g3 2 2 3 g3 g3 23 3 g3 2 2

. .2

3 31 3 g3 g3 23 g3 g3 23 2 g3 2 2 23 g3 2 2 23 2 2 g3 23 g3 23 23

.2

3 2 g3 2 2 23 g3 2 2 23 2 2 g3 23 g3 23 23

.. ..

2 12 3

s +2m d a s +2m d a s -2m d a s )

-θ θ (2m d a s +2d a s )+2θ θ (-a a s c -a a c s +a c +a c -2a c s )

+2θ θ (a a s c +a a c s -a c -a c +2a c s )

=θ J +θ .2.m (

..2 2

3g3 g3 23 g3 2 2 g3 2 2 23 2 2 g3 23 g3 g3

2 2 21 2 g2 2 2 1 g2 2 2 2 2 g3 23 23 1 2 2 1 g3 23 2 g3 23

. .

3 32 g3 3 1 3 g3 g3 23 g3 g3 23 g2 2 2

d a c -d a c )+θ 2(a a c c +a c +a s +a c )

-θ (m a 2s c -a a c +a 2s c +2a c s -2a a c -2a a c +2a a c )

-2θ a a s -θ θ (2m d a s +2d a s )+a m gc

 

29

Momen tác động lên khớp quay thứ ba:

 

2 2 23 3 1 g3 g3 23 3 2 g2 2 2 23 2 2 g3 23 g3 23 3 3 g3 3

3

.. .. . . ..2 2

3 1 2 3 23 g3 g3 3 23 1 23 3 g2 2 2 23 2 2 g3 23 g3 g3

3

.. .

2 23 g2 2 2 23

L=J .θ -2θ d a c m +2θ (a a c c +a s +a s +a c )m +2θ a m

θ

d L( )=J .θ -2d a m (θ c -θ s (θ +θ ))+2m θ (a a c c +a s +a s +a c )

dt θ

+2m θ -a a (s c θ +c

. . . . . . .2 2

2 3 2 2 3 2 32 23 2 2 g3 23 g3 23 23 g3 3

.. .. ..2 2 2

3 1 23 g3 3 3 g3 g3 23 3 g2 2 2 23 2 2 g3 23 g3 g3

2. .

3 21 2 g3 g3 23 3 1 g3 g3 23 3 3 g2

s (θ +θ ))+a c θ +a c (θ +θ )-a 2c s (θ +θ ) +2a m

= θ (J +2a m )-θ .2.m d a c +2θ m (a a c c +a s +a s +a c )

-2θ θ d a s m +2θ θ d a s m +θ (2m a

2 2 23 3 g2 2 2 23

.2 2

33 2 2 3 g3 23 3 g3 23 23 2 3 g2 2 2 23 3 g3 23 3 g3 23 23

. .2

3 33 1 2 g3 g3 23 1 g3 g3 23 2 g3 2 2 23 g3 23 g3 23 23

3

.2

1 g3 23 23

a s c -2m a a c s +

2m a c +2m a c -4m a c s )+θ θ (-2m a a c s +2m a c +4m a c s )

L=-2m θ θ d a s +2θ θ d a s +2θ θ (-a a c s +a c -2a c s

θ

+θ (2a c s 2a

1 g3 23 2 g3 23 2 2 g3 3 g3 3 3a c +2a a c )-2θ a a s +a gc m g

 

3

3 3

.. .. ..2 2 2

3 1 23 g3 3 3 g3 g3 23 3 g2 2 2 23 2 2 g3 23 g3 g3

2. .

3 21 2 g3 g3 23 3 1 g3 g3 23 3 3 g2 2 2 23 3 g2 2 2 23

23 2 2 3 g3 23 3 g3 23 2

d L LM = ( )-

dt θ θ

θ (J +2a m )-θ .2.m d a c +2θ m (a a c c +a s +a s +a c )

-2θ θ d a s m +2θ θ d a s m +θ (2m a a s c -2m a a c s +

2m a c +2m a c -4m a c s

.2

33 2 3 g2 2 2 23 3 g3 23 3 g3 23 23

. .2

3 33 1 2 g3 g3 23 1 g3 g3 23 2 g3 2 2 23 g3 23 g3 23 23

.2

1 g3 23 23 1 g3 23 2 g3 23 2 2 g3 3 g3 3 3

)+θ θ (-2m a a c s +2m a c +4m a c s )

-(-2m θ θ d a s +2θ θ d a s +2θ θ (-a a c s +a c -2a c s

+θ (2a c s 2a a c +2a a c )-2θ a a s +a gc m g)

 

30

c. Phương trình động lực học viết dưới dạng ma trận trạng thái:

Phương trình động lực học thanh nối 

.. .

..

1

1 11 111 12 13..

22 21 22 23 21 2

..31 32 333 31 3

3

T=H(Q)Q +V(Q,Q)+G(Q)

θM V GH H H

M = H H H θ + V + G

H H HM V Gθ

 

Với: 

211 1 1 g1

12 3 g3 g3 23 g3 2 2

13 3 g3 g3 23

21 3 g3 g3 23 g3 2 2

22 2

2 223 g3 2 2 23 2 2 g3 23 g3 23 3

31 3 g3 g3 23

2 232 g3 2 2 23 2 2 g3 23 g3 23 3

33

H =J +m a

H =2m (d a c -d a c )

H =-2m d a c

H =2m (d a c -d a c )

H =J

H =2(a a c c +a s +a s +a c )m

H =-2m d a c

H =2(a a c c +a s +a s +a c )m

H =J

23 g3 3

.2 2 2

211 2 g2 2 2 1 2 g2 2 2 2 2 g3 23 23 1 2 2 2 g3 23

2. .2

3 2g3 23 23 1 g3 3 2 g3 23 3 g3 2 2 3 g3 g3 23

3 g3 g3 23

+2a m

V =(2m a s c -2a m a c +2a s c +2a s c -2a a c +2a a c )θ

+(2a s c -2a a c +2a a c )θ +(2m d a c -2m d a s )θ

+2m d a s

31

.2 2 2

121 2 g2 2 2 1 g2 2 2 2 2 g3 23 23 1 2 2 2 g3 23

. . .

3 1 33 g3 g3 23 g3 g3 23

. . . .2

1 2 2 331 g3 g3 23 g3 2 2 23 3 g3 23 3 g3 23 23

g3 g3 23

V =-(2m a s c -a a c +2a s c +2a s c -2a a c -2a1ag3c23+2a a c )θ

-2a2ag3s3θ -(2m d a s +2d a s )θ θ

V =2d a s θ θ +2(-2a a c s +2m a c -4m a c s )θ θ

-4d a s θ. . . .

1 2 1 2g3 23 1 2 g3 23 2 g3 23

2.2

23 g2 2 2 23 g2 2 23 2 2 g3 23 g3 23 23

1

2 g2 2 2

3 g3 3 3

θ +(a s -a +a ).2a c θ -2a a s θ

+2m (-a a s c -a c c +a c +a c -2a c s )θ

0

a m gc

a gc m g

G

G

G

 

                

               

 

32

CHƯƠNG IV

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 dùng phương pháp PD bù trọng trường Bài toán đặt ra là xác định cấu trúc bộ điều khiển đảm bảo hệ thống ổn định tuyệt đố 

xung quanh điểm cân bằng, không phụ thuộc vào khối lượng thanh nối và tải.  Luật điều khiển này dưa trên tiêu chuẩn ổn định Liapunov.  

Mô hình bộ điểu khiển theo phương pháp PD bù trọng trường 

Tổng quan về bộ điều khiển PD

Phương trình động lực học: 

T=H Q Q+V Q,Q +G Q

 Hàm điều khiển : 

                                                     dkT =Kp.E+Kd.E+G(Q)  

Trong đó : 

                            

E Qd Q

E Qd        (1)

 

                            Qd: Giá trị đặt 

Mục đích; 

               Kp.E+Kd.E+G(Q) H Q Q+V Q,Q +G Q        (2)  

 

 

33

Tính chất phương trình động lực học: 

 

     +   H(Q): là hàm xác định dương , hàm đối xứng  

     +    1V Q,Q ( ) ( , )                  (3)

2H Q S Q Q  

     +   . ( , ). 0TQ S Q Q Q  

     +    . ( ). . ( ).           (4)T Ta H Q b a H Q b  

Mặt khác ta có: 

1 1      . ( ). . . ( ). ( . ( ). . ( ). . ( ). )

2 2

1                                                               = . ( ).     (5)

2

T T T T T

T

Q H Q Q Q H Q Q Q H Q Q Q H Q Q Q H Q Q

dQ H Q Q

dt

 

+      1      . . . .         (6)

2T Td

E Q E Kp E Q Kp Edt

 

 

Từ (1)(2)(3)1

         . . ( ). ( ) ( , )2

Kp E Kd Q H Q Q H Q S Q Q Q

 

           1

         . ( ). ( ) ( , ) .2

Kd Q H Q Q H Q S Q Q Q Kp E

  

 

 

Nhân cả hai vế của biểu thức trên với   TQ   

         

1 1. ( ). . . ( ). . . ( , ). . . . .   (7)

2 2T T T T TQ H Q Q Q H Q Q Q S Q Q Q Q Kp E Q Kd Q  

Thay (4)(5)(6) vào (7) ta được: 

  

               1

         . ( ). . . . .2

T T TdQ H Q Q E Kp E Q Kd Q

dt  

 

Từ biểu thức trên ta thấy được hàm Lyapunov: 

             1

V Q,Q . ( ). . . 02

T TQ H Q Q E Kp E  

 

             V Q,Q . .TQ Kd Q  

34

Ở chế độ làm việc xác lập các thành phàn tốc độ và gia tốc bằng không từ đó ta thấy 

được: 

                              0

       0

Q QdE

E Q Qd

 

Với luật bù trọng lực phi tuyến với bộ điều khiển PD ,tất cả các điểm làm việc cân 

bằng  sẽ  làm việc  cân  bằng không chịu  sự  ảnh hưởng của  trọng  lực Rôbôt. Mức độ ổn 

định và chất lượng của quá trình động phụ thuộc vào giá trị của Kd và Kp. 

 

2. Thiết kế quỹ đạo bậc ba cho Robot

Bài toán thiết kế quỹ đạo cho khớp là xác định đường biểu diễn của vị trí khớp ( góc 

quay của khớp quay hoặc độ di chuyển của khớp tịnh tiến ) theo thời gian khi di chuyển 

từ vị trí ban đầu �� đến vị trí cuối cùng �� trong thời gian �� ,với q là biến khớp tổng 

quát.Quỹ đạo di chuyển của khớp giữa hai vị trí sẽ thoả mãn 4 điều kiện : 

- Vị trí ban đầu và vị trí cuối cùng. 

- Tốc độ tại vị trí ban đầu và tại vị trí cuối cùng. 

Do đó đa thức bậc 3 sẽ thích hợp cho quỹ đạo của khớp robot : 

q(t)=�� +��t+����  +�� �� 

Các điều kiện đầu và cuối: 

�(��)=�� 

q(��)= �� 

�̇(��)= � ̇ � 

�̇(��)= � ̇ � 

Trong mô phỏng quỹ đạo khớp bằng simulink ta sẽ sử dụng phương pháp xây dựng quỹ 

đạo bậc 3 để tạo hàm tổng quát cho quỹ đạo di chuyển khớp. 

35

3. Mô phỏng 3 khớp đầu robot

a. Mô hình điều khiển PD bù trọng trường trên Simulink

 Mô hình bộ điều khiển PD bù trọng trường 

Thông số các thanh nối lần lượt như sau: 

m1 = 60(kg)  ;  

m2 = 30(kg) ;  

m3 = 20(kg) ; 

d1=0.275(m); 

a1=0.15(m); 

a2=0. 6(m); 

d3=0.17(m); 

a3=0.12(m); 

 Khâu khuếch đại và tỉ lệ đạo hàm được lựa chọn : 

1800 0 0 200 0 0

0 1800 0       Kd= 0 250 0

0 0 400 0 0 60

Kp va

Các giá trị đặt: 

qd1 3.4 rad ; 

qd2 2.1 rad ;

qd3 2.7 rad ;

36

Kết quả chạy mô phỏng:

Đáp ứng đầu ra với bộ điều khiển PD 

37

Nhận xét :

Nhờ việc chọn đúng thông số của hai bộ điều khiển Kp và Kd do vậy các đường đăc 

tính có chất lượng tốt : thời gian xác lập nhanh <0.6s, độ quá điều chỉnh của cả ba trường 

hợp gần như băng không . 

b. Mô phỏng phương pháp điều khiển PD bù trọng trường có thiết kế quỹ đạo bậc 3

Sơ đồ cấu trúc trên Simulink: 

  

Mô phỏng PD bù trọng trường quỹ đạo bậc 3 

Khâu khuếch đại và tỉ lệ đạo hàm được lựa chọn : 

2000 0 0

Kp= 0 1900 0    

0 0 500

250 0 0

  Kd= 0 320 0

0 0 60

Xd= -0.0870 0.5571-1.1409

   

38

Kết quả mô phỏng

  

Kết quả mô phỏng PD có quỹ đạo bậc 3 

   

39

c. Xây dựng Mô phỏng bằng Toolbox SimMechanics

Các khối cơ bản:

a. Khối body 1

Mục đích:  Biểu diễn một vật thể cứng tùy ý: 

 

Mô tả:   

+  Khối lượng của vật thể và tensor mômen quán tính.  

+ Toạ độ trọng tâm của vật thể (CG) 

+  Một số hệ toạ độ Body tuỳ ý (CSs) 

b. Khối body 2

Mục đích: Biểu diễn một vật thể cứng tùy ý. 

 

Mô tả: Khối body biểu diễn một vật thể cứng mà thuộc tính của nó là tùy ý. Sự miêu 

tả cần bao gồm: 

+ Khối lượng của vật thể và sensor mô men quán tính 

+ Tọa độ trọng tâm của vật thể 

+ Một số hệ tọa độ Body tùy ý. 

c. Khối Ground

Mục đích: Biểu diễn một điểm cố định trên nền giã đỡ, nơi đặt gốc của hệ tọa độ. 

 

40

Mô tả: Một khối Ground biểu diễn một điểm cố định trong hệ tọa độ tuyệt đối.Gắn 

khối này với một bên của khớp để ngăn chặn sự chuyển động cảu bên đó của khớp. Như 

vậy thì khối này tương ứng với ngàm. 

Ground nằm trong thư viện bodies và nó chính là một body đặc biệt.Nhung chúng ta 

chỉ có thể nối một đầu cảu nó với một khớp.Các khối ground tự động lấy hệ tọa độ có các 

trục song song với hệ tọa độ động(gắn tại một trong các ground khác trước đó) và gốc tại 

ground point. 

d. Khối revolute

Mục đích: Biểu diến một khớp quay kết hợp với bậc tự do tịnh tiến. 

 

Mô tả: Khối revolute miêu tả một bậc tự do quay đơn xung quanh một trục xác định 

giữa hai body. Khớp quay được xác định theo quy tắc bàn tay phải. 

e. Khối joint actuator

Mục đích: Tác động lực/mô men hoặc tạo chuyển động cho một khớp nguyên thủy. 

 

Mô tả : khớp giưã hai body thể hiện bậc tự do tương đối giữa những body. Khối Joint 

actuator kích hoạt một khối joint nối giữa hai body với một trong những tín hiệu sau: 

+ Một lực suy rộng: Lực do chuyển động tịnh tiến dọc theo khớp nguyên thủy lăng trụ 

hoặc mô men cho chuyển động quay quanh một khớp nguyên thủy quay. 

+ Một chuyển động: Chuyển động tịnh tiến cho khớp nguyên thủy lăng trụ, dưới dạng 

vị trí, vận tốc và gia tốc theo chiều dài. Tín hiệu vận tốc phải là đạo hàm của tín hiệu vị 

trí và gia tốc là đạo hàm cảu vận tốc. 

f. Khối Joint sensor

Mục đích: Đo chuyển động và lực /mô men của khớp nguyên thủy. 

 

41

Mô tả: Khối Joint sensor đo vị trí ,vận tốc và gia tốc của khớp nguyên thủy trong một 

khối Joint sensor đo chuyển động dọc/quay quanh trục khớp (hoặc quanh mộtđiểm đối 

với khớp cầu) trong hêt tạo độ quy chiếu đã xác định cho khớp nguyên thủy đó tronghộp 

thoại của khớp. joint nối một base body với một follwer quyêt định hướng chuyển động. 

c. Sơ đồ cấu trúc mô phỏng bằng SimMechanics:

 

Sơ đồ bộ điều khiển PD bù trọng trường cho một khớp:

 

42

Kết quả mô phỏng:

a.mô hình robot trước và sau khi quay. 

 

Vị trí ban đầu 

 

Sau khi quay với giá trị đặt 

 

      

 

 

 

 

 

 

 

 

 

 

 

43

b. Đồ thị quỹ đào các khớp: 

 

Đáp ứng của khớp 1,2,3 với giá trị đặt q1=45.q2=15,q3=-10. 

Kết luận:

    Nhờ việc chọn đúng thông số của hai bộ điều khiển Kp và Kd do vậy các đường đăc 

tính có chất lượng tốt, thời gian xác lập nhanh , độ quá điều chỉnh của các trường hợp gần 

như băng không. Bộ điều khiển bám theo quỹ đạo đặt. 

44

TÀI LIỆU THAM KHẢO

1. Bài giảng Robot công nghiệp ( TS. Nguyễn Phạm Thục Anh )

2. Robot công nghiệp – Nhà Xuất Bản Khoa Học Và Kỹ Thuật 2006 ( GS–TSKH.

Nguyễn Thiện Phúc )

3. Điều khiển Robot công nghiệp ( TS. Nguyễn Mạnh Tiến ) 

4. Matlab cho kỹ sư điều khiển và tự động hóa – Nhà xuất bản Khoa học và kỹ 

thuật 2007 ( GS-TSKH. Nguyễn Phùng Quang ) 

5. Tài liệu tham khảo trên mạng Internet.