Xây dựng và điều khiển robot dạng người

LỜI CAM ĐOAN Bằng kiến thức học đƣợc giúp đỡ tận tình giáo viên hƣớng dẫn PGS TS Nguyễn Phạm Thục Anh giảng viên Viện Điện, Đại học Bách Khoa Hà Nội Tôi cố gắng để hoàn thành thời hạn mục tiêu đặt Tuy nhiên, kiến thức hạn hẹp, việc tìm tài liệu khó khăn thời gian có hạn nên chắn có sai sót Tôi mong thầy, cô Hội đồng đánh giá luận văn thạc sỹ góp ý để hoàn thiện tốt đề tài Tôi xin cam kết luận văn thực hiện, kết nghiên cứu đƣợc đánh giá cách khách quan xác Tôi xin chịu trách nhiệm trƣớc Hội đồng nhƣ có vấn đề quyền tác giả Tôi xin chân thành cảm ơn! Hà nội, ngày 15 tháng năm 2016 Học viên Lƣu Tuấn Khanh MỤC LỤC LỜI CAM ĐOAN MỤC LỤC DANH MỤC CÁC KÝ HIỆU VÀ CÁC CHỮ VIẾT TẮT DANH MỤC CÁC BẢNG DANH MỤC HÌNH ẢNH MỞ ĐẦU CHƢƠNG TÌM HIỂU TỔNG QUAN VỀ ROBOT DÁNG NGƢỜI VÀ CÁC PHƢƠNG PHÁP ĐIỀU KHIỂN 14 1.1 TÌM HIỂU TỔNG QUAN VỀ ROBOT DÁNG NGƢỜI 14 1.1.1 Robot Asimo[1] 14 1.1.2 Robot Atlas[8] 22 1.1.3 Robot Qrio 25 1.1.4 Đánh giá .28 1.2 CÁC PHƢƠNG PHÁP ĐIỀU KHIỂN .29 1.2.1 Mô hình động học 29 1.2.2 Mô hình động lực học 31 1.2.3 Mô hình động học hai chân không khí .35 1.2.4 Điều kiện để robot bước vững chắc, ổn định [15] .37 CHƢƠNG THIẾT KẾ ROBOT DẠNG NGƢỜI 41 2.1 KẾT CẤU CƠ KHÍ 41 2.2 CHỌN VÀ ĐIỀU KHIỂN HỆ THỐNG TRUYỀN ĐỘNG 42 2.2.1 Chọn hệ thống truyền động 42 2.2.2 Điều khiển động RC servo .46 2.3 HỆ THỐNG CẢM BIẾN .49 2.4 BỘ ĐIỀU KHIỂN 52 2.4.1 Vi điều khiển 52 2.4.2 Thiết kế điều khiển 54 CHƢƠNG ĐIỀU KHIỂN ROBOT DẠNG NGƢỜI 58 3.1 ĐIỀU KHIỂN CÂN BẰNG 58 3.1.1 Cơ cấu cân .58 3.2 ĐIỀU KHIỂN ROBOT ĐI .60 3.2.1 (Bước 1) Robot lấy thăng 60 3.2.2 (Bước 2) Robot nhấc chân phải lên 60 3.2.3 (Bước 3) Đưa chân phía trước hạ chân xuống 61 3.2.4 (Bước 4) Robot nghiêng người sang phải 61 3.2.5 (Bước 5) Nhấc chân trái lên 62 3.2.6 (Bước 6) Đưa chân trái phía trước hạ xuống .63 3.2.7 (Bước 7) Nghiêng người sang trái (lặp lại bước 1) 63 CHƢƠNG CHẠY THÍ NGHIỆM 64 4.1 CÁC THÔNG SỐ .64 4.2 HÌNH ẢNH VỀ ROBOT 65 4.3 ĐÁNH GIÁ 66 CHƢƠNG KẾT LUẬN VÀ KIẾN NGHỊ 67 5.1 KẾT LUẬN 67 5.2 KIẾN NGHỊ 67 PHỤ LỤC 68 DANH MỤC CÁC KÝ HIỆU VÀ CÁC CHỮ VIẾT TẮT ZMP( Zero Moment Point) Điều khiển tiêu điểm ZMP DOF (Degree Of Freedom) Bậc tự DANH MỤC CÁC BẢNG BIỂU Bảng 1.1 thông số robot Asimo .21 Hình 1.12 Hệ thống động lực Atlas .22 Hình 1.13 Cánh tay robot Atlas .23 Hình 1.15 Khả Atlas .24 Bảng 1.2 thông số robot Qrio 26 Bảng 1.3 Vị trí robot 29 Bảng 1.4 Các thông số mô hình 30 Bảng 2.1 Ƣu, nhƣợc điểm hệ truyền động 43 Bảng 2.2 Thông số vi điều khiển dsPic30f4013 .53 DANH MỤC HÌNH ẢNH Hình Robot Elektro kết cấu bên 10 Hình Robot Asimo Nhật Bản 10 Hình Robot Hubo Hàn Quốc .11 Hình Robot Atlas hệ Mỹ di chuyển nhiều địa hình 12 Hình 1.1 Quá trình phát triển robot Asimo từ E0 đến New Asimo .14 Hình 1.2 Thế hệ robot E0 bậc tự 15 Hình 1.3 Trọng tâm robot dịch chuyển Robot di chuyển chậm nhanh .16 Hình 1.4 Thế hệ robot E1, E2, E3 16 Hình 1.5 Thế hệ robot E4, E5, E6 17 Hình 1.6 Các công nghệ điều khiển robot 18 Hình 1.7 Thế hệ robot P1, P2, P3 18 Hình 1.8 Kết hợp chuyển động chân tay 19 Hình 1.9 Robot Asimo 19 Hình 1.10 Robot Asimo sống ngƣời 20 Hình 1.11 Robot Atlas Boston Dynamics (Mỹ) 22 Hình 1.14 Mô máy tính robot Atlas hàn ống thép .24 Hình 1.16 Robot Qrio 25 Hình 1.17 Kết cấu bên chân robot Qrio 27 Hình 1.18 Robot Qrio lĩnh vực giải trí .27 Hình 1.19 Cấu hình phần chân robot dạng ngƣời 28 Hình 1.20 Mô hình robot chân link mặt phẳng chiều 29 Hình 1.21 Mô hình robot với chân không khí 31 Hình 1.22 Mô hình robot với hai chân không khí 35 Hình 1.23 Tiêu điểm ZMP trùng với 38 tâm điểm phản phản lực sàn 38 Hình 1.24 Tiêu điểm ZMP không trùng với tâm điểm phản lực sàn 38 Hình 1.25 Tiêu điểm ZMP trùng với tâm điểm phản lực sàn mặt sàn không phẳng 39 Hình 1.26 Tâm điểm phản lực sàn .40 Hình 1.27 Hiệu chỉnh vị trí tạo dáng đứng chân 40 Hình 2.1 Thiết kế robot dạng ngƣời 41 Hình 2.2 Hình dáng thông số robot 42 Hình 2.3 Động RC servo loại mini .44 Hình 2.4 Động RC servo loại standard 45 Hình 2.5 Động RC servo loại large 45 Hình 2.6 Động RC servo PowerHD 1501 MG 46 Hình 2.7 Cấu tạo bên động RC servo 47 Hình 2.8 Giản đồ xung kích Servo 48 Hình 2.9 Cảm biến GY521-MPU6050, ADXL345, L3G4200D 50 Hình 2.10 Bộ lọc Kalman có gắn cảm biến MPU6050 .51 Hình 2.11 Sơ đồ kết nối lọc Kalman với vi điều khiển 51 Hình 2.12 Vi điều khiển dsPic30f4013 dạng chip dán (bên trái) chip dán (bên phải) 52 Hình 2.13 Sơ đồ chân/cổng dsPic30f4013 53 Hình 2.14 Sơ đồ kết nối điều khiển 54 Hình 2.15 Kết nối không dây phía robot 55 Hình 2.16 Kết nối không dây phía máy tính .55 Hình 2.17 Sơ đồ nguyên lý mạch điều khiển 56 Hình 2.18 Mạch điều khiển .56 Hình 2.19 SBEC-20A 57 Hình 2.20 Nguồn điện cung cấp cho robot .57 Hình 3.1 Trọng tâm qua chân robot .58 Hình 3.2 Cơ cấu cân đối trọng 59 Hình 3.3 Trọng tâm dời sang trái phải robot 59 Hình 3.4 Robot lấy thăng 60 Hình 3.5 Robot nhấc chân phải 60 Hình 3.6 Robot đƣa chân phía trƣớc hạ chân xuống .61 Hình 3.7 Robot nghiêng ngƣời sang phải 62 Hình 3.8 Robot nhấc chân trái lên .62 Hình 3.9 Robot đƣa hạ chân trái xuống 63 Hình 3.10 Robot quay trở bƣớc 63 Hình 4.1 Phạm vi làm việc khớp 64 Hình 4.2 Giao diện phần mềm máy tính 65 Hình 4.3 Bộ điều khiển mặt trƣớc robot .65 Hình 4.4 Hình ảnh robot đứng ngồi ghế .66 Mở đầu MỞ ĐẦU  Lý lựa chọn đề tài Robot dạng ngƣời (Humanoid Robots) thuật ngữ để loại robot có hình dáng nhƣ ngƣời, có phƣơng thức di chuyển nhƣ ngƣời nghĩa hai chân Lĩnh vực nghiên cứu Robot dáng ngƣời cuối thập niên 1930 Đến nay, nhà khoa học khắp giới cho đời nhiều mẫu robot dáng ngƣời với chức ấn tƣợng nhƣ lại, leo cầu thang, vƣợt địa hình giao tiếp với ngƣời Nghiên cữu kỹ hơn, ta thấy việc tiến hóa loài ngƣời từ chi sang chi ƣu việt mà nhà khoa học cần phải nghiên cứu trình tìm mô hình robot di chuyển động nhất, thân thiện Vì vậy, đề tài “Xây dựng điều khiển Robot dạng người” đƣợc lựa chọn để tìm mô hình robot tự di chuyển hai chân nhƣ ngƣời  Lịch sử nghiên cứu Thuật ngữ Robot đƣợc nhắc đến lần kịch nhà văn Karel Chapek, Tiệp Khắc (cũ) năm 1921 Trong nhà văn sáng tạo vật thể hoạt động, biết suy nghĩ làm việc nhƣ ngƣời Chapek gọi vật thể Robota (trong tiếng Tiệp Khắc) tiếng Anh nghĩa ROBOT Thuật ngữ Humanoid Robots (Robot dạng người) để robot có hình dạng nhƣ ngƣời, di chuyển tƣơng tự cách di chuyển ngƣời, nghĩa hai chân Lĩnh vực nghiên cứu Humanoid robots đƣợc bắt đầu sớm từ cuối thập niên 1930, điển hình Humanoid Robots “Elektro” Elektro đƣợc phát minh tổng công ty điện Westinghouse (Westinghouse Electric Corporation) đƣợc giới thiệu vào năm 1939 Hội chợ toàn cầu New York (Mỹ) Tại lần ngƣời ta trông thấy robot dạng ngƣời cao 2.1 (m) nặng 120 (kg) di chuyển, hút thuốc, thổi bóng bay nói tới 700 từ Mở đầu Hình Robot Elektro kết cấu bên Châu Á đầu Nhật Bản bắt đầu nghiên cứu Robot dạng ngƣời từ năm 1980 mà đỉnh cao Robot Asimo (Advanced Step in Innovative MObility) đƣợc giới thiệu vào năm 2000 tập đoàn Honda lừng danh Asimo cao 130 (cm) nặng 54 (kg) giao tiếp với ngƣời, leo cầu thang, chạy đôi tay cầm nắm Hình Robot Asimo Nhật Bản Đến năm 2005, Viện Khoa Học Công Nghệ cao Hàn Quốc KAIST (Korea Advanced Institute of Science and Technology) bắt đầu cho đời hệ robot 10 Phụ lục if(counter_servo >= speed_servo[4] + 62) LAT_SERVO4=0; // MAX 240 // MIN 60 else LAT_SERVO4=1; if(counter_servo >= speed_servo[5] + 62) LAT_SERVO5=0; // MAX 240 // MIN 60 else LAT_SERVO5=1; if(counter_servo >= speed_servo[6] + 62) LAT_SERVO6=0; // MAX 240 // MIN 60 else LAT_SERVO6=1; if(counter_servo >= speed_servo[7] + 62) LAT_SERVO7=0; // MAX 240 // MIN 60 else LAT_SERVO7=1; // MIN 60 if(counter_servo >= speed_servo[8] + 62) LAT_SERVO8=0; // MAX 240 // MIN 60 else LAT_SERVO8=1; if(counter_servo >= speed_servo[9] + 62) LAT_SERVO9=0; // MAX 240 // MIN 60 else LAT_SERVO9=1; if(counter_servo >= speed_servo[10] + 62) LAT_SERVO10=0; // MAX 240 // MIN 60 else LAT_SERVO10=1; if(counter_servo >= speed_servo[11] + 62) 78 // MAX 240 Phụ lục LAT_SERVO11=0; // MIN 60 else LAT_SERVO11=1; if(counter_servo >= speed_servo[12] + 62) LAT_SERVO12=0; // MAX 240 // MIN 60 else LAT_SERVO12=1; } void config_servo(void){ ADPCFG = 0xffff; TRISB = 0x00; TRISC = 0x00; TRISD = 0x00; TRISF = 0xff; //RF0 RF8 Input LATB = 0x00; LATC = 0x00; LATD = 0x00; LATF = 0x00; } void timer1_init(void) { config_servo(); T1CON = 0; // Timer reset T1CONbits.TSIDL = 0; T1CONbits.TGATE = 0; //hoat dong ca o che ngu //Gated time accumulation enabled(TCS must be set to „0‟ when TGATE = Reads as „0‟ if TCS = 1) 79 Phụ lục //T1CONbits.TSYNC = 0; //bit duoc bo qua bit TCS = 0; chi quan tam TCS = 1; IFS0bits.T1IF = 0; // Reset Timer1 interrupt flag IPC0bits.T1IP = 0b111; // Timer1 Interrupt priority level=7 // muc uu tien 7//quan IEC0bits.T1IE = 1; // Enable Timer1 interrupt T1CONbits.TCKPS = 0b00; // Bo chia tan so : TMR1 = 0x0000; PR1 = 295 ; // Ngat dem tran 10us //PR1 = 29491 ; // ngat dem tran 1ms // T = (1/Fosc)s ma Fosc = (XTFREQ * PLL) /4 // PR1 = 29.4912 * * cycle (33.9084201388888888888888ns) = us T1CONbits.TCS = 0; // Timer1 Clock= Internal // Internal clock (FOSC/4) T1CONbits.TON = 1; // Enable Timer1 and start the counter } int goc(float goc){ float goc1 = 0; unsigned int goc2 = 0; if(goc == 0) return 0; else{ goc1 = 18 * goc; goc1 = goc1/18; goc2 = (int) goc1; }; return goc2; 80 Phụ lục } o Thư viện timer2.h: #include char flag_2; void timer2_init(void); void delay_ms(unsigned long delayms){ unsigned long counter_delay = 1; T2CONbits.TON = 1; // cho phep ngat IFS0bits.T2IF = 0; // xoa bit bao ngat while(delayms >= counter_delay){ if(flag_2 == 1) { counter_delay++; flag_2 = 0; } } T2CONbits.TON = 0; } void attribute ((interrupt, no_auto_psv)) _T2Interrupt(void){ IFS0bits.T2IF = 0; flag_2 = 1; } void timer2_init(void) { T2CON = 0; // Timer reset T2CONbits.TSIDL = 0; T2CONbits.TGATE = 0; //hoat dong ca o che ngu //Gated time accumulation enabled(TCS must be set to „0‟ when TGATE = Reads as „0‟ if TCS = 1) 81 Phụ lục //T1CONbits.TSYNC = 0; //bit duoc bo qua bit TCS = 0; chi quan tam TCS = 1; IFS0bits.T2IF = 0; // Reset Timer1 interrupt flag IPC1bits.T2IP = 0b011; // Timer1 Interrupt priority level=3 // muc uu tien 3.Ly : Khong quan IEC0bits.T2IE = 1; // Enable Timer1 interrupt T2CONbits.TCKPS = 0b11; // Bo chia tan so : 256 T2CONbits.T32 = 0; // time o che 16 bit TMR2 = 0x0000; PR2 = 115 ; // Ngat dem tran 10us //PR2 = 115; // ngat dem tran 1ms // T = (1/Fosc)s ma Fosc = (XTFREQ * PLL) /4 // PR2 = 115 * 256* cycle (33.9084201388888888888888ns) = ms T2CONbits.TCS = 0; // Timer1 Clock= Internal // Internal clock (FOSC/4) T2CONbits.TON = 1; // Enable Timer1 and start the counter } o Thư viện servo.h: #include #define FCY 29491200ULL //khai bao tan so de su dung delay thu vien libpic30 #include // -dac ta -//ham cho phep servo chuyen dong toi vi tri mong muon voi //cac toc khac // ket thuc dac ta 82 Phụ lục void ramp_servo(int two, // vi tri ket thuc int speed, // toc char number_servo); // so servo void ramp_servo_all( int two1, int two2, int two3, int two4, int two5, int two6, int two7, int two8, int two9, int two10, int two11, int two12, int speed_two1 ); void ramp_servo(int two,int speed,char number_servo){ int kc = 0,buff = 0,counter; buff = two - speed_servo[number_servo]; kc = fabs(buff); speed = speed * 1; if(buff > 0) for(counter = 0;counter 0) {speed_servo[5]++;buff[5] ;} else if(buff[5] < 0) {speed_servo[5] ;buff[5]++;} if(buff[6] == 0) speed_servo[6] = two6; else if(buff[6] > 0) {speed_servo[6]++;buff[6] ;} else if(buff[6] < 0) {speed_servo[6] ;buff[6]++;} if(buff[7] == 0) speed_servo[7] = two7; else if(buff[7] > 0) {speed_servo[7]++;buff[7] ;} else if(buff[7] < 0) {speed_servo[7] ;buff[7]++;} if(buff[8] == 0) speed_servo[8] = two8; else if(buff[8] > 0) {speed_servo[8]++;buff[8] ;} else if(buff[8] < 0) {speed_servo[8] ;buff[8]++;} if(buff[9] == 0) speed_servo[9] = two9; else if(buff[9] > 0) {speed_servo[9]++;buff[9] ;} else if(buff[9] < 0) {speed_servo[9] ;buff[9]++;} if(buff[10] == 0) speed_servo[10] = two10; else if(buff[10] > 0) {speed_servo[10]++;buff[10] ;} else if(buff[10] < 0) {speed_servo[10] ;buff[10]++;} if(buff[11] == 0) speed_servo[11] = two11; else if(buff[11] > 0) {speed_servo[11]++;buff[11] ;} else if(buff[11] < 0) {speed_servo[11] ;buff[11]++;} if(buff[12] == 0) speed_servo[12] = two12; 86 Phụ lục else if(buff[12] > 0) {speed_servo[12]++;buff[12] ;} else if(buff[12] < 0) {speed_servo[12] ;buff[12]++;} counter++; delay_ms(speed); } } o Thư viện uart.h: #include #include #define FCY 7372800*4 //add your operating frequency = tan so thach anh chia cho #define UART1_BAUD 9600 #define UART2_BAUD 9600 #define UBRG1_VALUE (FCY/UART1_BAUD)/16 - #define UBRG2_VALUE (FCY/UART2_BAUD)/16 - void uart_init() { _TRISF2=1; _TRISF3=0; U1BRG=UBRG1_VALUE; U1MODEbits.ALTIO=0; //IEC0bits.U1RXIE=1; U1STA&=0xfffc; U1MODEbits.UARTEN=1; U1STAbits.UTXEN=1; } void uart2_init() 87 Phụ lục { _TRISF4=1; _TRISF5=0; U2BRG = UBRG2_VALUE; // baud rate 9600 U2STA &= 0xfffc; IEC1bits.U2RXIE = 0; // enable RX2 interrupt U2MODEbits.UARTEN = 1; // UARTEN: UART Enable bit U2STAbits.UTXEN = 1; // transmition ON } void uart2_putchar(char data) { U2TXREG = data; while(!U2STAbits.TRMT){} } void uart2_send_number(unsigned int data) { unsigned char temp; temp=data/1000; uart2_putchar(temp+'0'); data=data-temp*1000; temp=data/100; uart2_putchar(temp+'0'); data=data-temp*100; temp=data/10; uart2_putchar(temp+'0'); data=data-temp*10; uart2_putchar(data+'0'); } void uart2_puts(const char * s) 88 Phụ lục { while(*s) uart2_putchar(*s++); } void uart_putchar(char data) { //U1TXREG = data; //while(!_TRMT); while (U1STAbits.TRMT==0); if(U1MODEbits.PDSEL == 3) U1TXREG = data; else U1TXREG = data & 0xFF; } void uart_puts(char *s) { while(*s) { U1TXREG = *s; while(!U1STAbits.TRMT); s++; } } void uart_send_number(int num) { int a,b,c,d,so; if(num