đồ án thiết kế robot gắp vật 5 bậc tự do

LỜI NÓI ĐẦU
Ngày nay robot được dùng rộng rãi trong các nhà máy, được ứng dụng trong nhiều
ngành khoa học công nghệ ví dụ như trong kỹ thuật hàn, trong kỹ thuật gia công cơ
khí, trong khoa học vũ trụ, đại dương và trong nhiều ngành khoa học khác. Robot có
thể thay con người làm việc trong mọi điều kiện khắc nhiệt và những công việc đòi hỏi
độ chính xác cao. Trong tương lai robot sẽ được ứng dụng rộng rãi hơn trong đời sống
hàng ngày.
Ở nước ta lĩnh vực robot đã được nghiên cứu ở các trường đại học và trong các
viện nghiên cứu và đã đặt nên móng cho sự phát triển của ngành khoa học non trẻ này
ở Việt Nam. Trong công nghiệp Việt Nam, robot cũng được ứng dụng trong các dây
chuyền sản xuất của nhà máy nhằm nâng cao năng suất, hiệu quả lao động và chế tạo
các sản phẩm có độ chính xác cao.
Ta có thể khái quát định nghĩa robot theo cách nhìn của cơ học là
một chuỗi động, mỗi khâu được ghép với nhau bởi các khớp nối,
hoạt động linh hoạt nhờ hệ dẫn động. Các cơ cấu tác động này có
thể hoạt động phối hợp với nhau để thực hiện những công việc phức
tạp dưới sự điều khiển củamột bộ phận có cấu tạo như máy tính, còn
gọi là những bộ điều khiển PC -based. Với những đặc điểm về cấu tạo
và hoạt động thì robot thường được sử dụng trong các hệ thống sản
xuất linh hoạt dạng workcell (FMS -Flexoble Manufacturing Systems)
và các hệ thống sản xuất tích hợp máy tính (CIM – Computer
Integrated Manufacturing). Càng ngày các dây chuyền sản xuất tự
động có sử dụng robot thay thế dần các dây chuyền sản xuất tự
động với chương trình hoạt động “cứng” trước đây.
Cùng với sự phát triển của khoa học, tin học và các ứng dụng của nó ngày càng trở
nên quan trọng. Máy tính được sử dụng như là một công cụ thay thế con người trong
việc tính toán các bài toán phức tạp. Nó giúp chúng ta đưa ra kết quả nhanh và chính
xác. Chương trình Maple là một trong những phần mềm tính toán mạnh và phổ biến
giúp chúng ta giải nhiều loại bài toán như: Bài toán phân tích, thống kê, bài toán cơ
học, bài toán nhiệt, bài toán điện kỹ thuật…Ngoài ra nó còn là một ngôn ngữ lập trình
thông dịch khá mạnh giúp cho người kỹ sư tính toán các bài toán cơ học quen thuộc

nhanh chóng và dễ dàng. Được sự hướng dẫn chỉ bảo tận tình của thầy NGÔ VĂN
AN, chúng em đã lựa chọn đề tài: “ Thiết kế robot để thực hiện nhiệm vụ gắp vật từ kệ
tới băng tải ” .


CHƯƠNG 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP .
1.1 Tổng quan về Robot công nghiệp.
1.1.1 Lịch sử phát triển.
Thuật ngữ robot xuất hiện vào năm 1920 trong một tác phẩm văn học của nhà văn
Tiệp Khắc tên là Karel Capek.
Thuật ngữ Inducstrial Robot (IR) xuất hiện đầu tiên ở Mỹ do công ty AMF
(Americal Machine and Foundry Company) quảng cáo mô phỏng một thiết bị mang
dáng dấp và có một số chức năng như tay người được điều khiển tự động thực hiện
một số thao tác để sản xuất thiết bị có tên gọi Versatran.
Quá trình phát triển của IR được tóm tắt như sau:
-

Từ những năm 1950 ở Mỹ xuất hiện viện nghiên cứu đầu tiên.
Vào đầu những năm 1960 xuất hiện sản phẩm đầu tiên có tên gọi là Versatran
của công ty AMF.
Ở Anh người ta bắt đầu nghiên cứu và chế tạo IR theo bản quyền của Mỹ từ
những năm 1967.
Ở những nước Tây Âu khác như: Đức, Pháp, Ý, Thụy Điển thì bắt đầu chế tạo
IR từ nhưng năm 1970.
Châu Á có Nhật Bản bắt đầu nghiên cứu ứng dụng của IR từ những năm 1968.

Đến nay, trên thế giới có trên 600 công ty sản xuất IR trong số đó có 400 công ty
của Nhật Bản, 130 công ty của Tây Âu,70 công ty của Mỹ và một số công ty của Nga,
Tiệp Khắc … Trong đó 6/10 công ty sản xuất Robot lớn nhất thế giới của Nhật Bản.
1.1.2 Cấu trúc chung của Robot công nghiệp.

Một Robot công nghiệp được cấu thành bởi các hệ thống sau:
-

-

Tay máy (Manipulator) là cơ cấu cơ khí gồm các khâu, khớp. Chúng hình thành
cánh tay để tạo các chuyển động cơ bản, cổ tay tạo lên sự khéo léo, linh hoạt và
bàn tay (End Effector) để trực tiếp hoàn thành các thao tác trên đối tượng.
Cơ cấu chấp hành tạo chuyển động cho các khâu của tay máy. Nguồn động lực
của các cơ cấu chấp hành là động cơ các loại: điện, thủy lực, khí nén hoặc kết
hợp giữa chúng.


-

Hệ thống cảm biến gồm các sensor và thiết bị chuyển đổi tín hiệu cần thiết
khác. Các robot cần hệ thống sensor trong để nhận biết trạng thái của bản thân
các cơ cấu của robot và các sensor ngoài để nhận biết trạng thái của môi trường.
Hệ thống điều khiển (controller) hiện nay thường là máy tính để giám sát và
điều khiển hoạt động của robot.

-

1.1.3 Phân loại robot công nghiệp.
a. Phân loại theo kết cấu.
Lấy hai hình thức chuyển động nguyên thủy làm chuẩn:
-

Chuyển động thẳng theo các hướng X, Y, Z trong không gian ba chiều thông
thường tạo nên những khối hình có góc cạnh, gọi là Prismatic (P).

Chuyển động quay quanh các trục X, Y, Z kí hiệu (R).

Với ba bậc tự do, robot sẽ hoạt động trong trường công tác tùy thuộc tổ hợp P và R
ví dụ:
+ PPP trường công tác là hộp chữ nhật hoặc lập phương.
+ RPP trường công tác là khối trụ
+ RRP trường công tác là khối cầu.
+ RRR trường công tác là khối cầu.

Hình 1.0 : Robot làm việc trong tọa độ lập phương .


Hình 1.1 : Robot làm việc trong tọa độ trụ.

Hình 1.2 : Robot làm việc trong tọa độ cầu .
b. Phân loại theo hệ thống truyền động.
Các dạng phổ biến là:
-

Hệ truyền động điện : Thường dùng các động cơ điện 1 chiều (DC : Direct
Current) hoặc các động cơ bước (step motor). Loại truyền động nay dễ điều
khiển, kết cấu gọn.


-

-

Hệ truyền động thuỷ lực : Có thể đạt được công suất cao, đáp ứng những điều
kiện làm việc nặng. Tuy nhiên hệ thống thuỷ lực thường có kết cấu cồng kềnh,

tồn tại độ phi tuyến lớn khó xử lý khi điều khiển.
Hệ truyền động khí nén : Có kết cấu gọn nhẹ hơn do không cần dẫn ngược nhưng
lại phải gắn liền với trung tâm tạo ra khí nén. Hệ nay làm việc với công suất
trung bình và nhỏ, kém chính xác, thường chỉ thích hợp với các robot hoạt động
theo chương trình định sẵn với các thao tác đơn giản “nhấc lên - đặt xuống” (Pick
and Place or PTP : Point To Point).
c. Phân loại theo ứng dụng.

Cách phân loại này dựa vào ứng dụng của robot. Ví dụ, có robot công nghiệp, robot
dùng trong nghiên cứu khoa học, robot dùng trong kỹ thuật vũ trụ, robot dùng trong
quân sự…
d. Phân loại theo cách thức và đặc trưng của phương pháp điều khiển.
Có 2 kiểu điều khiển robot: điều khiển hở và điều khiển kín .
- Điều khiển hở, dùng truyền động bước (động cơ điện hoặc động cơ thủy lực, khí
nén…) mà quãng đường hoặc góc dịch chuyển tỷ lệ với số xung điều khiển. Kiểu này
đơn giản, nhưng đạt độ chính xác thấp.
- Điều khiển kín (điều khiển kiểu servo), sử dụng tín hiệu phản hồi vị trí để tăng độ
chính xác điều khiển. Có 2 kiểu điều khiển servo: điều khiển điểm - điểm và điều
khiển theo đường(contour).
1.1.4 Ứng dụng của Robot.
Robot được sử dụng trong nhiều lĩnh vực sản xuất và đời sống của con người.
Trong điều kiện làm việc khắc nhiệt, khó khăn đối với con người thì robot công nghiệp
đóng vai trò quan trọng và nó được sử dụng trong nhiều lĩnh vực công nghiệp khác
nhau như:
- Trong gia công cơ khí: thường sử dụng trong các máy hàn tự động, máy khoan,
dây truyền lắp ráp.
- Trong dây truyền sản xuất: tham gia vào một số dây truyền sản xuất như gia công,
phun sơn, gắp sản phẩm, đóng gói bao bì...
+ Trong vận tải thường để bốc xếp hàng hóa.
- Một số ứng dụng quan trọng của robot trong công nghiệp:

+ Phục vụ máy CNC và các hệ thống tự động linh hoạt.


+ Đúc.
+ Lắp ráp, đóng gói.
+ Phun, phủ.
+ Tự động hàn.
+ Đảm nhận thực hiện cấp phôi phục vụ các nguyên công trong các dây
chuyền sản xuất tự động.
+ Chế tạo máy.
+ Kiểm tra.
+ Sơn.
+ Y học

Hình 1.3 : Ứng dụng robot tự động trong sản xuất ôtô.

Hình 1.4 : Robot lắp ráp ôtô.


Hình 1.5 : Robot hàn tự động trong công nghiệp .

Hình 1.6 : Robot gia công chi tiết .

Hình 1.7 : Ứng dụng robot phẫu thuật trong y tế .


1.2 Tổng quan robot gắp vật trên băng truyền trong sản xuất:
1.2.1 Tổng quan về robot gắp vật trên băng truyền .
Nhiệm vụ, yêu cầu đặt ra là phải làm thế nào để có thể thay thế con người làm công
việc mang vác vật nặng từ kệ tới băng tải, thì robot gắp vật là một lựa chọn tối ưu nhất.

Robot gắp trong công nghiệp là tay máy tự động được đặt cố định hoặc di động bao
gồm thiết bị dạng tay máy có số bậc tự do hoạt động và thiết bị điều khiển theo
chương trình, có thể tái lập trình để hoàn thành các chức năng hoạt động và điều khiển
trong quá trình sản xuất. Robot gắp vật trong công nghiệp được cấu hình bởi các yếu
tố sau:
1) Tay máy là cơ cấu cơ khí gồm các khâu khớp chúng hình thành cánh tay để tạo
các chuyển động cơ bản, cổ tay tạo nên sự khéo léo linh hoạt, bàn tay hoàn thành thao
tác trên đối tượng.
2) Cơ cấu chấp hành tạo chuyển động cho các khâu cổ tay máy, động cơ là nguồn
động lực của các cơ cấu chấp hành.
3) Hệ thống cảm biến gồm các cảm biến và các thiết bị chuyển đổi tín hiệu cần thiết
khác, các robot cần hệ thống cảm biến trong để nhận biết trạng thái của bản thân, các
cơ cấu của robot và các cảm biến ngoài để nhận biết trạng thái của môi trường.
4) Hệ thống điều khiển hiện nay thường là máy tính để giám sát và điều khiển hoạt
động của robot. Kết cấu tay máy gắp sản phẩm.
5) Tay máy là phần cơ sở quyết định khả năng làm việc của RBCN đó là thiết bị
đảm bảo cho robot khả năng làm việc nâng hạ vật.
6) Ban đầu người ta chế tạo tay máy phỏng tay người, còn hiện nay tay máy rất đa
dạng và nhiều loại khác xa tay người, tuy nhiên vẫn sử dụng thuật ngữ như vai, cánh
tay, cổ tay, bàn tay và khớp để chỉ các bộ phận của tay máy.
1.2.2 Thông số ảnh hưởng robot.
Trong thiết kế tay máy người ta quan tấn đến các thông số ảnh hưởng khả năng làm
việc:
1) Sức nâng, độ cứng vững lực kẹp của tay.
2) Tầm với của vùng làm việc.
3) Khả năng định vị, định hướng phần công tác.
1.2.3 Một số hình ảnh robot gắp sản phẩm trong công nghiệp.


Hình 1.8 Robot gắp hộp .

Hình 1.9 Robot cắt và gắp gạch tuynel.

Hình 1.10 Robot gắp hàng.


1.2.4 Kết luận.
Trong điều kiện làm việc mang vác nặng và điều kiện làm việc khắc nhiệt trong
công nghiệp, thì robot gắp vật là một lựa chọn tối ưu nhất, vì nó có thể thay thế sức
người và làm việc được trong thời gian dài. Ngoài ra khả năng tự động hóa cao, dễ
dàng điều khiển lập trình khi thay đổi công việc mà mình muốn.
Việc sử dụng robot gắp vật trong dây truyền sản xuất có rất nhiều mặt tích cực. Nó
giúp chúng ta tiết kiệm sức lực, thời gian và tiền bạc. Khả năng vận hành chính xác
cao, tự động hóa, làm việc chính xác và thay thế một số vị trí của con người trong dây
truyền sản xuất.
Để thực hiện được các công việc thì robot phải được thiết kế chính xác ,đầu tiên
phải giải quyết được bài toán động học robot,bởi vì động học robot là cơ sở để điều
khiển các biến khớp trên robot từ đó sẽ xác định được vị trí và hướng làm việc của tay
kẹp robot thực hiện các công việc chúng ta đặt trước ,được giải quyết trong chương 2:
động học robot .

CHƯƠNG 2: ĐỘNG HỌC ROBOT.
2.1: Tổng hợp cấu trúc động học.
Yêu cầu công nghệ và nhiệm vụ đặt ra là gắp vật từ kệ tới băng tải thì robot 5 bậc
tự do là đảm bảo được yêu cầu. Vì ngoài độ linh động và khả năng mang vác được tải
nặng thì robot này còn dễ dàng dàng điều khiển và lặp lại chính xác .
Robot gắp vật có 5 bậc tự do bao gồm các khớp xoay. Mỗi khớp xoay được dẫn
động bởi một động cơ và được điều khiển bởi hệ thống điều khiển PLC .
2.1.1 Bài toán động học thuận.
Bài toán động học thuận: Đây là bài toán cho trước chương trình chuyển động dưới

dạng quan hệ hàm qi(t) của các biến khớp, ta cần phải xác định quy luật biến đổi của
các tham số động học đặc trưng cho chuyển động của các khâu. Việc giải bài toán
thuận của động học robot chủ yếu nhằm thiết lập phương trình động học robot và xác
định vị trí của tay kẹp.
a, Phương pháp Denavit- Hartenberg.
Cơ sở của phương pháp Denavit-Hartenberg (D-H).
Theo DH, tại mỗi khớp ta gắn một hệ trục toạ độ, quy ước về cách đặt hệ toạ độ
này như sau:


-

Trục zi được liên kết với trục của khớp thứ i+1. Chiều của zi được chọn
tuỳ ý.

-

Trục xi được xác định là đường vuông góc chung giữa trục khớp i và khớp
i+1, hướng từ điểm trục của khớp i tới khớp i+1. Nếu hai trục song song
thì xi có thể chọn bất kỳ là đường vuông góc chung hai trục khớp. Trong
trường hợp hai trục này cắt nhau, xi được xác định theo chiều của zi zi 1

-

( hoặc quy tắc bàn tay phải).
Trục yi được xác định theo xi và zi theo quy tắc bàn tay phải.

Hình 2.0 Hệ trục tọa độ và các tham số DH của robot n bậc
Các thông số động học Denavit – Hartenberg được xác định như sau:
-

di : khoảng cách O và O theo trục z
i-1
i
i-1.

-

 i : góc giữa 2 đường vuông góc chung. Là góc quay quanh trục
zi-1 để trục xi-1 chuyển đến trục xi theo qui tắc bàn tay phải.

b, Đặt bài toán.

-

 i : góc xoay đưa trục z về z quanh z theo quy tắc bàn tay phải.
i-1
i
i

-

ai : khoảng dịch chuyển giữa 2 trục khớp động kề nhau.


Biết giá trị các biến khớp qi (i = 1, 2, 3, 4, 5) cần phải xác định vị trí và hướng của
tay kẹp, tức là xác đinh ma trận

�nx sx ax px �
�

�
n
s
a
p
y
y
y
y
0
0 1 2
3
4
�
TE  T5  A1 . A2 . A3 . A4 . A5  �
�nz sz az pz �
�
�
�0 0 0 1  �
c, Đặt các hệ trục tọa độ.
-

Chọn O0x0y0z0 làm hệ cơ sở gốc.
+ Tâm O0 trùng với tâm khớp 1.
+ z0 trùng với trục khớp 1, chiều tùy ý.
+ x0 tùy chọn,miễn là x0 vuông góc với z0.
+ y0 được chọn sao cho hệ tạo thành một tam diện thuận.

-

Hệ trục tọa độ Oixiyizi :
+ zi đặt tại khớp tiếp theo tương tự z0.
+ xi là đường vuông góc chung nhỏ nhất nối từ zi-1 đến zi ( chiều từ zi-1 đến zi
).
+ Oi là giao điểm của xi và zi.
+ yi xác định theo quy tắc bàn tay phải.

-

Hệ trục tọa độ đặt tại khâu tác động cuối Onxnynzn
+ On trùng với điểm P.
+ yn nằm trong mặt phẳng kẹp, vuông góc với phương kẹp,chiều tùy ý.
+ zn (hoặc xi ) hướng tới đối tượng.
+ trục còn lại là xn (hoặc zn): vuông góc với mặt phẳng kẹp, chiều đảm bảo hệ
tạo thành một tam diện thuận.


- Quy tắc D-H quy ước cách đặt các hệ tọa độ lên Robot
- Quy tắc D-H sử dụng 4 phép biến đổi để đưa hệ tọa độ Oi về trùng với hệ tọa độ
Oi+1.


R(z,α) là phép quay quanh trục zi một góc α để đưa trục tọa độ xi về trùng
với xi+1.



T(z,d) là phép tịnh tiến theo phương z một lượng d để 2 hệ tọa độ Oi và
Oi+1 có cùng cao độ.



T(x,a) là phép tịnh tiến theo phương xi+1 một lượng a để hệ tọa độ Oi về
trùng với Oi+1 .



R(x,α) là phép quay quanh trục xi+1 một góc β để đưa trục tọa độ zi về
trùng với zi+1.

Hình 2.1 Mô hình hoá Robot 5 bậc tự do.
Bảng 2.1 Bảng D-H của robot gắp vật 5 bậc tự do .

Khớp

RZ ,

TZ ,d

1

 1 

d1

Tx ,a

Rx ,

0

900


2
3
4

 2 
 3 

0

a2

00

0

a3

00

 4 

0
d5

0
0

900

 

5

5

00

d, Động học thuận về vị trí của robot.
- Như vậy hệ trục thứ i sẽ mô tả ma trận vị trí và hướng so với hệ trục thứ i-1 thông
qua phép biến đổi tọa độ thuần nhất D-H như sau :

�cos( )  sin( ).cos( ) sin( ).sin( ) a.cos( ) �
�
sin( ) cos( ).cos( )  cos( ).sin( ) a.sin( ) �
i 1
�
�
Ai 
�0
sin( )
cos( )
d �
�
�
0
0

1
�0
�
- Ma trận chuyển đổi hệ tọa độ từ khâu 0 sang khâu 1:
Cos(1 )
�
�
Sin(1 )
�
� 0
�
= � 0

0 Sin(1 )
0�
0  Cos(1 ) 0 �
�
`
1
0
d1 �
�
0
0
1�

- Ma trận chuyển đổi hệ toạ độ từ khâu 1 sang khâu 2:
Cos( 2 )  Sin( 2 )
�
�

Sin( 2 ) Cos( 2 )
�
� 0
0
�
0
=� 0

0 a2 Cos( 2 ) �
�
0 a2 Sin( 2 ) �
`
�
1
0
�
0
1
�

- Ma trận chuyển đổi hệ toạ độ từ khâu 2 sang khâu 3:
Cos(3 )  Sin(3 )
�
�
Sin(3 ) Cos(3 )
�
� 0
0
�
0

=� 0

0 a3 Cos(3 ) �
0 a3 Sin( 3 ) �
�
`
�
1
0
�
0
1
�


- Ma trận chuyển đổi hệ toạ độ từ khâu 3 sang khâu 4:
Cos( 4 )
�
�
Sin( 4 )
�
� 0
�
=� 0

0 Sin( 4 )
0  Cos( 4 )
1
0
0

0

0�
0�
�
`
0�
�
1�

- Ma trận chuyển đổi hệ toạ độ từ khâu 4 sang khâu 5:
Cos(5 )  Sin(5 )
�
�
Sin(5 ) Cos( 5 )
�
� 0
0
�
0
=� 0

0 0�
0 0�
�
`
1 d 5�
�
0 1�

- Ma trận biến đổi tọa độ thuần nhất 0An biểu diễn trạng thái khâu thao tác có thể
xác định từ cấu trúc động học robot. 0An có thể nhận được bằng cách nhân liên tiếp các
ma trận biến đổi tọa độ thuần nhất tương ứng với các phép dịch chuyển hệ tọa độ từ hệ
trục cố định tới hệ trục tọa độ gắn với khâu thao tác:
0

An  0 A11 A2 2 A3 �n 1 An

- Từ đó ta xác định được vị trí và hướng của khâu gốc với khâu cuối hệ tọa độ:
0

A5

=0A1*1A2*2A3*3A4*4A5

- Giải ma trận bằng matlab:
* Các ký hiệu giá trị góc quay

1 ,  2 , 3 ,  4 , 5

thay bằng q1, q2, q3, q4, q5.

* Các bước thực hiện trên matlab như sau:
+ Khai báo các biến khớp q2, q3, q4, q5)
+ Khai báo các biến , (d1, a2, a3, d5)
3
+ Khai báo 5 ma trận: , , A4 ,

+Kết quả giải trong matlab ta tìm được ma trận T:

- Ma trận là ma trận 4x4 có dạng:

nx
�
�
n
TE  T50  A10 . A21 . A32 . A43 . A5 4  �y
�
nz
�
�0

sx

ax

sy
sz

ay
az

0

0

px �
�
py �

pz �
�
1  �

TE T50
* Sau khi nhân xong trên matlab ta có giá trị
=
là:

TE

= [ sin(q1)*sin(q5) + cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))),
cos(q5)*sin(q1) - sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))),
cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +
sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)),
d5*(cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +
sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) + a2*cos(q1)*cos(q2) +
a3*cos(q1)*cos(q2)*cos(q3) - a3*cos(q1)*sin(q2)*sin(q3) ]
[ - cos(q1)*sin(q5) - cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))),
sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) +
sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) - cos(q1)*cos(q5),
cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)),
d5*(cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) + a2*cos(q2)*sin(q1) +
a3*cos(q2)*cos(q3)*sin(q1) - a3*sin(q1)*sin(q2)*sin(q3)]
[ cos(q5)*(cos(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) + sin(q4)*(cos(q2)*cos(q3) sin(q2)*sin(q3))),
-sin(q5)*(cos(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) + sin(q4)*(cos(q2)*cos(q3) sin(q2)*sin(q3))),

sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - cos(q4)*(cos(q2)*cos(q3) sin(q2)*sin(q3)),
d1 + a2*sin(q2) - d5*(cos(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) + a3*cos(q2)*sin(q3) +
a3*cos(q3)*sin(q2) ]
[ 0,

0,

Cho


TE  T

nx

0,

1]

5
0

ta được hệ phương trình động học thuận robot:

=sin(q1)*sin(q5)+cos(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3)-

cos(q1)*sin(q2)*sin(q3))sin(q4)*(cos(q1)*cos(q2)*sin(q3)+cos(q1)*cos(q3)*si
n(q2)))


ny

=-cos(q1)*sin(q5)-cos(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3)-

cos(q2)*cos(q3)*sin(q1)+sin(q4)*(cos(q2)*sin(q1)*sin(q3)+cos(q3)*sin(q1)*si
n(q2)))


nz

= cos(q5)*(cos(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))+

sin(q4)*(cos(q2)* cos(q3) -sin(q2)*sin(q3)))


S X = cos(q5)*sin(q1) - sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2)))



Sy

= sin(q5)*(cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) +

sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) - cos(q1)*cos(q5)


sz = -sin(q5)*(cos(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) +
sin(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))



ax = cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +
sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))



ay

= cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -

sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))


az

= sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - cos(q4)*(cos(q2)*cos(q3) -

sin(q2)*sin(q3))




px

= d5*(cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +

sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
a2*cos(q1)*cos(q2) + a3*cos(q1)*cos(q2)*cos(q3) a3*cos(q1)*sin(q2)*sin(q3)


py

= d5*(cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -

sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
a2*cos(q2)*sin(q1) + a3*cos(q2)*cos(q3)*sin(q1) - a3*sin(q1)*sin(q2)*sin(q3)


pz = d1 + a2*sin(q2) - d5*(cos(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) + a3*cos(q2)*sin(q3) +
a3*cos(q3)*sin(q2)

2.1.2 Động học ngược robot.
a, Đặt bài toán.

T T
Cho vị trí và hướng của bàn kẹp tức là biết ma trận E

5
0

cần phải xác định các

biến khớp αi (i=1 ... 5) theo vị trí và hướng bàn kẹp.
b, Phương pháp giải bài toán.
- Input: Ma trận T là tích các ma trận thành phần đã tính ở chương 2
Ma trận A là tọa độ thực đã biết.
- Output: Kết quả của biến khớp qi (i = 1,2,3,4,5).
Nội dung của bài toán động học thuận là cho biết chuyển động của các tọa độ khớp,
ta cần xác định chuyển động của các tọa độ khâu thao tác.

Để giải quyết được bài toán động học ngược robot gắp vật 5 bậc tự do, ta sẽ dùng
phần mềm EXCEL, cụ thể hơn là gói công cụ Solver trong phần mềm đó.
Ngược lại trong bài toán động học ngược, cho biết chuyển động của các tọa độ thao
tác, ta cần xác định chuyển động của các tọa độ khớp. Các phương pháp giải bài toán
động học ngược được phân thành hai nhóm: các phương pháp giải tích và các phương
pháp số.
Sau đây em sẽ trình bày phương pháp giải tích để giải bài toán động học ngược cho
robot gắp vật 5 bậc tự do.


- Từ ma trận

TE  T

5
0

ta được hệ phương trình động học nghịch:

�
( PX  a14 ) ^ 2  0
�
( PY  a24 ) ^ 2  0
�
�
( PZ  a34 ) ^ 2  0
�
�
(A  a 23 )^2 =0
� Y

�
 S X  a12  ^ 2  0
�

Với a14, a24, a34, a12, a23 là các tọa độ thực đã biết.
Vì Robot gắp vật có 5 bậc tự do, nên ta chỉ cần cần thiết lập 5 phương trình để giải
ra 5 ẩn tương ứng:


px

= d5*(cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +

sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
a2*cos(q1)*cos(q2) + a3*cos(q1)*cos(q2)*cos(q3) a3*cos(q1)*sin(q2)*sin(q3)


py

= d5*(cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -

sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
a2*cos(q2)*sin(q1) + a3*cos(q2)*cos(q3)*sin(q1) - a3*sin(q1)*sin(q2)*sin(q3)


pz = d1 + a2*sin(q2) - d5*(cos(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) + a3*cos(q2)*sin(q3) +
a3*cos(q3)*sin(q2)



ay

= cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) -

sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))


S X = cos(q5)*sin(q1) - sin(q5)*(cos(q4)*(cos(q1)*cos(q2)*cos(q3) cos(q1)*sin(q2)*sin(q3)) - sin(q4)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2)))

- Thông số các điểm làm việc trên quỹ đạo chuyển động của tay kẹp P sau khi giải
xong bài toán động học ngược trên excel (bảng 2.2).
Vị trí ban đầu của tay kẹp tại vị trí : (x,y,z)=( 495;0;50 ) khi đó các biến khớp q
(q1,q2,q3,q4,q5 ) bằng 0.
Bảng 2.2 Bảng thông số quỹ đạo chuyển động của robot .


P

q1

q2

q3

q4

q5

a14

a24

a34

L

0

0

0.225439

-0.368317

-0.46852

-6.03955E-07

400

0

10
0

2.45E-13
1.80E-13

1

0

0.516157

-0.470531

-0.4683

-4.65965E-07

400

0

20
0

2

0

0.591333

-0.136984

-0.63772

-5.35094E-07

400

0

30
0

2.77E-13

0

38
1

2.44E-14
0.000177

3

0

0.637389

0.1849225

-0.60909

1.59732E-07

400

4

0.184369

0.642796

0.1854

-0.76161

0.190043265

370

69

38
0

5

0.246293

0.597652

0.2913269

-0.84168

0.240754989

358

90

38
0

0.000167
0.000105

6

0.277161

0.571369

0.3630881

-0.90318

0.2716285

348

99

38
1

7

0.323546

0.564452

0.3733105

-0.97211

0.329669896

334

112

38
0

0.000163

320

13
1

38
1

0.000176

38
1

0.000113

8

0.388561

0.52733

0.4779643

-1.02658

0.385900154

9

0.520267

0.459754

0.6778008

-1.11995

0.514332566

288

16
5

10

0.708971

0.423212

0.8075175

-1.21526

0.708487694

239

20
5

38
1

0.000102
0.000136

11

0.836374

1.360655

-1.018165

-0.35387

0.844449226

186

206

38
0

12

0.976134

1.476208

-1.153427

-0.33473

0.980976253

140

207

38
0

0.000121

100

25
0

38
0

0.000109

80

270

38
0

0.000124

59

30
0

38
1

0.000128

38
1

0.000117

35

0.000118

13
14
15

1.19029
1.282739
1.376608

1.399772
1.348923
1.230496

-1.065596

-1.003517
-0.843335

-0.33994
-0.34769
-0.39784

-1.960269342
-1.863610929
1.380860874

16

1.508378

1.154911

-0.735524

-0.43003

-1.631155906

20

32
0

17

1.542232

1.041323

-0.661829

-0.39001

-1.602069706

10

35


18
19

1.570796
1.570796

0.617691
0.252306

0.0482777
0.6365985

-0.64328
-0.87684

1.570114724
1.574973463

0

9

0

40
0

34
6

0.000515

0

40
0

30
0

0.000163

CHƯƠNG 3 QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT .
Sau khi giải xong bài toán động học ngược robot ta có bảng thông số quỹ đạo
chuyển động của robot (bảng 2.2) .

Từ đó chúng ta biểu diễn sự chuyển động của các biến khớp q ( q1,q2,q3,q4,q5) và
chuyển động của tay kẹp robot (hình 3.2) .
3.1 Cách xây dựng đường cong nội suy .
3.1.1 Cơ sở lý thuyết .
Quỹ đạo không gian khớp có dạng hàm bậc 3:
(*)
Phương trình tiếp tuyến với quỹ đạo U(t) hay phương trình vận tốc tại thời
điểm t là :


Hình 3.1: Sơ đồ tính toán hệ số góc tiếp tuyến chung tại Pi .
- Xét :
Điều kiện về vận tốc công nghệ tại điểm đầu và cuối quỹ đạo.

-Điều kiện đi qua:

-Điều kiện chuyển tiếp trơn tại điểm chuyển tiếp:

+ Nếu
+ Nếu
Ta có hệ phương trình:
Giải hệ phương trình trên ta được phương trình .
3.1.2 Tính Toán .
+ Tính cho biến khớp q1
-

Bước 1: Tính các hệ số góc ( vận tốc trung bình trên các đoạn ) :

Công thức :

Hình 3.2 : Vẽ phác họa và quy ước tên các thông số cho khớp q.
Kc12 : k chung 1 và 2.
Kc23 : k chung 2 và 3.
ta có : K1 = 0

K2= =0 K3= = 0 K4= = 0.1844

Tính lần lượt từ k1 đến k19 ta có bảng giá trị:
Bảng thông số Ki :
K
Qi+
1–
Qi

K1
0

K2
0

K3
0

K4
0.184
4

K5
0.061

9

K6
0.030
9

K7
0.046
4

K8
0.065

K9
0.131
7

K
Qi+
1–
Qi

K11
0.127
4

K12
0.139
8

K13
0.214
2

K14
0.092
4

K15
0.093
9

K16
0.131
8

K17
0.033
9

K18
0.028
6

K19
0

-

Bước 2: tìm hệ số góc chuyển tiếp tại các điểm chốt:

+ Nếu
+ Nếu
Ta có:

K10
0.188
7


Kc12 == 0

Kc23 = = 0

Kc34 = = 0.0922

Kc45 = = 0.12315

Tính lần lượt từ kc12 đến kc1819 ta có bảng thông số:
Bảng thông số Kc :
Kc

Kc12

Kc23

Kc34

Kc45

Kc56

Kc67

Kc78

Kc910

Ki+1
- Ki
Kc

0

0

0.0922

0.0464

Kc111
2
0.1580
5

Kc121
3
0.1336

Kc131

4
0.1769

0.1231
5
Kc141
5
0.1533

0.0386
5
Kc161
7
0.0828
5

0.0558
5
Kc171
8
0.0312
5

0.0983
5
Kc181
9
0.0143

Ki+1

- Ki

Kc151
6
0.1128
5

Kc101
1
0.1602

Tính toán lần lượt từ q1 đến q5 có bảng thông số để vẽ cho các biến khớp q1 đến q5 .
-Bước 3: Xác định hệ số cho đa thức nội suy dựa theo điều kiện đi qua và điều kiện
vận tốc :
Ta có :
U1(t=1)= 0
U1 (t=2)= 0
U1’(t=1)=0
U1’(t=2)= Kc12= 0
Thay t=1s và t =2s vào phương trình (*) , ta có:
a1+b1+c1+d1= 0
27a1+9b1+3c1+d1= 0
3a1+2b1+c1=0
27a1+6b1+c1= 0
Giải Phương trình trên ta được : a1=0 ; b1=0 ; c1= 0 ; d1 =0 .
Sau khi giải xong bài toán động học ngược robot ta có bảng thông số quỹ đạo
chuyển động của robot
Từ đó chúng ta biểu diễn sự chuyển động của các biến khớp q ( q1,q2,q3,q4,q5) và
chuyển động của tay kẹp robot

3.1.3 Một số lệnh trong MATLAB .
>> plot ( t,u) : đồ thị.
>> hold on : giữ đồ thị trước đó vẽ tiếp vào.
>> grid on : chia lưới đồ thị.
>> plot ( t,u,’lineWidth’,2 ) // tăng độ nét dày hơn cho đường đồ thị =2 .
3.2 Đồ thị biến khớp q theo thời gian t.
Biến khớp q1
1.8
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2
0

0

2

4

6

8

10

12

14

16

18

20

18

20

Hình 3.3 Biến khớp q1 thay đổi theo thời gian t .
Biến khớp q2
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2
0

0

2

4

6

8

10

12

14

16

Hình 3.4 Biến khớp q2 thay đổi theo thời gian t .