(Đồ án hcmute) thiết kế và điều khiển thực tế ảo robot 6 bậc tự do

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ THIẾT KẾ VÀ ĐIỀU KHIỂN THỰC TẾ ẢO ROBOT BẬC TỰ DO GVHD: ĐẶNG XUÂN BA SVTH: TRẦN LƯU PHÚC HOÀ MSSV: 16151027 SVTH: NGUYỄN HẢI PHONG MSSV: 16151061 SKL 0 3 Tp Hồ Chí Minh, tháng 07/2020 an TRƯỜNG ĐẠI HỌC SƯ PHẠM KĨ THUẬT TPHCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN THỰC TẾ ẢO ROBOT BẬC TỰ DO SVTH: TRẦN LƯU PHÚC HOÀ MSSV: 16151027 NGUYỄN HẢI PHONG MSSV: 16151061 NGÀNH: CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ GVHD: TS ĐẶNG XUÂN BA Tp Hồ Chí Minh, tháng năm 2020 an TRƯỜNG ĐẠI HỌC SƯ PHẠM KĨ THUẬT TPHCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN THỰC TẾ ẢO ROBOT BẬC TỰ DO SV TH: TRẦN LƯU PHÚC HOÀ MSSV: 16151027 NGUYỄN HẢI PHONG MSSV: 16151061 NGÀNH: CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ GVHD: TS ĐẶNG XUÂN BA Tp Hồ Chí Minh, tháng năm 2020 an CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc *** -Tp Hồ Chí Minh, ngày 27 tháng năm 2020 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Trần Lưu Phúc Hoà MSSV: 16151027 Họ tên sinh viên 2: Nguyễn Hải Phong MSSV: 16151061 Ngành: CNKT Điều khiển tự động hóa Lớp: 16151CL1 Giảng viên hướng dẫn: TS Đặng Xuân Ba ĐT: 0945 853 990 Ngày nhận đề tài: 24 / 02 / 2020 Ngày nộp đề tài: 27 / / 2020 Tên đề tài: Thiết kế điều khiển thực tế ảo robot bậc tự Các số liệu, tài liệu ban đầu: Nội dung thực đề tài: Tính tốn thiết kế phần cứng mơ hình robot bậc tự Tìm hiểu giải thuật lập trình cho mơ hình robot bậc tự Tìm hiểu giải thuật thiết kế IMU theo dõi chuyển động cánh tay người Áp dụng giải thuật điều khiển xử lí hệ thống IMU vào mơ hình thực Sản phẩm: Mơ hình robot bậc tự Chương trình điều khiển Quyển báo cáo đồ án tốt nghiệp Đĩa CD TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc ******** PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Họ tên sinh viên 1: Trần Lưu Phúc Hoà MSSV: 16151027 Họ tên sinh viên 2: Nguyễn Hải Phong MSSV: 16151061 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế điều khiển thực tế ảo robot bậc tự Họ tên Giáo viên hướng dẫn: TS Đặng Xuân Ba NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Để thực đề tài này, cần khối lượng công việc lớn chiều sâu rộng, bao gồm thiết kế chế tạo, tính tốn điều khiển robot bậc tự kết nối với hệ thống cảm nhận chuyển động thông minh Ưu điểm: Nhóm ln làm việc cách nghiêm túc chăm để hoàn thành mục tiêu đề ban đầu đề tài Về bản, nhóm giải vấn đề lớn đề tài tính tốn, thiết kế điều khiển giám sát robot bậc tự do, đo lường mô chuyển động tay người sử dụng hệ cấu cảm biến thông minh Khuyết điểm: Do thời gian thực đề tài ngắn khối lượng công việc lớn, nên tồn vài vấn đề nhỏ mà nhóm chưa kịp giải Đề nghị cho bảo vệ hay không? Được bảo vệ Đánh giá loại: Xuất sắc Điểm: ……………… (Bằng chữ: … ) Tp Hồ Chí Minh, ngày 27 tháng năm 2020 Giáo viên hướng dẫn (Ký & ghi rõ họ tên) an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc ******** PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Họ tên sinh viên 1: Trần Lưu Phúc Hoà MSSV: 16151027 Họ tên sinh viên 2: Nguyễn Hải Phong MSSV: 16151061 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế điều khiển thực tế ảo robot bậc tự Họ tên Giáo viên phản biện: NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: Đề nghị cho bảo vệ hay không? Đánh giá loại: Điểm:……………….(Bằng chữ: ) Tp Hồ Chí Minh, ngày 27 tháng năm 2020 Giáo viên phản biện (Ký & ghi rõ họ tên) an LỜI CẢM ƠN Chúng em xin chân thành cảm ơn quý thầy, cô Trường Đại Học Sư Phạm Kỹ Thuật TP.HCM tận tình dạy dỗ chúng em suốt năm qua Trong phải kể đến quý thầy cô môn Tự động Điều khiển nói riêng Khoa đào tạo Chất lượng cao nói chung truyền đạt kiến thức, kinh nghiệm với đam mê khơi dậy lên lửa đam mê khám phá chúng em từ kiến thức, đam mê chúng em vận dụng vào đồ án tốt nghiệp, bàn đạp để bước vào cánh cửa lớn Đặc biệt, nhóm xin chân thành cảm ơn thầy Đặng Xuân Ba tận tình giúp đỡ chúng em trình lựa chọn đề tài hỗ trợ chúng em trình thực Nhờ có đóng góp, ý kiến thầy mà chúng em vượt qua khó khăn, bổ sung kiến thức cịn thiếu Ngồi việc hỗ trợ mặt kĩ thuật, thầy cịn đơn đốc, khuyến khích chúng em để chúng em vững bước hồn thành tốt khố luận Chúng cảm ơn bạn lớp 16151CL1 anh em lab “Dynamics and Robotics Control” ln sát cánh có góp ý để phát triển đề tài Cuối cùng, chúng xin dành hội để chân thành cảm ơn ba mẹ giúp đỡ, động viên suốt q trình học tập hồn thành khố luận TP.HCM, ngày 25 tháng năm 2020 Sinh viên thực đồ án an TÓM TẮT ĐỀ TÀI Trong luận án này, mơ hình cánh tay robot sáu bậc tự điều khiển hệ thống cảm biến đo lường qn tính (IMU) qua chuẩn truyền thơng không dây Bluetooth đề xuất Đồ án bao gồm ba phần: Xây dựng mơ hình cánh tay robot sáu bậc tự hoạt động ổn định; xây dựng điều khiển truyền thông qua Bluetooth; xây dựng hệ thống cảm biến đo lường quán tính đặt lên cánh tay người để điều khiển robot Mô hình cánh tay robot sáu bậc tự chế tạo theo dạng cánh tay robot công nghiệp kiểu cánh tay người, điều khiển bo mạch Arduino Mega Arduino Uno R3 Để đạt độ ổn định xác hoạt động, giải thuật động học robot thiết kế Giải thuật động học bao gồm hai phần: phần động học thuận tính tốn dựa việc xác định góc toạ độ khớp, thành lập bảng Denavit-Hartenberg tính tốn ma trận biến đổi đồng nhất; phần động học nghịch tính dựa biến đổi ma trận biến đổi đồng động học thuận vận dụng Góc Euler để xác định ma trận xoay điểm đầu cuối Tồn thuật tốn lập trình vào bo mạch Arduino Mega điều khiển sáu động vận hành robot, sau mơ hình quy hoạch quỹ đạo sử dụng để điều khiển động cách ổn định Hệ thống cảm biến đo lường quán tính (IMU) thiết kế để ước tính góc khớp vai khuỷu tay cách sử dụng bốn bo mạch cảm biến đo lường quán tính gắn lên ngực, cẳng tay, tay ngón tay Sau sử dụng phương pháp Euler phát triển Leonhard Euler để xác định hướng khớp quay không gian ba chiều, nhằm xác định vị trí bàn tay so với trái đất góc xoay bàn tay Dữ liệu vị trí góc xoay bàn tay truyền bo mạch điều khiển robot thông qua giao thức truyền thông Bluetooth Thơng thường để điều khiển mơ hình cánh tay robot vật thể xác định tay người, cần sử dụng phương pháp xử lí ảnh tốn nhiều tài nguyên cần điều khiển đủ mạnh để xử lí ví dụ máy tính nhúng Raspberry Pi Nhưng luận án đề xuất phương pháp điều khiển với độ xác tương đối tốn tài nguyên an MỤC LỤC NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP iii PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN iv PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN v LỜI CẢM ƠN vi TÓM TẮT ĐỀ TÀI vii MỤC LỤC viii DANH MỤC CÁC TỪ VIẾT TẮT xiii DANH MỤC CÁC BẢNG BIỂU xiv DANH MỤC CÁC HÌNH ẢNH, BIỂU ĐỒ xv CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI 1.1 Đặt Vấn Đề 1.2 Mục Tiêu Đề Tài 1.3 Nội Dung Nghiên Cứu 1.4 Giới Hạn 1.5 Bố Cục CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 Tổng Quan Về Thực Tế Ảo 2.1.1 Khái Niệm 2.1.2 Ứng Dụng Của Thực Tế Ảo 2.2 Tổng Quan Về Robot 2.2.1 Lịch Sử Phát Triển Robot 2.2.2 Các Khái Niệm Về Robot 2.2.2.1 Khái Niệm 2.2.2.2 Bậc tự Robot 2.2.2.3 Hệ tọa độ 2.2.2.4 Vùng Làm Việc Của Robot an 2.2.3 Cấu tạo Robot 2.2.3.1 Cấu tạo chung 2.2.3.2 Cơ cấu tay máy (Manipulator) 10 2.2.4 Phân Loại Robot 12 2.2.4.1 Phân Loại Theo Vùng Hoạt Động, Kết Cấu Tay Máy 12 2.2.4.2 Phân Loại Theo Mơ Hình Điều Khiển 12 2.2.4.2 Phân Loại Theo Phạm Vi Ứng Dụng 13 2.2.5 Động Học Tay Máy 13 2.2.5.1 Hệ Tọa Độ Thuần Nhất 13 2.2.5.2 Các Phép Biến Đổi 14 2.2.5.2.1 Phép Biến Đổi Tịnh Tiến (Translation) 14 2.2.5.2.2 Phép Quay (Rotation) 14 2.2.5.3 Ma Trận Chuyển Đổi Đồng Nhất 15 2.2.5.4 Quy Tắc Denavid-Hartenberg 15 2.2.5.5 Bài toán động học thuận 16 2.2.5.6 Bài Toán Động Học Nghịch 16 2.2.6 Thiết Kế Quỹ Đạo Chuyển Động Cho Robot 17 2.2.6.1 Giới Thiệu Và Cơ Sở Thiết Kế Quỹ Đạo 17 2.2.6.2 Thiết Kế Quỹ Đạo Chuyển Động Trong Không Gian Khớp 17 2.2.6.3 Thiết Kế Quỹ Đạo Trong Không Gian Làm Việc 18 2.3 Tổng Quan Về Cảm Biến Đo Lường Quán Tính (IMU) 20 2.3.1 Cảm Biến Đo Lường Quán Tính 20 2.3.2 Phương Pháp Xác Định Góc Nghiêng Trong Khơng Gian 21 2.3.2.1 Phương pháp sử dụng góc Euler 21 2.3.2.2 Phương pháp sử dụng Quaternion 23 2.3.3 Cảm Biến MPU 9250 24 2.3.3.1 Gia Tốc Kế 24 2.3.3.2.Xử Lý Tín Hiệu Gia Tốc Từ Gia Tốc Kế 24 2.3.3.3 Con Quay Hồi Chuyển 25 an } void quyhoach() { if (mode == 0) { int_t++; vtmm1_old = vtmm1_new; vtmm2_old = vtmm2_new; vtmm3_old = vtmm3_new; /*********tinh toan qui hoach qui dao va dong toc do*****************/ if (int_t (time / 10)) { Timer5.stop(); } } else if (mode == 1) { } } /******************dieu khien dong co buoc*****************************/ void stepmotor1() { if (vtmm1_new - vtmm1_old > 0) { digitalWrite(dirPin1, LOW); if (x1 < vtmm1_new - vtmm1_old) { digitalWrite(stepPin1, !digitalRead(stepPin1)); x1++; vt1++; } Trang | 78 an else Timer1.stop(); } else { digitalWrite(dirPin1, HIGH); if (x1 < abs(vtmm1_new - vtmm1_old)) { digitalWrite(stepPin1, !digitalRead(stepPin1)); x1++; vt1 ; } else Timer1.stop(); } } void stepmotor2() { if (vtmm2_new - vtmm2_old > 0) { digitalWrite(dirPin2, LOW); if (x2 < vtmm2_new - vtmm2_old) { digitalWrite(stepPin2, !digitalRead(stepPin2)); x2++; vt2++; } else Timer3.stop(); } Trang | 79 an else { digitalWrite(dirPin2, HIGH); if (x2 < abs(vtmm2_new - vtmm2_old)) { digitalWrite(stepPin2, !digitalRead(stepPin2)); x2++; vt2 ; } else Timer3.stop(); } } void stepmotor3() { if (vtmm3_new - vtmm3_old > 0) { digitalWrite(dirPin3, HIGH); if (x3 < vtmm3_new - vtmm3_old) { digitalWrite(stepPin3, !digitalRead(stepPin3)); x3++; vt3++; } else Timer4.stop(); } else { Trang | 80 an digitalWrite(dirPin3, LOW); if (x3 < abs(vtmm3_new - vtmm3_old)) { digitalWrite(stepPin3, !digitalRead(stepPin3)); x3++; vt3 ; } else Timer4.stop(); } } /*********************tinh toan du lieu thi***************************/ void chart_calculator() { a1 = (vt2 * 180) / ((3969 / 289) * * 200 * 2) * 2; a2 = (vt3 * 180) / ((3969 / 289) * * 200 * 2) * 2; a3 = (vt1 * 180) / ((3969 / 289) * * 200 * 2) * 2; value = ""; value += '*'; value += a1; value += '*'; value += a2; value += '*'; value += a3; value += '*'; value += a4; value += '*'; value += a5; value += '*'; value += a6; Trang | 81 an value += '*'; } /***********************void main**************************************/ void loop() { /********************guide*****************************************/ if (Serial.available() && Serial.find("{")) { buf_s = String("{") + Serial.readString(); StaticJsonBuffer jsonBuffer; JsonObject&object = jsonBuffer.parseObject(buf_s); Serial1.println(buf_s); y1 = object.get("dt1"); y2 = object.get("dt2"); y3 = object.get("dt3"); mode = object.get("dt8"); time = object.get("dt7"); chay(y1, y2, y3); } /*****************Bluetooth**************************************/ if (Serial2.available() && Serial2.find("{")) { int y1_o = y1; int y2_o = y2; int y3_o = y3; buf_s = String("{") + Serial2.readString(); StaticJsonBuffer jsonBuffer; JsonObject&object = jsonBuffer.parseObject(buf_s); Serial1.println(buf_s); Trang | 82 an Serial.println(buf_s); y1 = object.get("a"); y2 = object.get("b"); y3 = object.get("c"); y4 = object.get("d"); y5 = object.get("e"); y6 = object.get("f"); time = 400; chay(y3, y1, y2); mode = 1; } Send(); } void Send() { chart_calculator(); Serial.println(value); } void chay(float temp1, float temp2, float temp3){ z1 = ((temp1 * (3969 / 289) * * 200 * 2) / 360) * 2; z2 = ((temp2 * (3969 / 289) * * 200 * 2) / 360) * 2; z3 = ((temp3 * (3969 / 289) * * 200 * 2) / 360) * 2; vtmm1 = z1 - vt1; vtmm2 = z2 - vt2; vtmm3 = z3 - vt3; x1 = 0; x2 = 0; x3 = 0; the1 = vtmm1; the2 = vtmm2; the3 = vtmm3; Trang | 83 an if (mode == 0) { vtmm1_old = 0; vtmm2_old = 0; vtmm3_old = 0; vtmm1_new = 0; vtmm2_new = 0; vtmm3_new = 0; a_1 = vtmm1_old; b_1 = 0; c_1 = * (the1 - vtmm1_old) / pow(time, 2); d_1 = -2 * (the1 - vtmm1_old) / pow(time, 3); a_2 = vtmm2_old; b_2 = 0; c_2 = * (the2 - vtmm2_old) / pow(time, 2); d_2 = -2 * (the2 - vtmm2_old) / pow(time, 3); a_3 = vtmm3_old; b_3 = 0; c_3 = * (the3 - vtmm3_old) / pow(time, 2); d_3 = -2 * (the3 - vtmm3_old) / pow(time, 3); Timer5.start(); } else { vtmm1_old = vt1; vtmm2_old = vt2; vtmm3_old = vt3; vtmm1_new = vtmm1; vtmm2_new = vtmm2; vtmm3_new = vtmm3; tinhtocdo(); } int_t = 0; } void tinhtocdo() { max_t = max(abs(vtmm1 - vt1), max(abs(vtmm2 - vt2), abs(vtmm3 - vt3))); int_t1 = (max_t * time) / (abs(vtmm1 - vt1) + 1); int_t2 = (max_t * time) / (abs(vtmm2 - vt2) + 1); Trang | 84 an int_t3 = (max_t * time) / (abs(vtmm3 - vt3) + 1); } if ((int_t1 < 400) && (int_t2 < 400) && (int_t3 < 400)){ int_t1 = int_t2 = int_t3 = 400; Timer1.initialize(int_t1); Timer1.attachInterrupt(stepmotor1); Timer3.initialize(int_t2); Timer3.attachInterrupt(stepmotor2); Timer4.initialize(int_t3); Timer4.attachInterrupt(stepmotor3); Timer1.start(); Timer3.start(); Timer4.start(); } Code cho Arduino Uno R3 #include #include SoftwareSerial condiSerial(2, 3); #define servol1 #define servol2 10 #define servol3 11 int temp1,temp2,temp3,num4,num5,num6; int vtmm4, vtmm5, vtmm6; String buf_s; /*************3 motor Servo**********************************/ void servo1_on(){ if(vtmm4 >0){ if(temp1 = num4){ float goc4 = (temp1 * 9.44)+600; digitalWrite(servol1, HIGH); delayMicroseconds(goc4); digitalWrite(servol1, LOW); delayMicroseconds(20000-goc4); temp1 ; } else temp1 = num4; } if(vtmm4 == 0){ float goc4 = (temp1 * 9.44)+600; digitalWrite(servol1, HIGH); delayMicroseconds(goc4); digitalWrite(servol1, LOW); delayMicroseconds(20000-goc4); } } void servo2_on(){ if(vtmm5>0){ if(temp2 ){ Trang | 87 an if(temp3 = num6){ float goc6 = (temp3 * 9.44)+600; digitalWrite(servol3, HIGH); delayMicroseconds(goc6); digitalWrite(servol3, LOW); delayMicroseconds(20000-goc6); temp3 ; } else temp3 = num6; } if(vtmm6 == 0){ float goc6 = (temp3 * 9.44)+600; digitalWrite(servol3, HIGH); delayMicroseconds(goc6); digitalWrite(servol3, LOW); delayMicroseconds(20000-goc6); } } /***************************************************************/ Trang | 88 an void setup() { delay(1000); condiSerial.begin(115200); Serial.begin(9600); pinMode(servol1,OUTPUT); pinMode(servol2,OUTPUT); pinMode(servol3,OUTPUT); temp1 = temp2 = temp3 = 0; num4 = num5 = num6 = 0; vtmm4 = vtmm5 = vtmm6 = 90; } void loop() { /************truyen thong Bluetooth***********************/ if(Serial.available() && Serial.find("{")){ buf_s=String("{")+ condiSerial.readString(); StaticJsonBuffer jsonBuffer; JsonObject&object = jsonBuffer.parseObject(buf_s); num4 = object.get("d"); num5 = object.get("e"); num6 = object.get("f"); Serial.print("the4 the5 the6: "); Serial.print(num4); Serial.print(" "); Serial.print(num5); Serial.print(" "); Serial.println(num6); vtmm4 = num4 - temp1; vtmm5 = num5 - temp2; vtmm6 = num6 - temp3; } Trang | 89 an servo1_on(); servo2_on(); servo3_on(); chart_calculator(); } /******************thu thap du lieu*********************/ void chart_calculator() { String data_send =""; data_send += temp1; data_send += "|"; data_send += temp2; data_send += "|"; data_send += temp3; Serial.println(data_send); } Trang | 90 an S an K L 0 ... CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN THỰC TẾ ẢO ROBOT BẬC TỰ DO SVTH: TRẦN LƯU PHÚC HOÀ MSSV: 161 51027 NGUYỄN HẢI PHONG MSSV: 161 51 061 NGÀNH: CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HỐ GVHD:... tài: Thiết kế điều khiển thực tế ảo robot bậc tự Các số liệu, tài liệu ban đầu: Nội dung thực đề tài: Tính tốn thiết kế phần cứng mơ hình robot bậc tự Tìm hiểu giải thuật lập trình cho mơ hình robot. .. Trần Lưu Phúc Hoà MSSV: 161 51027 Họ tên sinh viên 2: Nguyễn Hải Phong MSSV: 161 51 061 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế điều khiển thực tế ảo robot bậc tự Họ tên Giáo viên hướng