Robot lau nhà bằng phát triển và cải tiến thuật toán di chuyển ziczac và thuật toán PID.

Trong bài báo cáo này Nhóm 2 chúng em xin trình bày phương pháp di chuyển robot – thuật toán “ Cày ziczag kết hợp với thuật toán PID “áp dụng để xây dựng ứng dụng “ robot lau nhà “. Hiện nay, các công trình nghiên cứu trên thế giới đã đưa ra nhiều thuật toán đường đi bao phủ để giải quyết một số vấn đề trong điều khiển Robot lau nhà và lịch trình đường đi bao phủ tối ưu khoảng cách. Song chủ yếu các thuật toán đưa ra hầu hết đều chỉ thực hiện yêu cầu trong môi trường tĩnh. Với thực tế đó, nhóm chúng em xin đề ra phương hướng giải quyết cho bài toán “ robot lau nhà “ trong môi trường động. Bên cạnh đó , còn đề ra một số giải pháp để cải tiến việc sai số trong định vị và dẫn đường của robot, sai số trong thu thập thông tin về môi trường.

TRÍ TUỆ NHÂN TẠO Robot lau nhà phát triển cải tiến thuật toán di chuyển ziczac thuật toán PID Lê Thị Mai1 , Đỗ Vũ Hiệp2, Nguyễn Văn Quý3 Đặng Vũ Đức4, Nguyễn Cao Thùy Trang5 Viện Đào Tạo Quốc Tế- Đại Bách Khoa Hà Nội ltmai93@gmail.com Từ khóa : Robot lau nhà thuật toán cày ziczag, cover path planning,… Tóm tắt: Trong báo cáo Nhóm chúng em xin trình bày phương pháp di chuyển robot – thuật toán “ Cày zic-zag kết hợp với thuật toán PID “áp dụng để xây dựng ứng dụng “ robot lau nhà “ Hiện nay, công trình nghiên cứu giới đưa nhiều thuật toán đường bao phủ để giải số vấn đề điều khiển Robot lau nhà lịch trình đường bao phủ tối ưu khoảng cách Song chủ yếu thuật toán đưa hầu hết thực yêu cầu môi trường tĩnh Với thực tế đó, nhóm chúng em xin đề phương hướng giải cho toán “ robot lau nhà “ môi trường động Bên cạnh , đề số giải pháp để cải tiến việc sai số định vị dẫn đường robot, sai số thu thập thông tin môi trường I.Bài toán **Yêu cầu toán: Robot lau nhà hoạt động phải làm toàn diện tích sàn làm việc , đồng thời phải phát tránh vật cản bàn ghế, đồ đạc phòng Các thuật toán bao phủ sử dụng cần thỏa mãn yêu cầu sau: - Phải qua điểm vùng hoạt động nói cách khác đường robot phải bao phủ toàn vùng hoạt động Robot không vùng chồng chất lẫn Robot phải hoạt động theo đường không lặp lại Robot phải tránh vật cản xuất vùng hoạt động Quỹ đạo chuyển động robot cần đơn giản Đường robot cần tối ưu hóa dựa vào điều kiên định ***Phân tích toán: Robot lau nhà + Các nhận thức : Vị trí mức độ VD: [A,bẩn] ,[B, bẩn] + Các hành động: Di chuyển sang phải, sang trái, hút bụi không làm ****Bảng hành động: [A, ] > Di chuyển sang phải [A, bẩn] -> Hút bụi [B, sạch] > Di chuyển sang trái [B, bẩn] -> Hút bụi Funtion: Relax-Agent([location,status]) return an action If status = dirty then return Suck Else if location = A then return Right Else if location = B then return Left *****Hiệu hoạt động: Mức độ làm , thời gian hút bụi, mức điện tiêu tốn, mức tiếng ồn gây ra,… *****Thuật toán đề xuất: Thuật toán Cày zic-zag đảm bảo diện tích sàn nhà lau tối đa,đồng thời tránh trùng lặp, kết hợp với thuật toán PID tốt để robot chuyển động theo hướng ổn định *****Phát vật cản: Có nhiều phương pháp tránh vật cản cho robot như: phương pháp điều khiển bám biên, phương pháp phát cạnh,phương pháp biểu diễn vật cản sơ đồ lưới,phương pháp trường thế,phương pháp dung Optical Flow,và phương pháp hướng dẫn thích nghi Ở em sử dụng phương pháp hướng dẫn thích nghi để điều khiển robot lau nhà tránh vật cản ****Nhưng cải tiến để ra: - Báo hết pin, tự động sạc điện hết pin Tránh vật cản di động, vật cản cố định Ổn định hướng cho robot II.Giải vấn đề 1.Thuật toán di chuyển robot: a Giới thiệu thuật toán: Có nhiều thuật toán di chuyển xoắn ốc, ngẫu nhiên, ziczag,loang… ziczag sử dụng đề tài ưu điểm tối ưu diện tích sàn nhà lau Tuy nhiên cần phải có thuật toán PID tốt để robot di chuyển theo hướng ổn định Ở lần lau robot bỏ sót bên vật cản, lần lau robot lau Vì đảm bảo diện tích lau cao TRANG Hình vẽ minh họa: Trong đó: Khoảng cách hai đường chạy liên tiếp d xác định vùng hút bụi robot Điều khiển vòng kín robot chạy theo đường ziczag theo đường tham chiếu , Đường tham chiếu dựa viền viên gạch lót nền.Sử dụng camera để dò đường viền Kết hợp với việc gắn camera đầu robot hình ảnh chụp từ camera qua sử lý ảnh tính sai số quỹ đạo di chuyển thực robot với quỹ đạo tham chiếu b Sơ đồ thuật toán di chuyển ziczag Code: void Zigzag() { Cambien(); TRANG s1=Cambien(); Tien(); if(s1==1) { k=1; } while(k) { Dung(); delay_ms(500); k1=1; k=0; } while(k1) { Lui(); delay_ms(500); 71 Dung(); k2=1; k1=0; } while(k2) { Retrai(); delay_ms(500); Dung(); k3=1; k2=0; } TRANG while(k3) { Tien(); delay_ms(500); k4=1; k3=0; } while(k4) { Retrai(); delay_ms(500); Dung(); k4=0; } } 2.Thuật toán tránh robot tránh vật cản: a Phương pháp: sử dụng phương pháp dẫn hướng thích nghi Sử dụng ba cảm biến siêu âm SRF04 để xác định khoảng cách Cảm biến thứ đặt hướng xuống giúp robot tránh bị rơi xuống cầu thang hay nơi có chênh lệch độ cao Hai cảm biến lại đặt hai bên giúp robot tránh vật cản quay đầu Gọi dc, dl dr khoảng cách theo phương,dl, dr đặt lệch so với dc góc α Khi cảm biến không phát vật cản hay d>d max , ngõ sensor= -1 Đây trường hợp tránh vật cản cho robot: TRANG • Phát vị trí (hình a) Góc dẫn hướng tránh vật cản θa(t): θ *a(t) = θ(t) +sgn(dl – dr)( - ε ) Với: ε = tan-1((dcosα - dc)/dsinα) ( |ε| < π/2 ) d = max(dl,dr) sgn(x) = • Phát vị trí(hình b+c+d) Ta có hình b: dc> drcosα dlĐích nằm bên phải robot , lúc robot quay góc anpha bên phải để hướng đích.Cách tính góc quay cho trường hợp sau: gama= arctan( ) gama>báo khoảng cách H H2 đủ lớn cho robot lọt qua , O’H tính hàm đoạn an toàn, O’H tương tự: O’H2=O’M4 x cos(X) = x cos(α-tan-1())  H1H2= O’H2 - O’H1 Cải tiến: Báo hết pin, tự động sạc hết pin Bằng cách lưu tọa độ vị trí ổ điện nhà cho robot đồ cài đặt sẵn, robot xác định thông số góc quay chuyển hướng tiến vị trí có ổ điện tự động sạc pin hết pin III Ý tưởng cài đặt thuật toán: Thuật toán xây dựng ngôn ngữ lập trình c++: Code thuật toán: Chương trình cho robot Hàm main.c //*********************************************************************// // PROJECT // Description: // Project Name : Smart cleaner Robot // Note : // Neu error heading_new > heading_old // Neu error >=0 > heading_new < heading_old // la ban so quay ve ben phia banh trai > tang heading > error giam heading > error >0 //********************************************************************* TRANG 14 // Define #include "msp430g2553.h" #include "I2C.h" #include "UART.h" #include "compass.h" #include "PID.h" #include "math.h" #include "GLCD.h" #include "PWM.h" #include "SRF05.h" #define L_speed 5000 #define R_speed 5000 #define xoay 8000 #define lui 8000 //****************************************************************** // Varible int current_value=0,setpoint=0,heading_error=0,bak=0,set=0; int temp=1; double hold=0; int dir=1; int i=0; int offset=0; char sensor1=0; int sensor2=0; unsigned int stop=1,dem=1,dem1=0; int SCL1=BIT2; int SDA1=BIT3; void stop_enable(void) { if(stop==0) TRANG 15 { PWM_LEFT(lui,0); PWM_RIGHT(lui,0); delay_cycles(4000000); stop=1; } } void stop_enable_1(void) { if(stop==0) { PWM_LEFT(0,0); PWM_RIGHT(lui,0); delay_cycles(1500000); PWM_LEFT(lui,1); PWM_RIGHT(0,0); delay_cycles(1500000); stop=1; } } //****************************************************************** main() { WDTCTL = WDTPW + WDTHOLD; BCSCTL1 = CALBC1_16MHZ; DCOCTL = CALDCO_16MHZ; BCSCTL3 |= LFXT1S_2; // Set P2.6 P2.7 in_output P2SEL &=~(BIT6+BIT7); P2SEL2 &=~(BIT6+BIT7); //====================== TRANG 16 P1DIR &=~(BIT0+BIT1); // Set input SRF04 trai, phai trai >P1.0,phai >P1.1 P1OUT |= (BIT0+BIT1); // P1DIR &=~BIT4 ; // set input cho encoder P2DIR &=~(BIT6+BIT7); // Set input SRF04 giua va cong tac hanh trinh SRF04 giua > P2.6, cong tac hanh trinh P2.7 P2OUT |= (BIT6+BIT7); // set pullup resistor P2REN |= BIT6; // Set internal pullup/pulldown resistor P2IE |= BIT6+BIT7; // Interrupt Enable in P2.6 P2IES |= BIT6+BIT7 ; // P2.6 Interrupt flag high-to-low transition P2IFG &= ~(BIT6+BIT7); // P2.6 IFG cleared //********************************* Configure ******************** P1DIR &=~BIT5 ; // set input cho encoder P1IE |= BIT5; // Interrupt Enable in P1.0 ( cau hinh cho encoder ) P1IES |= BIT5 ; // P1.0 Interrupt flag high-to-low transition P1IFG &= ~BIT5; // P1.0 IFG cleared //****************************************************************** configPWM() ; GLCD_Congfig(); // Khoi tao GLCD // xac dinh goc bak compass_init(); // khoi tao la ban so bak=(int)compass_heading(); // doi chan I2C SCL1=BIT6; SDA1=BIT7; // xac dinh goc setpoint compass_init(); // khoi tao la ban so set=(int)compass_heading(); // dat goc ban dau TRANG 17 display_setpoint(set) ; // hien thi setpoint display_bak(bak); // hien thi goc se comeback // Set toc ban dau PWM_LEFT(L_speed,1) ; PWM_RIGHT(R_speed,1) ; // delay_cycles(32000000); TA0CCTL0 = CCIE; // CCR0 interrupt enabled TA0CCR0 = 10000; TA0CTL = TASSEL_1 + MC_1+ TACLR ; // ACLK f= 12kHz enable_interrupt(); // enable all interrupts //================================================ while(1) { //**************************PID *********************** current_value=(int)compass_heading(); // xet goc hien tai heading_error=PID_output(); // tinh PID if(heading_error0) {PWM_RIGHT(R_speed+heading_error,1);PWM_LEFT(L_speed,1) ; } //=============================================================== ========== display_crt((int)(compass_heading())); // hien thi goc hien tai if(sensor1) // neu dap cong tac hanh trinh { // Quay huong ben trai if(temp==1) { setpoint=bak; TRANG 18 display_setpoint(setpoint) ; // hien thi setpoint PWM_LEFT(xoay,0); PWM_RIGHT(0,0); if(sensor2==1) //delay cho SRF04 giua { delay_cycles(15000000); sensor2=0; } else // delay cho cong tac hanh trinh delay_cycles(8000000); if((P1IN&BIT0)!=0) // ko co vat can ben trai { while(!(abs(current_value-setpoint)[...]... H1H2= O’H2 - O’H1 5 Cải tiến: Báo hết pin, tự động sạc khi hết pin Bằng cách lưu tọa độ vị trí ổ điện trong nhà cho robot trên bản đồ đã cài đặt sẵn, robot sẽ xác định các thông số góc quay và chuyển hướng tiến về vị trí có ổ điện và tự động sạc pin khi hết pin III Ý tưởng cài đặt thuật toán: Thuật toán được xây dựng bằng ngôn ngữ lập trình c++: Code thuật toán: Chương trình cho robot Hàm main.c //*********************************************************************//... đã lau Tránh được vật cản cố định và di động Với việc sử dụng thuật toán PID có thể ổn định được hướng đi cho robot giúp giảm thiểu chi phí tối đa, và tránh những sai số không cần thiết trong quá trình di chuyển của robot Nhược điểm: - Chỉ phù hợp với môi trường ít phức tạp và có kích thước vật cản đủ lớn TRANG 22 Tài liệu tham khảo 1 2 3 4 5 6 Minimum Time Coverage Planning for Floor Cleaning Robot. .. *********************** current_value=(int)compass_heading(); // xet goc hien tai heading_error=PID_output(); // tinh PID if(heading_error0) {PWM_RIGHT(R_speed+heading_error,1);PWM_LEFT(L_speed,1) ; } //=============================================================== ========== display_crt((int)(compass_heading())); // hien thi goc hien tai... thiết để thoát khỏi vật cản là anpha(), khi robot ở vị trí cách vật cản một đoạn.Góc anpha hoàn toàn tính được khi ta biết max_point, góc Ø ,và độ lớn O’L TRANG 11 a = Ø’ – Ø = cos-1 – Ø , quay về phía bên phải robot  Vật cản nằm bên phải robot >>> góc anpha được tính như sau: α = Ø’ – Ø = cos-1 – Ø , quay về phía bên trái robot  Vật cản nằm giữa đường robot >>> robot sẽ tìm góc quay né vật sao cho anpha... //*********************************************************************// // PROJECT // Description: // Project Name : Smart cleaner Robot // Note : // Neu error heading_new > heading_old // Neu error >=0 > heading_new < heading_old // la ban so quay ve ben phia banh trai > tang heading > error giam heading > error >0 //********************************************************************* TRANG... phải robot , lúc này robot sẽ quay một góc anpha về bên phải để hướng về đích.Cách tính góc quay cho các trường hợp như sau: gama= arctan( ) gama