Giáo trình môn học Kỹ thuật robot - Bùi Thư Cao

108 trang hapham 90

Download

Bạn đang xem 20 trang mẫu của tài liệu "Giáo trình môn học Kỹ thuật robot - Bùi Thư Cao", để tải tài liệu gốc về máy bạn click vào nút DOWNLOAD ở trên

Tài liệu đính kèm:

giao_trinh_mon_hoc_ky_thuat_robot_bui_thu_cao.pdf

Nội dung text: Giáo trình môn học Kỹ thuật robot - Bùi Thư Cao

BỘ CÔNG THƯƠNG TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP TPHCM KHOA CÔNG NGHỆ ĐIỆN TỬ - TỰ ĐỘNG GIÁO TRÌNH MÔN HỌC Biên soạn : Bùi Thư Cao Trần Hữu Toàn TP.HỒ CHÍ MINH, 03/10/2008
MỤC LỤC Chương 1. ng qu n v ro ot. 01 1.1. ch s ph t tri n Robot. 01 1.2. C c ứng d ng của Robot. 04 1.2.1. C c ưu đi m khi s d ng Robot. 04 1.2.2. Một số lĩnh vực ứng d ng. 05 1.3. C c kh i ni m v robot - robot c ng nghi p. 07 1.3. . nh nghĩa v robot c ng nghi p 07 1.3.2. C c thành phần của robot c ng nghi p 08 1.3.3. Bậc tự do của robot c ng ghi p. 10 .3.4. H toạ độ trong robot. 11 1.4. Phân loại robot. 13 1.4.1. Robot c ng nghi p. 13 . Robot nối tiếp. 13 2. Robot song song. 14 1.4.2. Robot di động 15 Chương 2. Phân tích hệ cơ cân ằng tĩnh và chuyển động t y máy. 17 2. . C c kh i ni m cơ bản và ti n đ tĩnh học. 17 2. . . Trạng th i cân bằng. 17 2. .2. ực. 17 2. .3. Momen của lực đối với tâm. 17 2. .4. Momen của lực đói với tr c. 17 2. .5. H lực. 18 2. .6. C c ti n đ tĩnh học. 18 2. .7. Một số m hình phản lực liên kết 20 2. .8. Sức b n vật li u. 22 1
2. .9. ực ma s t 23 2.2. Thiết kế h cơ cân bằng tĩnh. 24 2.2. . X c đ nh c c yếu tố đầu vào. 24 2.2.2. Thiết kế cơ khí. 24 2.2.3. Tính to n ki m tra cân bằng lực cho h . 25 2.3. Phân tích chuy n động tay m y. 28 2.3. . Giới thi u. 28 2.3.2. H toạ độ. 28 2.3.3. Quĩ đạo robot. 28 2.3.4. Phân tích chuy n động tổng qu t của tay m y. 28 2.3.5. Phép biến đổi h toạ độ. 29 2.4. Phân tích chuy n động của một số tay m y. 29 2.4. . Phân tích chuy n động của tay m y 2 khớp quay. 30 2.4.2. Phân tích chuy n động của tay m y 3 khớp quay. 32 2.4.3. Phân tích chuy n động của tay m y nhi u khớp nối 34 Chương 3. Các ph p i n đ i thu n nh t homogenous tr nsform tion 34 3.1. ectơ đi m và h toạ độ thuần nh t. 35 3.2. h c lại c c phép tính v vectơ và ma trận. 35 3.2. . Phép nhân vectơ. 36 3.2.2. C c phép tính v ma trận. 36 a. Phép cộng tr ma trận. 36 b. Tích hai ma trận. 37 c. Ma trận ngh ch đảo của ma trận thuần nh t. 37 d. ết của ma trận. 38 e. ạo hàm và tích phân của ma trận. 38 3.3. C c phép biến đổi d ng trong động học robot. 38 3.3. . Phép biến đổi t nh tiến. 39 3.3.2. Phép quay quanh c c tr c toạ độ. 40 3.3.3. Phép quay le Euler . 41 2
3.3.4. Phép quay Roll – Pitch -Yaw. 42 3.4. Biến đổi h toạ độ và mối quan h gi a c c h toạ độ. 42 2.4. . Biến đổi h toạ độ. 43 2.4.2. Mối quan h gi a c c h toạ độ. 44 3.5. M tả vật th – ối tư ng làm vi c của robot. 47 Chương 4. Phương tr nh động h c c ro ot inem tic equ tions 47 4. . D n nhập. 47 4. . . H toạ độ và mối quan h gi a c c khâu trên robot. 47 4. .2. hâu ch p hành cuối và đi m t c động cuối. 49 4.2. Bộ th ng số DE IT – HARTENBERG (DH). 49 4.2.1. D n nhập. 49 4.2.2. ộ dài ph p tuyến chung an và góc o n của khâu n αn. 50 4.2.3. hoảng c ch gi a hai khâu dn và góc quay của khâu n θn. 50 4.2.4. Bộ thông số DH. 51 4.3. G n h toạ độ cho robot. 51 a. Chọn gốc của h toạ độ. 51 b. Chọn tr c n. 51 c. Chọn tr c n. 51 d. G n h toạ độ cho robot SC R . 53 4.4. c trưng của c c ma trận 53 4.3. . h i ni m ma trận . 53 4.3.2. C c phép biến đổi ma trận . 54 4.5. X c đ nh c c ma trận T theo ma trận . 55 4.6. Trình tự thiết lập h phương trình động học của robot. 55 4.6. . C c bước thực hi n. 55 a. Chọn h toạ độ cơ bản và g n c c h toạ độ trung gian. 55 b. ập bảng th ng số DH. 55 c. X c đ nh c c ma trận i. 55 d. Tính c c ma trậpn T. 55 3
e. iết phương trình động học của robot. 56 4.6.2. í d thiết lập phương trình động học một số robot. 64 Chương 5. Động lực h c Ro ot và ứng dụng trong đi u hiển 64 5. . M c đích và phương ph p khảo s t động lực học Robot. 64 5.2. ộng lực học robot với phương trình Euler-Lagrange. 65 5.3. hảo s t bài to n động lực học của tay m y nhi u bậc tự do. 71 5.4. Phương trình động lực học tay m y. 71 5.4. . Tổng qu t. 72 5.4.2. Ma trận qu n tính. 74 5.4.3. ector Coriolis/hướng tâm. 74 5.4.4. ector trọng lực 75 5.5. Xây dựng Robot với đ c tính phi tuyến - Ứng d ng trong đi u khi n 78 Chương 6. Đi u hiển Ro ot 78 6. . Biến đổi quĩ đạo t h tọa độ Descartes sang kh ng gian khớp. 78 6. . ội suy đường đa thức. 80 6. .2. ội suy quĩ đạo theo thời gian nhỏ nh t. 82 6.2. i u khi n h robot phi tuyến. 82 6.3. i u khi n trự tiếp h robot. 93 6.4. Tính to n và đi u khi n theo momen - hồi tiếp tuyến tính h robot. 93 6.4. . ạo hàm của vòng hồi tiếp trong Deravition of Inner 93 Feedforward Loop) 6.4.2. Thiết kế PD vòng ngoài. 96 6.4.3. í d minh họa. 98 6.4.4. Thiết kê PID vòng ngoài. 100 6.4.5. Bảng tóm t t. 102 6.4.6. Áp d ng Matlab đ khảo s t c c bài to n c th 103 4
Chương 1: Tổng quan về Robot công nghiệp Chương 1 TỔNG QUAN VỀ ROBOT 1.1. Lịch sử phát triển Robot. Khái niệm Robot ra đời đầu tiên vào ngày 09/10/1922 tại NewYork, khi nhà soạn kịch người Tiệp Kh Karen Kapek đã tưởng tượng ra một cổ máy hoạt động một cách tự động, nó là niềm mơ ước của con người lúc đó. Từ đó ý tưởng thiết kế, chế tạo Robot đã luôn thôi thúc con người. Đến năm 1948, tại phòng thí nghiệm quốc gia Argonne, Goertz đã chế tạo thành công tay máy đôi (master-slave manipulator). Đến năm 1954, Goertz đã chế tạo tay máy đôi sử dụng động cơ servo và có thể nhận biết được lực tác động lên khâu cuối. Năm 1956 hãng Generall Mills đã chế tạo tay máy hoạt động trong việc thám hiểm dại dương. Năm 1968 R.S. Mosher, của General Electric đã chế tạo một cỗ máy biết đi bằng 4 chân. Hệ thống vận hành bởi động cơ đốt trong và mỗi chân vận hành bởi một hệ thống servo thủy lực. Năm 1969, đại học Stanford đã thiết kế được Robot tự hành nhờ nhận dạng hình ảnh. Hình 1.1 Robot Shakey Năm 1970 con người đã chế tạo được Robot tự hành Lunokohod, thám hiểm bề mặt của mặt trăng. 1
Chương 1: Tổng quan về Robot công nghiệp Trong giai đoạn này, ở nhiều nước khác cũng tiến hành công tác nghiên cứu tương tự, tạo ra các Robot điều khiển bằng máy tính có lắp đặt các loại cảm biến và thiết bị giao tiếp người và máy. Hình 1.2. Robot hàn điểm Hình 1.3. Robot phẫu thuật (Nguồn KUKA, Inc) (Nguồn Accury, Inc) Theo sự tiến bộ của khoa học kỹ thuật, các Robot ngày càng được chế tạo nhỏ gọn hơn, thực được nhiều chức năng hơn, thông minh hơn. Một lĩnh vực được nhiều nước quan tâm là các Robot tự hành, các chuyển động của chúng ngày càng đa dạng, bắt chước các chuyển động của chân người hay các loài động vật như : bò sát, động vật 4 chân, Và các loại xe Robot (robocar) nhanh chóng được ứng dụng rộng rãi trong các hệ thống sản xuất tự động linh hoạt (FMS). 2
Chương 1: Tổng quan về Robot công nghiệp Hình 1.4. Mobile Robot và ứng dụng công nghệ xử lý ảnh (Nguồn SRI, Stanford, CA) Từ đó trở đi con người liên tục nghiên cứu phát triển Robot để ứng dụng trong quát trình tự động hoá sản xuất để tăng hiệu quả kinh doanh. Ngoài ra Robot còn được sử dụng thay cho con người trong các công việc ở môi trường độc hại, khắc nghiệt, Chuyên ngành khoa học về robot “robotics” đã trở thành một lĩnh vực rộng trong khoa học, bao gồm các vấn đề cấu trúc cơ cấu động học, động lực học, quĩ đạo chuyển động, chất lượng điều khiển Tuỳ thuộc vào mục đích và phương thức tiếp cận, chúng ta có thể tìm hiểu lĩnh vực này ở nhiều khía cạnh khác nhau. Hiện nay, có thể phân biệt các loại Robot ở hai mảng chính : Các loại robot công nghiệp (cánh tay máy) và các loại robot di động (mobile robot). Mỗi loại có các ứng dụng cũng như đặc tính khác nhau. Ngoài ra, trong các loại 3
Chương 1: Tổng quan về Robot công nghiệp robot công nghiệp còn được phân chia dựa vào cấu tạo động học của nó : Robot nối tiếp (series robot) và robot song song (parallel robot). Hình 1.5. Robot song song 6 bậc tự do Merlet.( Nguồn : Dr. J. - P. Merlet và Prof. V. Hayward.) Chính công nghệ tiên tiến ở tất cả các lĩnh vực : cơ khí, vi mạch, điều khiển, công nghệ thông tin đã tạo ra nền tảng cũng như những thách thức lớn đối với khoa học nghiên cứu robot. Chính vì vậy, con người đã và đang tiếp tục phát triển và nâng cao mức độ hoàn thiện trong lĩnh vực đầy hấp dẫn này. 4
Chương 1: Tổng quan về Robot công nghiệp Hình 1.6. Nguyên bản của Robot Hexapod TU Munich ( Nguồn : Prof. F. Pfeiffer, TSI Enterprises, Inc.) 1.2. Các ứng dụng của Robot. 1.2.1. Các ưu điểm khi sử dụng Robot. Các loại Robot tham gia vào qui trình sản xuất cũng như trong đời sống sinh hoạt của con người, nhằm nâng cao năng suất lao động của dây chuyền công nghệ, giảm giá thành sản phẩm, năng cao chất lượng cũng như khả năng cạnh tranh của sản phẩm tạo ra. Robot có thể thay thế con người làm việc ổn định bằng các thao tác đơn giản và hợp lý, đồng thời có khả năng thay đổi công việc để thích nghi với sự thay đổi của qui trình công nghệ. Sự thay thế hợp lý của robot còn góp phần giảm giá thành sản phẩm, tiết kiệm nhân công ở những nước mà nguồn nhân công là rất ít hoặc chi phí cao như : Nhật Bản, các nước Tây Âu, Hoa Kỳ Tất nhiên nguồn năng lượng từ robot là rất lớn, chính vì vậy nếu có nhu cầu tăng năng suất thì cần có sự hỗ trợ của chúng mới thay thế được sức lao động của con người. Chúng có thể làm những công việc đơn giản nhưng dễ nhầm lẫn, nhàm chán. Robot có khả năng nghe được siêu âm, cảm nhận được từ trường Bên cạnh đó, một ưu điểm nổi bậc của robot là môi trường làm việc. Chúng có thể thay con người làm việc ở những môi trường độc hại, ẩm ướt, bụi bặm hay nguy hiểm. Ở những nơi như các nhà máy hoá chất, các nhà máy phóng xạ, trong lòng đại dương, hay các hành tinh khác thì việc ứng dụng robot để cải thiện điều kiện làm việc là rất hữu dụng. 5
Chương 1: Tổng quan về Robot công nghiệp 1.2.2. Mộ số lĩnh vực ứng dụng. a. Ứng dụng trong các lĩnh vực sản xuất cơ khí. Trong lĩnh vực cơ khí, robot được ứng dụng khá phổ biến nhờ khả năng hạot động chính xác và tính linh hoạt cao. Các loại robot hàn là một ứng dụng quan trọng trong các nhà máy sản xuất ôtô, sản xuất các loại vỏ bọc cơ khí Hình 1.7. Robot hàn trong công nghệ sản xuất cơ khí. Ngoài ra người ta còn sử dụng robot phục vụ cho các công nghệ đúc, một môi trường nóng bức, bụi bặm và các thao tác luôn đồi hỏi độ tin cậy. Đặc biệt trong các hệ thống sản xuất linh hoạt (FMS), Robot đóng vai trò rất quan trọng trong việc vân chuyển và kết nối các công đoạn sản xuất với nhau. Hình 1.8. Ứng dụng Robot trong các hệ thống sản xuất linh hoạt. b. Ứng dụng trong lĩnh vực gia công lắp ráp. Các thao tác này thường được tự động hoá bởi các robot được gia công chính xác và mức độ tin cậy cao 6
Chương 1: Tổng quan về Robot công nghiệp Hình 1.9. Robot được sử dụng trong công đoạn cấp liệu và lắp ráp. c. Ứng dụng trong các hệ thống y học, quân sự, khảo sát địa chất. Ngày nay, việc sử dụng các tiện ích từ Robot đến các lĩnh vực quân sự, y tế, rất được quan tâm. Nhờ khả năng hoạt động ổn định và chính xác, Robot đặc biệt là tay máy được dùng trong kĩ thuật dò tìm, bệ phóng, và trong các ca phẫu thuật y khoa với độ tin cậy cao. Hình 1.10. Các ứng dụng Robot trong các lĩnh vực thám hiểm, quân sự, vệ tinh Ngoài ra, tuỳ thuộc vào các ứng dụng cụ thể khác mà Robot được thiết kế để phục vụ cho các mục đích khác nhau, tận dụng được các ưu điểm lớn của chúng đồng thời thể hiện khả năng công nghệ trong quá trình làm việc. 1.3. Các khái niệm về Robot – Robot công nghiệp. Lĩnh vực nghiên cứu về Robot hiện nay rất đa dạng và phong phú. Trong 7
Chương 1: Tổng quan về Robot công nghiệp tài liệu này, chúng tôi chỉ trình bày các kiến thức chủ yếu trên các loại Robot công nghiệp, tức các cánh tay máy. Các bài toán cân bằng lực, các phương trình động học và động lực học là những nền tảng cơ bản để các bạn học viên có thể tiếp cận với chuyên nghành kĩ thuật Robot. 1.3.1. Định nghĩa về robot công nghiệp ( Industrial Robot ). Tuỳ thuộc mỗi quốc gia, tổ chức và mục đích sử dụng, chúng ta có nhiều định nghĩa về robot công nghiệp. Vì vậy trong nhiều tài liệu khác nhau, định nghĩa về robot công nghiệp cũng khác nhau. Theo từ điển Webster định nghĩa robot là máy tự động thực hiện một số chức năng của con người. Theo ISO ( International Standards Organization ) thì : Robot công nghiệp là tay máy đa mục tiêu, có một số bậc tự do, dễ dàng lập trình và điều khiển trợ động, 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. Tuy nhiên Robot công nghiệp được định nghĩa như vậy chưa hoàn toàn thoả đáng. Theo tiêu chuẩn của Mỹ RIA ( Robot Institute of America ) định nghĩa robot là loại 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ụ hay các thiế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. Hình 1.11. Biểu diễn không gian của cánh tay máy 1.3.2. Các thành phần cơ bản của của Robot công nghiệp. Sơ đồ tổng quan cấu thành một Robot công nghiệp chuyên dùng : Cảm biến 8 Giao diện và Bộ điều khiển Nguồn Cánh tay
Chương 1: Tổng quan về Robot công nghiệp a. Cánh tay Robot (Robot Arm ): Là bộ phận cơ khí gồm các khâu liên kết với nhau bởi các khớp nối, các bộ truyền động như: Bộ truyền bánh răng, bộ truyền đai, bộ truyền trục vít- bánh ví, vít me- đai ốc Hình 1.12. Cánh tay Robot. b. Nguồn động lực: Các thiết bị tạo chuyển động cho Robot, có thể là các thiết bị khí nén, thuỷ lực, điện. Đối với các chuyển động cần độ chính xác cao, yêu cầu gọn nhẹ người ta có thể dùng các loại nguồn truyền động là các motor bước, các motor servo. 9
Chương 1: Tổng quan về Robot công nghiệp Hình 1.13. Cấu tạo của motor một loại motor bước. c. Bộ điều khiển ( Controller ): Là thành phần quan trọng quyết định khả năng hoạt động và độ chính xác của Robot. Bộ phận này thông thường được tích hợp dưới dạng các board mạch điều khiển, có thể có các loại sau: IC diều khiển trung tâm (CPU) kết hợp với các card điều khiển phân theo modul. Các thiết bị điều khiển Robot sử dụng PLC ( Programable Logic Controller ). Sử dụng các bộ điều khiển PMAC ( Programable Multi-Axies Controller ). Các bộ điều khiển thiết kế theo các dạng điều khiển hiện đại như : Bộ điều khiển mờ, bộ điều khiển theo mạng neuron d. Cảm biến ( Sensor ): Là thiết bị chuyển các đại lượng vật lý thành các tín hiệu điện cung cấp cho hệ thống nhằm nâng cao khả năng linh hoạt và độ chính xác trong điều khiển. Như vậy Robot chính là một hệ thống điều khiển kín với vòng hồi tiếp ( Feedback ) được thực hiện từ tín hiêu thu về từ cảm biến.Các loại cảm biến thường gặp như: Cảm biến quang Cảm biến vị trí và dịch chuyển. Cảm biến đo góc. Cảm biến vận tốc. Cảm biến gia tốc và rung. Cảm biến lực và biến dạng. Các cảm biến trên có thể cho tín hiệu tương tự Analogue hoặc tín hiệu số ( Digital ), ngoài ra còn sử dụng các bộ mã hoá vị trí, mã hoá góc dịch chuyển Encoder, Resolver e. Các chương trình: Các chương trình luôn tương thích với các bộ điều khiển. Chính vì vậy các loại ngôn ngữ để viết chương trình điều khiển cho Robot cũng kha đa dạng, có thể là ngôn ngữ viết cho vi xử lý (ngôn ngữ máy ), ngôn ngữ viết cho PLC (thuộc các hãng khác nhau ), hay các ngôn ngữ trên máy tính như: Pascal, C, C++, Visual Basic, Matlab 10
Chương 1: Tổng quan về Robot công nghiệp 1.3.3. Bậc tự do của Robot công nghiệp. a. Khái niệm: Bậc tự do là số khả năng chuyển động của một cơ cấu để dịch chuyển được một vật thể nào đó 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 nhất định. Nói chung, cơ hệ của một robot là một cơ cấu hở ( là cơ cấu có một khâu nối giá ). Chuyển động của các khâu trong robot thường là một trong hai khâu chuyển động cơ bản là tịnh tiến hay chuyển động quay. b. Xác định số bậc tự do của robot (DOF- Defree Of Freedom). Số bậc tự do của robot được xác định: W= 6n - ∑i.Pi W: Số bậc tự do của robot. n: Số khâu động. Pi: Số khớp loại i. Trong đó, khớp loại i là khớp khống chế i bậc tự do. Hình 1.14. Robot PUMA 6 bậc tự do. Ví dụ: Xác định số bậc tự do của robot sau: 11
Chương 1: Tổng quan về Robot công nghiệp Hình 1.15. Bậc tự do của robot Xác định được số khớp loại 5 là 5 (4 khớp quay và một khớp tịnh tiến ), do đó n=5 và P5 =5 nên số bậc tự do của robot này: W= 6.5 – 5.5 = 5 bậc. Lưu ý: Hầu hết robot sử dụng khớp loại 5 ( khống chế 5 bậc tự do, chuyển động quay hoạc tịnh tiến ). Vì vậy số bậc tự do của nó cũng chính là số khâu động, robot có bậc tự do càng cao thì càng linh hoạt. Thông thường 3 bậc tự do đầu dùng để định vị, các bậc tự do sau để định hướng. 1.3.4. Hệ toạ độ trong robot. Mỗi robot thường bao gồm nhiều khâu liên kết với nhau ( links ) thông qua các khớp ( joints ) tạo thành một xích động học xuất phát từ một khâu cơ bản đứng yên. Hệ toạ độ gắn với khâu cơ bản gọi là hệ toạ độ cơ bản ( hay hệ toạ độ chuẩn ). Các hệ toạ độ trung gian khác gắn với các khâu động gọi là hệ toạ độ suy rộng. Tại từng thời điểm hoạt động các toạ độ suy rộng xác định cấu hình của robot bằng các chuyển dịch dài hoặc các chuyển dịch góc của các khớp tịnh tiến hoặc khớp quay. Các toạ độ suy rộng còn lại là các biến khớp. Tất cả các hệ toạ độ dùng trong robot phải tuân theo qui tắc bàn tay phải : Dùng bàn tay phải co hai ngón út và áp út, ngón cái trỏ theo phương diện trục z, ngón trỏ theo phương diện trục x, ngón giữa hướng trục y. x4 o4 d2 θ4 θ5 y4 z4 θ3 z0 θ0 y0 12 x0
Chương 1: Tổng quan về Robot công nghiệp Hình 1.16. . Hệ toạ độ của robot có n khâu. Các góc quay θ1, θ3, θ4, θ5 và độ dịch chuyển dài d2 là các toạ độ suy rộng ( các biến khớp ). Để khảo sát động học robot ta phải gắn trên mỗi khâu của robot một hệ toạ độ. Nguyên tắc chung để gắn hệ toạ độ sẽ được trình bày trong chương III trong khi xét đến phương trình động học của robot và bộ thông số Denavit- Hartenberg. Ví dụ: Xác định toạ độ cho robot SCARA (Robot có 4 bậc tự do ). d3 o0 y0 o1 y1 x2 o2 x0 z0 y2 x1 z1 z2 o3 x3 z3 y3 x4 o4 y4 z4 Hình 1.17. Xác định toạ độ cho các khâu của Robot Scara. 1.4. Phân loại Robot. 1.4.1. Robot công nghiệp. 1. Robot nối tiếp (series robot). Thực chất loại Robot này chính là các loại tay máy, các khâu và khớp nối của chúng được thiết kết liên tiếp nhau để hình thành nên các quĩ đạo chuyển động nhất định. Đối với loại robot này, chúng ta có nhiều cách phân loại khác nhau : a. Phân loại theo kiểu kết cấu. Robot kiểu toạ độ Đềcác. Tay máy có 3 chuyển động tịnh tiến theo 3 phương của hệ tọa độ Đềcác trong không gian. 13
Chương 1: Tổng quan về Robot công nghiệp Thường ứng dụng loại robot này trong việc vận chuyển phôi liệu, lắp ráp, hàn trong mặt phẳng Hình 1.18. Robot kiểu toạ độ Đề các Robot kiểu toạ độ trụ. Vùng làm việc của robot này có dạng hình trụ rỗng Robot Versatran (hãng AFM, Hoa Kỳ) là một robot thuộc loại này. Hình 1.19. Robot kiểu toạ độ trụ Robot kiểu toạ độ cầu. Vùng làm việc của robot có dạng hình cầu. Có hai loại cấu hình chính thuộc kiểu robot này : 3 khớp quay (RRR) 2 khớp quay, 1 khớp tịnh tiến ở khâu cuối (RRT) Hình 1.20. Robot kiểu toạ độ cầu Robot kiểu Scara. Robot có cấu trúc theo kiểu Scada ra đời từ năm 1979, tại trường đại học Yamanashi (Nhật Bản). Robot laọi này thường được ứng dụng trong các lĩnh vực lắp ráp, với cấu hình của 3 khâu đầu tiên là : RRT Hình 1.21. Robot kiểu Scara. b. Phân loại theo nguồn truyền động. Hệ truyền động điện. Hệ truyền động thuỷ lực. Hệ truyền động khí nén. c. Phân loại theo các ứng dụng. 14
Chương 1: Tổng quan về Robot công nghiệp Hình 1.22. Phân loại các loại robot chuyên dùng. (Nguồn : Reis Robotics, ABB Flexible Automation, CMB Automation) 2. Robot song song (Parallel Robot). Các loại Robot thuộc nhóm này có các khâu chuyển động song song tương đối với nhau. Thông thường chúng gồm 1 đế cố định và 1 đế di động. Hình 1.23. Một sản phẩm robot song song (Nguồn : PRSC’s) Tuỳ thuộc vào số lượng các nhánh của robot song song mà ta có thể phân loại chúng với nhau. Một loại robot song song có 6 nhánh được sử dụng rất phổ biến là Hexapod. 1.4.2. Robot di động (Mobile Robot). 15
Chương 1: Tổng quan về Robot công nghiệp Đây là hệ Robot có nhiều tính năng thông minh và linh hoạt trong quá trình ứng dụng nhờ khả năng di chuyển được theo lập trình. Hình 1.24. Mobile robot ứng dụng trong các lĩnh vực khác nhau. Hệ thống mobile robot là lĩnh vực thật sự hấp dẫn đối với các nhà nghiên cứu cũng như những người quan tâm, không chỉ nhờ những ưu điểm nổi bậc của nó mà còn ở tính đa dạng trong ứng dụng. Phân tích động học và động lực học mobile robot là những bài toán có mức độ phức tạp khác nhau, nó tuỳ thuộc vào kết cấu của robot cũng như yêu cầu về độ chính xác, tính thông minh trong xử lý tình huống Chúng ta xem xét một vài chuyển động mà con người mong muốn thiết kế các loại mobile robot + Chuyển động theo dạng trườn : + Chuyển động “slide” của các loài động vật bò sát. + Chuyển động chạy của động vật 4 chân. Loại chuyển động + Chuyển động đi bộ của con người. 16
Chương 1: Tổng quan về Robot công nghiệp Ngày nay con người đã hiện thực hoá được các ý tưởng này, mặc dù mức độ chính xác, độ tin cậy của mỗi loại, mỗi hãng sản xuất là khác nhau. Hình 1.25. Robot chuyển động bốn chân Hình 1.26. Mobile Robot tác vụ (Nguồn : AIBO, SONY, Nhật Bản) (Nguồn: SDR-4X, SONY, Nhật Bản) 17
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy Chương 2 PHÂN TÍCH HỆ CƠ CÂN BẰNG TĨNH VÀ CHUYỂN ĐỘNG CỦA TAY MÁY 2.1. Các khái niệm cơ bản và tiền đề tĩnh học 2.1.1. Trạng thái cân bằng . Hệ vật được xem như ở trạng thái cân bằng khi tổng các ngoại lực tác động lên nó bằng không. Lúc ấy hệ vật hoặc đừng yên hoặc chuyển động thẳng đều đối với hệ qui chiếu đó. . Trong thực tế luôn tồn tại lực ma sát nên khi hệ vật đạt trạng thái cân bằng thì nó đứng yên. 2.1.2. Lực . Lực đặc trưng cho tác dụng cơ học của vật thể này lên vật thể khác . Lực được biểu diễn bằng một vector {phương, chiều, độ lớn, điểm đặt} . Trong hệ trục {x,y,z} thì lực F (Fx , Fy , Fz ) 2.1.3. Mômen của lực đối với tâm  . Mômen của lực F đặt tại A đối với tâm O là m0 (F) OA F d F . m0 (F) có độ lớn bằng d.F, điểm đặt tại O, phương vuông góc với mặt phẳng (F,O), chiều thuận theo chiều xoay của OA, F m (F) A 0 F d O 2.1.4. Momen của lực đối với trục (∆) . Tách F F// F => m0 (F) dF . Vậy momen cua lực đối với trục bằng tích của thành phần hình chiếu vuông góc của lực (lên mặt phẳng vuông góc với trục) với khoãng cách từ lực hình chiếu đến trục. . Chiều của momen hường theo chiều xoay của lực quanh trục. 18
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy ( ) F F// F d O 2.1.5. Hệ lực . Hệ lực tác dụng vào một vật đang khảo sát (Fk ) (F1, F2 , , Fn ) . Hai hệ lực (Fk )  (Ph ) khi chúng có cùng tác dụng cơ học . Hợp lực của hệ lực: R được gọi là hợp lực của hệ lực (Fk ) khi R Fk . Hệ lực cân bằng khi R 0 2.1.6. Các tiên đề tĩnh học . Hai lực cân bằng khi chúng cùng phương, ngược hướng, cùng độ lớn. . Hợp lực của hai lực là vector lực đường chéo của hình bình hành. F1 R R F1 F2 F 2 . Khi hai vật tương tác với nhau, chúng tác lên nhau một lực: . Hai lực tương tác cùng phương, cùng độ lớn, nhưng ngược hướng. . Điểm đặt của 2 lực nằm ngay tại vị trí tiếp xúc của 2 vật và hướng vuông góc với tiếp tuyến tại điểm tiếp xúc. F N Tiếp tuyến . Vật tự do là vật có thể dịch chuyển tùy ý trong lân cận bé từ vị trí đang xét. Ngược lại gọi là vật không tự do . Vật khảo sát (S) được qui ước gọi là vật chịu liên kết. Các vật khác tương tác cơ học với S được gọi là vật gây liên kết. . Vật không tự do có thể xem là tự do nếu ta thay thế các vật gây liên kết bằng các phản lực liên kết. Ví dụ : 19
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy h M r ● m m . Điều kiện cân bằng của hệ tĩnh R 0 (F )  0 k , trong đó R là vector hợp lực và M 0 là mô men M 0 0 chính với tâm O của hệ (Fk ) . Ta có M m (F ) Rx  Fkx ox  x k k k R(Rx , Ry , Rz ) Ry  Fky , và M 0 (M ox, M oy, M oz) M oy my (Fk ) k k R F z  kz M oz mz (Fk ) k k  Fkx 0 k  Fky 0 k F 0  kz k Vậy điều kiện để hệ cân bằng tĩnh là (F)  0 mx (Fk ) 0 k m y (Fk ) 0 k mz (Fk ) 0 k 2.1.7. Một số mô hình phản lực liên kết a. Phản lực liên kết một chiều 20
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy N s N N N 2 s 1 b. Liên kết bản lề trụ s s s c. Liên kết bản lề cầu Rz R y Rx Ký hiệu qui ýớc d. Liên kết gối đỡ Ký hiệu qui ước ước e. Liên kết thanh Vi dụ: Xác định các phản lực liên kết của thanh trong hệ sau 21
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy A 0 A 1 45 A2 P m(5kg) y P 2.1.8. Sức bền của vật liệu Thanh liên kết , dy y z dx , x x y , z dz a. Các tác động lực lên thanh bao gồm: . Lực kéo: làm cho thanh dãn ra theo hai chiều của lực . Lực nén: làm cho thanh nén lại theo hai chiều của lực . Lực xoắn: làm cho thanh vặn cong Dưới tác động của các ngoại lực mỗi phần tử dv(dx,dy,dz) đều chịu tác động của các vector lực, được gọi là các tensor ứng suất. Các vector ứng suất này có được thể hiện như hình vẽ, theo từng cặp vector bằng nhau về độ lớn nhưng ngược chiều nhau, (dx,dx,,dy,dy,,dz,dz, ). b. Trạng thái vật lý của thanh khi bị xoắn bị kéo giãn  Nửa trên của thanh có xu hướng bị kéo giãn ra  Nửa dưới của thanh có xu hướng bị nén lại bị nén lại F 22
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy c. Khả năng chịu giãn và nén của các khi loại . Mỗi loại vật liệu có khản năng chụi giãn và nén khác nhau, chúng được gọi là các giá trị tới hạn nén Fn và giá trị tới hạn kéo FK . Nhưng nói chung khả năng chịu nén tốt hơn so với chịu giãn. . Khi bị nén quá mức giới hạn kim loại sẽ bị biến dạng, sau lần biến dạng này chúng sẽ có một giá trị tới hạn Fn khác, lớn hơn giá trị ban đầu. F Fn t . Khi bị kéo quá mức giới hạn kim loại sẽ bị biến dạng, sau lần biến dạng này chúng sẽ có một giá trị tới hạn Fk khác, nhỏ hơn giá trị ban đầu, và cứ như thế cho đến khi đứt rời ra. F Fk t 2.1.9. Lực ma sát a. Định nghĩa: Ma sát là lực sinh ra do sự cọ sát giữa hai vật. Vật này cọ sát sinh ra lực ma sát tác động lên vật kia và ngược lại 1 1 F m12 . Fm12  Fm21 F m21 2 2 . Fm12 Fm21 b. Phân loại: Có hai loại ma sát, là ma sát tĩnh và ma sát động Ma sát tĩnh là lực ma sát xuất hiện khi hai vật tiếp xúc nhau nhưng chưa chuyễn động 23
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy Ma sát động là lực ma sát xuất hiện khi hai vật tiếp xúc nhau và có sự chuyễn động tương đối giữa vật này với vật kia c. Tính chất của lực ma sát: Lực ma sát tỷ lệ với diện tích tiếp xúc và tốc độ cọ sát giữa hai vật d. Lợi điểm của lực ma sát: dùng để hãm, thắng động cơ, bánh xe e. Bất lợi của lực ma sát . Tốn công vô ích . Lực ma sát sinh ra nhiệt làm nóng hệ thống, nóng các điểm tiếp xúc và qua thời gian gây hư hỏng thiết bị (biến dạng bề mặt tiếp xúc) f. Phương pháp làm giảm bớt lực ma sát . Giảm diện tích tiếp xúc (Sử dụng các khe, các bánh xe, bac đạn, con trượt) . Giảm tốc độ cọ sát (tăng tốc từ từ) . Sử dụng các chất bôi trơn nơi tiếp xúc (nhớt, mở bò) 2.2. Thiết kế hệ cơ cân bằng tĩnh 2.2.1. Bước 1: Xác định các yếu tố đầu vào . Đối tượng phụ vụ: khối lượng, kích thước hình dạng, độ cứng . Chu trình phụ vụ: các thao tác, tiến trình thực hiện và các toạ độ, quĩ đạo của chu trình . Không gian phục vụ . Nguồn năng lượng cung cấp 2.2.2. Bước 2: Thiết kế khung cơ khí . Vẽ kết cấu hình học, xác định các khớp động . Xác định các nguồn lực cho các khớp động: motor(DC, AC, servo), khí nén, thủy lực . Xác định hệ truyền động cho các khớp: trực tiếp hay gián tiếp, vị trí đặt nguồn lực, khối lượng các nguồn lực . Tối ưu hoá các bước a, b, c để lợi về lực và đơn giản về kết cấu . Xác định vật liệu cho các thanh, dạng hình học và kích thước 2.2.3. Bước 3: Tính toán cân bằng lực cho hệ . Xác định các phản lực liên kết của các thanh . Dựa trên các phản lực liên kết, xác dịnh các nguồn lực: motor(ngẩu lực), khí nén(áp suất nén), 24
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy . Tính toán cân bằng lực cho cả hệ: tính toán cân bằng lực cho các khâu và cho đế tải trọng Ví dụ: Thiết kế hệ cân bằng tĩnh cho cánh tay Robot trong dây chuyền phân loại sản phẩm dưới đây Khâu 1 Thanh d2 Khâu 2 1 2 Thanh d1 Tay gắp dùng giác hút đế tải trọng M 1m Băng chuyền A Băng chuyền B 2m Bước 1: a. Vật thể M có khối lượng 0,5kg, kích thước hình trụ cao 10cm, có nhãn mác nên dễ trầy xước b. Nhấc vật M lên, di chuyễn từ băng chuyền A sang băng chuyền B, hạ vật B xuống c. Khoảng cách giữa 2 băng chuyền 2m, chiều cao của băng chuyền 1m, chiều cao của vật M là 10cm d. Nguồn năng lượng cung cấp khí nén Bước 2: a. Kết cấu hình học như hình vẽ  Khớp 1: xoay quanh trục  Khớp 2: khớp trượt lên xuống  Tay gắp: dùng giác hút  Thanh 1 có chiều cao: 1m + 0,1m +(chiều dài cylinder trượt)  Thanh 2 có chiều dài: 1m  Đế tải trọng có hình dạng và kích thước như hình vẽ 25
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy 0.5m Chân đế 0.25m 0.5m 0.25m b. Nguốn lực  Khớp 1: dùng vô lăng khí nén để truyền động xoay trực tiếp, khối lượng 1kg  Khớp 2: dùng cylinder khí nén truyền động trượt trực tiếp, khối lượng 1kg  Tay gắp: dùng van khí nén để điều khiển giác hút, khối lượng 200g c. Vật liệu làm cho các thanh là Inox  Thanh 1: loại thanh tròn, Φ34, khối lượng 8kg  Thanh 2: loại thanh tròn, Φ20, khối lượng 5kg  Tay gắp: phểu giác hút, Φ8  Đế tải trọng: Sắt tấm si Inox, dày 5mm, khối lượng 7kg Bước 3: a. Hoá rắn toàn hệ, xác định các phản lực liên kết của đế tải trọng, như hình vẽ P P  Do hệ đối xứng nên: cylinder volang N1 N4 và N2 N3  PT cân bằng của hệ lực:  Pk  Ni 0 Pthanh2 m (P ) m (N ) 0 PM  ( ) k  ( ) i  Tính cân b ằng lự c: N4 PM Pcylinder Pthanh2 Pvolang Pthanh1 Pde ( ) N3 N1 N 2 N3 N 4 0 P de 2N 2N 225(N) (1) 1 2 N1 P N2 thanh1 26
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy P  Phương trình cân bằng momen cylinder Pvolang 1m 0.75 0.75 (P P ) P 0.5 2N M cylinder 2 t1_1 2 0.25 P Pt1_ 2 0.25 Pvolang 0.25 (Pde Pthanh1 ) 0 P t1_ 2 2 t1_1 PM 11.25 14.0625 N2 1.5625 2.5 37.5 0 2N2 2N1 N2 16.25(N) (2) ( ) 0.25m Thay (2) vào (1) ta được N1 96.25(N) Pde P  Nhận xét: ta thấy N2 >0, nên hệ cân thanh1 bằng và ta không cần thêm đối trọng cho đế b. Xác định nguồn lực cho các khâu  Tay ghắp: dùng van hút chân không có áp suất m.g m.g P 2 1K(atm) , ta chọn P = 1.5K(atm) s .rM  Khâu 1: Cylinder khí nén có áp suất P ≥ 1K (atm), ta cũng chọn P = 1.5K (atm)  Khâu 2: Volang khí nén có áp suất P = 1.5K (atm) c. Áp suất nguồn khí nén cung cấp cho toàn hệ: ta chọn 2K(atm) 2.3. Phân tích chuyển động tay máy. 2.3.1. Giới thiệu về phân tích chuyển động . Với một hệ tay máy đã được thiết kế, vấn đề đặt ra là làm thế nào để xác định quỹ đạo của các khâu trong chu trình hoạt động của Robot . Việc phân tích chuyển động của tay máy nhằm mục đích tìm ra các quỹ đạo này, nhưng việc thực hiện được tiến hành theo hai bước: Xác định toạ độ của các khâu trung gian, rối từ đó định ra quỹ đạo của các khâu. . Để đơn giản cho việc phân tích chuyển động, thiết kế cơ khí và đều khiển Robot, ta thường đơn giản hoá các khâu ở một trong hai dạng cơ bản là khớp trượt và khớp bản lề . Khái niệm bậc chuyển động tự do thể hiện cho số khâu có trên Robot 27
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy 2.3.2. Hệ toạ độ Để khảo sát cho chuyển động các khâu, ta gắn vào đấy một hệ tọa độ (0xyz). Hệ trục này được đặt sao cho đơn giản cho việc khảo sát y4 y3 x4 x2 3 4 2 y1 x3 z2 y2 z3 z4 1 x1 z1 2.3.3. Quỹ đạo Để mô tả quỹ đạo của tay máy ta thể hiện thông qua các tọa độ suy rộng của các hệ tọa độ khâu. Ví dụ để mô tả quỹ đạo của tay máy tại vị trí M của tay gắp (khâu cuối) xM xM (q1x , q2x , , qnx) Trong đó, q1, q2, là các tọa độ suy rộng, yM yM (q1y , q2 y , , qny ) ứng với chuyển động zM zM (q1z , q2z , , qnz) của các khâu. 2.3.4. Phân tích chuyển động tổng quát của tay máy. a. Bài toán động học thuận Mô hình của bài toán là cho trước cơ cấu và quy luật chuyển động của hệ, thể hiện qua các tọa độ suy rộng. Ta phải xác định quy luật chuyển động của một vị trí xác định nào đó trong hệ. Bài toán này trong thực tế, nó thường được dùng sau khi giải quyết bài toán động học ngược, để xác định ranh giới chuyển động và kiểm tra cân bằng động của các phần tử trong hệ. b. Bài toán động học ngược Mô hình của bài toán là cho trước cơ cấu và quy luật chuyển động của khâu cuối, ta phải xác định quy luật chuyển động của các khâu thành viên, tức là xác định các tọa độ suy rộng. 28
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy Bài toán này cho vô số lời giải (vô số nghiệm của các qi). Trong thực tế khi giải quyết các bài toán này, ta thường thêm vào nó các điều kiện ràng buộc của chuyển động tay máy để cho lời giải tối ưu. 2.3.5. Phép biến đổi hệ tọa độ Cho hai hệ trục tọa độ (Oxyz) và (O1x1y1z1) như hình vẽ, i0 , j0 ,k0 là các vector chỉ phương đơn vị của hệ (Oxyz) z z1 a y1 k0 o o y 1 i0 j0 x x 1 Cho a trong hệ (Oxyz) được thể hiện a axi0 a y j0 azk0 Với : ax a cos(a,i0 ) ay a cos(a, j0 ) az a cos(a,k0 ) Định lý về phép chiếu hình học Hình chiếu của a theo hướng u bất ký là:  au ax cos(u, x) ay cos(u, y) az cos(u, z) Vậy chiếu của: lên x1 là ax1 ax cos(x1, x) ay cos(x1, y) az cos(x1, z)  lên y1 là ay1 ax cos(y1, x) ay cos(y1, y) az cos(y1, z) lên z là 1 az1 ax cos(z1, x) ay cos(z1, y) az cos(z1, z) Vậy trong hệ tọa độ (O1x1y1z1), a ax1i1 ay1 j1 az1k1 Lập bảng Cosin chỉ hướng cho hệ phương trình trên ta được x y z 1 cos(x1, x) , 2 cos(y1, x) , x1 1 1  1 y1 2  2  2 z1 3 3  3 Gọi ma trận cosin chỉ hướng từ hệ tọa độ (Oxyz) vào (O1x1 y1z1 ) là 29
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy 1 1  1 ax ax 1 MC   a MC a 10 2 2 2 => y1 10 y   a a 3 3 3 z1 z Tương tự như vậy nếu trong hệ tọa độ (O1x1y1z1), a ax1i1 ay1 j1 az1k1 Thì trong hệ tọa độ (Oxyz), sẽ có ma trận cosin chỉ hướng là: 1 2 3 a a x x1 T T MC01 1 2 3 MC MC a MC a => 01 10 => y 10 y1    a a 1 2 3 z z1 2.4. Phân tích chuyển động của một số tay máy. 2.4.1. Phân tích chuyển động của tay máy 2 khớp quay. x2 P x2 2 2 y2 y1 y1 z2 z2 y2 x1 1 x1 1 z1 z 1 Hình 1a) Hình 2a) Xét chuyển động của một tay máy hai bậc tự do như hình 1a, hình 2a, giả sử ta hoá rắn khâu 2, cho khâu 1 chuyển động xoay Ta thấy điểm P trong hệ tọa độ của khâu 2 không chuyển động, nhưng trong hệ tọa độ của khâu 1 thì nó chuyển động. Tọa độ của P được tính dựa vào hình 1b) và 2b) 30
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy x2 P r x2 2 2 2 r2 P y2 r1 y1 y1 d1 d1 z2 z2 r1 y2 x1 1 x1 1 z z 1 1 Hình 1b) Hình 2b) Vậy tọa độ của P trong hệ khâu 1 là (r1)1 (d1)1 (r2 )1 MC12(d1 r2 )2 2.4.2. Phân tích chuyển động của tay máy ba khớp quay. y3 x2 P 3 2 y1 x3 z2 y2 z3 z4 1 x1 z 1 Xem xét mô hình của tay máy ba bậc tự do như hình vẽ trên 31
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy y3 x2 P r3 3 2 d2 y 1 x 3 d1 r1 z2 y2 z3 z4 1 x1 z1 Từ mô hình vector ta thấy: r1 d1 d2 r3 r1 1 MC12 (d1 d 2 r3 )2 MC (d ) MC (d r ) 12 1 2 12 2 3 2 => MC12 (d1 )2 MC12 [MC23 (d 2 r3 )3 ] MC12 (d1 )2 MC12 MC23 (d 2 r3 )3 Nếu xem điểm P cũng là một khâu (khâu 4), ta được y4 y3 x4 x2 P r3 3 z4 2 d2 y 1 x 3 d1 r1 z2 y2 z3 1 x1 z 1 Vậy (r ) MC (d ) MC MC (d d ) 1 1 12 1 2 12 23 2 3 3 MC12 (d1)2 MC12 MC23 (d2 )3 MC12 MC23 MC34 (d3)4 ] 2.4.3. Phân tích chuyển động của tay máy nhiều khớp nối. 32
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy Mở rộng với hệ đa bậc tự do, ta có tọa độ của khâu cuối trong hệ tọa độ gốc là n n 1 i (rT (Ter min al) )1 di [MC j( j 1) (di )i 1 ] i 1 i 1 j 1 a. Các bước thực hiện cho việc phân tích chuyển dộng . Bước 1: Xác định hệ trục tọa độ  Xác định đặc tính các khớp: trượt hay bản lề  Đặt các hệ trục tọa độ sao cho trục quay của khớp trùng với trục z, trục thanh tay máy trùng với trục x  Xác định các góc quay, chọn chiều dương của góc quay hướng từ trục thanh(trục x) tới thanh quay (trong không gian 1/4 dương) . Bước 2: Xác định các ma trận MC . Bước 3: Viết phương trình xác định tọa độ của khâu cuối. . Bước 4: Tính toán vận tốc và gia tốc. b. Ví dụ1: Xác định tọa độ của khâu cuối P trong hệ tay máy như hình dưới. 0 0 0 Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, φ1 = 30 , φ2 = 60 , φ3 = 45 Giải z3 x2 3 x3 z4 2 2 d2 x1 d 3 1 d 1 z y2 3 P 2 r1 y3 x4 1 y1 y4 z 1 Ta có (rP )1 MC12 (d1)2 MC12 MC23 (d2 )3 MC12 MC23 MC34 (d3 )4 Mà: 33
Chương 2: Phân tích hệ cơ cân bằng tĩnh và chuyển động tay máy cos(x , x ) cos(y , x ) cos(z , x ) cos sin 0 1 2 3 2 1 2 1 2 1 1 1 M    cos(x , y ) cos(y , y ) cos(z , y ) sin cos 0 12 1 2 3 2 1 2 1 2 1 1 1  1  2  3 cos(x2 , z1 ) cos(y2 , z1 ) cos(z2 , z1 ) 0 0 1 cos(x , x ) cos(y , x ) cos(z , x ) cos 0 sin 3 2 3 2 3 2 2 2 M cos(x , y ) cos(y , y ) cos(z , y ) sin 0 cos 23 3 2 3 2 3 2 2 2 cos(x3, z2 ) cos(y3, z2 ) cos(z3, z2 ) 0 1 0 cos(x , x ) cos(y , x ) cos(z , x ) cos sin 0 4 3 4 3 4 3 3 3 M cos(x , y ) cos(y , y ) cos(z , y ) sin cos 0 34 4 3 4 3 4 3 3 3 cos(x4 , z3 ) cos(y4 , z3 ) cos(z4 , z3 ) 0 0 1 c. Ví dụ 2: Xác định tọa độ của khâu cuối P trong hệ tay máy như hình dưới. 0 0 0 Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, φ1 = 60 , φ2 = 30 , φ3 = 45 z4 z3 x4 x2 P d3 3 y4 2 2 d2 x 3 1 x 3 d1 r1 1 z2 y2 y3 z4 1 y1 z 1 34
Chương 3: Các phép biến đổi thuần nhất Chương 3 CÁC PHÉP BIẾN ĐỔI THUẦN NHẤT Ở chương 2, chúng ta đã tìm hiểu các kiến thức cơ bản về các hệ cân bằng lực cũng như động học của cánh tay máy. Đối với các robot có kết cấu đơn giản, chúng ta có thể áp dụng các phương thức trực tiếp về lực, momen và các thành phần động học để phân tích động học cho robot công nghiệp. Tuy nhiên, phương pháp này gặp nhiều khó khăn đối với các bài toán của robot có cấu hình phức tạp. Vì vậy, trong chương này chúng ta tìm hiểu cách thức tiếp cận khác trong vấn đề giải quyết bài toán động học robot, đó là các phép biến đổi trong hệ toạ độ thuần nhất (gọi tắt là các phép biến đổi thuần nhất). Phương pháp này là bước phát triển từ các nền tảng toán học, cơ học đã tìm hiểu ở chương trước. 3.1. Hệ toạ độ thuần nhất. Để biểu diễn 1 điểm trong không gian 3 chiều, người ta dùng vector điểm ( Point Vector) Các vector điểm thường được kí hiệu bằng các chữ viết thường. Ví dụ a,v, p Tuỳ thuộc hệ qui chiếu được chọn mà 1 điểm trong không gian có thể được biểu diễn bằng các vector điểm khác nhau Ví dụ : zC V VB zA B VA yC A xC yÂ xA Nếu gọi i , j,k là các vector định vị của hệ toạ dộ nào đó thì vector điểm v : v ai bj ck Với a,b,c là toạ độ vị trí của điểm v. o Nếu quan tâm đồng thời vấn đề vị trí và định hướng ta phải biểu diễn vector điểm trong không gian 4 chiều : 35
Chương 3: Các phép biến đổi thuần nhất x y x y z v , với a ; b ; c z w w w w Với w là hằng số thực (hằng số tỉ lệ). + Khi w=1 thì x=a; y=b; z=c : Hệ toạ độ thuần nhất (Lúc này toạ độ không gian 4 chiều trùng với toạ độ không gian 3 chiều) + Khi w=0 thì x, y, z →∞ : Thể hiện hướng của các trục toạ độ → Sử dụng hệ toạ độ với w=0 và w=1 thì có thể thể hiện cả vị trí và định hướng vật thể. + Ki w≠0, và w≠0 thì : v ai bj ck Ví dụ : v 2i 3 j k o Các trường hợp đặc biệt : + [0, ,0, 0, 0]T : Vector không xác định. + [0, 0, 0, n]T : Vector 0. + [x, y, z, 0]T : Vector chỉ hướng. + [x, y, z, 1]T : Vector trong hệ toạ độ thuần nhất. 3.2. Nhắc lại các phép tính về vector và ma trận. 3.2.1) Phép nhân vector : Cho 2 vector : a axi ay j azk b bxi by j bzk a. Tích vô hướng 2 vector : a.b axbx ayby azbz b. Tích có hướng hai vector (Tích hai vector) : i j k a.b c ax ay az bx by bz 3.2.2. Các phép tính về ma trận : a. Phép cộng trừ hai ma trận : Điều kiện : Các ma trận phải cùng bậc (cùng kích thước) 36
Chương 3: Các phép biến đổi thuần nhất Cộng (trừ) hai ma trận A,B cùng bậc ta có ma trận C cùng bậc với các phần tử Cij Aij Bij b. Tích hai ma trận : Điều kiện : Số cột của ma trận thứ nhất bằng số hàng của ma trận thứ hai. Tích của hai ma trận A(m,n) với ma trận B(n,p) là ma trận C(m,p). Ví dụ : 1 2 3 1 2 A 4 5 6 và B 34 7 8 9 56 22 28 A.B C 49 64 76100 Chú ý : + A.B ≠ B.A + (k.A).B = k.(A.B) = A.(k.B) + A.(B.C) = (A.B).C + (A+B).C = A.C+B.C + C.(A+B) = C.A+C.B c. Ma trận nghịch đảo : A.A 1 I Điều kiện : Ma trận A là khả đảo (det(A) ≠ 0) Có một số cách để tính ma trận nghịch đảo. Một trong số đó : + Tính định thức : det(A) + Tính ma trận C là ma trận phần phụ đại số của ma trận A : i j Cij ( 1) Dij với Dij det(Mij ) 1 + Tính ma trận nghịch đảo theo : A 1 CT det(A) d. Ma trận nghịch đảo của ma trận thuần nhất : Cho ma trận thuần nhất A : 37
Chương 3: Các phép biến đổi thuần nhất nx ox ax px n o a p y y y y A nz oz az pz 0 0 0 1 A n o a p Tìm ma trận nghịch đảo của ma trận thuần nhất : n n n n.p x y z o o o o.p x y z A ax ay az a.p 0 0 0 1 Ví dụ : Cho 0 0 1 1 0 1 0 2 A 1 0 0 3 0 0 0 1 0 0 1 3 0 1 0 2 A 1 1 0 0 1 0 0 0 1 Kiểm tra : 1 0 0 0 0 1 0 0 A.A 1 I 0 0 1 0 0 0 0 1 e. Vết của ma trận : Vết của ma trận vuông bậc n là tổng các phần tử trên đường chéo chính. n Kí hiệu : Trace(A) Tr(A) aii i 1 f. Đạo hàm và tích phân của ma trận : Nếu các phần tử của ma trận A là hàm nhiều biến thì các phần tử của ma trận đạo hàm bằng đạo hàm riêng của các phần tử ma trận A theo biến tương ứng. 38
Chương 3: Các phép biến đổi thuần nhất a b c A d e f g h k a b c t t t d e f A t t t g h k t t t Tương tự cho phép tích phân ma trận. 3.3. Các phép biến đổi ma trận dùng trong động học robot. Cho u là vector biểu diễn điểm cần biến đổi h là vector dẫn được biểu diễn b ma trận H là ma trận chuyển đổi : v H.u Là vector biểu diễn điểm sau khi chuyển đổi. 3.3.1. Phép biến đổi tịnh tiến. Giả sử cần tịnh tiến 1 điểm hay hay 1 vật thể theo vector dẫn : h a.i b. j c.k Ma trận chuyển đổi tịnh tiến theo vector dẫn : 1 0 0 a 0 1 0 b H 0 0 1 c 0 0 0 1 Gọi là vector biểu diễn điểm cần tịnh tiến : u= [x, y, z, 1]T 1 0 0 a x x a 0 1 0 b y y b v H.u 0 0 1 c z z c 0 0 0 1 1 1 Kí hiệu : v= Trans(a,b,c).u Ví dụ : Cho u 2.i 3. j 2.k h 4.i 3. j 7.k 39
Chương 3: Các phép biến đổi thuần nhất 1 0 0 4 2 6 0 1 0 3 3 0 v H.u 0 0 1 7 2 9 0 0 0 1 1 1 v=Trans(4, -3, 7).u 3.3.2. Phép quay quanh các trục toạ độ : Giả sử ta cần quay 1 điểm hay vật thể xung quanh 1 trục nào đó với góc quay θ0 ta lần lược có các ma trận chuyển động quay như sau : 1 0 0 0 0 cos sin 0 Rot(x, ) 0 sin cos 0 0 0 0 1 cos 0 sin 0 0 1 0 0 Rot(y, ) sin 0 cos 0 0 0 0 1 cos sin 0 0 sin cos 0 0 Rot(z, ) 0 0 1 0 0 0 0 1 Ví dụ : u 7.i 3. j 2.k Quay một góc 900 quanh trục z : Rot(z, 90), sau đó tiếp tục cho quay y 1 góc 900 : Rot(y, 90) Thực hiện chuyển đổi : v Rot(z,90 ).u 0 1 0 0 7 3 1 0 0 0 3 7 v R.u 0 0 1 0 2 2 0 0 0 1 1 1 Tiếp tục cho quay quanh y 1 góc 900 : 40
Chương 3: Các phép biến đổi thuần nhất W= Rot(y, 90).v 0 0 1 0 3 2 0 1 0 0 7 7 v R.u 1 0 0 0 2 3 0 0 0 1 1 1 Vậy có thể tính : W Rot(y,90 ).Rot(z,90 ).u Chú ý : + Phép quay cần tuân thủ theo đúng thứ tự trước sau . Trong ví dụ : quay quanh trục z trước, trục y sau, ta kí hiệu : Rot(y, 90).Rot(z, 90).u + Vì các phép quay cho các ma trận nên : Rot(y, 90).Rot(z, 90).u ≠ Rot(z,90).Rot(y,90).u 3.3.3. Phép quay Ơle( Euler) Trong thực tế việc định hướng khâu chấp hành cuối thường là kết quả của các phép quay quanh trục x, y, z. Phép quay Ơle mô tả khả năng định hướng của các khâu chấp hành cuối thông qua các góc quay , ,  bởi các phép biến đổi sau : + Quay 1 góc  quanh trục z. + Quay 1 góc  quanh trục y mới là y’ + Quay 1 góc  quanh trục z mới là z’’ Euler(, ,  ) Rot(z,).Rot(y,).Rot(z, ) Rot(z, ).Rot(y,).Rot(z,) Chú ý : Phép quay phải theo thứ tự trước sau , nhưng đặc biệt với phép quay Ơle thì sự thay đổi thứ tự không làm thay đổi kết quả. Công thức tính : Euler(, ,  ) Rot(z,).Rot(y,).Rot(z, ) cos 0 sin 0 cos sin 0 0 0 1 0 0 sin cos 0 0 Rot(z,) sin 0 cos 0 0 0 1 0 0 0 0 1 0 0 0 1 41
Chương 3: Các phép biến đổi thuần nhất cos cos cos sin sin cos cos sin sin cos cos sin 0 sin cos cos cos sin sin cos sin cos cos sin sin 0 sin cos sin sin cos 0 0 0 0 1 3.3.4. Phép quay roll - pitch – yaw. Là phép quay dùng để định hướng khâu chấp hành cuối thường được dùng trong thực tế. Ta tưởng tượng gắn hệ toạ độ xyz lên thân một con tàu YAW ROLL O PITCH + Roll- Chuyển động lắc của thân tàu tương ứng với trục z của thân tàu 1 góc  + Pitch- Chuyển động nhấp nhô của thân tàu tương ứng với việc quay quanh trục y 1 góc  + Yaw- Chuyển động lệch hướng tương ứng với việc quay quanh trục x 1 góc  z y x Người ta sử dụng phép quay này để biểu diễn chuyển động của Robot. Phương pháp này được sử dụng khá phổ biến. 42
Chương 3: Các phép biến đổi thuần nhất RPY (, ,  ) Rot(z,).Rot(y,).Rot(x, ) cos 0 sin 0 1 0 0 0 0 1 0 0 0 cos sin 0 Rot(z,) sin 0 cos 0 0 sin cos 0 0 0 0 1 0 0 0 1 cos cos cos sin sin sin cos cos sin cos sin sin 0 sin cos sin cos sin cos cos sin sin cos cos sin 0 sin cos sin cos cos 0 0 0 0 1 Hay có thể viết : C  C  C  S  S  S  C  C  S  C  S  S  RPY (,, ) S  C  S  S  S  C  C  S  S  C  C  S  S  C  S  C  C  3.4. Biến đổi hệ toạ độ và mối quan hệ giữa các hệ toạ độ 3.4.1. Biến đổi hệ toạ độ. Giả sử cần tịnh tiến gốc tạo độ Đề cac O(0,0,0) theo một vector dẫn h 4.i 3. j 7.k thì kết quả ta được toạ độ điểm OT : 0 0 0 4 0 4 0 1 0 3 0 3 O H.O T 0 0 1 7 0 7 0 0 0 1 1 1 Nếu ta tiếp tục thực hiện các phép quay đối với hệ toạ độ OT thì ta được hệ toạ độ mới : + Nếu chọn hệ qui chiếu là hệ toạ độ gốc thì ta thực hiện các phép biến đổi từ phải sang trái : A Rot(y,90 ).Rot(z, 90 ) 43
Chương 3: Các phép biến đổi thuần nhất zT z'T Rot(z,-90) Rot(y,90) T T OT x' OT x' OT yT xT y'T z'T y'T + Nếu chọn hệ qui chiếu là hệ toạ độ trung gian thì ta htực hiện các phép biến đổi từ trái sang phải : A Rot(y,90 ).Rot(z, 90 ) zT Rot (yT , 90) Rot (z'T ,-90) T OT OT x'' OT yT y'T z'T xT z''T y''T x'T 3.4.2. Mối quan hệ giữa các hệ toạ độ. Giả sử có 3 gốc hệ toạ độ A, B, C thì hệ toạ độ B có mối quan hệ với hệ toạ độ A được biểu diễn : A B A TB B C B TC 44
Chương 3: Các phép biến đổi thuần nhất zC P pC pA C zB yC xC B yB zA xB A yÂ xA Giả sử có điểm P trong hệ toạ độ C được biểu diễn pC . Xác định mối quan hệ của P trong hệ toạ độ A. B Trước hết cần xác định pB : pB TC .pC C A B pA TB .pB TB .TC A A B Vậy : TC TB .TC Tính chất : A B A TB B A B TA B A 1 TA (TB ) 3.5. Mô tả vật thể Vật thể là các đối tượng làm việc của Robot . Dựa vào đặc điểm hình học của chúng , ta có thể chia chúng thành 3 nhóm sau : + Nhóm các vật thể tròn xoay : ngoài giá trị của vị trí và kích thước, ta cần xác định toạ độ tâm và bán kính của đường cong. + Nhóm các vật thể có góc cạnh : Giá trị đặc trưng là toạ độ các điểm giới hạn. + Nhóm các vật thể có cấu trúc hỗn hợp Đối với hoạt động cầm nắm đối tượng và quá trình vận động của Robot thì việc mô tả vật thể cần phải gắn liền với các phép biến đổi thuần nhất. Ví dụ : Cho vật thể hình lăn trụ đặt trong hệ toạ độ oxyz như hình vẽ : 45
Chương 3: Các phép biến đổi thuần nhất C z B F D O y A E x Để mô tả vị trí của vật thể ta dùng ma trận của 6 điểm như sau, phần tử của hàng cuối cùng chính là giá trị w = 1. 1 1 1 1 1 1 0 0 0 0 4 4 A 0 0 2 2 0 0 1 1 1 1 1 1 A A B C D E F Yêu cầu : Thực hiện các phép biến đổi : H=Trans(4,0,0) Rot(y,90°) Rot(z,90°). Thực hiện nhân các ma trận thuần nhất của các phép biến đổi theo đúng thứ tự như trên , ta thu được ma trận H như sau : 0 0 1 4 1 0 0 0 H 0 1 0 0 0 0 0 1 0 0 1 4 1 1 1 1 1 1 1 0 0 0 0 0 0 0 4 4 A' H.A 0 1 0 0 0 0 2 2 0 0 0 0 0 1 1 1 1 1 1 1 4 4 6 6 4 4 1 1 1 1 1 1 A' 0 0 0 0 4 4 1 1 1 1 1 1 46
Chương 3: Các phép biến đổi thuần nhất Kiểm tra lại bằng hình vẽ : Dùng hệ qui chiếu là hệ toạ độ gốc. H=Trans(4,0,0) Rot(y,90°) Rot(z,90°) Thực hiện lần lược theo thứ tự : Quay quanh trục z , quay quanh trục y, tịnh tiến so với hệ toạ độ gốc. + Rot(z,90) : y' z' O x' + Rot(y,90) : y'' O x' z'' + Trans(4,0,0) : y'' O 4 x' z'' 47
Chương 4: Phương trình động học robot Chƣơng 4 PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT 4.1. Dẫn nhập Bất kỳ một Robot nào cũng bao gồm các khâu liên kết với nhau thông qua các khớp. Hai chuyển động cơ bản của các khâu thông qua khớp quay và khớp tịnh tiến. Hình 4.1. Khớp quay và khớp tịnh tiến trong chuyển động của robot. Ta đặt trên mỗi khâu của một Robot một hệ trục toạ độ. Sử dụng các phép biến đổi thuần nhất có thể mô tả vị trí tương đối và hướng giữa các hệ toạ độ này. Theo Denavit, mối liên hệ giữa hai khâu liền kề nhau (khâu n so với khâu (n-1)) được mô tả bởi ma trận A là ma trận biến đổi thuần nhất gồm có các phép quay và tịnh tiến giữa các hệ toạ độ với nhau. Hình 4.2. Đặt hệ trục toạ độ cho các khâu của robot Puma. Vậy, A1 là ma trận mô tả hướng và vị trí của hệ toạ độ gắn trên khâu thứ nhất so với hệ toạ độ gốc. 48
Chương 4: Phương trình động học robot Tương tự cho A2 , là ma trận mô tả mối quan hệ về hướng và vị trí của hệ toạ độ thứ hai so với hệ toạ độ gắn trên khâu thứ nhất. Tích của các ma trận A là ma trận T (Theo Denavit). Ví dụ : T3= A1.A2.A3 Hình 4.3. Các vector định vị và định hướng của tay máy. Lưu ý : + Nếu một Robot có 6 khâu thì : T6=A1A2A3 A4A5A6. T6 được gọi là ma trận vector cuối , mô tả hướng và vị trí của hệ toạ độ gắn lên khâu chấp hành cuối so với hệ toạ độ gốc. + Nếu một Robot có số bậc tự do w>3 thì 3 bậc tự do đầu tiên dùng để định vị, các bậc tự do còn lại để định hướng. + Hệ toạ độ biểu diễn khâu chấp hành cuối (điểm tác động cuối) n o a  : 3 vector chỉ phương của hệ toạ độ gán trên khâu chấp hành cuối, (điểm tác động cuối) xác định bởi : a : Vector có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng. o : Vector có hướng theo đó các ngón tay cầm nắm hay thả đối tượng. n : Vector pháp tuyến của và : n o.a nx ox ax px n o a p y y y y T6 nz oz az pz 0 0 0 1 4.2. Bộ thông số Denavit-Hartenberg (DH) 4.2.1. Các khái niệm : 49
Chương 4: Phương trình động học robot Một Robot gồm nhiều khâu cấu thành từ những khâu nối tiếp nhau thông qua các khớp động. Gốc chuẩn của 1 Robot là là khâu số 0 và không tính vào số các khâu. Khâu 1 nối với khâu chuẩn bởi khớp 1, không có khớp ở đầu mút khâu cuối cùng 4.2.2. Độ dài pháp tuyến chung và góc giữa hai trục khớp : Bất kỳ một khâu nào cũng được đặc trưng bởi hai yếu tố : + Độ dài pháp tuyến chung an + Góc giữa các trục khớp đo trong mặt phẳng vuuong góc với an , ký hiệu là n Hình 4.4. Chiều dài góc xoắn của khâu. :Góc xoắn của khâu n( Khớp n so với khớp (n+1)) an : Chiều dài của khâu n ( Khớp n so với khớp (n+1)) Hình 4.5. Các thông số của khâu : an, αn, dn, θn Các trường hợp đặc biệt : + =0,an =const(2 trục khớp song song) + / /=90, an =const (2 trục khớp vuông góc) + =0(180), an =0 (2 trục khớp trùng nhau ) 50
Chương 4: Phương trình động học robot + / n /=90, an =0 (2 trục khớp cắt nhau và vuông góc nhau) Hình 4.6. Các trường hợp đặc biệt của phương hai trục khớp 4.2.3. Khoảng cách giữa hai khâu và góc quay giữa hai khâu. Tiếp tục khảo sát mối quan hệ giữa các khâu liền kề nhau, phổ biến là hai khâu liên kết nhau ở chính trục của khớp : Hình 4.7. Khoảng cách hai khâu và góc quay giữa hai khâu. Mỗi trục khớp có hai đường pháp tuyến chung đói với nó, khoảng cách giữa hai đường pháp tuyến chung đo dọc theo trục khớp n gọi là dn dn còn gọi là khoảng cách giữa hai khâu : Khâu n so với khâu thứ (n-1) Góc giữa hai đường pháp tuyến chung đo trong mặt phẳng vuông góc với trục khớp thứ n là góc θn. θn là góc quay của khâu thứ n so với khâu thứ (n-1) 4.2.4. Bộ thông số Denavit-Hertenberg : 51
Chương 4: Phương trình động học robot Cả 4 thông số xác định ở trên chính là bộ thông số DH : n , an, dn, θn Với 4 thông số trên , ta có thể xác định vị trí và hướng của mỗi khâu so với nhau và so với toạ độ góc Nếu khớp nối hai khâu là khớp quay thì θn là biến khớp ( 3 thông số còn lại là hằng số) Nếu khớp nối là tịnh tiến thì dn là biến khớp :( θn =0, an =0, =const) 4.3. Gắn hệ toạ độ cho Robot . Để khảo sát động học của Robot ta phải gắn trên mỗi khâu của robot một hệ toạ độ. Nguyên tắc chung để gắn hệ toạ độ như sau : a. Gốc của hệ toạ độ : Gốc toạ độ của khâu thứ n nằm trên đường tâm của trục khớp thứ (n+1) và nằm tại giao điểm của đường pháp tuyến chung an với trục khớp thứ (n+1) (Tổng quát, chéo nhau) Nếu hai trục khớp cắt nhau thì gốc toạ độ on nằm tại chính điểm cắt đó. Nếu hai trục khớp song song nhau thì on nằm trên trục khớp thứ n+1 và tại một một vị trí đặc biệt nào đó để quá trình tính toán là thuận lợi nhất. b. Chọn trục Zn : Trục Zn nằm dọc theo trục khớp thứ n+1 và có hướng về phía các khâu. c. Chọn trục Xn : Trục Xn nằm dọc theo đường pháp tuyến chung hướng từ trục khớp thứ n đến trục khớp thứ n+1. Nếu hai trục khớp cắt nhau thì xn zn .zn 1 d. Chọn trục yn theo qui tắc bàn tay phải. Ví dụ 1: Gắn hệ toạ độ và xác định các thông số DH cho Robot có hai khâu phẳng : 52
Chương 4: Phương trình động học robot Hình 4.8. Xác định hệ trục tọa độ và bộ thông số DH robot hai khớp quay phẳng Bộ thông số DH của robot được xác định : Ví dụ 2: Gắn hệ toạ độ và xác định bộ thông số DH cho Robot Scara : a2 a1 d*3 o0 x0 o1 x1 x2 o2 y0 z0 y2 y1 z1 z2 o3 x3 y3 z3 x4 o4 y4 z4 Hình 4.9. Xác định hệ trục tọa độ và bộ thông số DH cho robot Scara. Bộ thông số DH : * 1 0 a1 0 1 * 2 0 a2 0 2 3 0 0 0 * d3 4 * 0 0 * 4 d4 4.4. Đặc trưng của các ma trận A. Ma trận A là ma trận mô tả mgh hướng và vị trí của hệ toạ độ gắn trên hai khâu liền kề nhau. Căn cứ vào thông số của bộ DH thì ma trận A được đặc trưng bới 4 phép biến đổi sau : i. Quay quanh trục zi-1 một góc i. ii. Tịnh tiến dọc trục zi-1 một quãng di. 53
Chương 4: Phương trình động học robot iii. Tịnh tiến dọc trục xi-1 (đã trùng với xi) một đoạn ai iv. Quay quanh trục x1 một góc i Bốn bước biến đổi này được biểu hiện bằng tích của các ma trận thuần nhất như sau: Ai = R (z,  i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i) cos sin 0 0 sin cos 0 0 Rot(z, ) 0 0 1 0 0 0 0 1 1 0 0 a 1 0 0 0 0 1 0 0 0 1 0 0 H H 1 0 0 1 0 2 0 0 1 d 0 0 0 1 0 0 0 1 1 0 0 0 0 cos sin 0 Rot(x, ) 0 sin cos 0 0 0 0 1 cosi cos i sini sin i sini ai cosi sin cos cos sin cos a sin A i i i i i i i Hay: i 0 sin i cos i di 0 0 0 1 Ma trận Ai được gọi là ma trận chuyển đổi thuần nhất, nó có dạng Ri pi : Ai với Ri là ma trận quay 3 x 3 và pi là vectơ tịnh tiến 3 x 1. 0 1 Lưu ý : Đối với khớp tịnh tiến thì i =a=0 nên: 1 0 0 0 0 cos sin 0 A i 0 sin cos d 0 0 0 1 54
Chương 4: Phương trình động học robot 4.5 Xác định các ma trận T theo ma trận A. Vậy, A1 là ma trận mô tả hướng và vị trí của hệ toạ độ gắn trên khâu thứ nhất so với hệ toạ độ gốc. Tương tự cho A2 , là ma trận mô tả mối quan hệ về hướng và vị trí của hệ toạ độ thứ hai so với hệ toạ độ gắn trên khâu thứ nhất. Tích của các ma trận A là ma trận T (Theo Denavit). Ví dụ : T3= A1.A2.A3 Nếu một Robot có 6 khâu thì : T6=A1A2A3 A4A5A6. T6 được gọi là ma trận vector cuối , mô tả hướng và vị trí của hệ toạ độ gắn lên khâu chấp hành cuối so với hệ toạ độ gốc. Hệ toạ độ biểu diễn khâu chấp hành cuối (điểm tác động cuối) n o a : 3 vector chỉ phương của hệ toạ độ gán trên khâu chấp hành cuối, (điểm tác động cuối) xác định bởi : + a : Vector có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng. + o : Vector có hướng theo đó các ngón tay cầm nắm hay thả đối tượng. + n : Vector pháp tuyến của và : n o.a nx ox ax px n o a p y y y y T6 nz oz az pz 0 0 0 1 Ta có thể xác định ma trận T thông qua hệ toạ độ trung gian : n n 1 Tn  Ai i 1 2 Với : T3 A3 1T A A 3 2 3 4.6. Trình tự thiết lập phương trình động học của robot. 4.6.1. Các bước thực hiện Để thiết lập phương trình động học của robot, ta thực hiện các bước sau : 1. Bước1: Chọn hệ toạ độ cơ bản và gán các hệ toạ độ trung gian khác : + Giả định vị trí ban đầu của Robot, là vị trí các biến khớp thường bằng 0 + Chọn gốc hệ toạ độ O0, O1 + Chọn trục Z0, Z1 theo nguyên tắc chung. 55
Chương 4: Phương trình động học robot Với các robot có w<= 3 thì không thể định hướng cho trục Zn chọn tuỳ ý. + Chọn các trục x0, x1 Vì ma trận Ai = R (z,  i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i) nên trục xn-1 chính là trục quay zn-1 thành trục Zn : Lúc này : αn= (Zn-1, Zn) + Chọn trục y theo nguyên tắc bàn tay phải. * Lưu ý : Trong quá trình gắn htd thì khi xuất hiịen các phéop biến đổi : Trans(0.y,0) và Rot(y,theta) thì vị trí giả định ban đầu là không đúng, cần thay đổi vị trí mới. 2. Bước 2: Lập bảng thông số DH. 3. Bước 3: Xác định các ma trận Ai 4. Bước 4: Tính các ma trận T từ ngọn tới gốc. T4=A1A2A3A4 Tính ngược từ sau ra trước (Thông thường) 5. Bước 5: Viết phương trình động học Robot 4.6.2. Các ví dụ thiết lập phương trình động học : 1. Ví dụ 1. Xác định phương trình động học của Robot hai bậc tự do RT Gắn hệ trục toạ độ cho Robot : z2 y2 O2 x2 z1 y1 x1 O l1 1 y0 O0 x0 z0 Hình 4.10. Gắn hệ toạ độ cơ bản và các hệ toạ độ trung gian cho Robot Khâu 1 : Quay quanh trục Z0, chọn X0 là pháp tuyến chung của (Z0, Z1). Khâu 2 : Tịnh tiến dọc theo trục Z1, chọn X1 nằm ngang. Xác định bộ thông số DH : 56
Chương 4: Phương trình động học robot Khâu i i ai di * 1 1 90 0 l1 * 2 0 0 0 d2 Các biến khớp : , Phương trình động học : + Các ma trận đặc trưng A : c1 0 s1 0 cosi cos i sini sin i sini ai cosi s1 0 c1 0 sini cos i cosi sin i cosi ai sini A A 1 i 0 1 0 l1 0 sin i cos i di 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 0 0 1 0 0 0 cos sin 0 A A 2 i 0 0 1 d2 0 sin cos d 0 0 0 1 0 0 0 1 + Ma trận vector cuối : c1 0 s1 0 1 0 0 0 c1 0 s1 d2s1 s1 0 c1 0 0 1 0 0 s1 0 c1 d c1 T A A 2 1 2 0 1 0 l1 0 0 1 d2 0 1 0 l1 0 0 0 1 0 0 0 1 0 0 0 1 + Phương trình động học thể hiện mối quan hệ về hướng và vị trí của ma trận vector cuối theo các biến khớp : Ba vector chỉ hướng : n,o,a a sin nx cos1 ox 0 x 1 n sin o 0 a cos y 1 , y , y 1 nz 0 oz 1 az 0 Vector định vị : p px d2 sin1 p y d2 cos1 pz l1 1. Ví dụ 2. Xác định phương trình động học Robot có cấu hình RRT 57
Chương 4: Phương trình động học robot Hình 4.11. Robot hai khâu RT i. Gắn hệ toạ độ cho Robot : Hình 4.12. Gắn hệ tọa độ tại Hình 4.13. Gắn hệ tọa độ tại vị trí lựa chọn vị trí ban đầu đã cho. ii. Bộ thông số DH : Khâu θ α ai di 1 * +90 0 d 1 1 2 * -90 0 0 2 3 0 0 0 * 3 iii. Xác định các ma trận A : cosi cos i sini sin i sini ai cosi sin cos cos sin cos a sin i i i i i i i Ai 0 sin i cos i di 0 0 0 1 58
Chương 4: Phương trình động học robot Qui uớc : cos1 = c1 cos2 = c2 c1c2-s1s2 = cos 1 2 = c12 s3c4+c3s4= sin 1 2 = s34 c1c23-s1s23= cos 1 2 3 = c123 c1 0 s1 0 s1 0 c1 0 A 1 0 1 0 d1 0 0 0 1 c2 0 s2 0 s2 0 c2 0 A 2 0 1 0 0 0 0 0 1 1 0 0 0 0 1 c2 0 A 2 0 0 1 d3 0 0 0 1 c1c2 s1 c1s2 c1s2d3 s1c2 c1 s1s2 s1s2d3 T 3 s2 0 c2 c2d3 d1 0 0 0 1 iv. Viết phương trình động học : nx ox ax px n o a p y y y y T3 nz oz az pz 0 0 0 1 3. Ví dụ 3 : Xác định phương trình động học cho Robot 3 khớp quay phẳng 59
Chương 4: Phương trình động học robot i. Bộ thông số DH : 1 * 0 a1 0 1 2 * 0 a2 0 2 3 * 0 a3 0 3 ii. Xác định các ma trận A cosi cos i sini sin i sini ai cosi sin cos cos sin cos a sin i i i i i i i Ai 0 sin i cos i di 0 0 0 1 iii. Tìm phương trình động học : Tương tự, thay vào tính A1 và T3: c123 s123 0 c123a3 c12a2 c1a1 s123 c123 0 s123a3 s12a2 s1a1 T 3 0 0 1 0 0 0 0 1 4. Ví dụ 4. Xác định phương trình động học của robot Puma 6 bậc tự do. Robot Puma là sản phẩm của công ty Unimate (USA), đó là loại robot có 6 bậc tự do được sử dụng tại nhiều nước trên thế giới. 60
Chương 4: Phương trình động học robot i. Gắn hệ tọa độ cho robot Puma. Hình 4. Gắn hệ tọa độ cho robot Puma. ii. Bộ thông số D-H của robot Puma : iii. Phương trình động học của robot Puma có số khớp n = 6 61
Chương 4: Phương trình động học robot c1 s1 0 0 c 2 s 2 0 0 s c 0 0 0 0 1 0 0 1 1 1 1T , 2T 0 0 1 0 s 2 c 2 0 0 0 0 0 1 0 0 0 1 c3 s3 0 a2 c3 s3 0 a2 s c 0 0 0 0 1 d 2 3 3 3 4 3T , 4T 0 0 0 d s c 0 0 3 4 4 0 0 0 1 0 0 0 1 c s 0 0 c 6 s 6 0 0 5 5 0 0 1 0 0 0 1 0 4T 5T 5 , 6 s5 c5 0 0 s 6 c 6 0 0 0 0 0 1 0 0 0 1 Ta có : r11 r12 r12 Px r r r Py 0T 0T 1T 2T 3T 4T 5T 21 22 23 6 1 2 3 4 5 6 r r r Pz 31 32 33 0 0 0 1 Trong đó : r11 c1[c23(c4c5c6 s4 s5 ) s23s5c5 ] s1 (s4c5c6 c4 s6 ) r21 s1[c23(c4c5c6 s4 s6 ) s23s5c6 ] c1 (s4c5c6 c4 s6 ) r31 s23(c4c5c6 s4 s6 ) c23s5c6 r12 c1[c23( c4c5 s6 s4c6 ) s23s5 s6 ] s1 (c4c6 s4c5 s6 ) r22 s1[c23( c4c5 s6 s4c6 ) s23s5 s6 ] c1 (c4c6 s4c5 s6 ) r s ( c c s s c ) c s s 32 23 4 5 6 4 6 23 5 6 r13 c1 (c23c4c5 s23c5 ) s1s4 s5 ] r23 s1 (c23c4c5 s23c5 ) c1s4 s5 ) r33 s23c4 s5 c23c5 Px c1[a2c2 a3c23 d 4 s23] d3 s1 62 Py s1[a2c2 a3c23 d 4 s23] d3c1 Pz a3 s23 a2 s2 d 4c23
r11 c1[c23(c4c5c6 s4 s5 ) s23s5c5 ] s1 (s4c5c6 c4 s6 ) r21 s1[c23(c4c5c6 s4 s6 ) s23s5c6 ] c1 (s4c5c6 c4 s6 ) r31 s23(c4c5c6 s4 s6 ) c23s5c6 r12 c1[c23( c4c5 s6 s4c6 ) s23s5 s6 ] s1 (c4c6 s4c5 s6 ) r22 s1[c23( c4c5 s6 s4c6 ) s23s5 s6 ] c1 (c4c6 s4c5 s6 ) r32 s23( c4c5 s6 s4c6 ) c23s5 s6 Chương 4: Phương trình động học robot r13 c1 (c23c4c5 s23c5 ) s1s4 s5 ] r23 s1 (c23c4c5 s23c5 ) c1s4 s5 ) r33 s23c4 s5 c23c5 Px c1[a2c2 a3c23 d 4 s23] d3 s1 Py s1[a2c2 a3c23 d 4 s23] d3c1 Pz a3 s23 a2 s2 d 4c23 63
Chương 5: Động lực học robot và ứng dụng trong điều khiển Chương 5 ĐỘNG LỰC HỌC ROBOT VÀ ỨNG DỤNG TRONG ĐIỀU KHIỂN 5.1. Mục đích và phương pháp khảo sát động lực học robot Với những mục đích thiết kế và điều khiển, cần thiết phải có một mô hình toán học mô tả động lực học của hệ thống. Vì thế, ở chương này ta sẽ xác lập phương trình chuyển động của tay máy dưới dạng phương trình vi phân. Phương pháp áp dụng ở đây là xây dựng phương trình chuyển động của cơ hệ dựa trên quan hệ năng lượng, xuất phát từ nguyên lý bảo toàn và chuyển hóa năng lượng trên cơ sở xác lập quan hệ giữa động năng và thế năng của cơ hệ tay máy, sau đó sử dụng phương trình vi phân của chuyển động trên cơ hệ với các đại lượng tham gia vào phương trình gồm lực, quán tính và năng lượng. Việc nghiên cứu động lực học Robot thường giải quyết hai nhiệm vụ sau : 1. Xác định momen và lực động trong quá trình chuyển động. Khi đó qui luật biến đổi của biến khớp qi(t) xem như đã biết. Việc tính toán lực cũng như momen trong cơ cấu tay máy là nhiệm vụ tất yếu trong việc lựa chọn công suất động cơ, tính toán kiểm tra độ bền, độ cứng vững, đảm bảo độ tin cậy cho Robot. 2. Xác định các sai số động, tức là sai số xuất hiện so với qui luật chuyển động trong chương trình. Có nhiều phương pháp nghiên cứu động lực học Robot, nhưng nhiều hơn cả là phương pháp cơ học Lagrange, cụ thể là phương trình Lagrange-Euler. Trong phạm vi nội dung của môn học này, chúng ta tìm hiểu nhiệm vụ thứ nhất, từ đó tạo cơ sở cho việc lập trình và điều khiển robot. 5.2. Động lực học robot với phương trình Euler-Lagrange. Hàm Lagrange của một hệ thống năng lượng được định nghĩa : L= K – P Trong đó : K là tổng động năng của cơ hệ L là tổng thế năng của cơ hệ K và P đều là những đại lượng vô hướng, nên có thể chọn bất kỳ hệ tọa độ nào để giả bài toán đơn giản. Xét một Robot có n khâu thì : 64
Chương 5: Động lực học robot và ứng dụng trong điều khiển n n K  Ki và P  Pi (2.1) i 1 i 1 Trong đó, Ki và Pi là động năng và thế năng của khâu thứ i xét trong hệ tọa độ đã chọn. Đó là các đại lượng phụ thuộc vào nhiều biến số : Ki K qi ,qi và Pi P qi ,qi (2.2) Với qi là tọa độ suy rộng của khớp thứ i. Định nghĩa : Lực (hay momen) tổng quát tác dụng lên khâu thứ i được xác định bởi phương trình Lagrange : d L L F dt q q 5.3. Khảo sát bài toán động lực học của tay máy nhiều bậc tự do Phương trình chuyển động Lagrange thiết lập cho một cơ hệ được cho bởi: d L L τ (2.3) dt q q Trong đó q là vectơ biểu diễn các toạ độ suy rộng của các khâu của Tay máy qi,  là vectơ biểu diễn các lực suy rộng của các khâu của tay máy và hàm Lagrange là sự chênh lệch giữa động năng và thế năng của cơ hệ : L K P (2.4) a. Ví dụ 1. Ta xét ví dụ xây dựng phương trình chuyển động của tay máy hai khâu phẳng liên kết bằng khớp bản lề. Trong ví dụ này, ta áp dụng các kết quả của bài toán động học đã được khảo sát ở phần trước. Để xây dựng bài toán động lực học, ta khảo sát cơ hệ với giả thiết rằng khối lượng của khâu được tập trung ở các khớp. Ma trận biến khớp là: T q 1 2  (2.5) và ma trận biểu diễn của lực suy rộng được thể hiện: T  1  2  (2.6) với 1, 2 là các mô men được cho bởi các cơ cấu tác động (chẳng hạn là mô men phát động của các động cơ điện). 65
Chương 5: Động lực học robot và ứng dụng trong điều khiển y (x2,y2) m2 a2 2 g a1 1 m1 x 0 Hình 5.1: Tay máy hai khâu bản lề Biểu thức động năng và thế năng Với khâu 1, ta có biểu thức của động năng và thế năng tương ứng là: 2 2 1  K1 2 m1a1 1 (2.7) P1 m1ga1 sin1 (2.8) Với khâu 2 ta có: x2 a1 cos1 a2 cos(1 2 ) (2.9) y2 a1 sin1 a2 sin(1 2 ) (2.10)    x2 a11 sin1 a2 (1 2 )sin(1 2 ) (2.11)    y2 a11 cos1 a2 (1 2 )cos(1 2 ) (2.12) Bình phương vận tốc là : 2 2 2 2 2 2   2 2   v2 x2 y2 a11 a2 (1 2 ) 2a1a2 (1 12 )cos2 (2.13) Do vậy động năng của khâu 2 là: 1 2 1 2 2 1 2   2 2   K2 2 m2v2 2 m2a11 2 m2a2(1 2 ) m2a1a2 (1 12 )cos2 (2.14) Thế năng cho khâu 2 là: P2 m2 gy2 m2 g[a1 sin1 a2 sin(1 2 )] (2.15) 66
Chương 5: Động lực học robot và ứng dụng trong điều khiển Phương trình Lagrange Hàm Lagrange cho Tay máy này là: 1 2  2 1 2   2 L K P K1 K2 P1 P2 2 (m1 m2 )a1 1 2 m2a2 (1 2 ) 2   (2.16 m2a1a2 (1 12 )cos2 (m1 m2 )ga1 sin1 m2 ga2 sin(1 2 ) ) Ta cần xác định các biểu thức : L (m m )a 2 m a 2 (  ) m a a (2  ) cos  1 2 1 1 2 2 1 2 2 1 2 1 2 2 1 d L (m m )a 2 m a 2 (  ) m a a (2  ) cos m a a (2   2 ) cos  1 2 1 1 2 2 1 2 2 1 2 1 2 2 2 1 2 1 2 2 2 dt 1 L (m1 m2 )ga1 cos1 m2 ga2 cos(1  2 ) 1 L m a 2 (  ) m a a  cos  2 2 1 2 2 1 2 1 2  2 d L m a 2 (  ) m a a  cos m a a   sin  2 2 1 2 2 1 2 1 2 2 1 2 1 2 2 dt  2 L  2   m2 a1a2 (1 1 2 )sin 2 m2 ga2 cos(1  2 )  2 Cuối cùng, phương trình chuyển động của cơ hệ tay máy được cho bởi hệ hai phương trình vi phân: 2 2  τ1 [(m1 m2 )a1 m2a2 2m1m2cos θ2 ]θ1 2     2 [m2a2 m2a1a2cos θ2 ]θ2 m2a1a2 (2θ1θ2 θ2 )sinθ2 (2.18) (m1 m2 )ga1cos θ1 m2 ga2cos (θ1 θ2 ) 2  2   2 τ2 [m2a2 m2a1a2cosθ2 ]θ1 m2a2 θ2 m2a1a2θ1 sinθ2 m2 ga2cos(θ1 θ2 ) Biểu diễn phương trình chuyển động của cơ hệ Tay máy dưới dạng ma trận Dưới dạng ma trận, phương trình chuyển động hay phương trình động lực học Tay máy dưới dạng ma trận có thể viết như sau: 67
Chương 5: Động lực học robot và ứng dụng trong điều khiển (m m )a2 m a2 2m m cosθ m a2 m a a cosθ θ 1 2 1 2 2 1 2 2 2 2 2 1 2 2 1 (2.19) 2 2  m2a2 m2a1a2cosθ2 m2a2 θ2 m a a (2θ θ θ 2 )sinθ (m m )ga cosθ m ga cos(θ θ ) τ 2 1 2 1 2 2 2 1 2 1 1 2 2 1 2 1  2 m2a1a2θ1 sinθ2 m2 ga2cos(θ1 θ2 ) τ2 Ta tìm được biểu thức động lực học tay máy dưới dạng chuẩn, được biểu diễn chung dưới dạng sau : M(q)q V(q,q) G(q) τ (2.20) M(q) là ma trận quán tính, V(q,q) là vectơ lực Coriolis hoặc/và lực hướng tâm và G(q) là vectơ trọng lực. Với biểu thức trên M(q) là ma trận đối xứng. b. Ví dụ 2. Xây dựng Phương trình động lực học của robot hai bậc tự do cấu hình RT. d2 O0  2 Hình 5.3. Cấu hình của Robot 2 bậc tự do RP Xuất phát từ phương pháp động lực học cho hệ cơ học tổng quát Phương trình chuyển động Lagrange thiết lập cho một cơ hệ được cho bởi: 68
Chương 5: Động lực học robot và ứng dụng trong điều khiển d L L τ (2.1) dt q q Trong đó q là vectơ biểu diễn các toạ độ suy rộng của các khâu của Tay máy qi,  là vectơ biểu diễn các lực suy rộng của các khâu của tay máy và hàm Lagrange là sự chênh lệch giữa động năng và thế năng của cơ hệ, với: L K P (2.2) Tương tự ví dụ 1, ta khảo sát cơ hệ với giả thiết rằng khối lượng của khâu được tập trung ở các khớp. Ma trận biến khớp là: T q 1 d2  (2.3) và ma trận biểu diễn của lực suy rộng được thể hiện: T  1  2  (2.4) với 1, 2 là các mô men được cho bởi các cơ cấu tác động (chẳng hạn là mô men phát động của các động cơ điện). Biểu thức động năng và thế năng y m2(x2,y2) l1 m1(x1,y1) d2 1 x Hình 5.4. Toạ độ của các khâu trên Robot + Với khâu 1 chuyển động quay, ta có biểu thức của động năng và thế năng 1 2  2 tương ứng là: K1 2 m1l1 1 (2.5) P1 m1gl1 sin1 (2.6) + Với khâu 2 chuyển động tịnh tiến, ta có: x2 d2 cos1 (2.7) y2 d2 sin1 (2.8) 69
Chương 5: Động lực học robot và ứng dụng trong điều khiển   x2 d2 cos1 d21 sin1 (2.9)   y2 d2 sin1 d21 cos1 (2.10) Bình phương vận tốc là : 2 2 2 2  2  2 v2 x2 y2 d2 1 d2 (2.11) Do vậy động năng của khâu 2 là: 1 2 1 2  2 1  2 K2 m2v2 m2d2 1 m2d2 2 2 2 (2.12) Thế năng cho khâu 2 là: P2 m2gy2 m2gd2 sin1 (2.13) Phương trình Lagrange Hàm Lagrange cho Tay máy này là: 1 2  2 1 2  2 1  2 L K P K1 K2 P1 P2 m1l1 1 m2d2 1 m2d2 m1gl1 sin1 m2 gd2 sin1 2 2 2 1 2 2  2 1  2 Vậy : L (m1l1 m2d2 )1 m2d2 (m1l1 m2d2 )g sin1 2 2 (2.14) Những hạng thức cần tính được thể hiện như dưới đây: 70
Chương 5: Động lực học robot và ứng dụng trong điều khiển L (m l 2 m d 2 )  1 1 2 2 1 1 d L m l 2 m (2d d  d 2)  1 1 1 2 2 2 1 2 1 dt 1 L (m1l1 m2d2 )g cos1 1 L m d (2.15)  2 2 d2 ) d L m d  2 2 dt d2 L  2 m2d21 m2 g sin1 d2 Cuối cùng, phương trình chuyển động của cơ hệ Tay máy được cho bởi hệ d L L τ hai phương trình vi phân: dt q q d L L  m l 2 m (2d d  d 2) (m l m d )g cos 1  1 1 1 2 2 2 1 2 1 1 1 2 2 1 dt 1 1 2 2    Vậy : 1 (m1l1 m2d2 )1 2m21d2d2 (m1l1 m2d2 )g cos1 d L L  m d m d  2 m g sin 2  2 2 2 2 1 2 1 dt d2 d2   2 Vậy :  2 m2d2 m2d21 m2 g sin1 Biểu diễn phương trình chuyển động của cơ hệ Tay máy dưới dạng ma trận Dưới dạng ma trận, phương trình chuyển động hay phương trình động lực học tay máy có thể viết như sau: 2 2    1 (m1l1 m2d2 )1 2m21d2d2 (m1l1 m2d2 )g cos1 2 2    m l m d 0 1 2m2d21d2 (m1l1 m2d2 )g cos1 τ1 1 1 2 2   2 m g sin θ τ 0 m2 d2 m2d2θ1 2 1 2 71
Chương 5: Động lực học robot và ứng dụng trong điều khiển 5.4. Phương trình động lực học tay máy. 5.4.1. Tổng quát. Chúng ta đã chỉ ra các ví dụ ứng dụng phương trình Lagrange để tính toán những phương trình động lực học của các Tay máy. Trong các ví dụ trên về động lực học ta nhận thấy biểu thức kết quả có dạng: M(q)q V(q,q) G(q) τ với q là biến khớp, ơ là vectơ lực hoặc mô men suy rộng. Để nhận được phương trình động lực học của tay máy ta bắt đầu từ việc xác định động năng và thế năng của cơ hệ, xây dựng hàm Lagrange, sau đó đưa các hạng thức vào phương trình Lagrange, thu gọn ta sẽ nhận được phương trình chuyển động của cơ hệ Tay máy. Để xây dựng mô hình động lực học tay máy bằng cách sử dụng phương trình Lagrange loại II, ta cần phải biết các thông số sau đây: Khối lượng cũng như tọa độ của khối tâm của các khâu, Vận tốc của điểm bất kỳ trên Tay máy thiết kế, Các thông số về ma sát động, ma sát tĩnh giữa các khâu, khớp và tác động nhiễu nếu có. Do trong thực tế, hoạt động của Tay máy luôn bị ảnh hưởng bởi các lực ma sát và nhiễu, nên ta sẽ khái quát mô hình động lực học Tay máy vừa nhận được như sau: M(q)q V(q,q) F(q) G(q) τd τ với q và  đã được định nghĩa ở trên. M(q) là ma trận quán tính, V(q,q) là vectơ lực Coriolis/hướng tâm và G(q) là vectơ trọng lực như đã phân tích ở trên. Ở phương trình khái quát trên, ta cộng thêm lực ma sát vào đó, với: F(q) Fv q Fd trong đó Fv là ma trận hệ số của ma sát tĩnh và Fd là ma sát động. Ta sẽ đưa thêm lượng nhiễu d vào phương trình, đại lượng này giúp mô tả phần bù cho trường hợp mô hình động lực học có sai sót mà ta chưa lường hết trong quá trình xây dựng mô hình toán. Việc xác định lực ma sát rất khó khăn, cách mô tả như vậy được chấp nhận. Hầu hết những trở lực nào chống lại chuyển động đều được các nhà nghiên cứu mô tả trong mô hình động lực học Tay máy theo cách như trên. Phương trình động lực học Tay máy cũng được biểu diễn dưới dạng: M(q)q N(q, q)  d  72
Chương 5: Động lực học robot và ứng dụng trong điều khiển Ở đó: N(q, q) V(q, q) F(q) G(q) biểu diễn cho cả các đại lượng phi tuyến. 5.4.2. Ma trận quán tính Ma trận quán tính M(q) n x n có các thành phần được định nghĩa bởi biểu thức: n T Ti Ti m jk (q) trace I i i 1 q j qk - Ti / q j mô tả sự thay đổi vị trí của điểm thuộc khâu thứ i gây nên bởi sự chuyển dịch của khâu thứ j. - Ii là ma trận quán tính giả của khâu i và được xác định dưới dạng khai triển như sau: x2dm yx dm zxdm x dm xy dm y 2dm zy dm y dm I i r i rT dm i i i 2 xz dm yz dm z dm z dm x dm y dm z dm dm Ở đây các giá trị được tính trên khâu thứ i. Đây là ma trận hằng số và xác định giá trị một lần cho mỗi khâu. Ma trận này phụ thuộc vào dạng hình học và sự phân bố khối lượng của khâu i. Trong đó các thành phần quán tính được phân biệt như sau: Mô men quán tính: 2 2 I xx ( y z )dm I (x2 z 2 )dm yy 2 2 I zz (x y )dm Mô men quán tính ly tâm: I xy xy dm I xz dm xz I yz yz dm 73
Chương 5: Động lực học robot và ứng dụng trong điều khiển mx x dm my y dm Mô men quán tính bậc nhất: mz z dm với m là tổng khối lượng khâu i, và: i T ri x y z 1 là bán kính vectơ biểu diễn trọng tâm khâu thứ i trong hệ tọa độ i. Ta có thể viết : I xx I yy I zz I xy I xz mx 2 I xx I yy I zz I xy I yz my Ii 2 I I I I I xx yy zz mz xz yz 2 mx my mz m Với Ti / q j = 0, j>i ta có thể viết ngắn gọn hơn : n T T Ti mjk (q)  trace Ii i max( j,k) q j qk Đây là một ma trận đối xứng dương 5.4.3. Vectơ coriolis/hướng tâm  1  T  K V(q,q) M(q)q (q M(q)q) Mq 2 q q Các thành phần của vectơ Coriolis/hướng tâm được xác định như sau:    V (q,q) vijkqiq j i, j 1 mkj mki mij vijk 2 qi q j qk 5.4.4.Vectơ trọng lực: Ta có 74
Chương 5: Động lực học robot và ứng dụng trong điều khiển P(q) G(q) q n  T  (g Ti (q))I i e4 i 1 q e4 = (0, 0, 0, 1) Từ đó , ta suy ra được: n  T G(q)  (g Ti (q))I i e4 i 1 q n T Ti G(q)  (I n g ) I i e 4 , j 1,2 , n i 1 q Ở đây thật sự ta có vectơ G(q) là: n T Ti  g I i e4 q i 1 1 n T g T i I e G(q)  i 4 i 2 q2  n T T i  g I i e4 i n qn Đến đây ta đã khảo sát bài toán động lực học Tay máy để từ đó thu được các giá trị lực hay mô men suy rộng trên mỗi khớp trong quá trình hoạt động của robot. Dựa trên những thông số này ta sẽ đưa ra những giải pháp thiết kế kết cấu cũng như điều khiển robot tốt hơn. Bởi bộ điều khiển sẽ đơn giản và có hiệu quả hơn nếu những đặc tính động lực học đã biết của Tay máy được kết hợp chặt chẽ ngay từ trong giai đoạn thiết kế. 5.5. Ứng dụng bài toán động lực học để mô tả đối tượng robot trong điều khiển. Sau khi thực hiện tính toán bài toán động lực học robot, chúng ta có thể sử dụng trực tiếp các mô hình toán thu được để xây dựng đối tượng trong việc mô phỏng và đưa ra các ý tưởng trong vấn đề điều khiển. Tất nhiên, việc xác định các thông số của robot là rất khó khăn, vì vậy chúng ta chỉ xây dựng đối tượng robot có tính chất mô phỏng để thực hiện các giải thuật điều khiển. Vì trong thực tế, các thông số của mô hình động lực học tay máy chịu ảnh hưởng của rất nhiều các yếu tố như : độ chính xác trong gia công cơ khí, ảnh hưởng của các tác nhân có tính chất như nhiễu, các sai số mô hình khi thực hiện tính toán Trong mục này, bằng các phần mềm hỗ trợ mô phỏng (Visual C, Visual Basic, Matlab, ) chúng ta thực hiện mô hình hóa các robot từ các phương trình 75
Chương 5: Động lực học robot và ứng dụng trong điều khiển động học và động lực học. Từ cơ sở này có thể thực hiện thiết kế và chế tạo các robot thực thi các mục tiêu đề ra. Chúng ta sẽ thực hiện việc mô hình hóa các đối tượng robot đã tìm hiểu ở các chương trước : a. Xây dựng mô hình mô phỏng điều khiển vị trí của robot Puma, dựa vào các phương trình động học đã tìm được ở chương 4. Hình 5.6. Mô phỏng robot Puma theo vị trí Hình 5.7. Mô phỏng quĩ đạo của robot Puma. b. Xây dựng mô hình toán cho robot hai bậc tự do cấu hình RT. Do tính chất phức tạp trong điều khiển, vấn đề của những nhà nghiên cứu là làm sao có thể tìm giải thuật điều khiển cho robot khi mà tất cả các khâu từ thiết 76
Chương 5: Động lực học robot và ứng dụng trong điều khiển kế đến thi công đều gặp nhiều khó khăn. Một công cụ rất hữu hiệu được đưa ra là mô hình toán của robot, nền tảng của mô hình toán là bài toán động lực học được xét đến. Mức độ chính xác , độ chênh lệch sai số mô hình phụ thuộc nhiều vào quá trình tính toán động lực học, trong đó không loại trừ các khả năng ảnh hưởng của nhiễu và các vấn đề khác liên quan đến động lực học cơ hệ. Chúng ta quay lại ví dụ 5.2, từ bài toán động lực học xây dựng cho robot hai bậc tự do, cấu hình RT thu được mô hình toán của đối tượng robot. Xét trên lĩnh vực điều khiển, hệ robot là các hệ phi tuyến, chính vì vậy việc điều khiển và sử dụng các giải thuật phải tuân theo các nguyên tắc điều khiển hệ phi tuyến. Xây dựng mô hình robot RT trong matlab : U1 Theta (Dien ap dieu khien motor khop 1) (Goc quay khop 1) U2 d (Do dai tinh tien d khop 2) (Dien ap dieu khien motor khop 2) ROBOT_2DOF Hình 5.8. Mô hình toán robot 2 bậc tự do RT Để mô phỏng thành công, chúng ta cần chọn các thông số của robot thích hợp. Các thông số này có thể thu thập số liệu hay lựa chọn theo các tài liệu đã được nghiên cứu. 1 u1 1 1 f(u) 1 s s theta theta_2dot theta_dot theta_ 2 u2 1 1 f(u) 2 s s d2 d_2dot d_dot d Hình 5.9. Mô hình toán từ phương trình động lực học robot. 77
Chương 6: Điều khiển Robot Chương 6 ĐIỀU KHIỂN ROBOT Vấn đề trọng tâm của chúng ta trong lĩnh vực nghiên cứu robot là điều khiển chúng theo các mục tiêu cụ thể. Trong chương này ta cần đưa ra các phương thức điều khiển làm cho tay máy đi theo quỹ đạo yêu cầu được cho trước. Quỹ đạo dự kiến đòi hỏi người lập trình điều khiển phải tìm kiếm đường đi có tính đến những vấn đề liên quan đến môi trường ứng dụng như tránh sự va chạm, các yêu cầu về tốc độ đáp ứng Trong các trường hợp ứng dụng, ta không thể điều khiển để tay máy di chuyển được chính xác tuyệt đối theo quỹ đạo dự kiến.Vì vậy cần thực hiện các thao tác sau để tìm quĩ đạo mong muốn trong quá trình điều khiển. Thứ nhất, ta sẽ chỉ ra cách thức biến đổi một quỹ đạo theo mong muốn từ hệ tọa độ Descartes (Cartesian coordinates) qua hệ tọa độ suy rộng (Joint coordinates - hay không gian khớp). Sau đó, đưa ra một bảng những điểm tựa, là những điểm thuộc quỹ đạo dự kiến đã được rời rạc hóa mà ta mong muốn điểm trên khâu tác động cuối sẽ đi qua và từ đó ta chỉ ra cách để xây dựng lại một quỹ đạo liên tục theo yêu cầu. 6.1. Biến đổi quĩ đạo từ hệ toạ độ Descartes sang không gian khớp Trong các ứng dụng của robot, một công việc cụ thể, về mặt lý thuyết ta có thể biểu diễn trong không gian Descartes; và ở đó, dịch chuyển của tay máy được mô tả dễ dàng trong mối quan hệ về vị trí của nó với các phần tử khác trong môi trường hoạt động bên ngoài. Tuy nhiên, việc điều khiển chuyển động của các khâu trên tay máy sao cho điểm làm việc trên khâu tác động cuối di chuyển đúng theo quỹ đạo cho trước lại yêu cầu phải sử dụng không gian khớp vì vậy ta cần sử dụng để giải quyết cả bài toán động lực học. Ở đây ta cũng chú ý một kết quả ở bài toán động học ngược mà ta đã biết ở phần trước, đó là có nhiều lời giải về chuyển động của các khâu thành viên trong không gian khớp qd(t) để cho điểm trên khâu tác động cuối di chuyển theo quỹ đạo đã cho (bài toán vô định). Vì vậy việc chọn lời giải duy nhất trong số những lời giải có thể có là một vấn đề cần quan tâm. Ngoài ra cách thực hiện dịch chuyển của điểm trên khâu tác động cuối giữa các điểm tựa (nội suy) ảnh hưởng đến khả năng và phương pháp điều khiển. Ở đây, chúng ta có thể thực hiện giải bài toán động học ngược trực tiếp hay theo phương pháp tách nhóm ba khâu. 6.1.1. Nội suy đường đa thức Giả định rằng một quỹ đạo yêu cầu đã được xác định và được thể hiện hoặc 78
Chương 6: Điều khiển Robot trong không gian Descartes hoặc dùng động học ngược, trong không gian khớp. Để thuận tiện, ta dùng biến không gian khớp q(t) cho ký hiệu. Sẽ không thuận tiện cho việc điều khiển khi dữ liệu về quỹ đạo với số lượng vô hạn các điểm được lưu trong bộ nhớ máy tính, cho nên ta thường lưu dưới dạng một số N hữu hạn các điểm tựa và hệ quả là sẽ có những giá trị qi(tk) tương ứng cho mỗi biến khớp i để mô tả những giá trị yêu cầu về vị trí của các khâu tại những điểm thời n gian rời rạc tk. Theo cách đó q(tk) là một điểm trong không gian R mà biến khớp sẽ đi qua tại thời điểm tk. Ta đã gọi chúng là những điểm tựa. Hầu hết các kế hoạch điều khiển robot yêu cầu một quỹ đạo liên tục. Để chuyển thành một bảng các điểm tựa qi(tk) cho quỹ đạo mong muốn qd(t), ta có thể sử dụng các cách thức nội suy tuỳ chọn. Dưới đây trình bày sơ lược về nội suy đa thức. Giả định rằng các điểm tựa là không gian đồng dạng trong thời gian và được xác định trên cơ sở lấy mẫu thời gian như sau: T tk 1 tk (6.1) Để di chuyển được trơn, trong mỗi khoảng thời gian [tk+1,tk] ta cần đến vị trí mong muốn qd(t) và vận tốc mong muốn qd (t) hợp với bảng điểm tựa. Ta có: q ( t ) q ( t ) di k i k q ( t ) q ( t ) di k i k q ( t ) q ( t ) di k 1 i k 1 q ( t ) q ( t ) di k 1 i k 1 (6.2) Để phù hợp với những điều kiện giới hạn, rất cần thiết dùng khoảng [tk,tk+1] để nội suy đa thức bậc 3: q (t) a (t t )b (t t )2 c (t t )3 d di i k i k i k i (6.3) trong đó có 4 biến tự do. Ơ đó: q (t) b 2(t t )c 3(t t )2 d di i k i k i (6.4) q (t) 2c 6(t t )d di i k i (6.5) cho nên gia tốc là tuyến tính trong mỗi mẫu thời gian. Ta dễ dàng giải ra được các hệ số và bảo đảm hợp với điều kiện giới hạn. Thực tế ta nhận thấy: 79
Chương 6: Điều khiển Robot 1 0 0 0 ai qi (tk ) 0 1 0 0 b q (t ) i i k 2 3 (6.6) 1 T T T ci qi (tk 1 ) 2 0 1 2T 3T di qi (tk 1 ) Ở đây, khi giải ra, ta nhận được các hệ số nội suy cần tính trong mỗi khoảng [tk,tk+1] ai qi (tk ) bi qi (tk ) 3[q (t ) q (t )] T[2q (t ) q (t )] c i k 1 i k i k i k 1 i T 2 (6.7) 2[q (t ) q (t )] T[q (t ) q (t )] d i k i k 1 i k i k 1 i T 3 Chú ý rằng với kỹ thuật này những vị trí và vận tốc mong muốn tại mỗi điểm lấy mẫu được yêu cầu lưu trữ dưới dạng bảng. Việc sử dụng nội suy bậc cao nhằm bảo đảm sự liên tục về vị trí, vận tốc và gia tốc tại mỗi thời gian tk . Mặc dù ta dùng ký hiệu biến khớp q(t), điều này vẫn làm nổi bật sự nội suy quỹ đạo có thể thực hiện được trong không gian Descartes. 6.1.2. Nội suy quỹ đạo theo thời gian nhỏ nhất Đây là phần quan trọng đặc biệt trong quỹ đạo LFPD. Giả định rằng gia tốc bị giới hạn bởi giá trị lớn nhất aM và mong muốn Tay máy đi từ điểm này đến điểm khác trong khoảng thời gian ngắn nhất. Để đơn giản, ta thừa nhận rằng vận tốc đầu và vận tốc cuối có giá trị về 0. Quỹ đạo thời gian nhỏ nhất được chỉ ra trong hình 6.16. Để cho biến khớp thứ i chạy từ vị trí q0 = qi(t0) tới vị trí mong muốn qf = qi(ti) trong khoảng thời gian nhỏ nhất tf , gia tốc lớn nhất aM, sẽ được áp dụng cho đến trước thời gian ngắt ts, là thời gian bắt đầu giảm tốc – aM lớn nhất sẽ được áp dụng trong khoảng thời gian tf. Chú ý rằng cả ts và tf đều phụ thuộc vào qo và qf. Ta có thể viết: 1 2 qi (ts ) q0 2 aM (ts t0 ) qi (ts ) aM (ts t0 ) 1 2 qi (t f ) qi (ts ) qi (ts )(t f ts ) 2 aM (t f ts ) qi (t f ) qi (ts ) aM (t f ts ) Ơ đó ta có phương trình vận tốc: qi (t f ) aM (ts t0 ) aM (t f ts ) 0 hoặc 80
Chương 6: Điều khiển Robot ts (t f t0 ) / 2 (6.8) Điều này có nghĩa là sự chuyển từ gia tốc lớn nhất đến giảm tốc lớn nhất xảy ra ở điểm giữa chu kỳ. Bây giờ ta có thể thực hiện những thao tác đơn giản trong phương trình vị trí: 1 2 1 2 qi (t f ) q0 2 aM (ts t0 ) aM (ts t0 )(t f ts ) 2 aM (t f ts ) q f q q f 0 1 2 1 2 2 (ts t0 ) (ts t0 )(t f ts ) 2 (t f ts ) aM Ở biểu thức trên : t f t0 (q f q0 ) / aM (6.9) Hình 6.1: Quỹ đạo thời gian ngắn nhất: (a) gia tốc; (b) vận tốc Tuy vậy, quỹ đạo dịch chuyển với thời gian nhỏ nhất trên cơ sở sử dụng gia tốc lớn nhất không liên quan trực tiếp trong robotics là vì trong thực tế là những tay máy luôn bị giới hạn mô men bảo hòa, M. Từ đặc điểm của phương trình chuyển động của Tay máy đã xây dựng trong phần trước là phi tuyến, do đó mà mô men bảo hòa thường sẽ không tương ứng với giới hạn hằng số trong gia tốc. 81
Chương 6: Điều khiển Robot Hình 6.2 (c): Quỹ đạo LFPB vị trí 6.2. Điều khiển hệ robot phi tuyến . Như đã đề cập ở chương trước, hệ robot là hệ phi tuyến, vì vậy trong điều khiển chúng ta phải xét đến các phương pháp điều khiển hệ phi tuyến. Một số phương pháp điều khiển phi tuyến có thể áp dụng cho hệ robot như : điều khiển tuyến tính hoá vào ra, phương pháp điều khiển trượt, phương pháp điều khiển ổn định hoá Trong giới hạn của môn học, chúng ta tìm hiểu hai phương thức cơ bản điều khiển một robot, sau khi đã giải quyết các bài toán động học và động lực học robot: i. Điều khiển trực tiếp robot bằng các giải thuật điều khiển phi tuyến. Các phương pháp điều khiển hiện đại, điều khiển thông minh dùng các công cụ như : tuyến tính hoá, logic mờ , mạng neural Tuy nhiên, một đặc thù rất riêng của robot là hệ phi tuyến nhiều đầu vào và nhiều đầu ra. Ở đây, để đơn giản chúng ta xét điều khiển một motor cho một khớp nối. Với hệ MIMO (Multi Input Multi Output) như robot, một phương thức thường được sử dụng để điều khiển trực tiếp hệ robot (có cấu hình không quá phức tạp) là điều khiển phân ly. Mỗi khớp nối sẽ được điều khiển bởi một nhánh của bộ điều khiển độc lập nhau. Lưu ý, phương pháp này chỉ thật sự hiệu quả khi cấu hình robot không quá phức tạp bởi tính chất phi tuyến của nó. ii. Điều khiển theo momen, dùng phương pháp hồi tiếp tuyến tính hệ phi tuyến robot. Phương pháp này thường xuất hiện trong điều khiển thô, điều khiển thích nghi, điều khiển theo hệ tự học 6.3. Điều khiển trực tiếp hệ robot. Để xây dựng giải thuật điều khiển phù hợp với robot trong các trường hợp ứng dụng khác nhau, trước tiên chúng ta cần xây dựng mô hình toán của đối tượng cần điều khiển. Tuỳ thuộc vào mục đích điều khiển, yêu cầu về chất lượng khác nhau, chúng ta cần lựa chọn các phương pháp thiết kế bộ điều khiển phù hợp. Đôi khi, quá trình lựa chọn này là quá trình thử sai để tìm phương pháp điều khiển tối ưu. Trong chương trước, chúng ta đã tìm được mô hình toán của các đối tượng robot từ phương trình động lực học của chúng. Để thuận tiện cho việc theo dõi, ở đây chúng ta khảo sát các bước viết giải thuật điều khiển cho một loại robot đã tìm hiểu trước đó. Phần mềm mô phỏng được sử dụng ở đây là phần mềm Matlab. Ví dụ : Xây dựng bộ điều khiển cho robot 2 bậc tự do RT bám theo quĩ đạo mong muốn. 82
Chương 6: Điều khiển Robot 1. Xây dựng đối tượng Robot 2 bậc tự do đã thiết lập phương trình động lực học ở trên. 1 u1 1 1 f(u) 1 s s theta theta_2dot theta_dot theta_ 2 u2 1 1 f(u) 2 s s d2 d_2dot d_dot d Hình 6.3. Đối tượng Robot 2 bậc tự do xây dựng trên sơ đồ Simulink Chọn các điều kiện đầu theo đúng sơ đồ phần cứng của Robot : + Điều kiện đầu của biến khớp bằng 0. + Điều kiện đầu của biến khớp d2 bằng l1 (Chọn =1m) Chọn các thông số cho Robot 2 bậc tự do : + Khối luợng khâu 1 : m1 = 0.5 kg. + Khối luợng khâu 2 : m2 = 0.3 kg. + Chiều dài khâu 1 là : l1 = 0.6 m. + Độ dài tịnh tiến tối đa của khâu 2 so với gốc toạ độ là : d2max = 1m. + Đặt trọng lượng các khâu tại các đầu mút của các khâu hay có thể chọn 2 2 Tensor quán tính : Izz1=0.015 kgm ; Izz2 = 0.008 kgm . 2. Thiết kế bộ điều khiển cho hệ Robot phi tuyến bám theo quĩ đạo mong muốn. Nhận xét : + Hệ tay máy hai bậc tự do là hệ phi tuyến MIMO (dựa vào phương trình động lực học) , có hai tín hiệu vào là điện áp (hay momen) đặt trên mỗi động cơ điều khiển lần lươt hai khớp quay và tịnh tiến, hai tín hiệu ra là góc quay θ1 và độ dài tịnh tiến d2. + Chuyển động tịnh tiến của khâu 2 có thể thực hiện được nhờ các bộ truyền cơ khí biến đổi chuyển động quay của trục động cơ thành chuyển động tịnh tiến của cơ cấu : bộ truyền bánh răng-thanh răng, bộ truyền vítme- đai ốc bi 83
Chương 6: Điều khiển Robot Hình 6.5. Kết cấu bộ Hình6.4. Bộ truyền bánh răng-thanh răng truyền vitme-đai ốc bi + Có thể thiết kế các bộ điều khiển SISO điều khiển cánh tay máy theo nguyên lý tách rời, mỗi bộ điều khiển sẽ kiểm soát hoạt động của một khớp liên kết của tay máy. + Vì đây là hệ có tính phi tuyến cao nên các bộ điều khiển thông thường không đảm bảo tốt khả năng điều khiển cơ hệ. Ta lựa chọn các bộ điều khiển thông minh để thực thi khả năng điều khiển cho hệ Robot này. Một phương án lựa chọn ở đây là sử dụng các bộ điều khiển mờ điều khiển hệ bám theo quĩ đạo mong muốn. + Qua quá trình lựa chọn và thử sai cho các bộ điều khiển ta nhận thấy các bộ điều khiển mờ trực tiếp, hay PI mờ, PD mờ chưa cho đáp ứng mong muốn. Chọn hai bộ điều khiển mờ PID để điều khiển mỗi khớp động của Robot. Trình tự thiết kế bộ điều khiển như sau : Mỗi bộ điều khiển PID mờ thiết kế cho từng khớp của Robot được chọn theo giải pháp bộ điều khiển PI mờ ghép song song với bộ điều khiển PD mờ. Sơ đồ mô phỏng thực thi các bộ điều khiển này : 84
Chương 6: Điều khiển Robot f(u) f1 Fcn2 theta_random.mat Random_theta theta0_theta theta_elip.mat PD_FUZZY_THETA Elip_theta U1 f3 PI_FUZZY_THETA Out1 f(u) ref1 End_Effector Fcn Trajectory Out1 PI_FUZZY_D ref2 ROBOT_2DOF PD_FUZZY_D d0_d d_elip.mat Elip_d U2 f(u) d_random.mat f4 Fcn1 Random_d f(u) f2 Fcn3 Hình 6.6. Xây dựng bộ điều khiển cho robot 2 bậc tự do RT a. Thiết kế bộ điều khiển mờ PI điều khiển góc quay khớp thứ nhất : Khối PI_FUZZY_THETA Bộ điều khiển mờ có tín hiệu vào là sai số (E) và vi phân sai số (DE), tín hiệu ra là vi phân điện áp điều khiển (DU). 1 k1 1 r1 k3 1 Gain s u1 Saturation Integrator Gain2 Fuzzy_PI du/dt k2 Derivative Gain1 Hình 6.7. Sơ đồ mô phỏng bộ điều khiển PI cho góc quay θ1. Do tay máy hoạt động trong tầm [0 pi] nên E . Vì vậy các hệ số chuẩn hoá chọn K1=2/pi ; K2=11/pi (K2 chọn phù hợp với đặc tính của Robot). Hệ số K3 được lựa chọn trong quá trình thử sai và tinh chỉnh cho bộ điều khiển. Các tập mờ biểu diễn cho các giá trị ngôn ngữ của biến vào và biến ra được chọn như sau ( lưu với tên file m1.fis ): 85
Chương 6: Điều khiển Robot Hình 6.8. Các tập mờ chọn cho bộ điều khiển PI mờ điều khiển góc quay θ1. Bằng kinh nghiệm và phương pháp thử sai, chúng ta có thể chọn hàm liên thuộc của E, hàm liên thuộc của DE, hàm liên thuộc của biến ra output_PI. Các luật mờ (hệ qui tắc mờ) được chọn : Vì chọn 5 biến ngôn ngữ cho mỗi đầu vào nên có 52 = 25 luật mờ được đưa ra. b. Thiết kế bộ điều khiển mờ PD điều khiển góc : Khối PD_FUZZY_THETA Bộ điều khiển mờ có tín hiệu vào là sai số (E) và vi phân sai số (DE), tín hiệu ra là vi phân điện áp điều khiển (DU). 1 k1 r1 Gain k3 1 u1 Saturation Gain2 Fuzzy_PD du/dt k2 Derivative Gain1 Hình 6.9. Sơ đồ mô phỏng bộ điều khiển PD mờ cho góc quay θ1. Vì các qui tắc phát biểu dạng ngôn ngữ của các tập mờ qui định cho các biến vào ra là E, DE ở trường hợp này hoàn toàn giống với truờng hợp thiết kế cho bộ PI mờ nên ta có thể sử dụng bộ mờ đã thiết kế cho sơ đồ điều khiển PD này . Các hệ số K được chọn như sau : K1 =0.2/pi; K2=2/pi, K3 =20. Các hệ số này được chọn thử sai trong quá trình thiết kế và tinh chỉnh bộ điều khiển. c. Thiết kế bộ điều khiển mờ PI điều khiển độ dài tịnh tiến d2 : Khối PI_FUZZY_D Bộ điều khiển mờ có tín hiệu vào là sai số (E) giữa tín hiệu đặt d2m với tín hiệu ra thực d2 và vi phân sai số (DE), tín hiệu ra là vi phân điện áp điều khiển (DU). 86
Chương 6: Điều khiển Robot 1 k4 1 r2 k6 1 Gain s u2 Saturation Integrator Gain2 Fuzzy_PI_d du/dt k5 Derivative Gain1 Hình 6.10. Sơ đồ mô phỏng bộ điều khiển PI mờ cho khoảng tịnh tiến d2 Do tay máy hoạt động trong tầm [l1 d2max] nên (d2max l1) E (d2max l1) . Vì vậy các hệ số chuẩn hoá chọn K4=6/0.4 ; K5=50/0.4 (K2 chọn phù hợp với đặc tính của Robot). Hệ số K6 được lựa chọn trong quá trình thử sai và tinh chỉnh cho bộ điều khiển. Các tập mờ biểu diễn cho các giá trị ngôn ngữ của biến vào và biến ra được chọn như sau ( lưu với tên file m4.fis ): Hình 6.11. Các tập mờ cho bộ điều khiển PI mờ điều khiển độ dịch chuyển d2 Hàm liên thuộc của E, hàm liên thuộc của DE, hàm liên thuộc của biến ra output_PI, các luật mờ chọn như trường hợp a. d. Thiết kế bộ điều khiển mờ PD điều khiển góc : Khối PD_FUZZY_THETA Bộ điều khiển mờ có tín hiệu vào là sai số (E) giữa tín hiệu đặt d2m với tín hiệu ra thực d2 và vi phân sai số (DE), tín hiệu ra là vi phân điện áp điều khiển (DU). 1 k4 r2 Gain k6 1 u1 Saturation Gain2 Fuzzy_PD_d du/dt k5 Derivative Gain1 Hình 6.12. Sơ đồ mô phỏng bộ điều khiển PD mờ cho khoảng tịnh tiến d2. 87
Chương 6: Điều khiển Robot Vì các qui tắc phát biểu dạng ngôn ngữ của các tập mờ qui định cho các biến vào ra là E, DE ở trường hợp này hoàn toàn giống với truờng hợp thiết kế cho bộ PI mờ cho d2 nên ta có thể sử dụng bộ mờ đã thiết kế với sơ đồ điều khiển PD này ( file m4.fis ). Các hệ số K được chọn như sau : K1 =2/0.4; K2=1/0.4, K3 =8. Các hệ số này được chọn thử sai trong quá trình thiết kế và tinh chỉnh bộ điều khiển. 3. Quĩ đạo đặt cho Robot. Như đã trình bày ở mục nội suy quĩ đạo cho Robot, ở đây chúng ta có thể cho trước một số quĩ đạo đạt mong muốn sao cho quĩ đạo này nằm trong vùng làm việc của Robot (vùng với đến) : Có thể là ½ đường tròn, ½ đường elip, quĩ đạo theo một hàm bất kỳ nằm trong ½ hình vanh khăn đã xác định trước. Giả sử như ta chọn quĩ đạo là ½ hình elip như sau : y Không gian làm việc của Robot Quĩ đạo là elip mong muốn End Effecttor 0.7m l1 1 l + dmax x 2m Hình 6.13. Quĩ đạo là elip với các độ dài trục lớn là 2, độ dài trục be là 1.4 Elip có phương trình : x2 y 2 1 Chọn a=1, b=0.7 như hình vẽ a2 b2 2 2 y x 1 0.49 Để tạo tín hiệu đặt là các hàm theo thời gian cho các biến khớp từ không gian Đề cac, trước tiên xuất phát từ quĩ đạo mong muốn, chúng ta xác định lần lượt các điểm tựa, ứng với từng điểm tựa này chúng ta thu thập được số liệu dạng bảng các giá trị của các biến khớp. Ở đây, giả sử chúng ta chọn các điểm tựa lần lượt ứng với hai biến khớp Ai(,d) như sau : A1 (0, 1) ; A2(pi/6, 0.8908) ; A3(pi/3, 0.7494) ; A4(pi/2, 0.7); A5(2pi/3, 0.7494); A6(5pi/6, 0.8908); A7(pi, 1). 88
Chương 6: Điều khiển Robot Thời gian lấy mẫu giữa các điểm tựa chọn là [tk tk+1]=5s. Vậy ta cần đạt được quĩ đạo mong muốn là elip khi khâu tác động cuối di chuyển các góc 1 cách đều nhau một góc 30° , d2 thay đổi từ [0.6 1] trong khoảng thời gian như nhau là 5s. Chọn thời gian lấy mẫu cho cả hệ thống và dữ liệu nội suy là 0.01s. Dùng phương pháp nội suy đường đa thức, chúng ta xác định được lần lượt các đa thức nối giữa các điểm tựa, tạo quĩ đạo mong muốn theo các biến khớp. Muốn tăng độ chính xác của quá trình nội suy, chúng ta có thể tăng số lượng các điểm tựa. Có thể viết m file để thực hiện thao tác nội suy này, sau đó lưu dữ liệu và đưa vào sơ đồ Simulink. Viết chương trình giải trực tiếp hàm nội suy hay dùng các hàm nội suy đa thức có sẵn của Matlab để tạo dữ liệu đặt cho các biến khớp. Với cách thức này, chúng ta hoàn toàn có thể xác định được tín hiệu đặt cho các biến khớp khi xác định quĩ đạo của Robot theo một đường cong bất kỳ. Kết quả nội suy cho biến khớp 1 và d2 theo quĩ đạo là elip trên : + Nội suy góc θ1(t) : + Nội suy d2(t) : 4. Kết quả thiết kế bộ điều khiển bám theo quĩ đạo mong muốn. a. Khi cho tín hiệu đặt bất kỳ cho các biến khớp nằm trong vùng làm việc của Robot: 89