Điều khiển robot chui ống dùng kỹ thuật điều khiển mờ

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP.HCM LÊ VŨ HÙNG ĐIỀU KHIỂN ROBOT CHUI ỐNG DÙNG KỸ THUẬT ĐIỀU KHIỂN MỜ LUẬN VĂN THẠC SỸ Chuyên ngành : Kỹ thuật Cơ – điện tử Mã số ngành : 60520114 TP HỒ CHÍ MINH, tháng 01 năm 2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP.HCM LÊ VŨ HÙNG ĐIỀU KHIỂN ROBOT CHUI ỐNG DÙNG KỸ THUẬT ĐIỀU KHIỂN MỜ LUẬN VĂN THẠC SĨ Chuyên ngành : Kỹ thuật Cơ – điện tử Mã số ngành : 60520114 HƯỚNG DẪN KHOA HỌC: TS NGUYỄN THANH PHƯƠNG TP HỒ CHÍ MINH, tháng 01 năm 2014 i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Các số liệu, kết nêu Luận văn trung thực chưa công bố cơng trình khác Tơi xin cam đoan giúp đỡ cho việc thực Luận văn cảm ơn thơng tin trích dẫn Luận văn rõ nguồn gốc Học viên thực Luận văn (Ký ghi rõ họ tên) Lê Vũ Hùng ii LỜI CÁM ƠN Sau thời gian học tập nghiên cứu trường, tơi hồn thành đề tài tốt nghiệp cao học Để có thành này, tơi nhận nhiều hỗ trợ giúp đỡ tận tình từ thầy cơ, gia đình bạn bè Tôi xin chân thành cảm ơn quý Thầy cô khoa Quản Lý Khoa Học - Đào Tạo Sau Đại Học, quý Thầy cô khoa Cơ - Điện - Điện Tử Trường Đại Học Kỹ Thuật Công Nghệ TP.HCM tận tình giúp đỡ, hỗ trợ tơi suốt q trình thực luận văn Với lòng tri ân sâu sắc, tơi muốn nói lời cám ơn đến Thầy TS Nguyễn Thanh Phương, người nhiệt tình hướng dẫn bảo cho suốt thời gian thực nghiên cứu Tp Hồ Chí Minh, tháng 01 năm 2014 Người thực luận văn Lê Vũ Hùng iii TÓM TẮT LUẬN VĂN Trong báo này, tìm hiểu hoạt động Pipeline robot phương pháp điều khiển mờ ứng dụng cho robot Đầu tiên khảo sát cấu trúc đường ống,từ lựa chọn phương án thiết kế tốt nhất,nghiên cứu thiết kế chế tạo điều khiển Pipeline robot Thứ hai xây dựng giải thuật điều khiển cho Pipeline robot sở phân tích chi tiết chuyển động Pipeline robot, Mô phương pháp điều khiển cho Pipeline robot phần mềm Matlab Sau hoạt động ổn định xác ứng dụng chế tạo Pipeline robot Thiết kế thi cơng mơ hình Pipeline robot,đánh giá kết đạt iv ABSTRACT In this thesis, we study about working of the Pipeline Robot and the fuzzy control method which apply for robot The first, we find out the pipe structure, from which select the best method to design and control the Pipeline robot The second, we construct the controlling algorism for the Pipeline robot which bases on analyzing the moving detail of Pipeline robot Reproduce the controlling methods for pipeline Robot on Matlab software After stable and exact working, then apply to make the Pipeline robot Design and produce the Pipeline robot model, then evaluate the getting result v MỤC LỤC Tên đề mục Trang Lời cam đoan i Lời cảm ơn ii Tóm tắt luận văn iii Abstract iv Mục lục v Danh mục từ viết tắt vii Danh mục bảng biểu viii Danh mục sơ đồ, hình ảnh ix Chương I: Tổng quan 1.1 Giới thiệu Tổng quan 1.2 Khảo sát đường ống lựa chọn phương án xây dựng mơ hình PIPELINE ROBOT 1.2.1 Những vấn đề xảy khảo sát thiết kế 1.2.2 Giải pháp thiết kế Pipeline robot 10 1.3 Mục đích nghiên cứu 13 1.4 Đối tượng nghiên cứu 13 1.5 Phạm vi nghiên cứu 14 1.6 Phương pháp nghiên cứu 14 1.7 Kế hoạch thực 14 1.8 Giá trị thực tiễn đề tài 14 1.9 Phát thảo nội dung luận văn 14 Chương II: Cơ sở Lý thuyết 2.1 Điều khiển mờ 16 2.2 Khái niệm 16 2.2.1 Định nghĩa tập mờ 16 2.2.2 Các thuật ngữ logic mờ 17 2.2.3 Biến ngôn ngữ 18 2.2.4 Các phép toán tập mờ 18 2.2.5 Luật hợp thành 18 vi 2.2.6 Giải mờ 20 2.3 Bộ điều khiển mờ 22 2.3.1 Cấu trúc điều khiển mờ 22 2.3.2 Nguyên lý điều khiển mờ 23 2.3.3 Phân loại điều khiển mờ 24 2.3.4 Thiết kế điều khiển mờ 25 2.4.Thiết kế điều khiển PID mờ 26 2.4.1 Sơ đồ điều khiển sử dụng điều khiển PID mờ : 27 2.4.2 Luật chỉnh định PID: 27 2.5 Hệ điều khiển mờ lai (F-PID) 28 2.6 Kết luận 29 Chương III: Xây dựng hệ thống điều khiển 3.1 Thiết kế điều khiển động DC PIPELINE ROBOT 31 3.1.1 Xây dựng phương trình động DC 31 3.1.2 Yêu cầu điều khiển – Thông số động 33 3.2 Thiết kế điều khiển PID kinh điển điều khiển động DC 34 3.2.1 Đặc tính điều khiển với PID 35 3.2.2 Hiệu chỉnh thông số điều khiển PID 36 3.2.5 Mơ q trình điều khiển PID kinh điển động DC 38 3.3 Thiết kế điều khiển PID mờ điều khiển động DC 38 3.3.1 Thiết kế điều khiển mờ PID mờ để chỉnh định tham số động DC 38 3.3.2 Sơ đồ điều khiển PID mờ điều khiển động DC 41 3.3.3 Mô trình điều khiển PID mờ cho động DC 42 3.4 Thiết kế PID mờ lai cho hệ thống điều khiển động DC 42 3.4.1 Thiết kế điều khiển mờ PID mờ lai điều khiển động DC 42 3.4.2 Sơ đồ điều khiển PID mờ lai điều khiển động DC 47 3.4.3 Mơ q trình điều khiển PID mờ lai cho động DC 48 Chương IV: Sơ đồ kết mô 4.1 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID kinh điển 50 vii 4.1.1 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID kinh điển 50 4.1.2 Sơ đồ kết mô hệ thống điều khiển moment động DC dùng điều khiển PID kinh điển 53 4.2 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID mờ 56 4.2.1 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID mờ 56 4.2.2 Sơ đồ kết mô hệ thống điều khiển moment động DC dùng điều khiển PID mờ 59 4.3 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID mờ lai 52 4.3.1 Sơ đồ kết mô hệ thống điều khiển tốc độ động DC dùng điều khiển PID mờ lai 52 4.3.2 Sơ đồ kết mô hệ thống điều khiển moment động DC dùng điều khiển PID mờ lai 70 Chương V: Thiết kế thi công mô hình PIPELINE ROBOT 5.1 Thiết kế mơ hình PIPELINE ROBOT 79 5.1.1 Giới thiệu sơ lược cấu tạo khí 79 5.1.2 Chọn vật liệu cho robot 80 5.1.3 Cơ chế co dãn chân robot: 81 5.2 Kích thước ROBOT 85 5.2.1 Phân tích hình học đoạn cong đường ống 86 5.2.2 Phân tích hình học nhánh 86 5.3 Cơ cấu truyền động ROBOT 88 5.4 Tính toán trục: 88 5.5 Tính tốn thiết kế nguyên lý hoạt động mạch điện sử dụng 92 5.5.1 Mạch robot 92 5.5.2 Tính tốn thiết kế: 100 5.5.3 Mơ hình thổng thể Pipeline robot 100 5.5.4 Code chương trình điều khiển 100 viii Chương VI: KẾT LUẬN Tài liệu tham khảo: 103 if (USART_get_packet(cmd,2) == 2) { USART_send_packet(cmd,2); lcd_clear(); lcd_gotoxy(0,0); lcd_putsf("Received command"); lcd_gotoxy(0,1); if (strcmp(cmd,"ST") == 0) //Stop { lcd_putsf("Stop!"); Stop(); break; } else { lcd_putsf("Not process!"); } } tmp = PINB & 0x02; //PB.1 > Limit SW max tmp1 = PINC & 0x80; //PC.7 > Cancel button if ((tmp == 0) || (tmp1 == 0)) { break; } } RELAY_ROTATE = DIR_CLOCKWISE; delay_ms(20); PWM_RotateMotor(255); //stop } } } ////////////////////////////////////////////////////////////// //Scan button ////////////////////////////////////////////////////////////// unsigned char Scan_Button(void) { unsigned char temp = NO_PRESS; unsigned char ret = NO_PRESS; DDRC = 0x00; //port C as inputs to read the button status ret = (PINC & NO_PRESS); while (ret == NO_PRESS) { ret = (PINC & NO_PRESS); delay_ms(10); } temp = ret; delay_ms(50); while (ret == temp) { ret = (PINC & NO_PRESS); delay_ms(10); } delay_ms(200); return temp; } ////////////////////////////////////////////////////////////// //Scan button with timeout ////////////////////////////////////////////////////////////// unsigned char Scan_Button_Timeout(unsigned long time_out) { unsigned char temp = NO_PRESS; unsigned char ret = NO_PRESS; unsigned long count = 0; count = time_out; DDRC = 0x00; //port B as inputs to read the button status ret = (PINC & NO_PRESS); while (ret == NO_PRESS) { ret = (PINC & NO_PRESS); if (count == 0) { return temp; } count ; delay_us(10); } temp = ret; count = time_out; delay_us(50); while (ret == temp) { ret = (PINC & NO_PRESS); if (count == 0) { return temp; } count ; delay_ms(10); } delay_ms(200); return temp; } ////////////////////////////////////////////////////////////// //get size of menu ////////////////////////////////////////////////////////////// unsigned char Get_Size_Menu(struct DefineMenu *Menu) { unsigned char i = 0; while (Menu[i].Id != 0xFF) { i++; } return (i + 1); } ////////////////////////////////////////////////////////////// //get function menu before ////////////////////////////////////////////////////////////// unsigned int Get_Function_Menu_Before(unsigned int funcSelected, struct DefineMenu *menu) { if(funcSelected = (Get_Size_Menu(menu) - 2)) { return 1; } return (funcSelected + 1); } ////////////////////////////////////////////////////////////// //display menu function ////////////////////////////////////////////////////////////// void Display_Menu_Function(void) { unsigned int FunctionMenuBefore,FunctionMenuAfter; unsigned int maxFunc; maxFunc = Get_Size_Menu(Menu) - 2; FunctionMenuBefore = Get_Function_Menu_Before(FunctionMenu, Menu); FunctionMenuAfter = Get_Function_Menu_After(FunctionMenu, Menu); lcd_clear(); lcd_gotoxy(0,0); lcd_puts(Menu[FunctionMenu].funcName); lcd_gotoxy(0,1); lcd_puts(Menu[FunctionMenuAfter].funcName); lcd_gotoxy(0,0); lcd_putsf(">"); } void Auto_Prog(void) { char cmd[3] = {0}; int i; lcd_clear(); lcd_gotoxy(0,0); lcd_putsf(" AUTO PROGRAM RECEIVE COMMAND "); while (1) { for (i = 0; i < 3; i++) { cmd[i] = 0; } if (USART_get_packet(cmd,2) == 2) { USART_send_packet(cmd,2); lcd_clear(); lcd_gotoxy(0,0); lcd_putsf("Received command"); lcd_gotoxy(0,1); if (strcmp(cmd,"GT") == 0) //Go straight { lcd_putsf("Go straight!"); Gostraight(MAX_SPEED, MAX_SPEED); } else if (strcmp(cmd,"GB") == 0) //Go back { lcd_putsf("Go back!"); Goback(MAX_SPEED, MAX_SPEED); } else if (strcmp(cmd,"MX") == 0) //Resize max { lcd_putsf("Resize max!"); Resize_Robot(RESIZE_MAX, MAX_SPEED); } else if (strcmp(cmd,"MN") == 0) //Resize { lcd_putsf("Resize min!"); Resize_Robot(RESIZE_MIN, MAX_SPEED); } else if (strcmp(cmd,"ST") == 0) //Stop { lcd_putsf("Stop!"); Stop(); } else { lcd_putsf("Unknown command!"); } } if (PINC.7 == 0) { break; } } lcd_clear(); Display_Menu_Function(); } void Resize_Up_Prog(void) { lcd_clear(); lcd_gotoxy(0,0); lcd_putsf(" RESIZE MAX PROGRAM "); Resize_Robot(RESIZE_MAX, MAX_SPEED); lcd_clear(); Display_Menu_Function(); } void Resize_Down_Prog(void) { lcd_clear(); lcd_gotoxy(0,0); lcd_putsf(" RESIZE MIN PROGRAM "); Resize_Robot(RESIZE_MIN, MAX_SPEED); lcd_clear(); Display_Menu_Function(); } ////////////////////////////////////////////////////////////// //action function ////////////////////////////////////////////////////////////// void Action_Function(unsigned char action) { switch (action) { case AUTO_PROGRAM: Auto_Prog(); break; case RESIZE_UP_PROGRAM: Resize_Up_Prog(); break; case RESIZE_DOWN_PROGRAM: Resize_Down_Prog(); break; default: break; } } ////////////////////////////////////////////////////////////// //init menu ////////////////////////////////////////////////////////////// void Init_Menu(void) { Menu = MainMenu; FunctionMenu = 1; Display_Menu_Function(); } ////////////////////////////////////////////////////////////// //Down_Menu_Inform ////////////////////////////////////////////////////////////// void Down_Menu_Inform(void) { unsigned short maxFunc; unsigned short FunctionMenuBefore, FunctionMenuAfter; maxFunc = Get_Size_Menu(Menu) - 2; if(FunctionMenu >= maxFunc) { FunctionMenu = 1; } else { FunctionMenu++; } FunctionMenuBefore = Get_Function_Menu_Before(FunctionMenu, Menu); FunctionMenuAfter = Get_Function_Menu_After(FunctionMenu, Menu); lcd_clear(); lcd_gotoxy(0,0); lcd_puts(Menu[FunctionMenu].funcName); lcd_gotoxy(0,1); lcd_puts(Menu[FunctionMenuAfter].funcName); lcd_gotoxy(0,0); lcd_putsf(">"); } ////////////////////////////////////////////////////////////// //Up_Menu_Inform ////////////////////////////////////////////////////////////// void Up_Menu_Inform(void) { unsigned short maxFunc; unsigned short FunctionMenuBefore, FunctionMenuAfter; maxFunc = Get_Size_Menu(Menu) - 2; if(FunctionMenu inputs DDRC = 0x00; PORTC = 0xFF; //PortD: //PortD.0: Input, PortD.1 PortD.7: Output DDRD = 0xFE; PORTD = 0xFF; // Timer/Counter initialization // Clock source: System Clock // Clock value: Timer Stopped // Mode: Normal top=0xFF // OC0 output: Disconnected TCCR0 = 0x00; TCNT0 = 0x00; OCR0 = 0x00; TCNT1H = 0x00; TCNT1L = 0x00; //Setup PWM for left motor and right motor TCCR1A = 0xA2; TCCR1B = 0x1B; ICR1H = 0x00; ICR1L = 0xFF; OCR1A = MIN_SPEED; //Left Motor stop OCR1B = MIN_SPEED;//Right Motor stop //Setup PWM for rotate motor ASSR = 0x00; TCCR2 = 0x6C; TCNT2 = 0xFF; OCR2 = MIN_SPEED;//Rotate motor stop // External Interrupt(s) initialization // INT0: Off // INT1: Off // INT2: Off MCUCR = 0x00; MCUCSR = 0x00; // Timer(s)/Counter(s) Interrupt(s) initialization TIMSK = 0x00; // USART initialization // Communication Parameters: Data, Stop, No Parity // USART Receiver: On // USART Transmitter: On // USART Mode: Asynchronous // USART Baud Rate: 9600 UCSRA=0x00; UCSRB=0xD8; UCSRC=0x86; UBRRH=0x00; UBRRL=0x33; // Analog Comparator initialization // Analog Comparator: Off // Analog Comparator Input Capture by Timer/Counter 1: Off ACSR=0x80; SFIOR=0x00; //Turn off Watchdog Timer WDTCR = 0x00; // Alphanumeric LCD initialization // RS - PORTA.7 // RD - PORTA.6 // EN - PORTA.5 // D4 - PORTA.3 // D5 - PORTA.2 // D6 - PORTA.1 // D7 - PORTA.0 // Characters/line: 16 lcd_init(16); // Global enable interrupts #asm("sei") printf("\r\nTunnel Robot Project"); lcd_clear(); lcd_gotoxy(0, 0); lcd_putsf("- TUNNEL ROBOT -"); lcd_gotoxy(0, 1); lcd_putsf(" VERSION 1.0.0 "); delay_ms(1000); //Init menu lcd_clear(); Init_Menu(); //Display main menu Display_Menu_Function(); while (1) { button = NO_PRESS; button = Scan_Button_Timeout(5000); Process_Button(button); } } ... đồ điều khiển PID mờ điều khiển tốc độ động DC 55 Hình 4.4 Sơ đồ điều khiển PID mờ điều khiển moment động DC 58 Hình 4.5 Sơ đồ hệ thống điều khiển tốc độ động DC dùng điều khiển PID mờ. .. thống điều khiển tốc độ động DC dùng điều khiển PID mờ 56 4.2.2 Sơ đồ kết mô hệ thống điều khiển moment động DC dùng điều khiển PID mờ 59 4.3 Sơ đồ kết mô hệ thống điều khiển. .. MRC Robot điều khiển tự động SISO Điều khiển mờ "Một vào - ra" MISO Điều khiển mờ "Nhiều vào - ra" MIMO Điều khiển mờ "Nhiều vào - nhiều ra" PID Robot kiểm tra FLC Fuzzy Logic BĐK Bộ điều khiển