TẢI FREE ZALO: 0934 573 149 NGHIÊN CỨU, THIẾT KẾ, MÔ PHỎNG ROBOT CÔNG NGHIỆP

1. TRƯỜNG ĐẠI HỌC KINH TẾ TÀI CHÍNH
BÁO CÁO THỰC TẬP CHUYÊN NGÀNH
ĐỀ TÀI : NGHIÊN CỨU, THIẾT KẾ, MÔ PHỎNG ROBOT CÔNG
NGHIỆP
Giáo viên hướng dẫn :
Họ và tên SV :
Lớp :
3/ 2019

2. LỜI NÓI ĐẦU
Theo quá trình phát triển của xã hội, nhu cầu nâng cao sản xuất và chất
lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động
hóa sản xuất. Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính
linh hoạt cao đã hình thành và phát triển mạnh mẽ…Vì thế ngày càng tăng
nhanh nhu cầu ứng dụng người máy, các cánh tay máy tự động (Robot) để tạo
ra các hệ sản xuất tự động linh hoạt.
Robot ứng dụng rộng rãi và đóng vai trò quan trọng sản xuất cũng như trong
đời sống. Robotlà cơ cấu đa chức năng có khả năng lập trình được dùng để di
chuyển nguyên vật liệu, các chi tiết, các dụng cụ thông qua các truyền động
được lập trình trước. Khoa học robotchủ yếu dựa vào các phép toán về đại số
ma trận.
Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển
động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển
bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot.
Chính vì vậy em chọn đề tài : “Nghiên cứu, thiết kế, mô phỏng robot công
nghiệp 3 bậc tự do”.
Đầu tiên em xin chân thành gửi lời cảm ơn sâu sắc tới các thầy cô giáo trong
khoa Công nghệ TĐH, cùng các bạn trong lớp KTĐ – ĐT K16D, đặc biệt là
thầy giáo Tiến sĩ … giảng viên trường Đại học …, người đã trực tiếp giảng
dạy và cho em kiến thức để hoàn thành đồ án này.
Trong quá trình làm đề tài mặc dù em đã nhiều cố gắng nhưng vẫn không
tránh khỏi sai xót. Rất mong các thầy thông cảm và giúp đỡ em nhiều hơn.
Em xin chân thành cảm ơn !

3. LỜI NÓI ĐẦU
CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG ............................................. 1
1.1. Sự ra đời của robot công nghiệp............................................................ 1
1.2. Các định nghĩa về robot công nghiệp .................................................... 2
1.3. Tay máy robot...................................................................................... 3
1.3.1. Kết cấu của tay máy ....................................................................... 4
1.3.2 Bậc tự do của robot ......................................................................... 5
1.3.3. Vùng làm việc của robot................................................................. 6
1.4. Ưu điểm của robot công nghiệp ............................................................ 7
1.5. Tình hình tiếp cận robot ở Việt Nam và ứng dụng của robot công nghiệp7
1.5.1. Tình hình tiếp cận robot ở Việt Nam ............................................... 7
1.5.2. Ứng dụng của robot công nghiệp..................................................... 8
CHƯƠNG 2: XÂY DỰNG PHƯƠNG TRÌNH ĐỘNG HỌC THUẬN VÀ
NGƯỢC CHO ROBOT 3 BẬC TỰ DO .................................................... 10
2.1. Sơ đồ động học của robot................................................................... 10
2.2. Thiết lập hệ tọa độ khảo sát và lập bảng D-H....................................... 11
2.3. Hệ phương trình động học Robot........................................................ 13
2.4. Phương trình động học thuận Robot.................................................... 14
2.5. Phương trình động học ngược Robot................................................... 17
CHƯƠNG 3: THẾT KẾ MÔ HÌNH ROBOT 3 BẬC TỰ DO..................... 19

4. 3.1. Giới thiệu các phần mềm mô phỏng.................................................... 19
3.1.1. Thiết kế cơ cấu robot trên SolidWorks........................................... 19
3.1.2. Thiết kế mô hình robot trên MATLAB/ Simulink. ......................... 19
3.2. Điều khiển chuyển động robot............................................................ 21
3.2.1. Điều khiển góc quay khớp với bộ điều khiển phản hồi PD.............. 21
3.2.2. Điều khiển robot theo quỹ đạo thẳng............................................. 24
3.2.3. Điều khiển robot theo quỹ đạo tròn............................................... 27
3.2.4. Kết hợp điều khiển robot theo quỹ đạo tròn và bộ điều khiển phản hồi
PD ........................................................................................................ 28
KẾT LUẬN.............................................................................................. 31
TÀI LIỆU THAM KHẢO......................................................................... 32
PHỤ LỤC……………………………………..…..…………………………33

5. 1
CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG
1.1. Sự ra đời của robot công nghiệp
Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày càng đòi hỏi
ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng tạo ra
những dây chuyền về thiết bị tự động có tính linh hoạt cao đang hình thành.
Các thiết bị này đang thay thế dần các máy tự động “cứng” chỉ đáp ứng một
việc nhất định trong lúc thị trường luôn luôn đòi hỏi thay đổi mặt hàng về
chủng loại, về kích cỡ và về tính năng v.v… Vì thế ngày càng tăng nhanh nhu
cầu ứng dụng robot để tạo ra các hệ thống sản xuất linh hoạt. Thuật ngữ
“robot” lần đầu tiên xuất hiện vào khoảng năm 1921 trong tác phẩm
“Rossum’s Universal Robot” của nhà viễn tưởng người Sec Karel Capek.
Trong vở kịch này, ông dùng từ “robot”, biến thể của từ gốc Slavơ “Robota”,
để gọi một thiết bị do con người tạo ta. Vào những năm 40 nhà văn viễn
tưởng người Nga, Issac Asimov, mô tả robot là một chiếc máy tự động, mang
diện mạo của con người. Asimov cũng đặt tên cho ngành nghiên cứu về robot
là Robotics, trong đó có 3 nguyên tắc cơ bản:
- Robot không gây tổn hại cho con người.
- Hoạt động của robot phải tuân theo các quy tắc do con người đặt ra. Các quy
tắc này không được vi phạm nguyên tắc thứ nhất.
- Một robot cần phải bảo vệ sự sống của mình, nhưng không được vi phạm 2
nguyên tắc trước. Và cũng trong những năm này, ước mơ viễn tưởng của
Kerel Capek đã bắt đầu thành hiện thực. Ngay sau chiến tranh thế giới thứ 2,
ở Hoa Kì đã xuất hiện những tay máy chép hình điều khiển từ xa trong các
phòng thí nghiệm về vật liệu phóng xạ.
Vào những năm 1950 bên cạnh các tay máy chép hình cơ khí đó, đã xuất hiện
các loại tay máy chép hình thủy lực và điện từ, như tay máy Minotaur I hoặc

6. 2
tay máy Handyman của General Electric. Năm 1954 George C. Devol đã thiết
kế một thiết bị có tên là “cơ cấu bản lề dùng để chuyển hàng theo chương
trình”. Đến năm 1956 Devol cùng với một kĩ sư trẻ của công nghiệp hàng
không Joseph F.Engelber, đã tạo ra loại robot đầu tiên năm 1959 ở công ty
Unimation. Chỉ đến năm 1975 công ty Unimation mới bắt đầu có lợi nhuận từ
các sản phẩm robot đầu tiên này. Chiếc robot công nghiệp được đưa vào ứng
dụng đầu tiên năm 1961 ở một nhà máy ô tô của General Motors tại Trenton,
New Jersey Hoa Kỳ.
Năm 1967 Nhật Bản mới nhập chiếc robot công nghiệp từ công ty AMF của
Hoa Kỳ (American Machine and Foundry Company). Đến năm 1990 có hơn
40 công ty Nhật Bản, trong đó có những công ty khổng lồ như công ty Hitachi
và công ty Mitsubishi đã đưa ra thị trường quốc tế nhiều loại robot.
1.2. Các định nghĩa về robot công nghiệp
Các nhà khoa học đã đưa ra rất nhiều định nghĩa về robot:
- Theo viện kỹ thuật robot của Hoa Kỳ: “Robot là loại tay máy nhiều chức
năng, với chương trình làm việc thay đổi được, dùng để thực hiện một số thao
tác sản xuất.”
- Theo ISO (International Standards Organization): “Robot công nghiệp là
một tay máy đa mục tiêu, có một số bậc tự do dễ dàng lập trình, điều khiển,
dùng để tháo lắp phôi, dụng cụ hoặc các vật dụng khác. Do chương trình thao
tác có thể thay đổi nên thực hiện nhiều nhiệm vụ đa dạng.”
- Theo tiêu chuẩn AFNOR (Pháp): “ Robot công nghiệp là một cơ cấu chuyển
động tự động có thể lập trình, lặp lại các chương trình, tổng hợp các chương
trình đặt ra trên các trục tọa độ; có khả năng định vị, định hướng, di chuyển
các đối tượng vật chất: chi tiết, dao cụ, gá lắp… theo những hành trình thay
đổi đã chương trình hóa nhằm thực hiện các nhiệm vụ công nghệ khác nhau.”

7. 3
- Theo RIA (Robot institute of America): “Robot là một tay máy vạn năng có
thể lặp lại các chương trình được thiết kế để di chuyển vật liệu, chi tiết, dụng
cụ hoặc cácthiết bị chuyên dùng thông qua các chương trình chuyển động có
thể thay đổi để hoàn thành các nhiệm vụ khác nhau.”
- Theo ΓOCT 25686-85 (Nga): “Robot công nghiệp là một tay máy tự động,
được đặt cố định hoặc di động được, liên kết giữa một tay máy và một hệ
thống điều khiển theo chương trình, có thể lập trình lại để hoàn thành các
chức năng vận động và điều khiển trong quá trình sản xuất.”
- Theo tiêu chuẩn VDI 2860/BRD: “ Robot là một thiết bị có nhiều trục, thực
hiện các chuyển động có thể chương trình hóa và nối ghép các chuyển động
của chúng trong những khoảng cách tuyến tính hay tuyến tính của động trình.
Chúng được điều khiển bởi các bộ phận hợp nhất ghép kết nối với nhau, có
khả năng học và nhớ các chương trình; chúng được trang bị dụng cụ hoặc các
phương tiện công nghệ khác để thực hiện các nhiệm vụ sản xuất trực tiếp và
gián tiếp.”
- Theo tiêu chuẩn GHOST 1980: “Robot là máy tự động liên kết giữa một tay
máy và một cụm điều khiển chương trình hóa, thực hiện một chu trình công
nghệ một cách chủ động với sự điều khiển có thể thay thế chức năng tương tự
của con người.” Bản chất của các định nghĩa khác nhau trên đây giúp ta thấy
được một ý nghĩa quan trọng là: Riêng một mình robot thì không thể hoàn
thành tốt được công việc. Nó phải được liên hệ chặt chẽ với máy móc, công
cụ và các thiết bị công nghệ tự động khác trong một hệ thống tự động tổng
hợp. Do đó trong quá trình phân tích và thiết kế, không thể quan niệm robot
như một đơn vị cấu trúc biệt lập, trái lại đó phải là những thiết kế tổng thể của
“hệ thống tự động linh hoạt robot hóa” cho phép thích ứng nhanh và đơn giản
khi nhiệm vụ sản xuất thay đổi.
1.3. Tay máy robot

8. 4
1.3.1. Kết cấu của tay máy
Tay máy là phần cơ sở nó quyết định đến khả năng làm việc của robot. Đó là
thiết bị cơ khí đảm bảo cho robot khả năng chuyển động trong không gian để
thực hiện các nhiệm vụ như nâng hạ, vận chuyển, lắp ráp.. Tay máy robot
thông thường là cơ cấu hở gồm một chuỗi các khâu liên kết với nhau bằng các
khớp, khâu đầu tiên được nối với giá cố định. Khớp tạo sự linh hoạt giữa các
khâu với nhau nói riêng và toàn bộ toàn bộ tay máy robot công nghiệp nói
chung. Thông qua khớp nối, các khâu trong cơ cấu tay máy được chuyển
động tương đối với nhau. Tùy theo yêu cầu về kết cấu của robot mà ta lựa
chọn loại khớp liên kết giữa các khâu khác nhau. Trong robot công nghiệp
hiện nay, người ta thường dùng chủ yếu hai loại khớp là khớp quay và khớp
trượt.
Khớp quay: (thường được kí hiệu là R) loại khớp này cho phép chuyển động
quay của khâu này và khâu khác quanh một trục quay. Loại khớp này hạn chế
năm khả năng chuyển động giữa hai thành phần khớp do đó có một bậc tự do.
Hình 1.1. Khớp quay
Khớp trượt: (thường được kí hiệu là T) loại khớp này cho phép hai khâu trượt
tương đối với nhau theo phương của một trục nào đó và hạn chế năm khả
năng chuyển động do đó khớp có một bậc tự do.

9. 5
Hình 1. 2. Khớp trượt
Ngoài ra trong một số trường hợp người ta còn dùng khớp cầu để tăng tính
linh hoạt cho robot. Với loại khớp này cho phép các khâu thực hiện các
chuyển động quay theo tất cả các hướng qua tâm khớp, và hạn chế chuyển
động tịnh tiến giữa các khâu. Do đó khớp cầu có số bậc tự do là ba.
Trong quá trình thiết kế tay máy robot, người ta quan tâm đến các thông số
ảnh hưởng lớn đến khả năng làm việc của robot như:
- Sức nâng, độ cứng vững, lực kẹp của tay…
- Tầm với hay vùng làm việc: kích thước và hình dáng vùng mà phần công tác
có thể với tới.
- Sự khéo léo của robot: thông số này liên quan đến bậc tự do của robot
1.3.2. Bậc tự do của robot
Bậc tự do là số khả năng chuyển động tịnh tiến hoặc quay của cơ cấu. Để dịch
chuyển được một vật thể trong không gian, cơ cấu chấp hành của robot phải
đạt được một số bậc tự do. Nói chung cơ hệ của robotlà một cơ cấu hở nên số
bậc tự do của nó có thể tính theo công thức:
w = 6n − ∑ ipi
i =1

10. 6
Trong đó:
n: Số khâu động
pi : Số khớp loại i (i=1,2,…,5: Số bậc tự do bị hạn chế)
Đối với các cơ cấu có các khâu được nối với nhau bằng khớp quay hoặc tịnh
tiến (khớp động loại 5) thì số bậc tự do bằng với số khâu động. Đối với cơ cấu
hở, số bậc tự do bằng tổng số bậc tự do của các khớp động.
Số bậc tự do của robot quyết định đến tính linh hoạt của robot trong quá trình
làm việc. Số bậc tự do càng lớn robot càng linh hoạt, càng nhiều phương án
để điểm thao tác thực hiện được yêu cầu công việc, điều này rất có ý nghĩa
trong trường hợp robot làm việc trong môi trường có nhiều chướng ngại vật.
Tuy nhiên số bậc tự do chuyển động này không nên lớn hơn sáu, bởi vì nếu
sáu bậc tự do nếu bố trí một cách hợp lý sẽ đủ để tạo ra khả năng linh hoạt
của khâu tác động cuối nhằm có thể tiếp cận đối tượng theo mọi hướng. Mặt
khác cũng phải thừa nhận rằng số bậc tự do lớn kéo theo hệ quả là: tăng sai số
dịch chuyển, tăng chi phí, thời gian sản xuất và bảo dưỡng robot. Do đó tùy
theo yêu cầu, chức năng mà người ta lựa chọn số bậc tự do cho robot thích
hợp.
1.3.3. Vùng làm việc của robot
Vùng làm việc của robot hay không gian làm việc của robot là toàn bộ thể tích
được quét bởi khâu chấp hành cuối khi robot thực hiện tất cả các chuyển động
có thể.
Vùng làm việc bị ràng buộc bởi các thông số hình học của robot cũng như các
ràng buộc cơ học của các khớp. Người ta thường dùng hai hình chiếu để mô tả
vùng làm việc của một robot.

11. 7
Hình 1.3. Biểu diễn vùng làm việc của robot
1.4. Ưu điểm của robot công nghiệp
- Làm việc không biết mệt mỏi, ít xảy ra nhầm lẫn trong quá trình làm việc.
- Làm việc được trong môi trường nguy hiểm, khắc nghiệt như: môi trường có
nhiều phóng xạ, môi trường có khí độc, dưới đáy đại dương, ngoài vũ trụ,…
- Thay đổi các thao tác bằng cách thay đổi chương trình điều khiển.
1.5. Tình hình tiếp cận robot ở Việt Nam và ứng dụng của robot công
nghiệp
1.5.1. Tình hình tiếp cận robot ở Việt Nam
Trong giai đoạn trước năm 1990 hầu như trong nước hoàn toàn chưa du nhập
về kỹ thuật robot, thậm chí chưa nhận được nhiều thông tin kỹ thuật về lĩnh
vực này. Nhưng với mục tiêu chủ yếu là tiếp cận kỹ thuật mới mẻ, trong nước
đã triển khai các đề tài nghiên cứu khoa học cấp nhà nước: Đề tài 58.01.03
trong giai đoạn 81-85 và đề tài 52B.03.01 trong giai đoạn 86-89. Kết quả
nghiên cứu của các đề tài này không những đáp ứng được yêu cầu tiếp cận mà
còn có những ứng dụng ban đầu trong kỹ thuật bảo hộ lao động và phục vụ
công tác đào tạo cán bộ kỹ thuật ở nước ta. Giai đoạn tiếp theo từ năm 1990
các ngành công nghiệp trong nước bắt đầu được đổi mới. Nhiều cơ sở đã nhập
ngoại nhiều dây chuyền thiết bị mới. Đặc biệt là ở một số cơ sở liên doanh với
nước ngoài đã nhập ngoại nhiều loại robot phục vụ các công việc.

12. 8
1.5.2. Ứng dụng của robot công nghiệp
Robot được áp dụng trong nhiều lĩnh vực, dưới góc độ thay thế sức người.
Mục đích sử dụng trong các dây chuyển sản xuất nhằm nâng cao năng suất,
chất lượng và hiểu quả sản xuất từ đó giảm giá thành sản phẩm, nâng cao khả
năng cạnh tranh.
- Trong ngành cơ khí, robot được sử dụng trong nhiều trong công nghệ đúc,
hàn, cắt kim loại, vận chuyển phôi, lắp ráp sản phẩm,…
- Robot được sử dụng trong các dây chuyền sản xuất từ động nhằm tạo ra linh
hoạt và tự động cao nhất cho dây chuyền, các robot này được điểu khiển bằng
một hệ thông các phương trình được lâp trình sẵn.
- Robot còn được sử dụng trong lĩnh vực y học, quốc phòng, vũ trụ, ….
Hình 1.4. Robot hàn trong công nghiệp

13. 9
Hình 1. 5. Robot phục vụ máy phay CNC
Robot có vai trò và ý nghĩa rất quan trọng trong nhiều lĩnh vực đời sống xã
hội của con người. Tuy nhiên, để chế tạo và đưa vào sử dụng một robot hoàn
chỉnh phải qua rất nhều quan trọng, từ thiết kế tính toán các thông số tới chế
tạo. Một trong nhưng công viêc đó là nghiên cứu, tính toán giải bài toán động
học của robot.

14. 10
CHƯƠNG 2: XÂY DỰNG CHƯƠNG TRÌNH ĐỘNG HỌC THUẬN VÀ
NGƯỢC CHO ROBOT 3 BẬC TỰ DO
2.1. Sơ đồ động học của robot
Sơ đồ động học của Robot RRR được cho trong hình 2.1. Trong các
tính toán và mô phỏng số sau này, ta sẽ lấy a1=5, a2=3, a3=2.
Hình 2.1. Sơ đồ động học robot và hệ tọa độ theo qui tắc D – H.
- Tính số bậc tự do của Robot
Công thức tổng quát:
trong đó:
ƒ: số bậc tự do của cơ cấu tay máy.

15. 11
λ: số bậc tự do của không gian trong đó tay máy thực hiện chuyển động.
n: số khâu động của tay máy.
k : số khớp của tay máy
fi : số bậc tự do chuyển động cho phép của khớp i
fc : số ràng buộc trùng
f p : số bậc tự do thừa
Với bài toán này, ta có: ƒ = 6(3-3) +(1+1+1)+0-0 = 3
Vậy Robotnày có 3 bậc tự do.
2.2. Thiết lập hệ tọa độ khảo sátvà lập bảng D-H
- Qui ước hệ tọa độ theo Denavit-Hartenberg:
+ Trục zi được chọn dọc theo trục của khớp thứ (i+1). Hướng của phép quay
và phép tịnh tiến được chọn tùy ý.
+ Trục xi được xác định dọc theo đường vuông góc chung giữa trục khớp thứ
i và (i+1), hướng từ khớp động thứ i tới trục (i+1).
+ Trục yi – xác định theo qui tắc bàn tay phải (hệ tọa độ thuận).
Hình 2.2. Qui ước hệ tọa độ theo Denavit – Hartenberg

16. 12
- Vị trí tương đối giữa hai hệ tọa độ liên tiếp j và (j-1) được mô tả bởi 4 tham
số động học là di, θi, ai, αi với:
+ di: dịch chuyển tịnh tiến giữa hai đường vuông góc chung của hai trục.
+ θi: góc giữa hai đường vuông góc chung. Là góc quay quanh trục zi-1 để
trục xi theo qui tắc bàn tay phải.
+ ai: khoảng dịch chuyển giữa hai trục khớp động kề nhau.
+ αi: góc lệch giữa trục của hai khớp động liền kề, là góc quay quanh trục xi
sao cho trục zi-1 chuyển đến trục zi theo qui tắc bàn tay phải.
-Từ qui ước trên, áp dụng cho đối tượng RobotRRR trong báo cáo, chọn
được hệ tọa độ như trong hình 2.1. Từ đó, lập được bảng D – H cho robot như
sau:
Các ma trận biến đổitọa độ thuần nhất D–H có dạng (với qui ước viết tắt:
c=cos,s=sin, q23=q2+q3
Ma trận i-1Ai cho phép biểu diễn quan hệ về tọa độ của một điểm trong hai hệ
tọa độ i và i-1. Ví dụ vị trí của một điểm P bất kì trong hệ tọa độ i là:
thì tọa độ của P trong hệ tọa độ thứ i-1 được xác định theo biểu thức:
Với Robot RRR, ta có các ma trận biến đổi tọa độ thuần nhất D-H lần lượt
như sau:

17. 13
2.3. Hệ phương trình động học Robot
Lập ma trận trạng thái khâu thao tác (End-effector, viết tắt EF) theo cấu trúc
động học
+ Với RobotRRR, ta có các biến khớp:
Ma trận mô tả hướng và vị trí của EF đối với hệ tọa độ cố định OX0Y0Z0
Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác
+ Ta đưa ra vector tọa độ định vị khâu thao tác (sử dụng các góc Cardan xác
định hướng vật rắn):
Trạng thái EF được biểu diễn qua các tọa độ suy rộng hoặc trực tiếp qua thời
gian, tùy bài toán cụ thể :

18. 14
Hoặc
Phương trình động học Robot
+ Phương trình động học dạng ma trận
Từ phương trình động học dạng ma trận, có thể suy ra được hệ phương trình
động học độc lập của Robot:
Nếu sử dụng vector các tọa độ suy rộng:
và gọi
thì hệ phương trình động học Robot ở trên có thể viết gọn lại thành:
2.4. Phương trình động học thuận Robot

19. 15
- Phương trình động học thuận: Trong phương trình động học thuận, các biến
khớp xem như đã biết, yêu cầu phải tìm vị trí của khâu tác động cuối đối với
hệ tọa độ cố định.
- Xây dựng quĩ đạo chuyển động của biến khớp
Giả sử ta có qui luật chuyển động của các biến khớp:
Phương trình động học Robot:
+ Vị trí của điểm tác động cuối:
+ Sử dụng ma trận định hướng của EF là ma trận Cardan, ta có:
+ So sánh trực tiếp các phần tử của hai ma trận quay ta được :

20. 16
Vận tốc điểm tác động cuối E:
- Vận tốc góc khâu thao tác:
Vậy
Vẽ quỹ đạo chuyển động của điểm E bằng phần mềm MATLAB
+ Thay số ta nhận được quỹ đạo chuyển động của EF:

21. 17
+ Mô phỏng và vẽ quỹ đạo bằng MATLAB và Robotics Toolbox:
Hình 2.3. Quỹ đạo EF biểu diễn với Matlab và Robotics Toolbox
2.5. Phương trình động học ngược Robot
- Phương trình động học ngược: vị trí các khâu thao tác xem như đã biết, yêu
cầu tìm giá trị các biến khớp ứng với vị trí cho trước đó.
- Để giải phương trình này, ta sử dụng phương trình động học robot:
Giải phương trình này tìm được các biến khớp tương ứng với quĩ đạo chuyển
động được chọn.
- Xây dựng một quỹ đạo chuyển động của Robot:

22. 18
Giải phương trình ngược với MATLAB, ta được 2 quỹ đạo nghiệm ứng với 2
trường hợp Elbow-up và Elbow-down của hai khớp q2 và q3.
Hình 2.4. Quĩ đạo biến khớp trong trường hợp Elbow-up
Hình 2.5. Quỹ đạo biến khớp trong trường hợp Elbow-down

23. 19
CHƯƠNG 3: THẾT KẾ MÔ HÌNH ROBOT 3 BẬC TỰ DO
3.1. Giới thiệu các phần mềm mô phỏng
3.1.1. Thiết kế cơ cấu robot trên SolidWorks
SolidWorks là một phần mềm CAD 3 chiều chạy trên hệ điều hành Windows
và đang được phát triển bởi bởi Dassault Systèmes SolidWorks Corp, một
công ty con của Dassault Systèmes, SA ( Vélizy, Pháp) . SolidWorks hiện
đang được sử dụng hơn 1,3 triệu kỹ sư và nhà thiết kế ở hơn 130.000 công ty
trên toàn thế giới.
Robot được vẽ từng thanh và được ghép lại nhau trên phần mềm SolidWorks
2007.
Hình 3.1. Cơ cấu robot
3.1.2. Thiết kế mô hình robot trên MATLAB/ Simulink
MATLAB là một môi trường tính toán số và lập trình, được thiết kế bởi
công ty MathWorks. MATLAB cho phép tính toán số với ma trận, vẽ đồ thị

24. 20
hàm số hay biểu đồ thông tin, thực hiện thuật toán, tạo các giao diện người
dùng và liên kết với các chương trình máy tính viết trên nhiều ngôn ngữ lập
trình khác.
Với thư viện Toolbox, MATLAB cho phép mô phỏng tính toán, thực nghiệm
nhiều mô hình trong thực tế và kỹ thuật.
Simulink là phần chương trình mở rộng của MATLAB nhằm mục đích mô
hình hóa mô phỏng và khảo sát các hệ thống động học. Giao diện đồ họa trên
màn hình của Simulink cho phép thể hiện hệ thống dưới dạng sơ đồ tín hiệu
với các khối chức năng quên thuộc.
Để thực hiện bài toán thiết kế bộ điều khiển cho robot, ta sử dụng thư viện
Simmechanics và phần mềm "SolidWorks-to-SimMechanics Translator” để
chuyển đổi cơ cấu robot từ SolidWorks sang dưới dạng khối Simulink.
Hình 3.2. Thư viện Simmechanics của Simulink
Cơ cấu robot được đưa vào Simulink qua phần mềm "SolidWorks-to-
SimMechanics Translator”

25. 21
Hình 3.3. Mô hình robot trên Simulink
3.2. Điều khiển chuyển động robot
3.2.1. Điều khiển góc quay khớp với bộ điều khiển phản hồi PD
Vị trí góc mong muốn:

26. 22
Hình 3.4. Mô hình điều khiển robot
Hình 3.5. Bộ điều khiển PD

27. 23
Hình 3.6. Mô hình robot
Hình 3.7. Đồ thị góc quay khớp

28. 24
Hình 3.8. Đồ thị vận tốc khớp
Nhận xét: Kết quả mô phỏng cho thấy góc quay đạt đến giá trị mong muốn
trong khoảng thời gian 0.5s và vận tốc góc cũng về giá trị 0 trong thời gian
tương ứng.
3.2.2. Điều khiển robot theo quỹ đạo thẳng
Hình 3.9. Mô hình điều khiển robot

29. 25
Hình 3.10. Quỹ đạo đặt
Hàm tính động học ngược được viết bằng khối Embedded MATLAB
Function:
function [theta1,theta2,theta3] = toado(Px,Py,Pz,a2,a3)
t1=atan2(Py,Px);
c3=(Px^2+Py^2+Pz^2-a2^2-a3^2)/(2*a2*a3);
s3=sqrt(1-c3^2);
t3=atan2(s3,c3);
a=a3*c3+a2;
b=a3*s3;
c=Pz;
ci=sqrt(a^2/(a^2+b^2));

30. 26
si=sqrt(1-ci^2);
i=atan2(si,ci);
sii=sqrt(c^2/(a^2+b^2));
cii=sqrt(1-sii^2);
ii=atan2(sii,cii);
t2=ii-i;
theta1=t1*180/pi;
theta3=t3*180/pi;
theta2=t2*180/pi;
Hình 3.11. Quỹ đạo đặt- quỹ đạo ra- robot ở điềm cuối hành trình

31. 27
3.2.3. Điều khiển robot theo quỹ đạo tròn
Hình 3.12. Mô hình điều khiển robot
Hình 3.13. Quỹ đạo đặt

32. 28
Hình 3.14. Quỹ đạo đặt- quỹ đạo ra- robot ở điểm cuối hành trình
3.2.4. Kết hợp điều khiển robot theo quỹ đạo tròn và bộ điều khiển phản
hồi PD
Hình 3.15. Mô hình điều khiển robot

33. 29
Hình 3.16. Khối tính sai lệch
Hình 3.17. Khối phản hồi

34. 30
Hình 3.18. Quỹ đạo đặt- quỹ đạo ra- robot ở cuối hành trình
Nhận xét: Robotchuyển động theo quỹ đạo với độ chính xác cao tuy nhiên có
thời điểm robot bị quay ngược do góc đặt khớp biến đổi tức thời trong thời
gian ngắn - thời điểm kết thúc chu kì trong đồ thị dưới.
Hình 3.19. Tín hiệu góc quay của ba khớp

35. 31
KẾT LUẬN
Sau một quá trình học tập và nghiên cứu cùng với sự hướng dẫn của
thầy giáo Tiến sĩ Nguyễn Vôn Dim và sự cố gắng nỗ lực em đã hoàn thành
báo cáo thực tập chuyên ngành về đề tài “Nghiên cứu, thiết kế, mô phỏng
robot công nghiệp 3 bậc tự do”.
Báo cáo đã thực hiện các nhiệm vụ:
- Thiết kế robot 3 trục quay trên SolidWorks và xây dựng sơ đồ khối trên
MATLAB/Simulink.
- Xây dựng phương trình động học thuận, động học ngược và lựa chọn các
tham số bộ điều khiển PD cho robot 3 trục quay.
- Thực hiện mô phỏng với kết quả tương đối chính xác.

36. 32
TÀI LIỆU THAM KHẢO
[1]. . TS. Nguyễn Mạnh Tiến , Điều khiển robot công nghiệp, NXB Khoa học
và kỹ thuật.
[2]. Nguyễn Phùng Quang , MATLAB và Simulink dành cho kĩ sư điều khiển
tự động, NXB Khoa học và kỹ thuật.
[3]. GS.TS. Nguyễn Công Hiền, TS Nguyễn Phạm Thục Anh, Mô hình hóa hệ
thống và mô phỏng, NXB Khoa học và kỹ thuật.

37. 33
PHỤ LỤC
Chương trình mô phỏng robot trên matlab symbolic và robotics toolbox:
clear all % Xóa tất cả các biến hiện có ở Workspace
clc % Xóa mọi dòng trên Command Window
startup_rvc % Khởiđộng Robotics Toolbox (file strartup_rvc của
ROBOTICSTOOLBOXphảiđang nằm ở thư
% mục hiện thời (Current Directory của MATLAB.
% Khaibáo các symbolic variables cùng các điều kiện của biến
syms q1 q2 q3 a1 a2 a3 dq1 dq2 dq3 m1 m2 m3 t g
% q1, q2, q3 là các biến khớp
% a1, a2, a3 là độ dài các thanh
% dq1, dq2, dq3 là đạohàm của q1, q2, q3
% m1, m2, m3 là khối lượng các thanh
% t : biến thời gian
% g : gia tốc trọng trường
assume(a1,'real');assume(a1>0);% Có nghĩa là ta cho MATLAB biết a1 là số
thực dương
assume(a2,'real');assume(a2>0);% việc này sẽ giúp cho việc rút gọn biểu
thức được hiệu quả hơn
assume(a3,'real');assume(a3>0);
assume(q1,'real');
assume(q2,'real');
assume(q3,'real');
assume(dq1,'real');
assume(dq2,'real');

38. 34
assume(dq3,'real');
assume(t,'real');assume(t>0);
assume(m1,'real');assume(m1>0);
assume(m2,'real');assume(m2>0);
assume(m3,'real');assume(m3>0);
assume(g,'real');assume(g>0);
q = [q1;q2;q3]; % Vector các tọa độ suy rộng q
dq = [dq1;dq2;dq3];% Vector các vận tốc dài
% Nhập các ma trận D-H
A_01=[cos(q1) 0 sin(q1) a1*cos(q1);sin(q1) 0 -cos(q1) a1*sin(q1);0 1 0
0;0 0 0 1];
A_12=[cos(q2) -sin(q2) 0 a2*cos(q2);sin(q2) cos(q2) 0 a2*sin(q2);0 0 1
0;0 0 0 1];
A_23=[cos(q3) -sin(q3) 0 a3*cos(q3);sin(q3) cos(q3) 0 a3*sin(q3);0 0 1
0;0 0 0 1];
% Tính các ma trận truyền
R_01=A_01(1:3,1:3);
A_03=A_01*A_12*A_23;
A_02=simplify(A_01*A_12);% Sau khi tính toán ta thu gọn kết quả ngay
bằng lệnh simplify
R_02=A_02(1:3,1:3);
disp('Ma tran chuyen tu khau 3 sang khau tac dong cuoi la')
A_03 = simplify(A_03)
R_03 = A_03(1:3,1:3);
disp(' ')

39. 35
% Tạo mô hình Robot trong Robotics toolbox
L(1)=Link([0,0,5,pi/2,0]);% Lệnh Link tạo một khâu của Robot
L(2)=Link([0,0,3,0,0]);
L(3)=Link([0,0,2,0,0]);
rob=SerialLink(L); % Lệnh SerialLink(L) tạo một robot nối tiếp gồm các
khâu của L
% Giải bài toán động học thuận
disp('Giaibai toan dong hoc thuan')
% 1. Tìm vi trí và tính vận tốc dài của khâu thao tác
rE = A_03(1:3,4) % Vector tọa độ khâu thaotác
v_qE = simplify(jacobian(rE,q)*dq)% Tính vector vận tốc khâu tác động
cuối
% 2. Tìm góc Cardan và Tính toán van toc góc cua khâu thao tác
R_0E = A_03(1:3,1:3)
diff_R_0E = diff(R_0E,q1)*dq1+diff(R_0E,q2)*dq2+diff(R_0E,q3)*dq3;
%Tinh dao ham cua R
omega_curve= diff_R_0E*R_0E.';
omega_curve= simplify(omega_curve)
disp('Van toc goc:')
omega = [omega_curve(3,2)omega_curve(1,3)omega_curve(2,1)]
% 3. Thayso bai toan dong hoc thuan
disp(' ')
disp('Thayso') % Khi thay số ta sử dụng lệnh subs
disp('Vi tri diem tac dong cuoi')
sub_rE = simplify(subs(rE,{q1 q2 q3 a1 a2 a3},{3*t2*tt 5 3 2}))

40. 36
disp('Van toc dai:')
sub_diff_qE= simplify(subs(v_qE,{q1q2 q3 dq1 dq2 dq3 a1 a2 a3},{t2*t3*t
3 2 1 5 3 2}))
disp('Van toc goc:')
sub_R_0E = simplify(subs(R_0E,{a1 a2 a3},{53 2}));
sub_omega = simplify(subs(omega,{q1q2 q3 dq1 dq2 dq3 a1 a2 a3},{t2*t
3*t 3 2 1 5 3 2}))
% 4. Tính toán và vẽ đồ thị
time=0:0.02:2*pi/6;
num_rE=zeros(3,length(time));
for j=1:length(time)
num_rE(:,j) = subs(sub_rE,t,time(j));
end
figure(1)
clf
title('Quy dao cua khau tac dong cuoi trong baitoan thuan')
hold on
grid on
axis([-10, 10, -10, 10 ,-5, 5])
pause
for j=1:length(time)
plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+');% Vẽ quĩ đạo chuyển
động bằng MATLAB
plot(rob,[3*time(j),2*time(j),time(j)]);% Vẽ hình ảnh chuyển động 3D của
Robot theo quĩ đạo
pause(1/30)% Dùng lệnh pauseđể tạo cảm giácgiống nhưmột đoạn phim
end

41. 37
% Giải bài toán động học ngược
figure(2)
clf
hold on
grid on
rE_solve = subs(rE,{a1 a2 a3},{53 2});
time=0:0.1:3;
j=length(time);
q1_num=zeros(2,j);
q2_num=zeros(2,j);
q3_num=zeros(2,j);
xE = zeros(1,j);yE = zeros(1,j);zE = zeros(1,j);
for i=1:j
xE(i) = 6*sin(time(i));
yE(i) = 6*cos(time(i));
zE(i) = 3;
f1 = rE_solve(1,1)-xE(i);% các phương trình động học Robot
f2 = rE_solve(2,1)-yE(i);
f3 = rE_solve(3,1)-zE(i);
f = [f1 f2 f3];
[q1_num(:,i) q2_num(:,i) q3_num(:,i)]= solve(f1,f2,f3);
% Dùng lệnh solve() để tìm nghiệm của hệ phương trình động học
end
subplot(311)
plot(time,q1_num(1,:))
grid

42. 38
subplot(312)
plot(time,q2_num(1,:))
grid
subplot(313)
grid
plot(time,q3_num(1,:))
grid
figure(3)
clf
title('Quy dao cua khau tac dong cuoi trong baitoan nguoc')
hold on
grid on
axis([-10, 10, -10, 10 ,-5, 5])
pause
close(2)
for i=1:length(time)
plot(rob,[q1_num(1,i),q2_num(1,i),q3_num(1,i)])
plot3(xE(i),yE(i),zE(i),'b+')
pause(1/5)
end
% Giải bài toán động lực học(Solve the dynamicsproblem)
% Cac tensor quan tinh (inertial moment)
I_c1 = [0 0 0;0 m1*a1^2/120;0 0 m1*a1^2/12];
I_c2 = [0 0 0;0 m1*a2^2/120;0 0 m1*a2^2/12];
I_c3 = [0 0 0;0 m1*a3^2/120;0 0 m1*a3^2/12];

43. 39
% Toa do cac trong tam (center of mass in moving coordinate)
A_c1 = [1 0 0 -a1/2;0 1 0 0;0 0 1 0;0 0 0 1];
A_c2 = [1 0 0 -a2/2;0 1 0 0;0 0 1 0;0 0 0 1];
A_c3 = [1 0 0 -a3/2;0 1 0 0;0 0 1 0;0 0 0 1];
% Chuyen sang toa do co dinh (transforming into fixed coordinate)
A_0_c1 = A_01*A_c1
A_0_c2 = simplify(A_02*A_c2)
A_0_c3 = simplify(A_03*A_c3)
% Cac ma tran vi tri
disp(' ')
disp('Toa do cac trong tam trong he toa do co dinh')
r_0_c1 = A_0_c1(1:3,4)
r_0_c2 = A_0_c2(1:3,4)
r_0_c3 = A_0_c3(1:3,4)
disp('Van toc cac khoi tam')
v_0_c1 = simplify(jacobian(r_0_c1,q)*dq)
v_0_c2 = simplify(jacobian(r_0_c2,q)*dq)
v_0_c3 = simplify(jacobian(r_0_c3,q)*dq)
v_c1 = simplify(v_0_c1.'*v_0_c1);
v_c2 = simplify(v_0_c2.'*v_0_c2);
v_c3 = simplify(v_0_c1.'*v_0_c3);
JT1=simplify(jacobian(r_0_c1,q));% Các Jacobian vận tốc dài
JT2=simplify(jacobian(r_0_c2,q));
JT3=simplify([diff(r_0_c3,q1),diff(r_0_c2,q2),diff(r_0_c3,q3)]);

44. 40
disp('Van toc goc cac khau')
tmp = A_0_c1(1:3,1:3);C2 = A_0_c2(1:3,1:3);C3 = A_0_c3(1:3,1:3);
W_c1=tmp.'*(diff(tmp,q1)*dq1+diff(tmp,q2)*dq2+diff(tmp,q3)*dq3);
w_c1=simplify([W_c1(3,2);W_c1(1,3);W_c1(2,1)])
W_c2=C2.'*(diff(C2,q1)*dq1+diff(C2,q2)*dq2+diff(C2,q3)*dq3);
w_c2=simplify([W_c2(3,2);W_c2(1,3);W_c2(2,1)])
W_c3=C3.'*(diff(C3,q1)*dq1+diff(C3,q2)*dq2+diff(C3,q3)*dq3);
w_c3=simplify([W_c3(3,2);W_c3(1,3);W_c3(2,1)])
JR1 = [0 0 0;1 0 0;0 0 0];
JR2 = [sin(q2) 0 0;0 cos(q2) 0;0 0 0];
JR3 =[sin(q2+q3)0 0;0 cos(q2+q3) 0;0 1 1]; % Các Jacobian vận tốc góc
disp('Dong nang cua cac khau') % Tính toán động năng từng khâu
T1 = simplify(1/2*m1*v_c1.'*v_c1+1/2*w_c1.'*I_c1*w_c1)
T2 = simplify(1/2*m1*v_c2.'*v_c2+1/2*w_c2.'*I_c2*w_c2)
T3 = simplify(1/2*m1*v_c3.'*v_c3+1/2*w_c3.'*I_c3*w_c3)
% ManipulatorInertia Matrix (Tính ma trận khối lượng của Robot)
disp('Phuongtrinh dong luc hoc tong quat co dang:')
disp(' M.ddq - Psi - Q = U')
disp(' Trong do:')
M =
JT1.'*m1*JT1+JT2.'*m2*JT2+JT3.'*m3*JT3+JR1.'*I_c1*JR1+JR2.'*I_c2*J
R2+JR3.'*I_c3*JR3;
M = simplify(M)
% Biểu thức thế năng - Potential Energy (PE)
PE = (1/2*m2+m3)*a2*cos(q2)*g+1/2*m3*a3*cos(q3)*g;

45. 41
% Centrifugal/Coriolis matrix and Psi (các ma trận lực hướng tâm/Coriolis)
tmp = sym(zeros(3));
Psi = sym(zeros(3,1));
for j=1:3
h = sym(0);
for k=1:3
for l=1:3
tmp(k,l) = 1/2*(diff(M(k,l),q(l))+diff(M(l,j),q(k))-
diff(M(k,l),q(j)))*dq(k)*dq(l);
h = h+tmp(k,l);
end
end
Psi(j) = -h-diff(PE,q(j));
end
Psi=simplify(Psi)
% Statics of Robot
syms Fx Fy Fz Mx My Mz % Các lực và momen tác dụng vào khâucuối
assume(Fx,'real');assume(Fy,'real');assume(Fz,'real');
assume(Mx,'real');assume(My,'real');assume(Mz,'real');
F_E3 = [Fx, Fy, Fz].'; M_E3 = [Mx, My, Mz].';
P_3 = [0,0,-m3*g].';P_2= [0,0,-m2*g].';P_1= [0,0,-m1*g].';
r_1 = [a1,0,0].';r_2 = [a2,0,0].';r_3 = [a3,0,0].';
r_c1 = [a1,0,0].';r_c2 = [a2,0,0].';r_c3 = [a3,0,0].';
F_32 = simplify(F_E3-P_3)
M_32 = simplify(M_E3+R_03*MatrixCurve(r_3)*F_32-
R_03*MatrixCurve(r_c3)*P_3)

46. 42
F_21 = simplify(F_32-P_2)
M_21 = simplify(M_32+R_02*MatrixCurve(r_2)*F_21-
R_02*MatrixCurve(r_c2)*P_2)
F_10 = simplify(F_21-P_1)
M_10 = simplify(M_21+R_01*MatrixCurve(r_1)*F_10-
R_01*MatrixCurve(r_c1)*P_1)