MỤC LỤC
LỜI MỞ ĐẦU

4

CHƯƠNG I: PHÂN TÍCH LỰA CHỌN CẤU TRÚC ROBOT

5

I/ Phân tích mục đích ứng dụng của Robot:

5

II/ Phân tích yêu cầu kĩ thuật thao tác:

6

2.1. Đối tượng thao tác, dạng thao tác:

6

2.2. Yêu cầu về vị trí:

6

2.3. Yêu cầu về hướng của khâu thao tác:

6

2.4 Yêu cầu về vận tốc, gia tốc khi thao tác:

6

2.5

6

Yêu cầu về không gian thao tác:

III/ Xác định các đặc trưng kĩ thuật:

7

3.1. Số bậc tự do cần thiết:

7

3.2. Vùng làm việc có thể với tới của Robot:

7

3.3. Yêu cầu về tải trọng:

7

IV/ Các phương án thiết kế cấu trúc Robot, cấu trúc các khâu khớp, phân tích,
chọn phương án thực hiện:
7
4.1. Các phương án thiết kế cấu trúc Robot:

7


4.2. Phân tích và lựa chọn cấu trúc Robot:

8

5.1. Thiết kế 3D:

9

5.2. Các thông số đặc trưng hình học – khối lượng:

9

CHƯƠNG II: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG

10

I/ Khảo sát động học thuận, khảo sát động học ngược:

10

1.1. Đặt hệ trục tọa độ theo Denavit – Hartenberg

10

1.2. Thiết lập bảng DH

11

1.3. Ma trận Denavit – Hartenberg


12

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

13

1.5.

Khảo sát bài toán động học ngược

19

II/ Xây dựng quy luật chuyển động thao tác của robot.

20

CHƯƠNG III: BÀI TOÁN TĨNH HỌC

27

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

33

I/ Phương trình Lagrange dạng ma trận:
~1~

33



II/ Thiết lập phương trình vi phân chuyển động của robot:

37

CHƯƠNG V: THIẾT KẾ HỆ DẪN ĐỘNG CHO ROBOT

43

I/ Tính toán hệ dẫn động:

44

1.1. Tính chọn động cơ cho khớp 1:

45

II/ Tính toán thiết kế bộ truyền bánh răng

50

2.1. Xác định khoảng cách trục

50

2.2. Xác định thông số ăn khớp

51

III/ Kiểm nghiệm bộ truyền bánh răng


52

3.1. Kiểm nghiệm độ bền tiếp xúc

52

3.2. Kiểm nghiệm độ bền uốn

53

IV/ Tính toán thiết kế khớp nối:

56

4.1. Tính chọn khớp nối

56

4.2. Chọn khớp nối

56

V/ Tính toán thiết kế trục:

57

5.1. Xác định lực và sơ đồ đặt lực

57


5.2. Xác định đường kính trục

57

5.3. Kiểm nghiệm độ bền trục

58

VI/ Chọn ổ lăn:

62

VII/ Thiết kế 3D và kiểm nghiệm bền các khâu:

64

CHƯƠNG VI: THIẾT KẾ ĐIỀU KHIỂN ROBOT

68

I/ Điều khiển phản hồi và điều khiển vòng kín:

68

II/ Thiết kế bộ điều khiển PD

69

2.1. Bộ điều khiển PD không bù trọng lực:


69

2.2. Bài toán điều khiển

69

KẾT LUẬN

76

TÀI LIỆU THAM KHẢO

77

~2~


LỜI MỞ ĐẦU
Chúng ta đang sống trong thời đại nền công nghiệp 4.0 - nền công nghiệp sản
xuất hàng loạt với sự trợ giúp của robot, máy tự động dưới sự điều khiển của máy tính.
Nền khoa học kỹ thuật ngày căng phát triển mạnh mẽ dẫn tới thay đổi lớn trong sản
xuất. Sự thay đổi lực lượng sản xuất trong mọi ngành nghề, sự thay thế hoạt động chân
tay của con người bằng máy móc, robot.
Robot có được vị trí như vậy là nhờ chúng có những ưu điểm đặc biệt về chất lượng,
độ chinh xác và tính kinh tế. Robot có thể làm việc không bị ảnh hưởng bởi những yếu
tố chủ quan, khách quan như có thể làm việc không biết mệt mỏi, làm việc trong môi
trường ô nhiễm, độc hại, làm việc nơi có nhiệt độ/áp suất cao, làm việc nơi nguy hiểm,
… không những vậy nhờ những thiết kế cơ khí chính xác và những thuật toán điều
khiển mà robot có khả năng làm những công việc yêu cầu độ cẩn thận, tinh tế, làm

việc chính xác không có sự nhầm lẫn như con người.
Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay máy Robot
đã dùng ở nhiều lĩnh vực sản xuất, xuất phát từ những ưu điểm mà tay máy Robot đó
và đúc kết lại trong quá trình sản xuất làm việc, tay máy có những tính năng mà con
người không thể có được, khả năng làm việc ổn định, có thể làm việc trong môi trường
độc hại, v.v…Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy Robot phục vụ
cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và tương lai.
Ngày nay việc dùng robot ứng dụng trong ngành công nghiệp cơ khí nặng là một trong
những nhu cầu rất cần thiết , việc di chuyển một cách khéo léo và chính xác là điều
khó khăn đối với con người, chính vì thế mà việc nghiên cứu chế tạo ra một thiết bị
như cánh tay robot để làm được việc đó có ý nghĩa rất lớn.
Việc tìm hiểu nghiên cứu Robot trong khuôn khổ môn học Tính toán thiết kế robot sẽ
là cơ sở để chúng em tính toán, thiết kế cũng như điều khiển các loại Robot trong công
nghiệp phục vụ sản xuất. Cụ thể, ở đây chúng em chọn đề tài tính toán, thiết kế mô
hình Robot ứng dụng gắp chi tiết đúc từ máy công nghệ đưa ra vị trí tháo khuôn.

~3~


CHƯƠNG I: PHÂN TÍCH LỰA CHỌN CẤU TRÚC
ROBOT
I/ Phân tích mục đích ứng dụng của Robot:
Trong ngành công nghiệp sản xuất cơ khí nặng, Công nghệ Đúc là công nghệ
chế tạo sản phẩm bằng phương pháp rót vật liệu ở dạng chảy lỏng vào khuôn để tạo ra
sản phẩm có hình dạng theo khuôn mẫu. Đa phần công nghệ đúc thực hiện với các vật
liệu kim loại. Việc gắp chi tiết đúc từ máy công nghệ đưa ra vị trí tháo khuôn là những
công việc lặp đi lặp lại rất đơn giản và nhàm chán, nếu sử dụng công nhân trực tiếp
làm việc này thì năng suất đạt được không cao, đồng thời gây căng thẳng mệt mỏi, ảnh
hưởng đến chất lượng và tiến độ công việc. Ngoài ra việc áp dụng khoa học công nghệ
sử dụng robot thay thế con người trong công việc này đang là xu thế, robot làm việc

không biết mệt mỏi, không bị ảnh hưởng bởi tâm lí và ít bị tác động bởi môi trường,
năng suất cao, độ chính xác tin cậy cao, tính ra giá trị lợi nhuận thu về thì sử dụng
robot rẻ hơn lao động con người rất nhiều. Do vậy, nhóm chúng em quyết định sẽ thiết
kế một mô hình robot để ứng dụng trong việc gắp chi tiết đúc từ máy công nghệ đưa ra
vị trí tháo khuôn.

II/ Phân tích yêu cầu kĩ thuật thao tác:
2.1. Đối tượng thao tác, dạng thao tác:
Đối tượng thao tác ở đây là chi tiết đúc được sản xuất từ máy công nghệ.
Dạng thao tác: Khâu tác động cuối cùng để gắp chi tiết đúc là hệ thống kẹp chi tiết và
nhấc lên vuông góc với bề mặt chứa chi tiết đúc và di chuyển theo chiều tịnh tiến.

~4~


2.2. Yêu cầu về vị trí:
Các chi tiết đúc khi được sản xuất từ máy công nghệ nằm trên một băng chuyền, khâu
tác động cuối của robot phải đưa đến đúng vị trí để tháo khuôn.
2.3. Yêu cầu về hướng của khâu thao tác:
Hướng của khâu thao tác không thể tuỳ ý mà phải phù hợp với hướng định trước của
chi tiết đúc được sản xuất ra từ máy công nghệ trên băng chuyền. Hướng của khâu tác
động cuối nằm trên mặt phẳng song song với mặt đất XoYo, định hướng bằng các góc
quay quanh trục Zo.
2.4 Yêu cầu về vận tốc, gia tốc khi thao tác:
Yêu cầu về vận tốc: Mỗi chu kỳ làm việc khoảng 4-5 giây.
Yêu cầu về gia tốc: Gia tốc của robot không được quá cao và thay đổi đột ngột có thể
làm chi tiết đúc ra khỏi đầu kẹp của khâu tác động cuối.
2.5 Yêu cầu về không gian thao tác:
Băng chuyền đã có, mỗi băng chuyền rộng 500mm, cao 1000mm, nằm vuông góc với
nhau. Không gian thao tác yêu cầu của robot cần phải bao quát được cả 2 bằng chuyển

để có thể lấy từ bằng chuyền 1 đưa sang băng chuyền 2 một cách dễ dàng. ( Ở đây
băng truyền 1 chính là vị trí ban đầu của chi tiết đúc khi vừa được sản xuất từ máy
Công nghệ, băng truyền 2 là vị trí mình đưa chi tiết đến để tháo khuôn)

III/ Xác định các đặc trưng kĩ thuật:
3.1. Số bậc tự do cần thiết:
Nhiệm vụ của robot là gắp các chi tiết đúc được đưa ra từ máy công nghệ rồi nhận biết
và đưa nó về vị trí để tháo khuôn sao cho đúng vị trí yêu cầu. Robot dự định thiết kế sẽ
là robot 3 bậc tự do RRT,RTT,TTT,TRT...
3.2. Vùng làm việc có thể với tới của Robot:
Là vị trí xa nhất mà robot có thể với tới, đó sẽ là vị trí chi tiết đúc ở vị trí xa nhất khi
được đưa ra từ máy công nghệ trên băng chuyền.
3.3. Yêu cầu về tải trọng:
Trong thực tế, chi tiết đúc ta có thể ước tính nặng trung bình khoảng 1kg/1chi tiết, mỗi
lần Robot thao tác được với 4 chi tiết, như vậy tải trọng ngoài tác dụng lên Robot sẽ là
4kg. Ta lấy tải trọng tính toán ngoài là 4 kg.

IV/ Các phương án thiết kế cấu trúc Robot, cấu trúc các khâu
khớp, phân tích, chọn phương án thực hiện:
Dựa vào yêu cầu kỹ thuật thao tác nhóm chúng em lên ý tưởng Robot dự định thiết kế
sẽ là robot 3 bậc tự do RRT,RTT,TTT,TRT...

~5~


Dự định thiết kế tay gắp chi tiết đúc sẽ là hệ thống kẹp và nhấc lên vuông góc với bề
mặt chứa các chi tiết đúc và di chuyển theo chiều tịnh tiến.
4.1. Các phương án thiết kế cấu trúc Robot:

Phương án 1


Phương án 2

~6~


Phương án 3

Phương án 4

4.2. Phân tích và lựa chọn cấu trúc Robot:
Như vậy để robot có thể hoạt động đúng yêu cầu thì cần có ít nhất 3 bậc tự do: 2 bậc
để xác định tọa độ điểm trong mặt phẳng, 1 bậc để xác định chiều cao trong không
gian.
Sau khi phân tích ,trong các cấu trúc đề xuất, ta thấy cấu trúc 2,3,4 kém linh hoạt do có
2 khâu tịnh tiến. Tuy nhiên cấu trúc 1 là đơn giản, gọn hơn cả. Chính vì thế nhóm lựa
chọn cấu trúc 1: Robot 3 bậc tự do RRT, gồm 2 khâu quay để xác định vị trí chi tiết
đúc trên mặt phẳng, 1 khâu tịnh tiến để xác định độ cao chi tiết đúc.

Khớp 1 là khớp quay, dùng bộ truyền bánh răng trụ răng thẳng.
Khớp 2 là khớp quay, dùng bộ truyền bánh răng trụ răng thẳng.
Khớp 3 là khớp tịnh tiến, dùng xi lanh khí nén.
Hành Trình: Khâu 1: 0-90 độ
Khâu 2: 0-90 đọ
Khâu 3: 300mm

~7~


V/ Thiết kế mô hình Robot:

5.1. Thiết kế 3D:
Dựa trên sự phân tích ở phần trước, nhóm đã thiết kế mô hình 3D trên phần mềm
Solidworks

5.2. Các thông số đặc trưng hình học – khối lượng:

mm

~8~


CHƯƠNG II: THIẾT KẾ QUỸ ĐẠO CHUYỂN
ĐỘNG
I/ Khảo sát động học thuận, khảo sát động học ngược:
1.1. Đặt hệ trục tọa độ theo Denavit – Hartenberg
Đối với các robot công nghiệp, Denavit – Hartenberg (1995) đã đưa ra cách chọn các
hệ trục tọa độ có gốc tại khớp thứ i như sau:
-

Trục zi-1 được chọn dọc theo trục của khớp động thứ i.

-

Trục xi-1 được chọn dọc theo đường vuông góc chung của 2 trục z i-2 và zi-1,
hướng đi từ trục zi-2 sang zi-1. Nếu trục zi-1 cắt trục zi-2 thì hướng của trục xi-1
được chọn tùy ý, miễn là vuông góc với zi-1. Khi 2 trục zi-1 và zi-2 song song với
nhau, trục xi-1 có thể chọn hướng theo pháp tuyến chung nào cũng được.

-


Gốc tọa độ Oi-1 được chọn tại giao điểm của trục xi-1 và trục zi-1

- Trục yi-1 được chọn sao cho hệ (Oxyz)i-1 là hệ quy chiếu thuận.
Chú ý:
-

Đối với hệ tọa độ (Oxyz)0 theo quy ước trên ta mới chỉ chọn được trục z 0, còn
trục x0 chưa có trong quy ước trên. Ta có thể chọn trục x 0 được chọn một cách
tùy ý, miễn là x0 vuông góc với z0.

-

Đối với hệ tọa độ (Oxyz)n, do không có khớp n+1, nên theo quy ước ta không
xác định được trục zn. Trục zn không được xác định duy nhất, trong khi trục x n
lại được chọn theo pháp tuyến của trục z n-1. Trong trường hợp này, nếu khớp là
khớp quay ta có thể chọn trục zn song song với trục zn-1. Ngoài ra ta có thể chọn
tùy ý sao cho hợp lý.

-

Khi khớp thứ i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục z i-1 một cách tùy
ý. Người ta thường chọn trục zi-1 dọc theo trục của khớp tịnh tiến này.

Từ quy tắc trên ta xây dựng được các hệ tọa độ như hình vẽ:

~9~


Hình 2.1 Hệ trục tọa độ của robot


1.2. Thiết lập bảng DH
Vị trí của hệ tọa độ khớp (Oxyz)i đối với hệ tọa độ khớp (Oxyz)i-1 được xác định
bởi bốn tham số Denavit- Hartenberg  i , d i , ai ,  i như sau:
-

i : góc quay quanh trục để trục trùng với trục ()

-

d i : dịch chuyển tịnh tiến dọc trục để gốc tọa độ chuyển đến là giao điểm của
trục và trục

-

ai : dịch chuyển dọc trục để điểm chuyển đến điểm

-

 i : góc quay quanh trục sao cho trục () trùng với trục

Hình 2.2. Các tham số Denavit – Hartenberg
Như vậy từ các hệ tọa độ đã xây dựng ở trên, ta có các tham số DH như sau:
Bảng 2.1. Các tham số Denavit – Hartenberg

~ 10 ~


Khâu

i


di

ai

i

1

1

d1

a1

0

2

2

d2

a2

0

3

0


d3

0

0

Trong đó 1 , 2 ,d3 là các biến khớp, còn a1 , a 2 ,d1 , d 2 là hằng số. Ta đặt

1  q1 ,  2  q2 , d3  q3

1.3. Ma trận Denavit – Hartenberg
Ta có thể chuyển hệ tọa độ khớp (Oxyz)i-1 sang hệ tọa độ (Oxyz)i bằng bốn phép
biến đổi cơ bản như sau:
-

Quay quanh trục zi-1 một góc i .

-

Dịch chuyển tịnh tiến dọc trục zi-1 một đoạn di.

-

Dịch chuyển tịnh tiến dọc trục xi một đoạn ai.

- Quay quanh trục xi một góc  i .
Như vậy ma trận của phép biến đổi hệ tọa độ khớp (Oxyz) i-1 sang hệ tọa độ (Oxyz)i ,
kí hiệu là


i 1

Ai , là tích của 4 ma trận biến đổi cơ bản và có dạng như sau:

cosi
�
�
sin 
i 1
Ai = � i
�0
�
�0
cosi
�
�
sin
� i
�0
�
�0

 sin i
cosi
0
0

0 0 ��
1
�

�
0 0 0
��
1 0 ��
0
��
0 1 ��
0

0 ��
1
�
�
1 0 0 0
��
0 1 di ��
0
��
0 0 1 ��
0

sin isin i

0 0 ai ��
1
0
�
�
1 0 0 0 cos  i
��

0 1 0 ��
0 sin  i
��
0 0 1 ��
0
0

0
 sin  i
cos  i
0

0�
0�
�
0�
�
1�

aicos i �
cos i cosi -sin icosi aisini �
�
 2.1
sin i
cosi
di �
�
0
0
1 �

Ma trận được xác định bởi công thức (2.1) được gọi là ma trận Denavit – Hartenberg.
Nó cho ta biết thông tin về vị trí của khâu thứ i của robot đối với hệ quy chiếu (Oxyz) i1.
Áp dụng liên tiếp các phép biến đổi (2.1) đối với robot n khâu ta có:
0

-cos isin i

0 0

0
R
�
An  A A2 ... An  � T n
�0
0

1
1

n 1

rE �
�
1 � (2.2)

~ 11 ~


r  x , y ,z
trong đó: E  E E E  là véc tơ mô tả vị trí điểm tác động cuối trong hệ tọa độ

T

(Oxyz)0
0

Rn là ma trận cosin chỉ hướng của khâu thao tác đối với hệ tọa độ (Oxyz)
0

1.4. Bài toán động học thuận robot
Nhiệm vụ của bài toán động học thuận là xác định vị trí của khâu thao tác, hay nói
cách khác là vị trí điểm tác động cuối và hướng của khâu thao tác đối với hệ tọa độ cố
định với điều kiện các biến khớp đã biết. Ở đây ta sẽ xác định từ ma trận DH của khâu
0

thao tác A3
Tính các ma trận DH
Đặt cosq1 = C1, sinq1 = S1, cosq2 = C2, sinq2 = S2, cos(q1 + q2) = C12, cos(q2 + q3) = C23,
và thay các giá trị trong bảng thông số DH vào công thức (2.1) và ta được các ma trận
biến đổi DH giữa các hệ trục tọa độ như sau:

cos(q1 (t ))  sin( q1 (t ))
�
�
sin(q (t )) cos(q1 (t ))
0
A1  � 1
� 0
0
�
0

� 0

0 a1 cos(q1 (t )) � �
C1  S1
�
�
0 a1 sin( q1 (t ))
S
C1
� �1
� �0
1
d1
0
��
0
1
0
� �0

0 a1C1 �
0 a1S1 �
�
1 d1 �
�
0 1 �

(2.3)

cos(q2 (t ))  sin( q2 (t ))

�
�
sin(q (t )) cos(q2 (t ))
1
A2  � 2
� 0
0
�
0
� 0
1
�
�
0
2
A3  �
�
0
�
0
�

0 0

0 ��
1
�
�
1 0
0

0
� �
0 1 q3 (t ) � �
0
��
0 0
1 ��
0

0 a2 cos( q2 (t )) � �
C2
0 a2 sin( q2 (t )) � �
S
� �2
� �0
1
d2
��
0
1
� �0

 S2
C2
0
0

0 a2C2 �
0 a2 S 2 �
�

1 d2 �
�
0
1 �

0 0

0�
1 0 0�
�
0 1 d3 �
�
0 0 1�

cos(q1 (t )  q2 (t ))  sin(q1 (t )  q2 (t ))
�
�
sin(q (t )  q2 (t )) cos(q1 (t )  q2 (t ))
0
A2  0 A1.1 A2  � 1
�
0
0
�
0
0
�
C12  S12 0 a1C1  a2C12 �
�
�

0
0
S12 C12 0 a1S1  a2 S12 � �
�
� �R2  q  r2  q  �

�
�0
0
1
d1  d 2 � � 0T
1 �
�
�
0
0
1
�0
�

~ 12 ~

0 a2 cos( q1 (t )  q2 (t ))  a1 cos(q1 (t )) �
0 a2 sin(q1 (t )  q2 (t ))  a1 sin(q1 (t )) �
�
�
1
d1  d 2
�
0

1
�

( 2.4)


C12
�
�
S
0
0
1
2
A3  A1. A2 . A3  �12
�0
�
�0

 S12
C12
0
0

0 a1C1  a2C12 �
0
0 a1S1  a2 S12 � �
� �R3  q 
1 d1  d 2  q3 � � 0T
�

0
1
�

0

r3  q  �
�
1 �

(2.5)
Xác định vị trí điểm tác động cuối:
Từ (2.2) và (2.5) suy ra:

rE   a2 cos(q1  q2 )  a1 cos( q1 ) a2 sin(q1  q2 )  a1 sin( q1 ) q3  d 2  d1 

T

r  x , y ,z
Mà E  E E E  nên:
T

xE  a2 cos( q1  q2 )  a1 cos( q1 )
yE  a2 sin(q1  q2 )  a1 sin( q1 )

(2.6)

z E  q3  d 2  d1
Xác định hướng khâu thao tác.
Theo (2.2) và (2.5) suy ra:


cos(q1  q2 )  sin(q1  q2 ) 0 �
�
0
R3  �
sin(q  q ) cos(q1  q2 ) 0 �
� 1 2
�
�
0
1�
� 0
�
0
Hướng của khâu thao tác so với hệ tọa độ (Oxyz) 0 được xác định qua ma trận R3 .

Ngoài ra còn có thể xác định qua các góc α,β, η trong phép quay Cardan.
Chú thích: Phép quay Cardan là phép biến đổi hệ tọa độ cố định (Oxyz)i sang hệ tọa độ
động (Oxyz)i+3 bằng cách quay liên tiếp quanh các trục của hệ tọa độ động, cụ thể là:
-

Quay (Oxyz)i góc α quanh trục xi. Hệ (Oxyz)i (Oxyz)i+1

-

Quay (Oxyz)i+1 góc β quanh trục yi+1. Hệ (Oxyz)i+1(Oxyz)i+2

-

Quay (Oxyz)i+2 góc η quanh trục zi+2. Hệ (Oxyz)i+2 (Oxyz)i+3


~ 13 ~


zi

zi1

zi 2 , zi 3

yi  3







O

xi , xi1

yi 1 , yi2
yi




xi 2


xi 3

Hình 2.3. Các góc quay Cardan
Ma trận quay Cardan biểu diển hướng của hê tọa độ động (Oxyz)i+3 so với hệ tọa độ
cố định (Oxyz)I là tích của ba ma trận quay thành phần cơ bản, được tính
theo công thức sau:
cos cosη
�
�
 RCD   �sin sin cos +cos sin
�
-cos sin cos +sin sin
�

-cos sin
sin �
-sin sin sin +cos cos -sin cos �
�
cos sin sin +sin cos cos cos �
�

 2.7 

0
Như vậy bằng cách so sánh 2 ma trận quay R CD và R 3 , ta có thể tìm được các góc

Cardan  ,  ,  biểu diễn hướng của khâu thao tác đối với hệ tọa độ cố định (Oxyz)0.
0

R3  RCD


hay:

cos(q1 +q 2 ) -sin(q1 +q 2 ) 0 � �
cosβcosη
-cosβsinη
sinβ �
�
�
�
�
sin(q +q ) cos(q1 +q 2 ) 0 = sinαsinβcosη+cosαsinη -sinαsinβsinη+cosαcosη -sinαcosβ �
� 1 2
��
�
�
�
�
�
0
0
1
-cosαsinβcosη+sinαsinη
cosαsinβsinη+sinαcosη
cosαcosβ
�
��
�
So sánh các phần tử tương ứng của 2 ma trận, kết hợp hệ trục tọa độ trong hình 2.2 ta có:


 2.8

 = 0,  =  ,  = q1  q2
Tính vận tốc, gia tốc điểm tác động cuối và vận tốc góc, gia tốc góc các khâu.

Sau khi đã biết các biến khớp q1 , q2 , q3 và các vận tốc khớp, gia tốc khớp, ta sẽ đi tính
vận tốc góc, gia tốc góc các khâu cũng như vận tốc, gia tốc điểm tác động cuối.
- Vận tốc, gia tốc điểm thao tác cuối vE :

&
v E =r&
E = xE

z&E 

T

a E =v&E =  &
x&E &
y&E &
z&E 

T

y&E

(2.9)

~ 14 ~



- Vận tốc góc, gia tốc góc các khâu:
Toán tử sóng của véc tơ vận tốc góc khâu i trong hệ tọa độ động (Oxyz) i được tính
theo công thức sau:

%11 ω
%12
ω
�
i
%i = 0 R iT 0 R&i = �
%21 ω%22
ω
ω
�
%31 ω
%32
�
ω
�

ω%13 �
ω%23 �
�
ω%33 �
�

(2.10)

Khi đó vận tốc góc của khâu i tính trong hệ tọa độ động (Oxyz)i là:

i

ωi =  ω%32

%13
ω

%21 
ω

T

(2.11)

a, Tính vận tốc góc, gia tốc góc các khâu
- Xét khâu 1, từ (2.2) và (2.3) ta có:
C1 -S1 0 �
�
�
0
R1 = �
S1 C1 0 �
�
�
�0 0 1 �
�
Suy ra:
1

%1 = R

ω
0

T 0
1

C1 S1 0 ��
-q&1S1 -q&1C1 0 � �0 -q&1 0 �
�
�
�
�
�q& 0 0 �
R&1 = �
-S1 C1 0 ��
q&1C1 -q&1S1 0 �
=
� �1
�
�
�
�
�
�
�
0
0
1
0
0

0
0
0
0
�
��
��
�

Như vậy vận tốc góc và gia tốc góc của khâu 1 tính trong hệ tọa độ động gắn với
khâu là:
T
1
ω1 =  0 0 q&1 
1

&1 =  0 0 &
ε1 = 1 ω
q&
1

- Xét khâu 2, từ (2.2) và (2.4) ta có:
C12 -S12 0 �
�
�
0
R2 = �
S12 C12 0�
�
�

0 1�
�0
�
Suy ra:
2

%2 = R
ω
0

T 0
2

C12
�
�
R&2 = �
-S12
�
�0

S12
C12
0

0 ��
-(q&1 +q&2 )S12
�
�
0 ��

(q&1 +q&2 )C12
1�
��
� 0

-(q&1 +q&2 )C12
-(q&1 +q&2 )S12
0

0� � 0
�& &
0�
=
 q1 +q 2 
��
0�
��
� 0

-  q&1 +q&2 
0
0

Như vậy vận tốc góc, gia tốc góc của khâu 2 tính trong hệ tọa độ động gắn chặt với khâu là :

~ 15 ~

0�
�
0�

0�
�


2

ω2 =  0 0 q&1 +q&2 

2

&
&
ε 2 = 2 ω&2 =  0 0 &
q&
1 +q 2 

T
T

- Xét khâu 3, từ (2.2) và (2.5) ta có:
C12 -S12 0 �
�
0
R3= �
S12 C12 0 �
�
�
�
�
0

0
1
�
�
Suy ra:

C12
�
�
3
0 T 0 &
ω%3 = R 3 R 3 = �
-S12
�
�0

0 ��
-(q&1 +q&2 )S12
�
0�
(q&1 +q&2 )C12
��
1�
��
� 0

S12
C12
0


-(q&1 +q&2 )C12
-(q&1 +q&2 )S12
0

0� � 0
�q&+q&
0�
 1 2
�= �
0�
��
� 0

-(q&1 +q&2 ) 0 �
0
0�
�
0
0�
�

Như vậy vận tốc góc, gia tốc góc của khâu 3 tính trong hệ tọa độ động gắn chặt với
khâu là :
3

ω3 =  0 0 q&1 +q&2 

3

&

&2 
&3 =  0 0 &
ε3 = 3 ω
q&1 +q

T
T

b, Tính vận tốc, gia tốc điểm tác động cuối
Từ (2.6) ta có véc tơ vị trí điểm tác động cuối là:

xE � �
a 2C12 +a1C1 �
�
�
�
rE = �
yE �
�
�= �a 2S12 +a1S1 �
�
zE �
�
��
�q 3 +d 2 +d1 �
�
Lần lượt đạo hàm rE theo thời gian, ta thu được vận tốc và gia tốc điểm tác cuối là:
�
& &
& �

�- a 2 ( q1 + q 2 )S12 -a1 q1 S1 �
�
�
v E =r&
a 2 ( q&1 + q&2 )C12 + a1 q&1 C1 �
E=�
�
�
q&3
�
�
�
�
2
�
&
& &
&
&
&
&2
& & �
�- a 2 ( q1 + q 2 )S12 - a1 q1 S1 -a1C1q1 -a 2C12  q1 +q 2  �
�
�
2
a E =v&E = �
-a 2S12  q&1 +q&2  +a 2 ( &
q&1 + &
q&2 )C12 - a1q&12S1 +a C1&

q&
1�
�
�
&
q&3
�
�
�
�

Mô phỏng bằng matlab động học thuận
Chọn quy luật chuyển động của các khâu của robot

~ 16 ~


Cho các hằng số: d1=600mm, a1=500mm, a2=400mm, d2=10mm
q1  

 
3 3

t; q2   
t ; q3  5t , t  0,1, 2,...
2 100
4 200

Các biến khớp:
Cho t=[0,100];

Khi đó vị trí của điểm tác động cuối:

5

xE  400sin( 
t )  500sin(
t)
4 200
100

5

yE  400 cos( 
t )  500 cos(
t)
4 200
100
zE  5t  610

Hình 2.4. Đồ thị q1(t)

Hình 2.5. Đồ thị q2(t)

Hình 2.6. Đồ thị q3(t)

Hình 2.7. Đồ thị xE

~ 17 ~



Hình 2.8. Đồ thị yE

Hình 2.9. Đồ thị zE

Hình 2.10. Miền làm việc của điểm cuối trong không gian (x,y,z)
1.5. Khảo sát bài toán động học ngược
Nhiệm vụ của bài toán động học ngược là tìm các biến khớp với điều kiện tọa
độ và hướng khâu thao tác đã biết. Cụ thể trong bài toán này, bài toán động học ngược
có nhiệm vụ tìm q1 , q2 , q3 với điều kiện xE , yE , z E đã biết.
tacó:

�xE = a2C12  a1C1
�
�yE = a2 S12  a1S1
�z  q  d  d
3
2
1
�E

xE , yE ta có:

-Bình phương 2 vế của phương trình
�xE2  a22C122  a12C12  2a2C12 a1C1
�2
2 2
2 2
�yE  a2 S12  a1 S1  2a2 S12 a1S1

~ 18 ~



xE2  yE2  a12  a22  2a2 a1 (C12C1  S12 S1 )
�
xE2  yE2  a12  a22  2a2 a1C2
xE2  yE2  a12  a22
cos(q 2 )=
2a2 a1
sin(q2 )  � 1  C22

q2  atan 2(sin(q 2 ),cos(q 2 ))
Ta thay sin(q2),cos(q2) vào 2 phương trình

xE , yE ta có:

�xE = (a2C2  a1 )C1  a2 S1S 2
�
�yE = a2 S 2C1 +(a2C2  a1 )S1

Giải hệ phương trình đại số tuyến tính:


a1  C2 a2
a2 S2

 S 2 a2
 a12  a22  2C1a1a2  xE2  y E2
a1  C2 a2

 S2 a2

 a2 (C 2 xE  S 2 yE )  xE a1
a1  C2 a2

1 

xE
yE

2 

a1  C2 a2
S 2 a2

xE
 a2 (C2 yE  S 2 xE )  yE a1
yE

Từ đó ta có:
� cos(q1 )=
sin( q1 )=

a2 (C 2 xE  S 2 yE )  xE a1
xE2  yE2

a2 (C 2 yE  S2 xE )  yE a1
xE2  yE2

q1  atan 2(S1,C1 )
Từ:


z E  q3  d 2  d1  q3  z E  (d 2  d1 )

Như vậy đã giải quyết xong bài toán động học ngược.

II/ Xây dựng quy luật chuyển động thao tác của robot.
2.1. Khái niệm, nhiệm vụ của thiết kế quỹ đạo
Thiết kế quỹ đạo chuyển động là xây dựng quy luật chuyển động cho các khâu của
robot đảm bảo thỏa mãn yêu cầu về vị trí, hướng, hoặc thỏa mãn cả vị trí và hướng của

~ 19 ~


khâu thao tác trong không gian theo thời gian. Mục tiêu là tạo ra bộ tham số đầu vào
cho hệ thống điều khiển chuyển động để đảm bảo Robot thực hiện theo quỹ đạo đã
được lập trình.
Có hai kỹ thuật thiết kế quỹ đạo chính là:
-Thiết kế quỹ đạo trong không gian khớp.
-Thiết kế quỹ đạo trong không gian thao tác.
Các thông số của Robot:
Chiều dài các khâu: a1=0,5 m, a2=0,4m,d1=0,6m,d2=0,01m
Giới hạn các góc khớp:
q1 [min =
q2 [min =





2 , max = 2 ] (rad)




3
3
4 , max = 4 ] (rad)

q3 [min =0, max = 500] (mm)
2.2. Thiết kế quỹ đạo trong không gian khớp
Thiết kế quỹ đạo trong không gian khớp cho Robot
Trong trường hợp chuyển động từ điểm tới điểm, người ta có thể quan tâm hoặc không
quan tâm đến việc khâu thao tác đi qua một số điểm trung gian. Khi đó kỹ thuật thiết
kế quỹ đạo trong không gian khớp hay được sử dụng. Thiết kế quỹ đạo trong không
gian khớp đòi hỏi phải xây dựng được các hàm phụ thuộc thời gian của các biến khớp
và các đạo hàm bậc nhất, bậc hai của nó. Qua đó mô tả được chuyển động cần thiết
của Robot.
a. Đề bài
Đối với Robot, ta chọn nội dung thiết kế quỹ đạo như sau: Chọn hai điểm A, B bất
kì trong không gian làm việc, biết tọa độ (xE, yE, zE), hướng của khâu thao tác  và thời
gian làm việc tc. Yêu cầu xây dựng hàm phụ thuộc thời gian của các biến khớp và đạo
hàm bậc nhất của chúng, sao cho thỏa mãn điều kiện: điểm tác động cuối đi từ điểm A
đến điểm B trong thời gian tc, vận tốc tại điểm A và điểm B đều bằng 0.
b.Lời giải

Giả sử: khi điểm tác động cuối ở điểm A ( x A , y A , z A ,  A , t  t0 ), ta có:
qi  t0   i q0 , q&i  t0   i q&0
tại điểm B ( xB , y B , zB , B , t  tc ), ta có:
qi  tc   i qc , q&i  tc   i q&c

(2.12)
Chọn hàm phụ thuộc thời gian của biến khớp qi là đa thức bậc ba có dạng:


~ 20 ~


qi (t)= i a0 + i a1t + i a2t 2 + i a3t 3

(2.13)

i
i
i
i
Trong đó a0 , a1 , a2 , a3 làcác hằng số cần xác định.
Theo giả thiết tại thời điểm t0 và tc, Robot đứng yên nên:
i
q&0  0 ; i q&c  0
(2.14)
Đạo hàm phương trình (2.13) ta có:
q&i (t) = i a1 + 2 i a2t + 3 i a3t 2

(2.15)

Sử dụng (2.12), (2.13), (2.14),(2.15) ta nhận được hệ bốn phương trình sau:
�
qi (t0 ) = i a0 = i q0
�
qi (tc ) = i a0 + i a1tc + i a2tc2 + i a3tc3 = i qc
�
�
q&i (t0 ) = i a1 = 0

�
�
q&i (tc ) = i a1 + 2 i a2tc + 3 i a3t 2 c = i q&c = 0
�
i
a , i a1 , i a2 , i a3 theo các công thứcsau:
Giải các phương trình trên ta được các hệ số : 0
i
q0 - i qc
q0 - i qc
i
a0 = q0 ; a1 = 0;
a2 = - 3
; a3 = 2
(2.16)
tc2
tc3
Từ bốn hệ số trên ta thu được hàm phụ thuộc thời gian của các biến khớp dạng đa
thứcbậc ba (2.13). Những giá trị đó là tín hiệu đặt cho bộ điều khiển vị trí để truyển
động khớp di chuyển đến vị trí tương ứng.
Ví dụ
i

i

i

i

i


Bàitoán: Thiết kế quỹ đạo trong không gian khớp cho Robot sao cho điểm tác động cuối





50 2; 450 2;610
101.2; 341.9;660 
đi từ điểm A
đến điểm B 
trong thời gian tc =
10s.
Giải:
Trước hết ta giải bài toán động học ngược Robot, để tìm giá trị cụ thể các biến khớp tại
vị trí các điểm A và B.
Sử dụng các công thức ở tính toán động học ngược, ta tính toán được cụ thể:
Tại điểm



A 50 2; 450 2;610

 ,suy ra q
1

0

π
π

= - ; 2 q0 = - ; 3q0 = 0
4
2

3π 2
3π 3
;
q
=
; qc = 50
c
101.2; 341.9;660 
10
4
Tại điểm B 
suyra
a. Xây dựng đa thức phụ thuộc thời gian của các biến khớp.
Sử dụng công thức (2.16) với t0 = 0; tc = 10 ta nhận được các hệ số sau:
1

qc = -

Khớp 1:
π
-3π 1
π
1
a0 = - ; 1a1 =0; 1a2 =
; a3 =
4

2000
10000

~ 21 ~


Khớp 2:

-π
;
2

2

a0 =

3

a0 = 0 ;

2

2

a1 =0;

a2 =

-3π
;

400

2

a3 =

π
2000

Khớp 3:
3

a1 =0;

3

a2 =1.5 ;

3

a3 = -0.1

Suy ra:
q1(t) =

-π
3π 2
π 3
-3π
3π 2

t +
t ; q&1  t  =
t+
t ;
4 2000
10000
1000
10000

&
q&
1 t =

-3π
3π

t
1000 5000

q2 (t) = -

π
3π 2
π 3
-3π
3π 2
t +
t ; q&2  t  =
t
t ;

2 400
2000
200
2000

&
q&
2  t =

-3π
3π

t
200 1000

1 3
3
3
&
t ; q&3  t  = 3t - t 2 ; q&
t
3  t = 3 10
10
5
q1  t  , q2  t  , q3  t 
đã xây dựng ở trên, ta vẽ được các đồ thị sau:

q3 (t) = 1.5t 2 Với các đa thức

Hình 2.11. Đồ thị vị trí, vận tốc và gia tốc khớp 1 theo thời gian


~ 22 ~


Hình 2.12. Đồ thị vị trí, vận tốc và gia tốc khớp 2 theo thời gian

Hình 2.13. Đồ thị vị trí, vận tốc và gia tốc khớp 3 theo thời gian
2.3. Thiết kế quỹ đạo trong không gian thao tác
Thiết kế quỹ đạo trong không gian thao tác
Trong trường hợp chuyển động liên tục trên đường dịch chuyển, kỹ thuật thiết kế
quỹ đạo trong không gian thao tác hay được sử dụng.

~ 23 ~


Đối với Robot này, ta chọn bài toán thiết kế quỹ đạo trong không gian thao tác sao
cho quỹ đạo của điểm tác động cuối là đường thẳng từ điểm A đến điểm B (bất kì
trong vùng làm việc)
Với điểm A có tọa độ A(x0,y0,z0) và điểm B(xc,yc,zc), ta có phương trình đường
thẳng trong không gian thao tác :
y0 xc - yc x0
� yc - y0
y
=
x
+
� x -x
xc - x0
x - x0
y - y0

z - z0
�
c
0
=
=
��
 2.17 
z
z
z
x
z
x
xc - x0 yc - y0 zc - z0
�z = c 0 x + 0 c c 0
�
xc - x0
� xc - x0
Ta chọn x = x(t)là đa thức bậc 3 có dạng:
x  t   a0  a1t  a2 t 2  a3t 3
x t = x A , x  tc  = x B , v  t0  = 0, v  tc  = 0
Điều kiện:  0 
Tương tự như thiết kế quỹ đạo trong không gian khớp, ta tìm được các hệ số
a0 , a1 , a2 , a3 theo công thức :
a0  x  t0  ;

a1  0;

a2 


3  x  tc   x  t0  
t

2
c

;

a3 

2  x  tc   x  t0  

(2.18)

tc3

Ví dụ
Bài toán:Thiết kế quỹ đạo chuyển động trong không gian thao tác cho Robot, sao cho





50 2; 450 2;0
điểm tác động cuối di chuyển từ điểm A
đến điểm B
 101.2; 341.9;0  với quỹ đạo là đường thẳng trong thời gian 10s
Giải:
Ta chọn x = x(t) là đa thức bậc 3 có dạng:

x  t   a0  a1t  a2 t 2  a3t 3
x t = x A , x  tc  = x B , v  t0  = 0, v  tc  = 0
Điều kiện:  0 
Tương tự như thiết kế quỹ đạo trong không gian khớp, ta tìm được các hệ số
a0 , a1 , a2 , a3 theo công thức :

a0  x  t0  ;

a1  0;

a2 

3  x  tc   x  t0  

;

a3 

2  x  tc   x  t0  

tc2
tc3
Thay tọa độ của điểm A và điểm B, tc=10 vào công thức (2.19) ta tìm được:
a 0 =50 2;

a1 =0;

a2=




3 -101.2-50 2
100

 =-5,16;

a3=



-2 -101.2-50 2

x  t  = 50 2 - 5,16t 2 + 0.34t 3

341,9  450 2
( 450 2)( 101, 2)  341,9.50 2
x+
101, 2  50 2
101, 2  50 2
 1.71x  515.26
y=

~ 24 ~

1000

 =0.34

(2.19)


(2.20)


y  t  =  636.18 + 8.8t 2 - 0,58t 3
Với x(t), y(t), như trên ta vẽ được các đồ thị sau:

Hình 2.14. Đồ thị xE, yE
-Sau khi xác định được quỹ đạo chuyển động ta áp dụng bài toán động học ngược tìm
ra các quỹ đạo chuyển động của biến khớp phụ thuộc theo hàm thời gian t:
Lần lượt là:
q2  atan 2(sin(q 2 ),cos(q 2 ))

q1  atan 2(S1 ,C1 )
z E  q3  d 2  d1  q3  z E  (d 2  d1 )

~ 25 ~