Đang tải... (xem toàn văn)
nghiên cứu bài toán động học, động lực học và thiết kế bộ điều khiển RISE cho robot 3 bậc tự do RRP. Thiết kế bộ điều khiển thích nghi để so sánh, đánh giá chất lượng của bộ điều khiển RISE. Ứng dụng matlab simulink để giải ài toán robot và thiết kế bộ điều khiển.
1 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền MỤC LỤC SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 2 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền DANH MỤC CÁC THUẬT NGỮ, CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT STT Ký hiệu n DH Diễn giải nội dung đầy đủ Khoảng cách từ trục đến trục đo dọc theo trục Góc trục trục xác định theo trục Khoảng cách từ trục đến trục đo dọc theo trục Góc trục trục xác định theo trục Số bậc tự Robot Denavit- Hartenberg Vận tốc biến khớp thứ i 10 11 Gia tốc biến khớp i Ma trận biến đổi mô tả quan hệ khâu i-1 khâu i Ma trận biến đổi cos(qi ) 12 sin ( 2.q2 ) 13 cos ( q1 ) cos(q ) 14 cos ( q1 ) sin(q ) 15 sin(q1 ).cos ( q2 ) 16 sin(qi ) 17 sin ( q1 ) sin ( q2 ) 18 19 20 21 22 23 24 25 26 27 28 ,, ,, ,, ,, ,, Vector tọa độ khối tâm hệ quy chiếu khâu i Các momen quán tính độc cực Các momen quán tính tích Vector gia tốc trọng trường Ma trận giả quán tính khâu i Khối lượng khâu i Lực tổng quát khâu thứ i Chiều dài khâu i Vector mô tả vị trí Vector có hướng mà bàn tay tiếp cận đối tượng Vector có hướng mà theo ngón tay nắm vào cầm nắm đối tượng SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 3 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền DANH MỤC HÌNH VẼ SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền LỜI MỞ ĐẦU Trong năm gần đây, với phát triển công nghệ hệ thống máy móc đại, Robot không ngừng phát triển số lượng khả chúng Robot ngày thông minh, đảm nhận thay công việc mà người thực như: làm việc môi trường có tia phóng xạ, giải cứu người hỏa hoạn, thu thập thông số môi trường nơi khắc nghiệt Các loại Robot ngày linh hoạt chúng điều khiển xử lý trung tâm tích hợp thêm hệ thống điều khiển thông minh Robot phát minh vĩ đại người mục đích giải phóng người khỏi công việc nặng nhọc, nhàm chán công việc (do lặp lại thao tác công việc nhiều lần nguy hiểm môi trường lao động môi trường nóng lò hơi, ô nhiễm bụi bặm hầm mỏ, hay nguy hiểm duới đáy đại dương, không gian vũ trụ… tăng tính tự động hóa dây truyền sản xuất Nhưng nước ta việc nghiên cứu chưa mở rộng phát triển sử dụng sở sản xuất lớn Các Robot đa số thường nhập từ nước với giá thành đắt, vận dụng Robot vào sản xuất gặp nhiều khó khăn cần hướng dẫn bạn bè quốc tế, lực lượng cán kỹ sư có trình độ cao … Từ thực trên, với đam mê, ham học hỏi, nghiên cứu khoa học công nghệ tiên tiến giới nên em định chọn đề tài liên quan tới lĩnh vực nghiên cứu Robot Cụ thể đề tài: “Nghiên cứu động học, động lực học thiết kế điều khiển RISE cho robot bậc tự RRP” TS Đinh Thị Thanh Huyền hướng dẫn Hoàn thành đồ án tốt nghiệp, em xin chân thành cảm ơn hướng dẫn, bảo cô giáo TS Đinh Thị Thanh Huyền giúp đỡ em hoàn thành môn học Măc dù em cố gắng hạn chế mặt kiến thức nên chắn đề tài em không tránh khỏi sai sót, kính mong nhận bảo thầy cô góp ý bạn để đề tài em hoàn thiện Em xin chân thành cảm ơn! Sinh viên thực Lê Bá Thành SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền CHƯƠNG 1: TỔNG QUAN VỀ ROBOT 1.1 Định nghĩa Robot - Định nghĩa theo tiêu chuẩn AFNOR (Pháp): Robot cấu chuyển động tự động lập trình, lặp lại chương trình, tổng hợp chương trình đặt trục tọa độ; có khả định vị, định hướng, di chuyển đối tượng vật chất: chi tiết, dụng cụ, gá lắp,… theo hành trình thay đổi chương trình hóa nhằm thực nhiệm vụ công nghệ khác - Định nghĩa theo RIA (Robot Institute of America): Robot tay máy vạn lặp lại chương trình thiết kế để di chuyển vật liệu, chi tiết, dụng cụ thiết bị chuyên dùng thông qua chương trình chuyển động thay đổi để hoàn thành nhiệm vụ khác - Định nghĩa theo ГOCT (Nga): Robot máy tự động, đặt cố định di động được, liên kết tay máy hệ thống điều khiển theo chương trình, lập trình lại để hoàn thành chức vận động điều khiển trình sản xuất Robot có khả chương trình hóa linh hoạt nhiều trục chuyển động, linh hoạt biểu thị cho số bậc tự chúng 1.2 Vai trò Robot Robot áp dụng nhiều lĩnh vực góc độ thay sức người Mục đích sử dụng dây chuyền sản suất nhằm nâng cao suất, chất lượng hiệu sản xuất từ giảm giá thành sản phẩm nâng cao khả cạnh tranh SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Hình 1.1: Robot công nghiệp Đặc điểm Robot: - Làm việc mệt mỏi, tượng nhầm lẫn làm việc Làm việc môi trường nguy hiểm phóng xạ, độc hại Thay đổi thao tác làm việc dễ dàng cách thay đổi chương trình lập trình Trong ngành khí, Robot sử dụng nhiều công nghệ đúc, hàn, cắt kim loại, - sơn, phun phủ kim loại, vận chuyển phôi, lắp ráp sản phẩm,… Trong dây chuyền sản xuất tự động, nhờ có mặt Robot mà dây chuyền đạt mức độ tự động hóa cao, mức độ linh hoạt cao Các Robot điều khiển hệ thống chương trình lập trình sẵn Robot sử dụng lĩnh vực y học, quốc phòng, chinh phục vũ trụ, công nghiệp nguyên tử,… SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Hình 1.2: Robot công nghiệp dây truyền sản xuất ô tô Nhật Bản Rõ ràng Robot có vai trò ý nghĩa quan trọng nhiều lĩnh vực sống, nhiên Robot chưa thể linh hoạt người, trình làm việc bị hỏng làm ảnh hưởng tới hoạt động dây chuyền Vì vậy, Robot phải hoạt động giám sát người 1.3 Bậc tự Robot Bậc tự Robot số khả chuyển động độc lập cấu Robot Để dịch chuyển vật thể không gian, cấu chấp hành Robot phải đạt số bậc tự Hầu hết hệ Robot cấu hở, bậc tự tính theo công thức: (1.1) Trong đó: + n: Số khâu động + : Số khớp loại i (i= 1, 5: số bậc tự bị hạn chế) Đối với cấu nối với khớp quay tịnh tiến (khớp động loại 5) số bậc tự số khâu động Đối với cấu hở, số bậc tự tổng số bậc tự khớp động Để định vị định hướng khâu chấp hành cuối cách tùy ý không gian chiều Robot cần có bậc tự do, bậc tự để định vị bậc tự để SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền định hướng Đối với số công việc đơn giản nâng hạ, xếp,… yêu cầu số bậc tự Các Robot hàn, sơn,… thường yêu cầu bậc tự Trong số trường hợp cần khéo léo, linh hoạt cần tối ưu hóa quỹ đạo,… người ta dùng Robot với số bậc tự lớn 1.4 Hệ tọa độ Mỗi Robot bao gồm nhiều khâu (links) liên kết với khớp (joints), tạo thành xích động học xuất phát từ khâu (base) đứng yên Hệ quy chiếu gắn với khâu gọi hệ quy chiếu chuẩn Các hệ quy chiếu trung gian khác gắn với khâu động gọi hệ quy chiếu suy rộng Trong thời điểm hoạt động, tham số hệ quy chiếu suy rộng xác định cấu hình Robot chuyển động dài chuyển dịch góc khớp tịnh tiến khớp quay Hình 1.1 Các tham số hệ quy chiếu suy rộng gọi biến khớp Các hệ quy chiếu gắn khâu Robot phải tuân theo quy tắc bàn tay phải: Dùng tay phải, nắm hai ngón tay út áp út vào lòng bàn tay, xòe ngón: cái, trỏ theo phương vuông góc với Nếu chọn ngón làm phương trục Z ngón trỏ phương, chiều trục X ngón biều thị phương, chiều trục Y Hình 1.2 Hình 1.3 Các tọa độ suy rộng robot SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Hình 1.4 Quy tắc bàn tay phải Cấu trúc robot Các thành phần robot 1.5 1.5.1 Một Robot thường bao gồm thành phần như: Cánh tay Robot, nguồn động lực, dụng cụ lắp khâu chấp hành cuối, cảm biến, điều khiển, thiết bị dạy học, máy tính… phần mềm lập trình gọi thành phần hệ thống Robot Mối quan hệ thành phần Robot Hình 1.3 Hình 1.5 Các thành phần hệ thống robot - Cánh tay Robot kết cấu khí gồm khâu liên kết với khớp động để - tạo nên chuyển động Robot Nguồn động lực động điện chiều động bước, hệ thống xy - lanh thủy lực, khí nén để tạo động lực cho tay máy hoạt động Dụng cụ thao tác gắn khâu cuối Robot, dụng cụ nhiều kiểu khác như: dạng bàn tay để nắm bắt đối tượng công cụ làm việc mỏ hàn, - đá mài, đầu phun sơn… Thiết bị dạy học dùng để dạy cho Robot thao tác cần thiết theo yêu cầu trình làm việc, sau Robot tự lặp lại động tác dạy để làm việc SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp - GVHD: TS Đinh Thị Thanh Huyền Các phần mềm để lập trình chương trình điều khiển Robot cài đặt máy tính, dùng để điều khiển Robot thông qua điều khiển Bộ điều khiển hay gọi Module điều khiển, thường kết nối với máy tính Một Mudule điều khiển có cổng Vào-Ra (I/O port) để làm việc với nhiều thiết bị khác cảm biến giúp Robot nhận biết trạng thái thân, xác định vị trí đối tượng làm việc; điều khiển băng tải cấu cấp phôi hoạt động phối hợp với Robot… 1.5.2 Kết cấu tay máy Như nói trên, tay máy thành phần quan trọng, định khả làm việc robot Các kết cấu nhiều tay máy theo cấu tạo chức tay người; nhiên ngày nay, tay máy thiết kế đa dạng, nhiều cánh tay Robot có hình dáng khác xa cánh tay người Trong thiết kế sử dụng tay máy, cần quan tâm tới thông số hình học-động học, thông số liên quan đến khả làm việc robot như: tầm với (hay hướng công tác), số bậc tự (thể khéo léo linh hoạt Robot), độ cứng vững, tải trọng vật nặng, lực kẹp, - … Các khâu robot thường thực hai chuyển động bản: Chuyển động tịnh tiến theo phương x, y, z không gian Đề các, thông thường tạo nên hình khối, chuyển động thường ký hiệu T (Translation) P - (Prismatic) Chuyển động quay quanh trục x, y, z ký hiệu R (Rotation) Tùy thuộc vào số khâu tổ hợp chuyển động (R&T) mà tay máy có kết cấu khác với vùng - làm việc khác Các kết cấu thường gặp Robot robot kiểu tọa độ Đề các, tọa độ trụ, tọa độ cầu, robot kiểu SCARA, hệ tọa độ góc (phóng sinh)… Robot kiểu tọa độ Đề các: tay máy có chuyển động tịnh tiến theo phương trục hệ tọa độ gốc (hình 1.4) Không gian làm việc có dạng khối chữ nhật Do kết cấu đơn giản, loại tay máy có độ cứng vững cao, độ xác khí dễ đảm bảo thường dùng để vận chuyển phôi liệu, lắp ráp, hàn mặt phẳng… SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp Tức cần GVHD: TS Đinh Thị Thanh Huyền T & ˆ = ⇔ −eT2 Yθ%= θ% −eT2 Yθ%−θ%T Eθ& Eθˆ ( ) T T T −eT2 Yθ%= −eT2 Yθ% = −θ% ( eT2 Y ) = −θ%T YTe2 Ta có & & T T T & ⇒ −θ% Y e = θ% Eθˆ ⇒ −Y T e = Eθˆ ⇒ θˆ = −E −1Y T e Từ ta chọn hàm f = − Ke2 + Yθˆ với & θˆ = − E −1Y T e2 4.4.2 Mô hình hệ thống điều khiển thích nghi cho robot BTD Hình 4.23: Mô hình hệ thống điều khiển thích nghi cho robot BTD Khi mô với thời gian t=30(s) kết thu sai số sau điều khiển sau: SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Hình 4.24: Kết sai số khâu Hình 4.25: Kết sai số khâu SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Hình 4.26: Kết sai số khâu Bộ điều khiển thích nghi đạt yêu cầu mong muốn, giúp cho robot chuyển động theo quỹ đạo cho trước (e =>0) 4.4.3 So sánh kết hai điều khiển Cùng quỹ đạo mong muốn ban đầu sử dụng hai điều khiển khác với hệ số mô khác cho kết khác Dưới khác biệt hai điều khiển RISE thích nghi Hình 4.27: So sánh sai số khâu BĐK SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Kết sai số khâu thông qua mô cho thấy điều khiển RISE có độ vọt lố nhỏ so với điều khiển thích nghi Hình 4.28: So sánh sai số khâu BĐK Tuy nhiên khâu lại có kết ngược lại, điều khiển thích nghi lại thể tốt có độ vọt lố nhỏ hơn, độ vọt lố BĐK thích nghi 1/3 độ vọt lố BĐK RISE Hình 4.29: So sánh sai số khâu BĐK SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền Bộ điều khiển RISE cho thời gian xác lập ngắn thời gian xác lập điều khiển thích nghi Cụ thể thời gian xác lập BĐK RISE 1,1s điều khiển thích nghi có thời gian xác lập 5s Tuy nhiên, điều khiển thích nghi lại cho độ vọt lố thấp điều khiển RISE Như vậy, hai điều khiển giúp cho robot chuyển động theo quỹ đạo mong muốn Tuy nhiên điều khiển có lợi riêng, có hướng tiếp cận khác để giúp cho robot làm việc xác SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CỦA ĐỀ TÀI Trong nội dung nghiên cứu đồ án xây dựng điều khiển RISE cho mô hình robot bậc tự RRP để robot làm việc hiệu theo ý muốn người điều khiển Kết đạt Trong đồ án tìm hiểu nội dung sau: • Tìm hiểu khía cạnh tổng quát Robot định nghĩa, vị trí, vai trò, ứng dụng, … • Phương pháp giải toán động học, động lực học cho Robot ba bậc tự RRP, thiết kế quỹ đạo cho Robot • Nghiên cứu, tính chọn, thiết kế điều khiển RISE cho Robot RRP dựa lý thuyết ổn định Lyapunov • Tính toán, thiết kế điều khiển thích nghi so sánh kết đạt so với điều khiển RISE • Thiết lập mô hình ảnh thông qua phần mềm Matlab Simulink Hướng phát triển đề tài Bộ điều khiển thiết kế dựa lý thuyết ổn định Lyapunov, mô qua hình ảnh máy tính sử dụng phần mềm Matlab Simulink hướng phát triển đề tài là: • Nghiên cứu thiết kế điều khiển RISE nhằm nâng cao tính ổn định đồng thời kết hợp với số điều khiển khác để tăng tính thực thi chế tạo • Nghiên cứu, chế tạo Robot ba bậc tự RRP nhiều ba bậc tự ứng dụng vào thực tiễn ta mô xác máy tính SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền TÀI LIỆU THAM KHẢO Tiếng Việt Bộ Môn Kỹ Thuật Máy trường Đại học Giao thông vận tải, Bài giảng “Điều Khiển Đa Biến”, Hà Nội Nguyễn Trường Giang (2001), “Điều khiển Robot”, Hà Nội Trường Đại học Giao thông vận tải (2002), “MATLAB tìm hiểu ứng dụng giải Một Số Bài Toán Kĩ Thuật”, Hà Nội Nguyễn Thị Phương Hà (chủ biên)- Huỳnh Thái Hoàng (2005), “Lý thuyết điều khiển tự động ”, Nxb Đại Học Quốc Gia Thành Phố Hồ Chí Minh, Tp Hồ Chí Minh Nguyễn Phùng Quang (2004), “ Matlab Simulink dành cho kỹ sư điều khiển tự động”, Nxb Khoa học kỹ thuật, Hà Nội Tiếng Anh Mark W Spong, Seth Hutchinson, and M Vidyasagar, Robot Modeling and Control, 10 (1), pp.387-392 R Kelly, V Santibanez and A Loria (2005), Control of Robot Manipulators in Joint Space Web: http://data.tailieuhoctap.vn/books/cong-nghe-thong-tin/do-hoa-ungdung/file_goc_770047.pdf http://doc.edu.vn/tai-lieu/do-an-thiet-ke-tay-may-3-bac-tu-do-1297/ SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền PHỤ LỤC CODE PHUONG TRINH DONG LUC HOC ROBOT RRP clc clear all syms q1 q2 q3 qd1 qd2 qd3 qdd1 qdd2 qdd3 m1 m2 m3 l1 l2 l3 g %Ma tran bien doi thuan nhat T01= [cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 l1;0 0 1] ; T12= [cos(q2+pi/2) -sin(q2+pi/2) 0;0 -1 0;sin(q2+pi/2) cos(q2+pi/2) 0;0 0 1] ; T23= [1 0 0;0 -1 l2+q3;0 0;0 0 1] ; T34= [0 0;1 0 ;0 l3;0 0 1]; T01; T02=T01*T12; T03=T02*T23; T04=T03*T34; a1=diff(T01,q1); a2=diff(T02,q2); a3=diff(T03,q3); %Ma tran gia quan tinh Ixx1=0; Iyy1=m1*l1^2/12; Izz1=m1*l1^2/12; Ixx2=0; Iyy2=m2*l2^2/12; Izz2=m2*l2^2/12; Ixx3=0; Iyy3=m3*l3^2/12; Izz3=m3*l3^2/12; J1=[(-Ixx1+Iyy1+Izz1)/2+m1*l1^2/4 0 m1*l1/2; (Ixx1-Iyy1+Izz1)/2 0; 0 (Ixx1+Iyy1-Izz1)/2 0; m1*l1/2 0 m1]; J2=[(-Ixx2+Iyy2+Izz2)/2+m2*l2^2/4 0 m2*l2/2; (Ixx2-Iyy2+Izz2)/2 0; 0 (Ixx2+Iyy2-Izz2)/2 0; m2*l2/2 0 m2]; SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền J3=[(-Ixx3+Iyy3+Izz3)/2+m3*l3^2/4 0 m3*l3/2; (Ixx3-Iyy3+Izz3)/2 0; 0 (Ixx3+Iyy3-Izz3)/2 0; m3*l3/2 0 m3]; %Vecto gia toc truong Vg=[0;-g;0;1]; %Vecto toa khoi tam he quy chieu cua khau rRG1=[l1/2;0;0;1]; rRG2=[l2/2;0;0;1]; rRG3=[l3/2;0;0;1]; %He phuong trinh vi phan chuyen dong F1= trace(a1*J1*transpose(a1))*qdd1+trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd 1+trace(a2*J2*transpose(diff(T02,q1)))*qdd2+ trace(diff(T03,q1)*J3*transpose(diff(T03,q1)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q1)))*qdd2+trace(a3*J3*transpose(diff(T03, q1)))*qdd3+ trace(diff(a1,q1)*J1*transpose(a1))*qd1*qd1+trace(diff(diff(T02,q1),q1)*J2*transpos e(diff(T02,q1)))*qd1*qd1+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+trace(diff(a2,q2) *J2*transpose(diff(T02,q1)))*qd2*qd2+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q1)))*qd1*qd1+2*trace((diff(diff(T 03,q1),q2))*J3*transpose(diff(T03,q1)))*qd1*qd2+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q1)))*qd2*qd3+2*trace(diff(diff( T03,q1),q3)*J3*transpose(diff(T03,q1)))*qd1*qd3+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q1)))*qd2*qd2+trace(diff(a3,q3)*J3 *transpose(diff(T03,q1)))*qd3*qd3- m1*transpose(Vg)*a1*rRG1-m2*transpose(Vg)*diff(T02,q1)*rRG2m3*transpose(Vg)*diff(T03,q1)*rRG3; F2= trace(diff(T02,q1)*J2*transpose(a2))*qdd1+trace(a2*J2*transpose(a2))*qdd2+trace(di ff(T03,q1)*J3*transpose(diff(T03,q2)))*qdd1+ SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền trace(diff(T03,q2)*J3*transpose(diff(T03,q2)))*qdd2+trace(a3*J3*transpose(diff(T03, q2)))*qdd3+trace(diff(diff(T02,q1),q1)*J2*transpose(a2))*qd1*qd1+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(a2))*qd1*qd2+trace(diff(a2,q2)*J2*transp ose(a2))*qd2*qd2+trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q2)))*qd1*qd1+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q2)))*qd1*qd3+2*trace(diff(diff( T03,q1),q2)*J3*transpose(diff(T03,q2)))*qd1*qd2+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q2)))*qd2*qd3+trace(diff(diff(T0 3,q2),q2)*J3*transpose(diff(T03,q2)))*qd2*qd2+ trace(diff(a3,q3)*J3*transpose(diff(T03,q2)))*qd3*qd3-m2*transpose(Vg)*a2*rRG2m3*transpose(Vg)*diff(T03,q2)*rRG3 ; F3= trace(diff(T03,q1)*J3*transpose(a3))*qdd1+trace(diff(T03,q2)*J3*transpose(a3))*qdd 2+trace(a3*J3*transpose(a3))*qdd3+ trace(diff(diff(T03,q1),q1)*J3*transpose(a3))*qd1*qd1+trace(diff(diff(T03,q2),q2)*J3 *transpose(a3))*qd2*qd2+ trace(diff(a3,q3)*J3*transpose(a3))*qd3*qd3+2*trace(diff(diff(T03,q1),q2)*J3*transp ose(a3))*qd1*qd2+2*trace(diff(diff(T03,q1),q3)*J3*transpose(a3))*qd1*qd3+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(a3))*qd2*qd3m3*transpose(Vg)*a3*rRG3 ; %Don gian hoa he phuong trinh vi phan chuyen dong F1=simplify(F1) F2=simplify(F2) F3=simplify(F3) CODE CỦA KHỐI QUỸ ĐẠO ĐIỀU KHIỂN RISE function x0 = FCN(t) x0=[sin(0.1*t)*(1-exp(-3*t));sin(0.1*t)*(1-exp(-3*t));0.02*sin(0.1*t)*(1-exp(3*t));0.1*cos(0.1*t)+3*exp(-3*t)*sin(0.1*t)-0.05*exp(-3*t)*cos(0.1*t);0.1*cos(0.1*t) SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền +3*exp(-3*t)*sin(0.1*t)-0.1*exp(-3*t)*cos(0.1*t);0.1*0.02*cos(0.1*t)+3*0.02*exp(3*t)*sin(0.1*t)-0.1*0.02*exp(-3*t)*cos(0.1*t)]; CODE MATLAB ĐIỀU KHIỂN RISE ROBOT RRP function xdd = ROBOT(u,x) % Hang so l1 = 0.5;l2 = 0.5; l3 = 1.5 ; % chieu dai l1 l2 l3 (m) m1 = 5; m2=4; m3=6; % khoi luong m1 m2 m3 (kg) g=9.81; % gia toc truong (m/s^2) % Dat cac hang so a1= m1*l1^2/2; a2=m2*l2^2/6+m3*l3^2/6; a3=m3*l2^2/2; a4=m3*l2; b1=m3; b2=m3*l2*l3/2; b3=m3*l3/2; b4=l2*m3; c1=m1*l1*g; c2=l2*l3*m3; % Bien trang thai q1= x(1); q2= x(2); q3= x(3); qd1= x(4);qd2= x(5);qd3= x(6); xd= zeros(6,1); x2= [qd1;qd2;qd3]; % Phuong trinh vi phan x1d = x2; % Khai bao cac ma tran M(q), C(q,qd), G(q); M= [2*a1/3+2*a2+2*a2*(cos(q2))^2+b1*q3^2*(cos(q2))^2+2*a3*(cos(q2))^2b2*sin(2*q2)-b3*sin(2*q2)+2*b4*q3*(cos(q2))^2 0; 2*a2+2*a3+b1*q3^2+2*b4*q3 -b3; -b3 b1]; C= [2*a4*qd3*(cos(q2))^2+2*b1*q3*qd3*(cos(q2))^2-b3*qd3*sin(2*q2) c2*qd1+2*b3*q3*qd1+2*a2*qd1*sin(2*q2)-2*q3*qd1*sin(2*q2)b1*q3^2*qd1*sin(2*q2)+2*c2*qd1*(cos(q2))^2-4*b3*qd1*q3*(cos(q2))^22*b4*q3*qd1*sin(2*q2) 0; SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền -2*qd1-b3*q3*qd1a2*qd1*sin(2*q2)+a3*qd1*sin(2*q2)+b1*q3^2*qd1*sin(2*q2)/2+c2*qd1*(cos(q2))^ 2+2*b3*q3*qd1*(cos(q2))^2+a4*q3*qd1*sin(2*q2) 2*a4*qd2+2*b1*q3*qd2; -a4*qd1*(cos(q2))^2-b1*q3*qd1*(cos(q2))^2+b1*qd1*sin(2*q2)/4 -a4*qd2q3*b1*qd2 0]; G= [c1*cos(q1)/2+a4*g*cos(q1)*cos(q2)+b1*g*q3*cos(q1)*cos(q2)g*l2*m2*cos(q1)*sin(q2)/2-g*l3*m3*cos(q1)*sin(q2)/2; -g*l2*m2*cos(q2)*sin(q1)/2- g*l3*m3*cos(q2)*sin(q1)/2- g*b4*sin(q1)*sin(q2)b1*g*q3*sin(q1)*sin(q2); b1*g*cos(q2)*sin(q1)]; % Phuong trinh vi phan x2d= M^(-1)*(u-C*x2-G); xdd=[x1d;x2d]; CODE MATLAB BỘ ĐIỀU KHIỂN RISE function [u,e1,Kd] = BODIEUKHIEN(x,x0,K) % Hang so Ks=200;anpha1=10;anpha2=10;beta=1; e02=[0;1;0]; q1=[x(1);x(2);x(3)]; qd1=[x0(1);x0(2);x0(3)]; q2=[x(4);x(5);x(6)]; qd2=[x0(4);x0(5);x0(6)]; e1=q1-qd1; e2=q2-qd2+anpha1*e1; Kd=(Ks+1)*anpha2*e2+beta*sign(e2); u=-(Ks+1)*e2+(Ks+1)*e02-K; CODE MATLAB BỘ ĐIỀU KHIỂN THÍCH NGHI function [u,e1,thetamud]= BDK(x1,x2,qm,qmd,qmdd,thetamu) % Hang so SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền anpha1= 10 ; anpha2 = 10.0; anpha3 = 30.5 ; E=0.050; K=30.0; g=9.81; q1= x1(1); q2= x1(2); q3= x1(3); qd1= x2(1);qd2= x2(2);qd3= x2(3); qm1=qm(1); qm2=qm(2); qm3=qm(3); e11=x1(1)-qm1; e12=x1(2)-qm2; e13=x1(3)-qm3; e1=[e11;e12;e13]; qmd1=qmd(1); qmd2=qmd(2); qmd3=qmd(3); e21=x2(1)-qmd1+anpha1*e11; e22=x2(2)-qmd2+anpha2*e12; e23=x2(3)-qmd3+anpha3*e13; e2=[e21;e22;e23]; qmdd1=qmdd(1); qmdd2=qmdd(2); qmdd3=qmdd(3); M1=qmdd1+anpha1^2*e11-anpha1*e21; M2=qmdd2+anpha2^2*e12-anpha2*e22; M3=qmdd3+anpha3^2*e13-anpha3*e23; V1=e21-anpha1*e11+qmd1; V2=e22-anpha2*e12+qmd2; SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền V3=e23-anpha3*e13+qmd3; d1=M1; d2=M1+cos(q2)^2*M1+sin(q2)*cos(q2)*e11-g*cos(q1)*sin(q2)/2+2*sin(2*q2)*V2; d3=cos(q2)^2*M1+sin(q2)*cos(q2)*e11; d4=g*cos(q1)*cos(q2)+2*cos(q2)^2*qd3*V1; d5=(q3)^2*cos(q2)^2*M1+ (q3)^2*sin(q2)*cos(q2)*e11+q3*cos(q2)^2*e11+g*q3*cos(q1)*cos(q2)+2*q3*cos(q2 )^2*qd3*V1-(q3)^2*sin(2*q2)*V2; d6=-sin(2*q2)*M1+cos(2*q2)*e11+2*q3*V2; d7=-sin(2*q2)*qd3*V1-4*q3*cos(q2)^2*V2; d8=2*q3*cos(q2)^2*M1+2*q3*sin(q2)*cos(q2)*e11-cos(q2)^2*e112*q3*sin(2*q2)*V2; d9=cos(q1)/2; d10=V2+2*cos(q2)^2*V2; d11=-e11-2*q3*sin(2*q2); h1=0; h2=M2-g*sin(q1)*cos(q2)/2-sin(2*q2)*qd1*V1; h3=M2+sin(2*q2)*qd1*V1/2; h4=q3*sin(2*q2)*qd1*V1+2*qd2*V3; h5=(q3)^2*M2-g*q3*sin(q1)*sin(q2)+(q3)^2*sin(2*q2)*qd1*V1/2+2*q3*qd2*V3; h6=0; h7=-M3+2*q3*cos(q2)^2*qd1*V1-q3*qd1*V1; h8=2*q3*M2-g*sin(q1)*sin(q2); h9=0; h10=qd1*cos(q2)^2*V1; h11=-e12-2*qd1*V1; k1=0; k2=0; k3=-qd2*V2; k4=-cos(q2)^2*qd1*V1-qd2*V2; k5=M3+g*sin(q1)*cos(q2)-q3*cos(q2)^2*qd1*V1+sin(2*q2)*qd1*V1/4; SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền k6=0; k7=-M2; k8=0; k9=0; k10=0; k11=-e13; Ymut=[d1 h1 k1;d2 h2 k2;d3 h3 k3;d4 h4 k4;d5 h5 k5;d6 h6 k6;d7 h7 k7;d8 h8 k8;d9 h9 k9;d10 h10 k10;d11 h11 k11]; thetamud=-(E^(-1))*Ymut*e2; Y=[d1 d2 d3 d4 d5 d6 d7 d8 d9 d10 d11 ;h1 h2 h3 h4 h5 h6 h7 h8 h9 h10 h11 ;k1 k2 k3 k4 k5 k6 k7 k8 k9 k10 k11]; u=-K*e2+Y*thetamu; SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53 [...]... sqrt(px^2+py^2+(l1-pz)^2)-l2-l3; N4 = [q1d;q2d;q3d]; End SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K 53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền CHƯƠNG 3 :ĐỘNG LỰC HỌC ROBOT RRP 3. 1 Nhiệm vụ và phương pháp Nghiên cứu động lực học Robot là công việc cần thiết khi phân tích cũng như tổng hợp quá trình điều khiển chuyển động Việc nghiên cứu động lực học Robot giải quyết hai nhiệm vụ sau đây: Xác định moment và lực động xuất hiện trong... tốt nghiệp 1.7 GVHD: TS Đinh Thị Thanh Huyền Thiết kế mô hình Robot 3 bậc tự do trên phần mềm Topsolid Thiết kế mô hình là một nhiệm vụ rất quan trọng trước khi tính toán động học và mô phỏng Robot Việc thiết kế mô hình có ý nghĩa quan trọng, nó giúp cho nhà thiết kế có thể nghiên cứu biết được đặc điểm, kết cấu của từng khâu trên Robot cũng như toàn thể Robot Tùy theo yêu cầu làm việc mà ta xây dựng... s1.s2 c2 0 c1 c2 s1.c2 s2 0 l3 c1 c2 − (l2 + q3 ).c1 c2 l3 s1 c2 − (l2 + q3 ).s1 c2 l1 − l3 s2 + (l2 + q3 ).s2 1 (2.1) Phương trình (2.1) gọi là phương trình động học của Robot RRP Code Matlab thiết lập phương trình động học function [px, py, pz] = giaidonghocthuan(q1, q2, q3) % Hang so l1 = 0.5;l2 = 0.50; l3 = 1.50 ; % chieu dai l1 l2 l3 (cm) %Ma tran bien doi thuan nhat T01= [cos(q1) -sin(q1)... Đinh Thị Thanh Huyền Hình 1.6 Robot kiểu tọa độ Đề các Robot kiểu tọa độ trụ: Vùng làm việc của Robot có dạng trụ rỗng Thường khớp thứ nhất chuyển động quay Ví dụ robot 3 bậc tự do, cấu hình R.T.T Hình 1.7 Robot kiểu tọa độ trụ Robot kiểu tọa độ cầu: Vùng làm việc có dạng hình cầu, thường độ cứng vững của loại Robot này thấp hơn so với hai loại trên Ví dụ robot 3 bậc tự do, cấu hình R.R.R hoặc R.R.T... phương pháp nghiên cứu động lực học Robot, nhưng trong đồ án này sử dụng phương pháp cơ học Lagrange, cụ thể là phương trình Lagrange Euler Đối với các khâu khớp của Robot, với các nguồn động lực và kênh điều khiển riêng biệt, không thể bỏ qua các hiệu ứng trọng trường, quán tính, tương hỗ, ly tâm,… mà những khía cạnh này chưa được xét đến đầy đủ trong cơ học cổ điển Cơ học Lagrange nghiên cứu các vấn... 0 0 3 1 4T = 3T 4T = 0 0 0 1 3 T = 4 0 0 c1 c2 s1 c2 s2 0 0 1 c1.c2 ( l2 + q3 ) c2 s1 ( l2 + q3 ) l1 + s2 ( l2 + q3 ) 1 l3 c1 c2 − (l2 + q3 ).c1 c2 l3 s1 c2 − (l2 + q3 ).s1 c2 l1 − l3 s2 + (l2 + q3 ).s2 1 T Cân bằng các hệ số với các hệ số mô tả vị trí và hướng của bàn tay Robot, ta có hệ phương trình động học Robot RRP: nx n y nz 0 ox oy oz 0 ax... Thành – Lớp: Cơ Điện Tử K 53 Đồ án tốt nghiệp GVHD: TS Đinh Thị Thanh Huyền T 23= [1 0 0 0;0 0 -1 l2+q3;0 1 0 0;0 0 0 1] ; T34= [0 1 0 0;1 0 0 0 ;0 0 1 l3;0 0 0 1]; T01; T02= T01*T12; T 03= T02*T 23; T04= T 03* T34; px = l3*cos(q1)*sin(pi/2 + q2) - cos(q1)*sin(pi/2 + q2)*(l2 + q3); py = l3*sin(q1)*sin(pi/2 + q2) - sin(q1)*sin(pi/2 + q2)*(l2 + q3); pz = l1 + cos(pi/2 + q2)*(l2 + q3) - l3*cos(pi/2 + q2); Sau khi... việc của Robot RRP Điểm tác động cuối của cánh tay Robot sẽ làm việc trong phạm vi toàn bộ không gian bán cầu này 2.2 Động học ngược 2.2.1 Tổng quan - Giải bài toán động học ngược là giải hệ phương trình động học nhằm xác định các biến trong bộ thông số DH khi biết được vị trí và hướng của khâu chấp hành cuối của Robot - Nhiệm vụ của bài toán là xác định tập nghiệm ( θi , di ) khi đã biết vị trí và hướng... hình robot nhờ những ưu điểm như khả năng đồ họa mạnh, kết hợp với sự hiểu biết trước đó từ môn học “Thực tập chuyên ngành”, thư viện phần tử lớn thuận lợi trong thiết kế, cho phép xuất ra kết quả dưới nhiều dạng khác nhau Phần mềm Topsolid là hệ thống CAD/CAM/CAE 3D hoàn chỉnh và có thể gia công chi tiết khi muốn chế tạo Robot Vì vậy em chọn phần mềm này để thiết kế Robot Hình 1.15 : Mô hình robot RRP. .. Thị Thanh Huyền 2.1.6 Bài toán động học thuận Robot RRP Từ mô hình thiết kế trên phần mềm TopSolid, ta đơn giản hóa mô hình sau đó đặt các hệ trục tọa độ cho từng khâu như sau: Dựa vào mô hình ta xây dựng được bảng thông số DH như sau: Bảng 2.1 Bảng thông số DH của Robot 3 bậc tự do RRP Khâu i 1 2 0 90 0 0 3 4 90 0 0 0 q2 + 90° 0 0 90 Thay các giá trị từ bảng DH vào ma trận tổng quát Ta có: c1 s