Tài liệu Phân tích và mô phỏng động học rôbốt 6 bậc tự do cấp phôi cho các máy phay cnc

TRƯỜNG ĐẠI HỌC HÀNG HẢI VIỆT NAM VIỆN CƠ KHÍ THUYẾT MINH ĐỀ TÀI NCKH CẤP TRƯỜNG ĐỀ TÀI PHÂN TÍCH VÀ MÔ PHỎNG ĐỘNG HỌC RÔBỐT 6 BẬC TỰ DO CẤP PHÔI CHO CÁC MÁY PHAY CNC Chủ nhiệm đề tài: TS. HOÀNG MẠNH CƯỜNG Thành viên tham gia: KS. NGUYỄN ĐỨC SANG Hải Phòng, tháng 05/2016 i MỤC LỤC MỞ ĐẦU ............................................................................................................... 1 CHƯƠNG 1. GIỚI THIỆU CHUNG VỀ ROBOT CÔNG NGHIỆP................... 3 1.1 Lịch sử hình thành và phát triển Robot công nghiệp ...................................... 3 1.2. Cấu trúc chung của robot công nghiệp........................................................... 5 1.3 Phân loại robot công nghiệp ............................................................................ 6 1.4 Các chỉ tiêu đánh giá và các thông số kỹ thuật ............................................... 7 1.5. Các bài toán thường gặp đối với robot công nghiệp ...................................... 9 CHƯƠNG 2. PHÂN TÍCH ĐỘNG HỌC ROBOT UR ...................................... 11 2.1. Phân tích động thuận rôbốt UR .................................................................... 11 2.2. Phân tích động học ngược robot UR ............................................................ 18 2.3. Một số kết quả mô phỏng ............................................................................. 32 CHƯƠNG 3. MÔ PHỎNG HOẠT ĐỘNG CỦA ROBOT UR .......................... 38 3.1. Giới thiệu chung về kỹ thuật mô phỏng ....................................................... 38 3.2. Giới thiệu thư viện Simmechanics ............................................................... 39 3.3 Mô phỏng hoạt động của Robot cấp phôi ..................................................... 42 KẾT LUẬN ......................................................................................................... 46 TÀI LIỆU THAM KHẢO ................................................................................... 47 ii MỞ ĐẦU 1. Tính cấp thiết của vấn đề nghiên cứu Với sự phát triển mạnh mẽ của khoa học công nghệ, việc tự động hóa quá trình sản xuất ngày càng nâng cao, làm cho công nghệ sản xuất chuyển sang thời kỳ mới: thời kỳ công nghệ sản xuất tiên tiến mà trong đó robot công nghiệp đóng vai trò hết sức quan trọng, các robot tham gia vào hầu hết các nguyên công và dần thay thế con người trong những công việc nhàm chán, nặng nhọc, nguy hiểm, …. Do vậy, việc nghiên cứu ứng dụng robot vào sản xuất tự động đang được nhiều người quan tâm nghiên cứu. 2. Tổng quan về tình hình nghiên cứu thuộc lĩnh vực đề tài Robot công nghiệp được sử dụng phổ biến trong các hệ thống sản xuất linh hoạt, đã có nhiều cấu hình robot được nghiên cứu ứng dụng trong sản xuất và được đề cập trong nhiều tài liệu [3, 4, 5, 6, 7]. Tuy nhiên, cấu hình robot UR, với đặc điểm nhỏ gọn và linh hoạt, nó vẫn đang được nhiều người quan tâm nghiên cứu ứng dụng trong nhiều các hệ thống sản xuất khác nhau. 3. Mục tiêu, đối tượng, phạm vi nghiên cứu - Mục tiêu của đề tài là nghiên cứu đưa ra được nghiệm giải tích của bài toán phân tích động học ngược đối với một cấu hình robot công nghiệp, dựa vào nghiệm này sẽ đi xây dưng chương trình mô phỏng chuyển động mô hình robot đó. - Đối tượng nghiên cứu của đề tài là robot UR, đây là một cấu hình robot đang được nhiều người quan tâm nghiên cứu trong nhiều ứng dụng. - Phạm vi nghiên cứu, với giới hạn của một đề tài nghiên cứu cấp trường công trình này cũng chỉ dừng lại ở bài toán phân tích động học và mô phỏng chuyển động đối robot UR. 4. Phương pháp nghiên cứu, kết cấu của công trình nghiên cứu Phương pháp nghiên cứu sử dụng trong công trình này là phương pháp tính toán mô phỏng mô hình trên máy tính. 1 Kết cấu của thuyết minh đề tài, ngoài mở đầu, kết luận, nội dung chính của đề tài được chia làm 3 chương: chương 1, giới thiệu về những nét cơ bản về robot công nghiệp, chương 2, tập trung phân tích động học đối với cấu hình robot UR, chương 3, tập trung xây dựng chương trình mô phỏng chuyển động đối với robot UR. 5. Kết quả đạt được của đề tài Một số kết quả đạt được trong đề tài này đó là: - Phân tích động học thuận robot UR. Xác định vị trí, vận tốc, gia tốc các khâu và thiết lập phương trình động học của robot. - Phân tích động học ngược robot UR. Đã đưa ra được nghiệm giải tích của các biến khớp, việc tìm ra được nghiệm giải tích sẽ giúp cho việc giải quyết bài toán điều khiển sau này trở nên đơn giản hơn. - Xây dựng được chương trình tính toán động học ngược và mô phỏng chuyển động của robot UR. 2 CHƯƠNG 1 GIỚI THIỆU CHUNG VỀ ROBOT CÔNG NGHIỆP 1.1 Lịch sử hình thành và phát triển Robot công nghiệp Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng tạo ra những dây chuyền về thiết bị tự động có tính linh hoạt cao đang hình thành. Các thiết bị này đang thay thế dần các máy tự động “cứng” chỉ đáp ứng một việc nhất định trong khi thị trường luôn đòi hỏi thay đổi mặt hàng về chủng loại, về kích cỡ và về tính năng...Vì thế ngày càng tăng nhanh nhu cầu ứng dụng robot để tạo ra các hệ thống sản xuất tự động linh hoạt. Thuật ngữ “robot” lần đầu tiên xuất hiện năm 1921 trong tác phẩm viễn tưởng “Rossum’s Universal Robot” của Karel Capek. Theo tiếng Séc thì robot là người làm tạp dịch. Trong tác phẩm này nhân vật Rossum và con trai của ông đã tạo ra những chiếc máy gần giống con người để hầu hạ con người. Hình 1.1. Ba chú robot trong vở kịch của Karel Capek, 1921 Hơn 20 năm sau, ước mơ viễn tưởng của Karel Capek đã bắt đầu hiện thực. Ngay sau chiến tranh thế giới lần thứ 2, ở Hoa Kỳ đã xuất hiện các loại tay máy chép hình điều khiển từ xa trong các phòng thí nghiệm về vật liệu phóng xạ. Vào những năm 50, bên cạnh các tay máy chép hình cơ khí , đã xuất hiện các loại tay máy chép hình thủy lực và điện từ, như tay máy Minotaur I hoặc tay máy 3 Handyman của General Electric. Năm 1954 George C. Devol đã thiết kế một thiết bị có tên là “ Cơ cấu bản lề dùng để chuyển hàng theo chương trình”. Đến năm 1965 Devol cùng với Joseph F. Engelber, một kỹ sư trẻ của công nghiệp hàng không, đã tạo ra loại robot công nghiệp đầu tiên năm 1959 ở Công ty Unimation. Chỉ đến năm 1975 Công ty Unimation mới bắt đầu có lợi nhuận từ sản phẩm robot đầu tiên này. Chiếc robot công nghiệp được đưa vào ứng dụng đầu tiên, năm 1961, ở một nhà máy ô tô của General Motors tại Trenton, New Jersey Hoa Kỳ. Năm 1967 Nhật Bản mới nhập khẩu chiếc robot công nghiệp đầu tiên từ Công ty AMF của Hoa Kỳ ( American Machine and Foundry Company). Đến năm 1990 có hơn 40 công ty Nhật Bản, trong đó có những công ty khổng lồ như Công ty Hitachi và Công ty Mitsubishi, đã đưa ra thị trường quốc tế nhiều loại robot nổi tiếng. Từ những năm 70, việc nghiên cứu nâng cao tính năng của robot đã chú ý nhiều đến sự lắp đặt thêm các cảm biến ngoại tín hiệu để nhận biết môi trường làm việc. Tại trường Đại học Tổng hợp Standford người ta đã tạo ra loại robot lắp ráp tự động điều khiển bằng máy vi tính trên cơ sở xử lý thông tin từ các cảm biến lực và thị giác. Vào thời gian này, Công ty IBM đã chế tạo loại robot có các cảm biến xúc giác và cảm biến lực, điều khiển bằng máy tính để lắp ráp các máy in gồm 20 cụm chi tiết. Vào giai đoạn này ở nhiều nước khác cũng tiến hành các công trình nghiên cứu tương tự, tạo ra các loại robot tự hành theo hướng bắt chước chân người hoặc súc vật. Các robot này còn chưa có nhiều ứng dụng trong công nghiệp. Tuy nhiên các loại xe robot (robocar) lại nhanh chóng được đưa vào hoạt động trong các hệ thống sản xuất tự động linh hoạt. Từ những năm 80, nhất là vào những năm 90, do áp dụng rộng rãi các tiến bộ kỹ thuật về vi xử lý và công nghệ thông tin, số lượng robot công nghiệp đã gia tăng, giá thành đã giảm đi rõ rệt, tính năng đã có nhiều bước tiến vượt bậc. Nhờ vậy robot công nghiệp đã có vị trí quan trọng trong các dây chuyền tự động sản xuất hiện đại. 4 Trước khi bước vào nghiên cứu các nội dung tiếp theo, chúng ta cũng cần thống nhất về thuật ngữ “ robot công nghiệp” ( Industrial robot). Trong nhiều tài liệu khác nhau, định nghĩa về robot công nghiệp cũng khác nhau. Khi “ robot công nghiệp” đầu tiên ra đời, Công ty AMF đã quảng cáo nó là loại máy tự động vạn năng. Trong từ điển Webster định nghĩa robot là những máy tự động có thể thực hiện được một số chức năng của con người. Nhưng nếu vậy thì có nhiều loại máy khác nhau cũng có thể gọi là robot. Viện Kỹ thuật robot của Hoa Kỳ định nghĩa robot là loại tay máy nhiều chức năng, với chương trình làm việc thay đổi được, dùng để thực hiện một số thao tác sản xuất. Có nhiều tài liệu khi định nghĩa robot rất lưu ý đến tiêu chí điều khiển bằng máy tính nhưng trong phân loại robot công nghiệp theo tiêu chuẩn của Nhật Bản (JIS B 0134- 1979) có cả nhóm tay máy điều khiển bằng tay. Theo ISO ( International Standards Organization) thì: “ robot công nghiệp là một tay máy đa mục tiêu, có một số bậc tự do, dễ dàng lập trình, điều khiển 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ư thế chưa hoàn toàn thỏa đáng. Robot công nghiệp có thể được hiểu là những thiết bị tự động linh hoạt, bắt chước được các chức năng lao động công nghiệp của con người. Nói đến thiết bị tự động linh hoạt là nhấn mạnh đến khả năng thao tác với nhiều bậc tự do, được điều khiển trợ động và lập trình thay đổi được. Còn nói đến sự bắt chước các chức năng lao động công nghiệp của con người là có ý nói đến sự không hạn chế từ các chức năng lao động chân tay đơn giản đến trí khôn nhân tạo, tùy theo loại hình công việc lao động cần đến chức năng đó hay không. Đồng thời cũng nói đến mức độ cần thiết bắt chước được như con người hay không. 1.2. Cấu trúc chung của robot công nghiệp Trên hình 1.2 giới thiệu các bộ phận chủ yếu của robot công nghiệp thông thường. Tay máy gồm các bộ phận: đế 1 đặt cố định hoặc gắn liền với xe di động 2, thân 3, cánh tay trên 4, cánh tay dưới 5, bàn kẹp 6. 5 Hình 1.2. Các bộ phận cấu thành robot công nghiệp Bên trong hoặc ở bên ngoài tay máy còn đặt nhiều bộ phận khác nữa: + Hệ thống truyền dẫn động có thể là cơ khí, thủy khí hoặc điện khí, là bộ phận chủ yếu tạo nên sự chuyển dịch ở các khớp động. + Hệ thống điều khiển đảm bảo sự hoạt động của robot theo các thông tin đặt trước hoặc nhận biết được trong quá trình làm việc. + Hệ thống cảm biến tín hiệu thực hiện việc nhận biết và biến đổi thông tin về hoạt động của bản thân robot (cảm biến nội tín hiệu) và của môi trường- đối tượng mà robot phục vụ (cảm biến ngoại tín hiệu). Các thông tin đặt trước hoặc cảm biến được sẽ đưa vào hệ thống điều khiển sau khi xử lý bằng máy vi tính, rồi tác động vào hệ thống truyền dẫn động của tay máy. Trực tiếp liên hệ với bàn kẹp là các dụng cụ (tools) thao tác với môi trường và đối tượng làm việc. 1.3 Phân loại robot công nghiệp Ngày nay robot công nghiệp đã phát triển rất đa dạng. Có thể phân loại robot công nghiệp theo nhiều cách khác nhau: + Theo vị trí “ công tác” phân ra các loại robot cấp thoát phôi, robot vận chuyển, robot vạn năng... + Theo dạng công nghệ chuyên dụng phân ra các loại robot sơn, robot hàn, robot lắp ráp... 6 + Theo cách thức và đặc trưng điều khiển phân ra: robot điều khiển tự động, robot điều khiển bằng dạy học, robot điều khiển bằng tay, robot nhìn được ( vision)... + Theo các hệ tọa độ được dùng khi thực hiện các chuyển động cơ bản phân ra các robot hoạt động theo hệ tọa độ trụ, cầu hoặc phỏng sinh... 1.4 Các chỉ tiêu đánh giá và các thông số kỹ thuật Để các cơ cấu tay máy hoạt động linh hoạt tức là có thể thực hiện được dễ dàng các chuyển dịch muôn màu muôn vẻ, chúng cần phải có một số bậc tự do chuyển động cần thiết. Như đã biết, với các cơ cấu tay máy dùng các cơ cấu hở không gian có các khớp động loại 5 thì số bậc tự do bằng số khâu động. Khi tăng số bậc tự do tức là tăng số khâu động và tăng số thiết bị động lực cho các khâu động đó nên sẽ tăng độ phức tạp về kết cấu và chế tạo. Vấn đề đặt ra là khi cùng số bậc tự do có thể chọn lựa cơ cấu tay máy nào đảm bảo tính linh hoạt cao hơn. Tính linh hoạt của cơ cấu tay máy là một chỉ tiêu tổng hợp được thể hiện qua các yếu tố sau đây: 1.4.1 Độ động của cơ cấu Khâu thao tác robot được xác định bằng 6 thông số xE , yE , z E , ,  ,  , trong đó 3 thông số đầu là vị trí của gốc hệ tọa độ gắn với khâu thao tác, còn 3 thông số sau xác định hướng của khâu thao tác. Trong lúc mỗi cấu hình của cơ cấu tay máy được xác định bằng n giá trị biến khớp q1,..., qn. Số bậc tự do n của cơ cấu tay máy có thể bằng hoặc khác 6. Có thể xảy ra 3 trường hợp sau: + Nếu n=6, khi điểm E thực hiện di chuyển nhỏ  xE ,  yE , zE , , , sang một vị trí mới nào đó, thì có thể xác định  q1 ,..., qn một cách đơn trị. + Nếu n  6 thì không phải lúc nào điểm E cũng đạt tới vị trí với định hướng như yêu cầu được. + Nếu n  6 thì có nhiểu lời giải để điểm E đạt tới vị trí với định hướng đã yêu cầu. Hiệu số n-6=m được gọi là độ cơ động của tay máy. Có thể xác định độ cơ động m bằng số bậc tự do còn lại của cơ cấu nếu giữ cố định bàn kẹp lại. Ví dụ, trong trường hợp cơ cấu hình 2.4 nếu bàn kẹp vật ở vị trí cố định, tức là khâu 3 sẽ 7 trở nên cố định và số khâu động còn lại là 2. Tính theo công thức (2.1) lúc này cơ cấu còn lại 1 bậc tự do (w=1). Sự tồn tại độ cơ động ( m  1 ) là có lợi vì khi đó cơ cấu tay máy có thể đạt tới đích với nhiều phương án khác nhau. Điều đó càng quan trọng nhất là khi môi trường làm việc có các chướng ngại. Tuy nhiên dễ có độ cơ động cao, tức là cần số bậc tự do cao thì độ phức tạp kết cấu tay máy cũng tăng theo và sẽ không tránh khỏi việc tăng giá thành và giảm độ chính xác chuyển động. 1.4.2 Hệ số phục vụ Trong vùng làm việc, tức là trong khoảng không gian mà bàn kẹp tay máy có thể thao tác được, không phải ở bất cứ điểm nào trong vùng này bàn kẹp tay máy cũng thao tác dễ dàng như nhau. Để đánh giá mức độ dễ dàng thao tác đó người ta dùng khái niệm hệ số phục vụ. Hệ số phục vụ  là tỷ số giữa góc phục vụ  so với 4. Góc phục vụ  là góc nón quét một vùng không gian mà chỉ ở phía trong đó bàn kẹp mới có thể hướng tới tọa độ cần thiết:   4 Giá trị của  và  không những phụ thuộc vào vị trí điểm thao tác trong vùng làm việc, mà còn phụ thuộc vào kết cấu của tay máy. 1.4.3 Độ dễ điều khiển của cơ cấu tay máy Trong thực tế điều khiển hoạt động của tay máy, từ khi nhận được tín hiệu về định vị và định hướng của” điểm tác động cuối” E tại một điểm của quỹ đạo, cho tới khi điều khiển để đạt được mục tiêu đó, robot phải thực hiện hoạt động đó trong một khoảng thời gian nhất định. Thời gian đó bao gồm thời gian tính toán để xác định các thông số điều khiển và thời gian thực hiện di chuyển. Tổng các thời gian đó có thể gọi là thời gian điều khiển. Trong đó thời gian tính toán giá trị các biến khớp qi theo các thông số định vị và định hướng tại điểm E, phụ thuộc vào loại cơ cấu tay máy. Qua thông số thời gian điều khiển nói trên có thể xác định mức độ khó dễ điều khiển, như một chỉ tiêu đánh giá cơ cấu tay máy. 8 1.4.4 Các thông số kỹ thuật của robot công nghiệp Robot công nghiệp thường được đặc trưng bằng bảng các thông số kỹ thuật cơ bản xem bảng (1.1). Hệ truyền dẫn động được ghi rõ là thủy lực, khí nén, động cơ điện một chiều, động cơ bước...Trong chương IX sẽ phân tích về khả năng ứng dụng trong các hệ truyền dẫn động này. Hệ điều khiển được xác định theo chu kỳ, theo vị trí hoặc theo chu tuyến...Trong chương VI sẽ đề cập đến các hệ điều khiển ứng dụng trong robot công nghiệp. Sai số định vị của bàn kẹp (mm) là độ sai lệch giữa vị trí thực so với vị trí yêu cầu. Mức chính xác thấp  >  1(mm) áp dụng cho các loại robot vận chuyển, phun phủ...Mức chính xác trung bình 0,1    1,0(mm) thích hợp với các việc như lắp ráp có khe hở, vặn vít, hàn hồ quang...Mức chính xác cao   0,1(mm) dùng khi đo lường, lắp ráp khít... Bảng 1.1. Đặc trưng của robot Thông số kỹ thuật Đơn vị Thông số kỹ thuật Đơn vị Số bậc tự do - Vận tốc tịnh tiến lớn nhất mm/s Tải nâng Kg Tầm với (max/min) mm Tầm cao (max/min) mm Giá trị giới hạn (max, min) * Biến khớp quay rad Sai số định vị mm * Biến khớp tịnh tiến mm Hệ truyền dẫn động - Vận tốc góc lớn nhất khi quay rad/s Hệ điều khiển - 1.5. Các bài toán thường gặp đối với robot công nghiệp Trong thực tế, để chế tạo ra một robot công nghiệp hoàn chỉnh và có thể thương mại hóa nó phải trải qua rất nhiều bước. Tuy nhiên, ở đây ta chỉ giới hạn ở các vấn đề tính toán lý thuyết cơ bản thì các bài toán liên quan đến robot công nghiệp bao gồm: + Phân tích động học: Tìm mối quan hệ giữa chuyển động của khâu thao tác (bàn kẹp, đầu hàn, sơn, phun phủ,…) và chuyển động của các khớp (góc quay của các động cơ hoặc chuyển động tịnh tiến đặt ở mỗi khớp). Sau đó, chúng ta cần phải 9 giải mối quan hệ này theo cả hai chiều: cho trước chuyển động của khâu thao tác, cần tìm chuyển động của các khớp hoặc ngược lại. Bên cạnh đó, vấn đề tính toán vận tốc dài, vận tốc góc cũng là 1 vấn đề quan trọng, làm đầu vào cho bài toán động lực học. Các yếu tố đầu vào của bài toán động học bao gồm cấu trúc động học và kích thước các khâu của robot. + Phân tích động lực học: Ở bài toán động lực học, chúng ta cần quan tâm đề nguyên nhân gây ra chuyển động tức là mối quan hệ giữa momen ( hoặc lực) đặt vào các khớp quay (hoặc tịnh tiến) và chuyển động tương ứng của các khớp đó. Các yếu tố đầu vào của bài toán động lực học bao gồm kết quả của bài toán động học và các yếu tố về khối lượng, momen quán tính khối của các khâu của robot. + Thiết kế quỹ đạo và điều khiển: Đây là bài toán sau cùng cũng là bài toán phức tạp nhất của tính toán robot. Chúng ta cần thiết kế đường di chuyển cho khâu thao tác và chuyển động của các khớp cùng với các yếu tố về vận tốc, gia tốc để đáp ứng yêu cầu kỹ thuật trong sản xuất. Bài toán điều khiển nhằm đảm bảo robot sẽ hoạt động bám theo đúng những thông số ta đã thiết kế trước, chất lượng của điều khiển quan hệ mật thiết với chất lượng của một robot. 10 CHƯƠNG 2 PHÂN TÍCH ĐỘNG HỌC ROBOT UR Trong chương này tập trung giải quyết hai bài toán đối với phân tích động học là bài toán động học thuận và bài toán động học ngược. Bài toàn động học thuận có nhiệm vụ chủ yếu là xác định ví trí và hướng khâu thao tác dưới dạng hàm của các biến khớp. Phương pháp sử dụng để giải quyết bài toán này là phương pháp ma trận Denavit-Hartenberg [1, 2]. Đối với bài toán động học ngược, cho biết chuyển động của khâu thao tác ta cần phải xác định chuyển động của các tọa độ khớp hai phương pháp được giới thiệu để giải bài toán này là phương pháp giải tích và phương pháp số [2]. 2.1. Phân tích động thuận rôbốt UR 2.1.1. Mô hình rôbốt và các hệ tọa độ Denavit-Hartenberg (DH) Trên hình 2.1 là hinh ảnh của một số dạng robot UR, các robot đó có kích thước khác nhau nhưng hình dáng nói chung là giống nhau, xuất phát từ hình ảnh thực tế, ta xây dựng được mô hình chuyển động của các robot UR có dạng như trên hình 2.2 dưới đây, trong đó ta đặt các kích thước trên hình bằng các tham số như sau: O0O1 = d1, O1A = d2, AO2 = a2, O2B = d3, BC = a3, CO3 = d4, O4O5 = d5, O5O6 = d6. Dựa vào phương pháp ma trận DH [1, 2], ta xây dựng được các hệ trục tọa độ khớp đối với rôbốt có dạng như trên hình 2.2. Sau khi xây dựng được các hệ tọa độ khớp, ta xác định được bảng các tham số động học DH được cho như trong bảng 2.2 Hình 2.1: Bộ sản phẩm robot công nghiệp UR3, UR5 và UR10 11 z q5 4 x 4 O 6 x x z q B 2 O 3 5 x C 5 x’ O O q 3 4 z 2 q 2 3 4 3 6 x z 5 O 6 6 z 6 O1 q x 2 A z z 1 0 q 1 1 O 0 x’ 1 x 0 Hình 2.2. Mô hình chuyển động của rôbốt UR Bảng 2.1: Bảng các tham số động học DH của rôbốt UR Hệ trục i di ai αi 1 q1 d1 0 900 2 q2 d2 a2 00 3 q3 d 4  d3 a3 00 4 q4 0 0 -900 5 q5 d5 0 900 6 q6 d6 0 00 2.1.2. Xác định các ma trận DH Từ cách xây dựng hệ trục ở trên ta thấy, để chuyển hệ trục thứ i – 1 sang hệ trục thứ i ta cần thực hiện bốn bước như sau [1, 2]: đầu tiên là quay quanh trục zi1 một góc i, tiếp theo dịch chuyển dọc trục zi-1 một đoạn di sau đó dịch chuyển dọc trục xi một đọa ai và cuối cùng là quay quanh trục xi một góc i. Vậy khi đó ma trận chuyển từ hệ trục i – 1 sang hệ trục i là tích của 4 ma trận quay thuần nhất cơ bản nói trên, khi đó ta có 12 H i i 1 H i  Tz (i ).Transz (di ).Transx (ai ).Tx ( i ) cosi  sin  i   0   0  sin i cos  i cosi cos i sin  i 0 sin i sin  i  cosi sin  i cos i 0 ai cosi  ai sin i   di   1  (2.1) Từ bảng các tham số động học DH (bảng 2.1), thay các tham số tương ứng vào (2.1) ta lần lượt nhận được các ma trận DH địa phương như sau: c1 s H1   1 0  0 c3 s H3   3 0  0 0 s1 0 c1 0 c2  s2 s 0 c2 ; H2   2 0 1 0 d1  0   0 0 1 0 0  s3 0 a3c3  c4 0 s c3 0 a3 s3  0  ; H4   4  0 1 0 1 d 4  d3    0 0 1  0 0 0 c5 0 s5 c6  s6  s 0 c 0  s c6 5 5 ; H5   H6   6  0 1 0 d5  0 0    1 0 0 0 0 0 0 a2c2  0 a2 s2   1 d2   0 1   s4 0  c4 0  0 0  0 1 0 0 0 0  1 d6   0 1 Từ các ma trận DH địa phương ở trên, ta xác định được các ma trận DH toàn cục cho các khâu của robot được cho bởi công thức [1, 2] Di  0 H i  0 H1 1 H 2  i 1 H i  H1H 2  H i (2.2) Áp dụng (2.2) khi cho i = 1  6, ta được các ma trận Di như sau: c1 s D1   1 0  0 0 s1 0 c1 1 0 0 0 c1c2 c1s2 c s s s 1 2 D2   2 1  s2 c2  0  0 0 0  d1   1 s1 c1 0 0 d 2 s1  a2c1c2  a2c2 s1  d 2c1   d1  a2 s2   1  13 c23c1  s23c1 s1 d 2 s1  s1 (d3  d 4 )  a2c1c2  a3c1c23   c s  s s c c ( d  d )  d c  a c s  a c s  23 1 1 1 3 4 2 1 2 2 1 3 23 1  D3   23 1  s23  c23 0 d1  a3 s23  a2 s2   0 0 1  0  c234c1  s1  s234c1 d 2 s1  s1 (d3  d 4 )  a2c1c2  a3c1c23   c s c  s s c (d  d )  d c  a c s  a c s  1 234 1 1 3 4 2 1 2 2 1 3 23 1  D4   234 1  s234  0 c234 d1  a3 s23  a2 s2   0 0 1  0   c234 c1c5  s1s5 c s  c c s D5   1 5 234 5 1  s234 c5  0   s234 c1 c5 s1  c234 c1s5  s234 s1 c234 s1s5  c1c5 c234 s234 s5 0 0  c6 ( s1s5  c234 c1c5 )  s234 c1s6  c (c s  c c s )  s s s 234 1 6 D6   6 1 5 234 5 1  c234 s6  s234 c5c6  0  (d 2  d3  d 4 ) s1  a2 c1c2  d5 s234 c1  a3c1c23  (d3  d 4  d 2 )c1  a2 c2 s1  d5 s234 s1  a3c23 s1   d1  a3 s23  a2 s2  d5c234  1  s6 ( s1s5  c234 c1c5 )  s234 c1c6 c5 s1  c234 c1s5  s6 (c1s5  c234 c5 s1 )  s234 c6 s1 c234 s1s5  c1c5 c234 c6  s234 c5 s6 s234 s5 0 0 D6 [1, 4]  D6 [2, 4] D6 [3, 4]  1  Trong đó D6 [1, 4]  d 6 (c5 s1  c234 c1s5 )  ( d 2  d3  d 4 ) s1  a2c1c2  d5 s234 c1  a3c1c23 D6 [2,4]  (d 2  d3  d 4 )c1  d 6 c1c5  a2c2 s1  d5 s234 s1  a3 s1c23  d 6c234 s1s5 D6 [3,4]  d1  a3 s23  a2 s2  d 5c234  d 6 s5 s234 Chú ý. Trong các ma trận ở trên, ta đưa vào các ký hiệu ci  cos( qi ); cij  cos  qi  q j  ; cijk  cos  qi  q j  qk  ; si  sin( qi ); sij  sin  qi  q j  ; sijk  sin  qi  q j  qk  (2.3) 2.1.3. Xác định vận tốc góc và gia tốc góc các khâu của rôbốt UR Để tìm vận tốc góc các khâu của rôbốt UR ta sử dụng công thức dưới đây [1]  AT , ω  ,A    i(0)  A  i( i )  ATi A ω i i i i  R0 dA i dt  i( i ) [2,3] ω  i( i ) [1,3] ω  i( i ) [1, 2] ω i( i )   ω (2.4) T (2.5) 14 Trong đó Ai là ma trận côsin chỉ hướng của khâu thứ i đối với hệ trục tọa độ cố định, các ma trận này được xác định dựa vào các ma trận Di như sau: như đã biết ma trận Di bao gồm các thành phần [1]  0 Ai Di  H i   T  0 rO(0)i   1  0 (2.6) Từ (2.6) ta dễ dàng xác định được các ma trận Ai có dạng như sau  c1 0 s1   c1c2 A1   s1 0 c1  ; A 2   c2 s1     0 1 0   s2  c234 c1 A 4   c234 s1   s234  s1 c1 0 c1s2  s1s2 c2 s1  c23c1 c1  ; A 3   c23 s1    s23 0   s234 c1   c234c1c5  s1s5   s234 s1 ; A 5   c1s5  c234c5 s1    c234  s234 c5  c6 ( s1s5  c234 c1c5 )  s234c1s6 A 6   c6 (c1s5  c234c5 s1 )  s234 s1s6   c234 s6  s234c5c6  s23c1  s23 s1 c23 s1  c1   0   s234c1 c5 s1  c234 c1s5   s234 s1 c234 s1s5  c1c5    c234 s234 s5 s6 ( s1s5  c234c1c5 )  s234c1c6  s6 (c1s5  c234c5 s1 )  s234c6 s1 c234 c6  s234c5 s6 c5 s1  c234c1s5  c234 s1s5  c1c5    s234 s5 Sau khi có được các ma trận côsin chỉ hướng, áp dụng công thức (2.4) và để ý đến (2.5) ta xác định được các véctơ vận tốc góc các khâu của rô bốt UR như sau: * Vận tốc góc các khâu của robot UR khi chiếu lên các hệ tọa độ khớp ω1(1)   0 q1  ω (2) 2   s2 q1 ω 3(3)   s23q1  ω (4) 4   s234 q1 T 0 c2 q1 c23q1 T q2  T q 2  q3   q 2  q3  q4 T c234 q1  ω 5(5)  c5 s234 q1  s5 ( q2  q3  q4 )    c234 q1  q5        s234 s5 q1  c5 ( q2  q3  q4 )  ω (6) 6  (c234 s6  c5c6 s234 ) q1  c6 s5 ( q2  q3  q 4 )  s6 q5    (c5 s234 s6  c234c6 ) q1  s5 s6 ( q2  q3  q4 )  c6 q5      s234 s5 q1  c5 ( q 2  q3  q4 )  q6 * Vận tốc góc các khâu của robot UR khi chiếu lên hệ tọa độ cố định 15 T ω1(0)   0 0 q1   ω (0) 2   s1 q2 c1q2 T q1  T ω 3(0)   s1 ( q2  q3 ) c1 ( q2  q3 ) q1  T    ω (0)  c1 ( q2  q3  q4 ) q1  4   s1 ( q2  q3  q4 ) ω 5(0)  s1 ( q2  q3  q4 )  c1s234 q5    c1 (q2  q3  q 4 )  s1s234 q5      q1  c234 q5 ω(0) 6  s1 (q2  q3  q4 )  c1s234q5  (c1c234 s5  c5 s1 )q6    c1 (q2  q3  q4 )  s1s234q5  (c234 s1s5  c1c5 )q6      q1  c234q5  s234 s5q6 Tương tự như với vận tốc góc, bằng cách đạo hàm công thức (2.4) theo thời gian, ta được ma trận sóng của véctơ gia tốc góc của các khâu có dạng:  ( i )  A  TA   AT A  ε i(i )  ω i i i i i (2.7)  (0)  A  AT  A  A T ε i(0)  ω i i i i i (2.8) Từ (2.7) và (2.8) ta dễ dàng suy ra được véctơ gia tốc góc các khâu của rôbốt. 2.1.4. Vận tốc và gia tốc điểm định vị khâu thao tác của rôbốt UR Theo (2.6) ta thấy ma trận D6 của rôbốt có dạng  0 A 6 rO(0)6   0 A 6 rP(0)  D6   T  T  1  0 1   0  Từ đó suy ra vị trí điểm định vị của khâu thao tác chính là ba thành phần đầu tiên của cột thứ tư của ma trận D6, khi đó ta có (0) P r  d 6 (c5 s1  c234c1s5 )  ( d 2  d3  d 4 ) s1  a2c1c2  d5 s234c1  a3c1c23    ( d 2  d3  d 4 )c1  d 6c1c5  a2c2 s1  d5 s234 s1  a3 s1c23  d 6c234 s1s5  (2.9)     d1  a3 s23  a2 s2  d5c234  d 6 s5 s234 Đạo hàm (2.9) theo thời gian ta được vận tốc của điểm định vị khâu thao tác như sau: 16 vPx  d 6 [c1q1c5  s1s5 q5  s234 ( q2  q3  q4 ) c1 s5  c234 s1q1s5  c234c1c5 q5 ]  ( d 2  d 3  d 4 )c1q1  s1q1c2 a2  c1s2 q 2 a2  d5c234 ( q 2  q3  q 4 )c1  d5 s234 s1q1  a3 s1q1c23  a3c1s23 ( q 2  q3 ) vPy  (d 2  d3  d 4 ) s1q1  d 6 s1q1c5  d 6 c1s5 q5  c1q1c2 a2  s1s2 q2 a2  d5c234 (q2  q3  q4 ) s1  d5 s234c1q1  a3c1q1c23  a3 s1s23 (q2  q3 )  d 6 s234 (q2  q3  q4 ) s1s5  d 6c234c1q1s5  d 6c234 s1c5 q5 vPz  a3c23 (q2  q3 )  c2q2a2  d5s234 (q2  q3  q4 )  d6c5q5 s234  d6 s5c234 (q2  q3  q4 ) Bằng cách đạo hàm lần nữa các hình chiếu vận tốc ta được hình chiếu véctơ gia tốc điểm định trên các trục cố định. 2.1.5. Thiết lập phương trình động học Robot Hệ các phương trình động học rôbốt cho ta mối quan hệ giữa vị trí điểm định vị và hướng của khâu thao tác và các tọa độ khớp. Hê phương trình này có một vai trò rất quan trọng trong việc phân tích bài toán động học ngược sau này, trong phần này ta sẽ đi thiết lập các phương trình đó. Từ trên ta đã biết, ma trận thuần nhất biểu diễn mối quan hệ giữa hệ tọa độ gắn với khâu thao tác và hệ tọa độ cố định có dạng  c6 ( s1s5  c234 c1c5 )  s234 c1s6  c (c s  c c s )  s s s 234 1 6 D6   6 1 5 234 5 1  c234 s6  s234 c5c6  0  s6 ( s1s5  c234 c1c5 )  s234 c1c6 c5 s1  c234 c1s5  s6 (c1s5  c234 c5 s1 )  s234 c6 s1 c234 s1s5  c1c5 c234 c6  s234 c5 s6 s234 s5 0 0 D6 [1, 4]  D6 [2, 4] D6 [3, 4]  1  Mặt khác, ma trận thuần nhất mô tả khâu thao tác có dạng:  nx n T y  nz  0 sx ax xP  s y a y yP   (2.10) s z az z P   0 0 1    trong đó các vecto n  nx , n y , nz  ; s  sx ,s y ,s z  ; a  a x ,a y ,a z  là các vecto đơn vị của 3 trục tọa độ của hệ tọa độ gắn với khâu thao tác. 3 vecto này thể hiện hướng của  khâu thao tác, còn vecto rP  xP , yP , z P  thể hiện vị trí khâu thao tác (chính là vị trí 17 gốc tọa độ của hệ tọa độ gắn với khâu thao tác) trong hệ tọa độ cố định. Từ đây ta thu được hệ phương trình động học dưới dạng: T  D6  H1H2H3H4H5H6 (2.11) Khai triển (2.11) ở dạng tường minh, ta thu được hệ phương trình động học robot UR như sau:  nx  c6  s1s5  c234c1c5   s234c1s6 ; n y  c6  c1s5  c234c5 s1   s234 s1s6   nz  c234 s6  s234c5c6 s  s  s s  c c c   s c c ; s  s  c s  c s c   s s c 234 1 6 y 6 1 5 234 1 5 234 1 6  x 6 1 5 234 1 5  sz  c234c6  s234c5 s6 (2.12)  a  s c  c s c ; a   c c  s s c ; a  s s y 1 5 1 5 234 z 234 5  x 1 5 1 5 234 x  d c s  c c s   d  d  d  s  a c c  d s c  a c c 6 5 1 234 1 5 2 3 4 1 2 1 2 5 234 1 3 1 23  P  yP    d 2  d3  d 4  c1  d 6c1c5  a2c2 s1  d5 s234 s1  a3 s1c23  d 6 s234 s1s5   z P  d1  a3 s23  a2 s2  d 5c234  d 6 s5 s234 Chú ý rằng ở (2.12) các biểu thức vế trái là các thành phần biểu diễn khâu thao tác, còn các biểu thức vế phải chứa kích thước các khâu cũng như góc quay của các khớp. (2.12) có 12 phương trình, gồm 3 phương trình liên quan tới vị trí và 9 phương trình liên quan tới hướng, tuy nhiên chỉ có 3 trong 9 phương trình về hướng là độc lập. Trong nhiều tài liệu người ta thường chọn 6 phương trình dưới đây làm hệ các phương trình động học của rôbốt  nx  c6  s1s5  c234c1c5   s234c1s6   s y   s6  c1s5  c234 s1c5   s234 s1c6 a  s s  z 234 5 (2.13)  x  d c s  c c s  d  d  d s  a c c  d s c  a c c     6 5 1 234 1 5 2 3 4 1 2 1 2 5 234 1 3 1 23  P y   d  d  d c  d c c  a c s  d s s  a s c  d s s s  2 3 4  1 6 1 5 2 2 1 5 234 1 3 1 23 6 234 1 5  P  z P  d1  a3 s23  a2 s2  d 5c234  d 6 s5 s234 2.2. Phân tích động học ngược robot UR Đối với bài toán động học ngược, xuất phát từ hệ phương trình động học (2.11) với các thành phần về hướng và vị trí trong ma trận T ở (2.10) đã cho trước 18