Nghiên cứu mô hình robot 2 bánh tự cân bằng dùng arduino

0 BỘ CÔNG THƯƠNG TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP THỰC PHẨM TP HCM KHOA CÔNG NGHỆ KỸ THUẬT ĐIỆN – ĐIỆN TỬ ĐỒ ÁN HỌC PHẦN 2 ĐỀ TÀI MÔ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG GV Hướng Dẫn LÊ MINH THANH SV Thực hiện H.

BỘ CÔNG THƯƠNG TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP THỰC PHẨM TP.HCM KHOA CÔNG NGHỆ KỸ THUẬT ĐIỆN – ĐIỆN TỬ ĐỒ ÁN HỌC PHẦN ĐỀ TÀI: MƠ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG GV Hướng Dẫn: LÊ MINH THANH SV Thực hiện: HUỲNH BÁ MẪN MSSV: 2002140327 Lớp: 05DHDT3 TP HCM Tháng Năm 2017 NHIỆM VỤ ĐỒ ÁN HỌC PHẦN Họ tên sinh viên: Huỳnh Bá Mẫn MSSV: 2002140327 Khóa: 2014 Lớp: 05DHDT3 I TÊN ĐỀ TÀI: MƠ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG II NHIỆM VỤ VÀ NỘI DUNG: III NGÀY GIAO NHIỆM VỤ: 10/04/2017 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 16/6/2017 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: ThS LÊ MINH THANH LỜI CẢM ƠN  Trong suốt trình thực đề tài, gặp phải nhiều khó khăn giúp đỡ, hỗ trợ kịp thời từ quý Thầy Cô bạn nên Đồ án hoàn thành tiến độ Em xin chân thành cảm ơn thầy Lê Minh Thanh tận tình hướng dẫn, bảo kinh nghiệm quý báu hỗ trợ phương tiện thí nghiệm suốt trình tìm hiểu, nghiên cứu đề tài Em xin cảm ơn thành viên lớp 05DHDT3 có ý kiến đóng góp, bổ sung, động viên khích lệ giúp em hồn thành tốt đề tài Mặc dù nhóm thực đề tài cố gắng hoàn thiện đồ án, q trình soạn thảo kiến thức cịn hạn chế nên cịn nhiều thiếu sót Nhóm thực đề tài mong nhận đóng góp ý kiến quý thầy cô bạn sinh viên Sau nhóm thực xin chúc Thầy sức khoẻ, thành công tiếp tục đào tạo sinh viên giỏi đóng góp cho đất nước Chúc anh (chị), bạn sức khỏe, học tập thật tốt để không phụ công lao Thầy Cô giảng dạy Nhóm thực xin chân thành cảm ơn Trân trọng Người thực Huỳnh Bá Mẫn NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Tp HCM, ngày … tháng … năm 2017 Giáo viên hướng dẫn ThS Lê Minh Thanh Mục lục Chương Tổng quan đề tài 1.1 Đặt vấn đề 1.2 Mục tiêu đề tài 1.3 Giới hạn đề tài 1.4 Phương pháp nghiên cứu Chương 2.1 Các sở lý thuyết Đặc tính động lực học 2.1.1 Mơ hình hóa robot bánh tự cân địa hình phẳng .9 2.1.2 Mơ hình hóa robot hai bánh tự cân địa hình phẳng Matlab Simulink 14 2.2 Bộ lọc Kalman 15 2.2.1 Giới thiệu lọc Kalman .15 2.2.2 Quá trình ước lượng: 17 2.2.3 Bản chất xác suất lọc .18 2.2.4 Thuật toán Kalman rời rạc 19 2.3 Giải thuật điều khiển 21 2.3.1 Cấu trúc điều khiển PID cho robot hai bánh tự cân 21 2.3.2 Bộ điều khiển PID với thông số cố định 23 2.3.3 Các thành phần mơ hình 25 Chương 3.1 Thiết kế hệ thống 28 Thiết kế phần cứng mơ hình Robot bánh tự cân 28 3.1.1 Thiết kế khí 28 3.1.2 Kết nối phần cứng .28 3.2 Thiết kế phần mềm 29 3.2.1 Bộ lọc Kalman 29 3.2.2 Lưu đồ giải thuật điều khiển .32 3.2.3 Code arduino Simulink 33 Chương Kết luận hướng phát triển đề tài 38 4.1 Những kết đạt 38 4.2 Hạn chế đề tài 38 4.3 Kết luận 38 4.4 Hướng phát triển 38 4.5 Tài liệu tham 39 Chương Tổng quan đề tài 1.1 Đặt vấn đề Trong ngành tự động hóa - điều khiển tự động nói chung điều khiển học nói riêng, mơ hình lắc ngược đối tượng nghiên cứu điển hình đặc thù đặc tính động khơng ổn định mơ hình nên việc điều khiển đối tượng thực tế đặt thử thách Kết nghiên cứu mơ hình lắc ngược bản, ví dụ mơ hình xe-con lắc, lắc ngược quay… ứng dụng kế thừa sang mơ hình tương tự khác có tính ứng dụng thực tiễn hơn, chẳng hạn mơ hình tên lửa, mơ hình xe hai bánh tự cân bằng… khắc phục nhược điểm vốn có robot hai ba bánh kinh điển Các robot hai ba bánh kinh điển, theo có cấu tạo gồm bánh dẫn động bánh tự (hay khác) để đỡ trọng lượng robot Nếu trọnglượng đặt nhiều vào bánh lái robot khơng ổn định dễ bị ngã, đặt vào nhiều bánh hai bánh khả bám Nhiều thiết kế robot di chuyển tốt địa hình phẳng khơng thể di chuyển lên xuống địa hình lồi lõm mặt phẳng nghiêng Khi di chuyển lên đồi, trọng lượng robot dồn vào đuôi xe làm khả bám trượt ngã Robot dạng bánh xe di chuyển địa hình phẳng trọng lượng chia cho bánh lái bánh dẫn nhỏ Robot dạng bánh xe di chuyển địa hình phẳng Robot dạng bánh xe xuống dốc, trọng lực dồn vào bánh sau khiến xe bị lật úp Robot dạng bánh xe xuống dốc Robot dạng bánh xe lên dốc, trọng lượng dồn vào bánh trước khiến lực ma sát giúp xe bám mặt đường không đảm bảo Robot dạng bánh xe lên dốc Ngược lại, robot dạng hai bánh đồng trục lại thăng linh động di chuyển địa hình phức tạp, thân robot hệ thống không ổn định Khi robot di chuyển địa hình dốc, tự động nghiêng trước giữ cho trọng lượng dồn hai bánh Tương tự, di chuyển xuống dốc, nghiêng sau giữ trọng tâm rơi vào bánh Vì vậy, khơng có tượng trọng tâm xe rơi vùng đỡ bánh xe để gây lật úp Robot bánh di chuyển địa hình khác theo hướng bảo toàn thăng 1.2 Mục tiêu đề tài Mục tiêu đề tài xây dựng mơ hình robot bánh tự cân dựa tảng lý thuyết mơ hình lắc ngược Trong thời gian làm đề tài, mục tiêu đề tài đặt sau:  Tìm hiểu mơ hình xe, robot bánh tự cân nguyên lý cân  Tìm hiểu áp dụng Bộ lọc Kalman để lọc nhiễu cho cảm biến, xây dựng thuật tốn bù trừ để có giá trị góc xác  Xây dựng thuật tốn điều khiển động cơ, giữ thăng cho robot 1.3 Giới hạn đề tài Trong khuôn khổ đề tài này, mơ hình robot bánh tự cân đứng hay chạy tiến lùi mà chưa thể xoay vòng hay lên dốc Phương pháp điều khiển: sử dụng phương pháp điều khiển PID với thông số cố định, chưa ứng dụng phương pháp mạng neuron thích nghi, phương pháp mạng tồn phương tuyến tính (LQR) 1.4 Phương pháp nghiên cứu Xây dựng mơ hình lý thuyết robot bánh tự cân tính tốn thơng số PID Xây dựng mơ hình thực:  Thiết kế khí: khung sườn mơ hình  Ứng dụng mạch điện tử, mạch công suất, cảm biến, vi xử lý  Mạch cảm biến bù trừ giá trị cảm biến SỔ TAY ARDUINO Góc dị lớn Để tăng độ nhậy cho đầu dò, Bạn dùng kính Fresnel, thiết kế cho loại đầu có cảm biến, góc dị lớn, có tác dụng ngăn tia tử ngoại Nguyên lý làm việc loại đầu dị PIR nhƣ hình sau: Các nguồn nhiệt (với người vật nguồn thân nhiệt) phát tia hồng ngoại, qua kính Fresnel, qua kích lọc lấy tia hồng ngoại, cho tiêu tụ cảm biến hồng ngoại gắn đầu dò, tạo điện áp khuếch đại với transistor FET Khi có vật nóng ngang qua, từ cảm biến cho xuất tín hiệu tín hiệu khuếch có biên độ đủ cao đưa vào mạch so áp để tác động vào thiết bị điều khiển hay báo động Sơ đồ chân cảm biến PIR Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO  GND – kết nối với mặt đất  OUT – kết nối đến chân digital Arduino  5V – kết nối với 5V Linh kiện thí nghiệm:  1x PIR Motion Sensor  1x Arduino UNO (hoặc bất kỳ)  1x LED loa Sơ đồ kết nối arduino cảm biến: Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Lập trình điều khiển: int ledPin = 13; // chọn chân 13 báo hiệu LED int inputPin = 2; // chọn ngõ tín hiệu vào cho PIR int pirState = LOW; // Bắt đầu với khơng có báo động int val = 0; int pinSpeaker = 10; //chọn chân cho chuông có đột nhập void setup() { pinMode(ledPin, OUTPUT); pinMode(inputPin, INPUT); pinMode(pinSpeaker, OUTPUT); Serial.begin(9600); } void loop() { val = digitalRead(inputPin); // đọc giá trị đầu vào Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO if (val == HIGH) // giá trị mức cao.(1) { digitalWrite(ledPin, HIGH); // LED On playTone(300, 160); // thời gian chuông kêu delay(150); if (pirState == LOW) { Serial.println("Motion detected!"); pirState = HIGH; } } else { digitalWrite(ledPin, LOW); playTone(0, 0); delay(300); if (pirState == HIGH) { Serial.println("Motion ended!"); pirState = LOW; } } } void playTone(long duration, int freq) { Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO duration *= 1000; int period = (1.0 / freq) * 1000000; long elapsed_time = 0; while (elapsed_time < duration) { digitalWrite(pinSpeaker,HIGH); delayMicroseconds(period / 2); digitalWrite(pinSpeaker, LOW); delayMicroseconds(period / 2); elapsed_time += (period); } } 13 CẢM BIẾN KHOẢNG CÁCH ( SRF05) PHẦN NÀY BỔ XUNG Đối với robot, chế độ tự động, phải lấy thơng tin môi trường xung quanh như: khoảng cách, nhiệt độ, ánh sáng Và tiến hành phân tích liệu đó, cuối định phù hợp Ví dụ robot tự tìm đường phải tính xem phía trước có vật cản hay khơng, tiến hành tới hay rẽ sang hướng khác Bài viết hướng dẫn bạn đọc hiểu sử dụng cảm biến khoảng cách phổ biến - SRF05 Thiết bị cần chuẩn bị Các bạn cần chuẩn bị thiết bị sau:     x Arduino Uno R3 x cảm biến siêu âm SRF05 x breadboard để cắm mạch test Dây cắm loại Cảm biến siêu âm SRF05 Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Sóng siêu âm (sonar) loại sóng cao tầng mà người nghe thấy Tuy nhiên, ta thấy diện sóng siêu âm khắp nơi tự nhiên Ta có lồi động vật dơi, cá heo dùng sóng siêu âm để liên lạc với nhau, để săn mồi hay định vị không gian Việc tính tốn khoảng cách cịn phụ thuộc nhiều vào mơi trường truyền dẫn, ví dụ sóng âm truyền môi trường nước hay kim loại nhanh nhiều so với sóng âm truyền mơi trường khơng khí Lƣu ý sóng âm truyền đƣợc môi trƣờng chân không Cảm biến siêu âm SRF05 hoạt động theo nguyên tắc trên, thiết bị gồm có loa - thu phát - với chân để kết nối với Arduino, tầm hoạt động tối đa cảm biến nằm khoảng 5m Chức chân nhƣ sau: Vcc: cấp nguồn cho cảm biến Trigger: kích hoạt q trình phát sóng âm Q trình kích hoạt chu kì điện cao / thấp diễn Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Echo: bình thường trạng thái 0V, kích hoạt lên 5V có tín hiệu trả về, sau trở 0V Gnd: nối với cực âm mạch OUT: không sử dụng Ta tiến hành lắp đặt cảm biến khoảng cách SRF05 theo sơ đồ sau: Vcc: nối với nguồn 5V Adruino Gnd: nối với PIN GND Trigger: nối với PIN Echo: nối với PIN Lập trình điều khiển const int trig = 8; // chân trig HC-SR05 const int echo = 7; // chân echo HC-SR05 void setup() { Serial.begin(9600); // giao tiếp Serial với baudrate 9600 pinMode(trig,OUTPUT); // chân trig phát tín hiệu pinMode(echo,INPUT); // chân echo nhận tín hiệu } void loop() { unsigned long duration; // biến đo thời gian int distance; // biến lƣu khoảng cách /* Phát xung từ chân trig */ digitalWrite(trig,0); // tắt chân trig Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO delayMicroseconds(2); digitalWrite(trig,1); // phát xung từ chân trig delayMicroseconds(5); // xung có độ dài microSeconds digitalWrite(trig,0); // tắt chân trig /* Tính toán thời gian */ // Đo độ rộng xung HIGH chân echo duration = pulseIn(echo,HIGH); // Tính khoảng cách đến vật distance = int(duration/2/29.1); /* In kết Serial Monitor */ Serial.print(distance); Serial.println("cm"); delay(200); } Giải tích code: Ta biết thời gian âm truyền khơng khí 20°C 344 m/s Bằng quy tắc tam suất đơn giản ta dễ dàng tính sóng âm di chuyển cm khơng khí 1000 / 344 * 100 ~= 29.1 Arduino cung cấp cho ta hàm pulseIn có tác dụng trả thời gian (tính milisec) kể từ hàm gọi có tín hiệu PIN định trước, hay trả không nhận tín hiệu / thời gian timeout Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO 14 CẢM BIẾN MÀU ( TCS3200) Module cảm biến màu TCS3200 module cảm biến phát đầy đủ màu sắc, bao gồm cảm biến màu TCS3200 với khả nhận biết màu RGB đèn LED trắng Các TCS3200 phát đo lường gần tất màu sắc nhìn thấy Ứng dụng bao gồm kiểm tra đọc dải, phân loại theo màu sắc, cảm biến ánh sáng xung quanh hiệu chuẩn, kết hợp màu sắc, vài ứng dụng TCS3200 có tách sóng quang, có lọc màu sắc lọc màu đỏ, xanh, màu xanh, khơng có lọc ( rõ ràng) Các lọc màu phân bố khắp mảng để loại bỏ sai lệch vị trí điểm màu Bên dao động tạo đầu sóng vng có tần số tỷ lệ thuận với cường độ màu sắc lựa chọn Thông số kỹ thuật: Điện áp cung cấp: (2.7V đến 5.5V) Chuyển đổi từ cường độ ánh sáng sang tần số với độ phân giải cao Có khả lập trình để nhận biết đầy đủ màu sắc Điện tiêu thụ thấp Giao tiếp trực tiếp với vi điều khiển S0 ~ S1: Dùng để lựa chọn tỉ lệ tần số đầu S2 ~ S3: Dùng để lựa chọn kiểu photodiode OUT Pin: Đầu tần số OE Pin: Tần số đầu cho phép hoạt động (hoạt động mức thấp) Hỗ trợ đèn LED bổ sung kiểm soát ánh sáng Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Kích thước: 28.4x28.4mm Nguyên tắc nhận biết màu: TCS3200 có lọc màu, lựa chọn lọc màu cho phép nhận biết màu màu khác bị chặn Ví dụ, lựa chọn lọc màu đỏ, Chỉ có ánh sáng tới màu đỏ thông qua, màu xanh màu xanh ngăn chặn Vì nhận cường độ ánh sáng màu đỏ Tương tự vậy, lựa chọn lọc khác mà nhận ánh sáng màu xanh màu xanh Cách thiết lập màu sắc cho TCS3200: Tần số đầu TCS3200 khoảng 2HZ~500KHZ Tần số đầu có dạng xung vng với tần số khác mà màu sắc khác cường độ sáng khác Chúng ta lựa chọn tỉ lệ cừng độ sáng màu sắc theo bảng sau Trên module có chân LED EN chân điều khiển đèn LED để tắt bật Cách thiết lập tần số cho TCS3200: Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Sơ đồ nối dây: Lập trình điều khiển: #define S0 #define S1 #define S2 #define S3 #define sensorOut int frequency = 0; void setup() { 10 pinMode(S0, OUTPUT); 11 pinMode(S1, OUTPUT); 12 pinMode(S2, OUTPUT); 13 pinMode(S3, OUTPUT); 14 pinMode(sensorOut, INPUT); 15 16 // Setting frequency-scaling to 20% 17 digitalWrite(S0,HIGH); 18 digitalWrite(S1,LOW); 19 20 Serial.begin(9600); 21 } 22 23 void loop() { 24 // Setting red filtered photodiodes to be read 25 digitalWrite(S2,LOW); 26 digitalWrite(S3,LOW); Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO 27 frequency = pulseIn(sensorOut, LOW); 28 Serial.print("R= ");//printing name 29 Serial.print(frequency);//printing RED color frequency 30 Serial.print(" "); 31 delay(100); 32 digitalWrite(S2,HIGH); 33 digitalWrite(S3,HIGH); 34 frequency = pulseIn(sensorOut, LOW); 35 Serial.print("G= ");//printing name 36 Serial.print(frequency);//printing RED color frequency 37 Serial.print(" "); 38 delay(100); 39 digitalWrite(S2,LOW); 40 digitalWrite(S3,HIGH); 41 frequency = pulseIn(sensorOut, LOW); 42 Serial.print("B= ");//printing name 43 Serial.print(frequency);//printing RED color frequency 44 Serial.println(" "); 45 delay(100); 46 } Sau upload chương trình kết nối xong lấy mẫu giấy màu đưa sát vào led màu trắng cảm biến, sau mở serial port lên xem giá trị trả màu Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO 15 CẢM BIẾN GIA TỐC ( MPU 6050) Cảm biến gia tốc MPU 6050 cảm biến có trục thiết kế cho ứng dụng nguồn ni thấp, giá rẻ, có hiệu xuất cao, ứng dụng điện thoại máy tình bảng, thiết bị di động khác MPU-6000/6050 gồm có trục quay hồi chuyển (gyroscope) trục gia tốc (accelerometer ) thiết kế nhân chip Digital Motion Processor (DMP) sử dụng thuật toán MotionFusion thuật tốn MotionFusion có khả giao tiếp với số cảm biến khác qua chân I2C phụ trợ (auxiliary master I2C bus), giúp cho thiết bị trang bị đầy đủ loại cảm biến hệ thống điều khiển Board MPU6050 thiết kế với IC nguồn 3.3V trện board trở kéo bus I2C Thông số kĩ thuật cảm biến gia tốc MPU 6050 : - Điện áp cấp: 3~5v - góc quay hồi chuyển với độ nhạy lên đến 131 LSBs/sps đầy đủ độ ±250, ±500, ±1000, and ±2000dps - góc gia tốc kế với đầy đủ khả lập trình với ±2g, ±4g, ±8g and ±16g - Kích thước: 14 x 21mm Sơ đồ nối dậy: Web: tdhshop.com.vn - Chuyên Kit TỰ HỌC ARDUINO SỔ TAY ARDUINO Lập trình điều khiển: Trƣớc hết ta phải cài thƣ viện MPU6050 I2C vào arduino IDE #include const int MPU_addr=0x68; // I2C address of the MPU-6050 int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; void setup(){ Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); Serial.begin(9600); } void loop(){ Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers AcX=Wire.read()