Thiết kế giải thuật điều khiển robot tự hành trong môi trường tĩnh có vật cản

TĨM TẮT ĐỒ ÁN Trong thời đại cơng nghiệp ngày nay, Robot ngày sử dụng phổ biến sản xuất sống người Robot có vị trí quan trọng khó thay được, giúp người để làm việc điều kiện nguy hiểm, khó khăn Ngồi ra, Robot dùng vào lĩnh vực thám hiểm khơng gian, qn sự, giải trí… Mobile Robot ngày chiếm quan tâm nhà nghiên cứu xã hội Từ tình hình thực tế đó, việc xây dựng chương trình hoạt động cho Robot điều thiết yếu đặc biệt Mobile Robot Trong đề tài “Robot tự hành tránh vật cản”, Robot hoạt động môi trường tĩnh có vật cản cho trước Robot xây dựng hồn chỉnh có khả tự hành tránh chướng ngại vật quãng đường di chuyển Robot di chuyển độc lập mơi trường mà khơng có can thiệp từ bên ngồi Robot có ba cảm biến siêu âm sử dụng để dò đường phát thông tin vật cản môi trường Robot sau có liệu vật cản mơi trường truyền liệu lên máy tính thơng qua module bluetooth kết hợp với giao diện Visual Studio để điều khiển truyền nhận liệu đến Robot iii ABSTRACT In today’s world robotics are a fast growing and interesting field The robot has a sufficent intelligence to cover the maximum area of provided space Obstacle avoidance is one of the most critical factors in the design of autonomous vehicles such as a mobile robot Obstacle avoidance divided into two parts, obstacle detection, and avoidance control Numerous methods for obstacle avoidance have been suggested and research in this area of robotics is done extensively It has a sensor which is used to sense the obstacles coming in between the path of the robot It will move in a particular direction and avoid the obstacle which is coming in its path Obstacle Avoiding Robot is robots that can perform desired tasks in unstructured environments without continuous human guidance The aim of this project is to develop an avoidance behaviors program for a robot that consists of servo motors that been employed at the legs It contains a servo motor, for turning right or left direction An Arduino Mega Controller has been implemented to act as a brain for the robot that controls the walking and turning algorithm The ultrasonic sensor was also developed to act as an “eye” to the system and tell the brain about the existence of an obstacle in front As a resultant, the obstacle avoidance robot system is been successfully developed that allows and will navigate the robot to move through the environment freely iv MỤC LỤC LỜI NÓI ĐẦU i LỜI CÁM ƠN ii TÓM TẮT ĐỒ ÁN iii ABSTRACT iv MỤC LỤC vi DANH MỤC HÌNH ẢNH VÀ SƠ ĐỒ ix DANH MỤC BẢNG xi DANH MỤC VIẾT TẮT xi CHƯƠNG 1: TỔNG QUAN 1.1 Giới thiệu chung 1.2 Phân loại robot tự hành 1.2.1 Robot tự hành di chuyển chân (Legged Robot) 1.2.2 Robot tự hành di chuyển bánh (Wheel Robot) 1.3 Phương pháp điều hướng cho robot tự hành 1.3.1 Phương pháp điều hướng có tính tốn 1.3.2 Phương pháp điều hướng robot theo phản ứng[10, 11] 10 1.3.3 Phương pháp điều khiển lai ghép 11 1.4 Mục tiêu đề tài 12 1.5 Nội dụng đề tài 12 1.6 Giới hạn đề tài 12 1.7 Phương pháp, đối tượng nghiên cứu 12 1.7.1 Phương pháp nghiên cứu .12 1.7.2 Đối tượng nghiên cứu .12 CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 13 2.1 Mơ hình động học cho robot 13 2.1.1 Mơ hình bánh xe robot 13 2.1.2 Phương trình động học robot 14 2.2 Phương pháp dẫn động cho robot tự hành 14 2.2.1 Tịnh tiến trước quay sau 15 vi 2.2.2 Tịnh tiến quay đồng thời 16 2.2.3 Quay trước tịnh tiến sau 16 2.3 Một số phương pháp dẫn đường cho robot di động 19 2.4 Phương pháp định vị cho robot tự hành 20 2.5 Phương pháp tránh vật cản 21 2.5.1 Phương pháp bám tường (wall-following method)[8] 21 2.5.2 Phương phát phát cạnh (edge-detection) .25 2.5.3 Phương pháp biểu diễn vật cản sơ đồ lưới xác định 25 2.5.4 Phương pháp trường (Potential Field Method)[6, 9] 26 2.5.5 Phương pháp dùng optical flow 29 2.6 [5] THUẬT TỐN DẪN HƯỚNG THÍCH NGHI (ADAPTIVE NAVIGATION) 32 2.6.1 Mơ hình robot kĩ thuật dẫn hướng thích nghi 32 2.6.2 Cách tránh vật cản 34 2.6.3 Xây dựng thuật tốn dẫn hướng thích nghi .36 2.7 Tổng kết phương pháp tránh vật cản 38 CHƯƠNG 3: XÂY DỰNG MƠ HÌNH ROBOT TỰ HÀNH 39 3.1 Xây dựng sơ đồ khối hoạt động robot 39 3.2 Xây dựng khối điều khiển cho robot tự hành 41 3.2.1 Khối điều khiển .41 3.2.2 Khối cảm biến 44 3.2.3 Khối công suất 49 3.2.4 Cổng I/O 51 3.3 Xây dựng mơ hình khí 52 3.4 Xây dựng điều khiển PID 53 3.4.1 Điều khiển động DC Servo .53 3.4.2 Điều khiển vận tốc động PID 55 3.5 Xây dựng thuật toán tránh vật cản 59 3.5.1 Môi trường di chuyển robot 59 3.5.2 Giải thuật phát tránh vật cản 59 vii 3.6 Giải thuật vẽ lại biên dạng vật cản hướng di chuyển robot 64 3.7 Thiết kế xây dựng chương trình điều khiển, giám sát máy tính 70 CHƯƠNG : KẾT QUẢ, THỰC NGHIỆM, PHÂN TÍ CH, TỔNG HỢP 71 4.1 Kết mơ hình khí 71 4.2 Kết điều khiển vẽ lại sơ đồ 73 4.2.1 Giải thuật di chuyển robot môi trường tĩnh .73 4.2.2 Kết điều khiển 76 CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN .82 5.1 Kết luận 82 5.2 Hướng phát triển 82 TÀI LIỆU THAM KHẢO 83 PHỤ LỤC 84 viii DANH MỤC HÌNH ẢNH VÀ SƠ ĐỒ Hình 1.1: Robot SDR-4X, chế tạo năm 2003 hãng Sony Hình 1.2: Robot chân Hình 1.3: Mơ hình robot chân Hình 1.4: Các loại bánh xe dùng Robot tự hành Hình 1.5: Robot Sojourner sử dụng thám hiểm Hỏa năm 1997 Hinh 1.6: Robot AIRDUCT Hình 1.7: Robot Mbari’s Altex Auv (Autonomous Underware Vehicle) Hình 1.8: Robot Khepera dùng để nghiên cứu học tập Hình 1.9: Robot dẫn đường AGU (Autonomous Guided Vehicle) Hình 2.1: Mơ hình bánh xe lý tưởng hóa Hình 2.2: Mơ hình robot tự hành Hình 2.3: Mặt phẳng chuyển động robot Hình 2.4: Phương pháp dẫn động tịnh tiến trước – quay sau Hình 2.5: Phương pháp dẫn động tịnh tiến quay đồng thời Hình 2.6: Phương pháp dẫn động quay trước – tịnh tiến sau Hình 2.7: Sự chuyển động cặp bánh chủ động robot quay Hình 2.8: Robot tự hành tình bám biên Hình 2.9: Robot di chuyển qua lại hai tường Hình 2.10: robot vào góc nhỏ có khe hở nhỏ Hình 2.11: Robot gặp vật cản bám tường Hình 2.12: Sơ đồ lưới: giá trị d đo chứng tỏ tồn vật cản vùng màu đen Hình 2.13: Histogram Grid Hình 2.14: Trường hợp robot khơng qua khe vật cản Hình 2.15: Optical Flow bàn tay Hình 2.16: Mơ hình tốn dẫn hướng thích nghi Hình 2.17: Các trường hợp phát vật cản robot Hình 3.1: Arduino Mega2560 Hình 3.2: Cấu tạo Arduino Mega2560 Hình 3.3: Cảm biến siêu âm SRF 05 Hình 3.4: Phạm vi góc hoạt động tốt Module Hình 3.5: Sơ đồ tính khoảng cách SRF05 Hình 3.6: Biểu đồ thời gian cảm biến siêu âm SRF05 Hình 3.7: Nguyên lý TOF Hình 3.8: Hiện tượng đọc chéo Hình 3.9: Sự phản xạ sóng siêu âm Hình 3.10: Hiện tượng Forescasting Hình 3.11: Cảm biến la bàn số HMC5883L Hình 3.12: Mạch điều khiển động L298 Hình 3.13: Cách nối dây module HC-05 với Arduino Hình 3.14: Mơ hình khí xây dựng SOLIDWORKS 3 7 8 13 14 14 15 16 16 17 22 24 24 24 26 27 29 30 32 35 41 42 44 44 45 45 46 46 47 47 48 49 51 52 ix Hình 3.15: Động DC servo giảm tốc Hình 3.16: Sơ đồ chân kết nối động Hình 3.17: Đọc xung thơng qua cổng COM máy tính Hình 3.18: Vận tốc trả hiển thị cổng COM Hình 3.19: Mơi trường di chuyển robot Hình 3.20: Vị trí cảm biến robot Hình 3.21: Cảm biến trung tâm quét phát vật cản Hình 3.22: Cảm biến trung tâm phát vật cản Hình 3.23: Cảm biến phải trả d3 từ vẽ lại biên dạng vật cản Hình 3.24: Cảm biến phải trả d3 từ vẽ lại biên dạng vật cản Hình 3.25: Robot di chuyển đến hết biên dạng mặt phẳng trước vật cản Hình 3.26: Cảm biến thu thập liệu biên dạng Hình 3.27: Các pixel hiển thị khơng gian 2D Hình 3.28: Giao diện phần mềm sau thiết kế Hình 4.1: Mặt trước mơ hình robot Hình 4.2: Mặt bên mơ hình robot Hình 4.3: Robot di chuyển từ trái sang phải Hình 4.4: Robot di chuyển từ phải sang trái Hình 4.5: Robot di chuyển tránh vật cản Hình 4.6: Mơ hình robot mơi trường di chuyển từ bên trái Hình 4.7: Hình ảnh thu thập từ phần mềm Hình 4.8: Mơ hình robot mơi trường di chuyển từ bên phải Hình 4.9: Hình ảnh thu thập từ phần mềm 53 54 54 58 59 60 60 61 61 62 62 Sơ đồ 1.1: Sơ đồ cấu trúc phương pháp điều khiển có tính tốn Sơ đồ 2.1: Sơ đồ nguyên lý khâu định vị mobile robot Sơ đồ 3.1: Sơ đồ thuật toán hoạt động Mobile robot Sơ đồ 3.2: Sơ đồ khối robot Sơ đồ 3.3: Sơ đồ cấu trúc Arduino Mega2560 Sơ đồ 3.4: Sơ đồ khối mạch nguồn Sơ đồ 3.5: Sơ đồ giải thuật vẽ lại biên dạng vật cản hướng di chuyển robot Sơ đồ 4.1: Sơ đồ giải thuật tránh vật cản robot Sơ đồ 4.2: Sơ đồ phòng robot di chuyển từ bên trái Sơ đồ 4.3: Sơ đồ phòng robot di chuyển từ bên phải 19 40 41 46 50 63 65 70 72 73 74 74 75 78 79 81 81 64 76 77 80 x DANH MỤC BẢNG Bảng 1.1: Sơ đồ bánh xe Robot tự hành Bảng 1.2: Kí hiệu loại bánh xe DANH MỤC VIẾT TẮT PID APM RPM TOF GUI Proportional-Integral-Derivative Absolute Positioning Methods Relative Positioning Methods Time of Flight Graphical User Interface xi CHƯƠNG 1: TỔNG QUAN 1.1 Giới thiệu chung Ngày nay, Robot học đạt thành tựu to lớn sản xuất cơng nghiệp Những cánh tay robot có khả làm việc với tốc độ cao, xác liên tục làm xuất lao động tăng nhiều lần Chúng làm việc mơi trường độc hại hàn, phun sơn, nhà máy hạt nhân, hay lắp ráp linh kiện điện tử tạo điện thoại, máy tính….một cơng việc địi hỏi tủ mỉ, xác cao Tuy nhiên robot có hạn chế chung hạn chế không gian làm việc Không gian làm việc chúng bị giới hạn số bật tự tay máy vị trí gắn chúng Ngược lại, Robot tự hành lại có khả hoạt động cách linh hoạt môi trường khác Robot tự hành loại Mobile robot có khả tự hoạt động, thực thi nhiệm vụ mà không cần can thiệp người Với cảm biến, chúng có khả nhận biết môi trường xung quanh Robot tự hành ngày có nhiều ý nghĩa ngành công nghiệp, thương mại, y tế, ứng dụng khoa học phục vụ đời sống người Với phát triển ngành Robot học, robot tự hành ngày có khả hoạt động mơi trường khác nhau, tùy lĩnh vực áp dụng mà chúng có nhiều loại khác robot sơn, robot hàn, robot cắt cỏ, robot thám hiểm đại dương, robot làm việc vũ trụ Cùng với phát triển yêu cầu thực tế, robot tự hành tiếp tục đưa thách thức cho nhà nghiên cứu Vấn đề robot tự hành làm để robot tự hành hoạt động, nhận biết môi trường thực thi nhiệm vụ đề Vấn đề di chuyển, Robot tự hành nên di chuyển cấu di chuyển lựa chọn tốt Điều hướng vấn đề nghiên cứu chế tạo Robot tự hành có hướng nghiên cứu khác nhau: - - Hướng thứ nhất: nghiên cứu Robot tự hành có khả điều hướng tốc độ cao nhờ thông tin thu từ cảm biến, loại robot có khả hoạt động mơi trường phịng mơi trường bên ngồi Loại robot u cầu khả tính tốn đồ sộ trang bị cảm biến có độ nhạu cao, dải lớn để điều khiển robot di chuyển tốc độ cao mơi trường có địa hình phức tạp Hướng thứ hai: nhằm giải vấn đề loại robot tự hành dùng để hoạt động môi trường phịng Loại robot tự hành có kết cấu đơn giản loại trên, thực nhiệm vụ đơn giản Bài toán điều hướng cho robot tự hành chia làm loại: tốn tồn cục (global) toán cục (local) Ở toán tồn cục, mơi trường làm việc robot hồn tồn xác định, đuờng vật cản hoàn toàn biết trước Ở tốn cục bộ, mơi trường hoạt động robot chưa biết trước biết phần Các cảm biến thiết bị định vị cho phép robot xác định vật cản, vị trí mơi trường giúp tới mục tiêu Các vấn đề gặp phải điều hướng cho Robot tự hành thường không giống loại robot khác Để điều hướng cho Robot tự hành, định theo thời gian thực phải dựa vào thông tin liên tục môi trường thông qua cảm biến, phịng ngồi trời, điểm khác biệt lớn so với kỹ thuật lập kế hoạch ngoại tuyến Robot tự hành phải có khả tự định phương thức điều hướng, định hướng chuyển động để tới đích thực nhiệm vụ định Điều hướng cho robot tự hành cơng việc địi hỏi phải thực số khả khác nhau, bao gồm: khả di chuyển mức bản, ví dụ hoạt động tới vị trí cho trước; khả phản ứng kiện theo thời gian thực, ví dụ có xuất đột ngột vật cản; khả xác định vị trí robot đồ đó; khả thiết lập kế hoạch để tới đích tránh tình khơng mong muốn khả thích nghi với thay đổi môi trường hoạt động Việc điều hướng gặp nhiều khó khăn nhiều vấn đề phức tạp Vấn đề gây trở ngại hạn chế việc ước tính lượng, khó khăn việc phát nhận biết đối tượng, khó khăn việc tránh xung đột với đối tượng khác nhau, khó khăn liên quan tới việc sử dụng thông tin cung cấp từ môi trường 1.2 Phân loại robot tự hành Robot tự hành chia làm loại loại robot tự hành chuyển động chân robot tự hành chuyển động bánh Ngoài số loại robot hoạt động môi trường đặc biệt nước hay khơng trung chúng trang bị cấu di chuyển đặc trưng 1.2.1 Robot tự hành di chuyển chân (Legged Robot) Ưu điểm lớn loại robot thích nghi di chuyển địa hình gồ ghề Hơn chúng cịn qua vật cản hố, vết nứt sâu Nhược điểm robot loại chế tạo phức tạp Chân robot kết cấu nhiều bậc tự do, nguyên nhân làm tăng trọng lượng robot đồng thời giảm tốc độ di chuyển Các kĩ cầm, nắm hay nâng tải nguyên nhân làm giảm độ cứng vững robot Robot loại linh hoạt chi phí chế tạo cao Sơ đồ 4.1 Sơ đồ giải thuật tránh vật cản robot 75 4.2.2 Kết điều khiển  Trường hợp robot di chuyển từ bên trái phòng: Sơ đồ 4.2 Sơ đồ phịng robot di chuyển từ bên trái Trong đó: + Vật cản đánh số 1-4 + Diện tích phịng kích thước vật cản theo đại lượng cm 76 Hình 4.6 Mơ hình robot mơi trường giả định di chuyển từ bên trái Robot di chuyển phía trước – gặp vật cản số – tránh di chuyển lên gặp vật cản số – tránh di chuyển lên gặp tường – tránh di chuyển sang phải gặp góc trường – tránh di chuyển xuống gặp vật cản số – tránh di chuyển xuống gặp vật cản số – tránh di chuyển xuống – kết thúc hành trình Trong trình di chuyển, robot thu thập số liệu khoảng cách từ cảm biến, từ vẽ lại sơ đồ đường robot hiển thị chúng dạng ảnh tập hợp pixel 77 Hình ảnh thu thập từ phần mềm robot kết thúc hành trình: Hình 4.7 Hình ảnh thu thập từ phần mềm Từ hình ảnh trả về, thấy hướng di chuyển robot (các pixel màu trắng) biên dạng vật cản (các pixel màu vàng) 78  Trường hợp robot di chuyển từ bên phải phòng: Sơ đồ 4.3 Sơ đồ phòng robot di chuyển từ bên phải Robot di chuyển phía trước – Gặp vật cản số – Rẽ tránh di chuyển lên gặp tường – Rẽ tránh di chuyển sang trái gặp góc tường – Rẽ tránh di chuyển xuống gặp vật cản số – Rẽ tránh di chuyển xuống – Kết thúc hành trình 79 Hình 4.8 Mơ hình robot mơi trường di chuyển từ bên phải Hình ảnh thu thập từ phần mềm robot kết thúc hành trình Hình 4.9 Hình ảnh thu thập từ phần mềm Ta thấy robot di chuyển lướt qua vật cản số số nên biên dạng vật cản thể mặt bên vật cản số số biểu mặt bên 80  Trường hợp vị trí Trường hợp robot khơng xuất phát từ bên trái hay bên phải phịng robot di chuyển thẳng phát vật cản, lúc robot lựa chọn hướng di chuyển ưu tiên khoảng cách lớn từ cảm biến trái phải Nếu d2>d3: rẽ phải Nếu d2 2*PI) heading -= 2*PI; // Convert radians to degrees for readability float headingDegrees = heading * 180/M_PI; curr_heading=(int)headingDegrees; delay(30); } 85 Code chương trình đọc xung encoder #define phase_a // kênh A encoder nối với chân PWM 02 Mega2560 #define phase_b // kênh B encoder nối với chân PWM 03 Mega2560 #define interrupt0 // định nghĩa ngắt tương ứng Mega2560 /* Khai bao xung encoder */ volatile int pulse = 0; // biến đếm số xung/vòng void setup() { Serial.begin(9600); // Encoder -// pinMode(phase_a, INPUT_PULLUP); // digitalWrite(phase_a, HIGH); pinMode(phase_b, INPUT_PULLUP); digitalWrite(phase_b, HIGH); // interrupt -// attachInterrupt(interrupt0, int_, RISING); // gọi hàm int_ trạng thái chân digital chuyển từ mức điện áp thấp sang mức điện áp cao } // void-interrupt dem xung encoder -// void int_() { if (digitalRead (phase_a)==HIGH) { pulse++; } else { pulse ; } } void loop() { Serial.println(pulse); //hiển thị số xung qua cổng Serial delay(10); } 86 Code chương trình điều khiển PID volatile float V = 0; //biến chứa giá trị vận tốc ban đầu volatile float V_sp = 200.0 ; //vận tốc mà ta mong muốn Vsetpoint = 200v/p /* Thong so bo PID */ volatile float del_err,sum_err =0; volatile float Kp=2.2,Ki = 0.0005,Kd =0.000125; volatile float error,pre_error; volatile float p_part,i_part,i_part_last,d_part; volatile int PWM; volatile float rpm;// biến chứa giá trị vận tốc thực thời điểm mà ta xét volatile float sampletime = 0.001; // -PID // int PID_control () { error = V_sp - V; //tính tốn sai số p_part = Kp * error; i_part = i_part_last + Ki * error * sampletime; d_part = Kd *(error - pre_error) / sampletime; i_part_last = i_part; pre_error = error; //khâu p //khâu i //khâu d //reset lại sai số giá trị lien quan đến PWM = (int)(p_part + i_part + d_part); //output xung PWM if (PWM >= 255) PWM = 255; if (PWM 30) d2 = 30; if (d3 > 30) d3 = 30; if (hd == 'n') { bt.SetPixel((width / 2) - d2, (height / 2), System.Drawing.Color.Yellow); bt.SetPixel((width / 2) + d3, (height / 2), System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); if (height > 5) height = height - 5; } if (hd == 'e') { bt.SetPixel((width / 2), (height / 2) - d2, System.Drawing.Color.Yellow); bt.SetPixel((width / 2), (height / 2) + d3, System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); width = width + 5; } if (hd == 'w') { bt.SetPixel((width / 2), (height / 2) + d2, System.Drawing.Color.Yellow); bt.SetPixel((width / 2), (height / 2) - d3, System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); if (width > 5) width = width - 5; } if (hd == 's') { 88 bt.SetPixel((width / 2) + d2, (height / 2), System.Drawing.Color.Yellow); bt.SetPixel((width / 2) - d3, (height / 2), System.Drawing.Color.Green); bt.SetPixel((width / 2), (height / 2), System.Drawing.Color.White); height = height + 5; } bt.RotateFlip(RotateFlipType.Rotate180FlipXY); ImageSource img = Bitmap2ImageSource(bt); imbox.Source = img; bt.Save("E:\\Image.bmp"); } btd.Content = "Drawing"; this.Close(); } private ImageSource Bitmap2ImageSource(Bitmap bitmapImage) { using (MemoryStream outStream = new MemoryStream()) { MemoryStream ms = new MemoryStream(); bitmapImage.Save(ms, ImageFormat.Bmp); ms.Seek(0, SeekOrigin.Begin); BitmapImage bi = new BitmapImage(); bi.BeginInit(); bi.StreamSource = ms; bi.EndInit(); bi.Freeze(); return bi; } } 89 ... cung cấp từ môi trường 1.2 Phân loại robot tự hành Robot tự hành chia làm loại loại robot tự hành chuyển động chân robot tự hành chuyển động bánh Ngoài số loại robot hoạt động môi trường đặc biệt... hình robot tự hành có khả tự định hướng vẽ lại sơ đồ đường biên dạng vật cản  Ngoài khả tự định hướng, robot phải tránh vật cản có quãng đường di chuyển  Giải thuật tránh vật cản cho robot. .. Phân loại robot tự hành 1.2.1 Robot tự hành di chuyển chân (Legged Robot) 1.2.2 Robot tự hành di chuyển bánh (Wheel Robot) 1.3 Phương pháp điều hướng cho robot tự hành