ĐỒ án học PHẦN 2 đề tài mô HÌNH ROBOT HAI BÁNH tự cân BẰNG

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 III NGÀY GIAO NHIỆM VỤ: 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 q 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, 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 q thầy bạn sinh viên Sau nhóm thực xin chúc Thầy cô 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 Các sở lý thuyết 2.1 Đặc tính động lực học 2.1.1 Mơ hình hóa rob 2.1.2 Matlab Simulink 2.2 Mơ hình hóa rob Bộ lọc Kalman 2.2.1 Giới thiệu 2.2.2 Quá trình ước lư 2.2.3 Bản chất xác suấ 2.2.4 Thuật toán Kalm 2.3 Giải thuật điều khiển 2.3.1 Cấu trúc điều 2.3.2 Bộ điều khiển PI 2.3.3 Các thành phần c Chương Thiết kế hệ thống 3.1 Thiết kế phần cứng mơ hình Robot bánh tự câ 3.1.1 Thiết kế khí 3.1.2 Kết nối phần cứn 3.2 Thiết kế phần mềm 3.2.1 Bộ lọc Kalman 3.2.2 Lưu đồ giải thuậ 3.2.3 Code arduino Chương Kết luận hướng phát triển đề tài 4.1 Những kết đạt 4.2 Hạn chế đề tài 4.3 Kết luận 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ã, cịn đặ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 Chương Thiết kế hệ thống 3.1 Thiết kế phần cứng mơ hình Robot bánh tự cân 3.1.1 Thiết kế khí Khung sườn sử dụng chất liệu mica mica màu 4mm, cố định theo cấu ghép rãnh Cơ cấu chuyền động encorder áp bánh bố trí mạch theo chiều dựng đứng Mơ hình Robot thực tế 3.1.2 Kết nối phần cứng Board Arduino Mega 2560 Chân Chân Chân Chân Chân Chân Chân Chân 10 Chân 20 Chân 21 3.2 Thiết kế phần mềm 3.2.1 Bộ lọc Kalman Robot sử dụng hai cảm biến: cảm biến gia tốc (accelecrometer) cảm biến quay hồi chuyển(gyroscope) để đo góc nghiêng vận tốc góc nghiêng.Tuy nhiên, vấn đề đặt cần phải kết hợp thơng tin từ hai cảm biến để xác định xác góc nghiêng thực hệ robot loại bỏ ảnh hưởng nhiễu đo nhiễu trình Để giải vấn đề này, giải thuật lọc Kalman sử dụng, với mục đích ước lượng giá trị góc nghiêng hệ robot từ hai loại cảm biến loại bỏ ảnh hưởng nhiễu Bộ lọc Kalman khảo sát với mơ hình biến trạng thái sau: Hình 3.5: Mơ hình lọc Kalman với biến trạng thái Với mơ hình này, lọc sử dụng biến ngõ vào vận tốc góc nghiêng từ cảm biến quay hồi chuyển góc nghiêng từ cảm biến gia tốc; biến ngõ góc nghiêng ước lượng Ma trận đặc trưng cho tương quan sai số: ℎ Với ℎ angle, q_bias góc, vận tốc sử dụng tính toán lọc R tượng trưng cho giá trị nhiễu coveriance Trong trường hợp này, ma trận 1x1 mong đợi có giá trị 0.08 rad ≈4,5 độ từ cảm biến gia tốc: R_angle=0.08 Q ma trận 2x2 tượng trưng cho tiến trình nhiễu coveriance Trong trường hợp này, mức độ tin cậy cảm biến gia tốc quan hệ với cảm biến gyro: 29 “Giai đoạn dự báo” Trong khoảng chu kỳ quét dt với giá trị sở gyro canh chỉnh tùy theo nhu cầu sử dụng cách lắp khí người dùng module IMU Giai đoạn cập nhật góc thời vận tốc ước lượng Vecto giá trị: = [ Nó chạy ước lượng giá trị qua hàm giá trị: ̇ ̇ =[ − ]=[ _ _ ] [3.5] Và cập nhật ma trận covariance qua hàm : ̇ =+′+ = + ̇ ′ + ý ℎ ế à( = + ) A Jacobian ( ) với giá trị mong đợi : ( ) =[ [3.6] [3.7] [3.8] Để dễ dàng việc lập trình, ma trận P khai triển đến mức tối thiểu : ̇ [ = − [1][1] −1 [0][0] =[ ][ 0 [1][0] − [1 =[ Lưu trữ giá trị ước lượng chưa bias gyro : ==_−_ (Với rate sai số góc tiên đốn) 30 Cập nhật ước lượng góc : ̇ =+=+ Cập nhật ma trận covariance : + = =[ [0][0] [1][0] “Giai đoạn cập nhật giá trị lọc Kalman“ Ma trận C ma trận 1x2 (giá trị x trạng thái), ma trận Jacobian giá trị đo lường với giá trị mong đợi Trong trường hợp C : =[ Vì giá trị góc đáp ứng trực tiếp với góc ước lượng giá trị góc khơng quan hệ với giá trị gyro bias nên C_0 cho thấy giá trị trạng thái quan hệ trực tiếp với trạng thái ước lượng nào, C_1 cho thấy giá trị trạng thái không quan hệ với giá trị sở gyro ước lượng Error giá trị khác giá trị đo lường giá trị ước lượng Trong trường hợp này, khác hai gia tốc kế đo góc góc ước lượng = − [3.15] Tính sai số ước lượng Từ lọc Kalman : =+ =[1 0][ [3.16] _0 = [0] _1 = [0] [0][0] [1][0] [3.17] [3.18] Ước tính lọc Kalman đạt từ lý thuyết lọc Kalman : [ ]=[ _0/ =(1− ] _1/ ) = − = − Ta có phép nhân điểm trơi (floating point) [3.19] [3.20] 31 [0][1]] + [ _− [1][1] [0][1] [3.21] [3.22] Cập nhật giá trị ước lượng Lần nữa, từ Kalman : =[ ]=+( _ _ =_ _ [3.23] [3.24] 3.2.2 Lưu đồ giải thuật điều khiển Dựa tảng thuật toán điều khiển PID, giải thuật cân điều khiển bám theo vị trí ban đầu mơ hình robot xây dựng theo lưu đồ sau: 32 3.2.3 Code arduino Simulink 3.2.3.1 Code arduino #include #include #include Kalman kalmanX; //IMU 6050====================================== ============== int16_t accX, accY, accZ; int16_t tempRaw; int16_t gyroX, gyroY, gyroZ; float accXangle; float gyroXangel; float kalAngelX; unsigned long timer; uint8_t i2cData[14]; float CurrentAngle; / MOTOR================================ ==================== int AIN1 = 4; int AIN2 = 5; int BIN1 = 6; int BIN2 = 7; int CIN1 = 9; int CIN2 = 10; int speed; / PID=================================== ================= const float Kp = 10; const float Ki = 0; const float Kd = 2; float pTerm, iTerm, dTerm, integrated_error, last_error, error; const float K = 1.9*1.12; #define GUARD_GAIN 10.0 #define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t)) void setup() { pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT); pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT); Serial.begin(9600); Wire.begin(); 33 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, KHz sampling i2cData[2] = 0x00; i2cData[3] = 0x00; while(i2cWrite(0x19,i2cData,4,false)); while(i2cWrite(0x6B,0x01,true)); while(i2cRead(0x75,i2cData,1)); if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register Serial.print(F("Error reading sensor")); while(1); } delay(100); //Kalman=================================== ================= while(i2cRead(0x3B,i2cData,6)); accX = ((i2cData[0]