Điều khiển xe tự cân bằng sử dụng arduino đồ án tốt nghiệp khoa công nghệ điện

ĐẠI HỌC CÔNG NGHIỆP TP.HCM KHOA CÔNG NGHỆ ĐIỆN ĐỒ ÁN TỐT NGHIỆP ĐIỀU KHIỂN XE CÂN BẰNG SỬ DỤNG ARDUINO SINH VIÊN : BÙI DUY TIN MSSV : 14064861 LỚP : DHDKTD10B GVHD TUẤN :THS NGUYỄN NGỌC ANH Đồ án tốt nghiệp TP HCM, NĂM 2018 PHIẾU GIAO ĐỀ TÀI ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên/nhóm sinh viên đƣợc giao đề tài Bùi Duy Tin, MSSV: 14064861 Tên đề tài ĐIỀU KHIỂN XE CÂN BẰNG SỬ DỤNG ARDUINO Nội dung Mục tiêu đồ án xây dựng robot có khả di chuyển hai bánh,làm phương tiện di chuyển hiệu linh động, dễ dàng xoay trở không gian chật hẹp Kết Ơn lại tìm hiểu thêm kiến thức điện tử, động lực học Sử dụng thành công lọc kalman mơ hình robot để tính tốn góc Xây dựng thành cơng giải thuật cân bám theo vị trí ban đầu dựa thuật toán PID Giảng viên hướng dẫn Tp HCM, ngày tháng Sinh viên i năm 20 NHẬN XÉT CỦA GIẢNG VIÊN HƢỚNG DẪN - ii MỤC LỤC MỤC LỤC iii DANH SÁCH HÌNH VẼ v DANH SÁCH BẢNG BIỂU vii CHƢƠNG 1: TỔNG QUAN VỀ ROBOT HAI BÁNH TỰ CÂN BẰNG 1.1 Giới thiệu chung 1.2 Thế robot hai bánh tự cân .1 1.3 Tại phải thiết kế robot hai bánh tự cân 1.4 Mục tiêu đồ án 1.5 Phƣơng pháp nghiên cứu .4 1.6 Giới hạn đồ án CHƢƠNG 2: THIẾT KẾ CHẾ TẠO 2.1 Sơ đồ khối 2.2 Thiết kế phần cứng 2.3 Mạch điện tử 2.3.1 Nguồn điện .8 2.3.2 Bộ điều khiển trung tâm Aduino Mega 2560 2.3.3 Mạch điều khiển động robo shield 15 2.3.4 Cảm biến MPU6050 .16 2.3.5 Động Encoder 334 Xung + Giảm tốc RP201 18 2.4 Cách phƣơng thức giao tiếp .18 2.4.1 Điều chế độ rộng xung PWM 19 2.4.2 Giao thức I2C .21 2.4.3 Giao tiếp UART 23 2.5 Giải thuật lập trình .24 iii 2.5.1 Nguyên lý điều khiển cân 24 2.5.2 Bộ lọc Kalman 25 2.5.3 Giải thuật điều khiển PID 36 2.5.4 Nguyên lý hoạt động 40 CHƢƠNG 3: THỬ NGHIỆM VÀ KẾT LUẬN 41 3.1 Thử nghiệm 41 3.2 Kết 41 3.3 Hƣớng phát triển .41 TÀI LIỆU THAM KHẢO 42 PHỤ LỤC .43 LỜI CẢM ƠN 52 iv DANH SÁCH HÌNH VẼ Hình 1 Mơ tả nguyên lý cân Hình Mơ tả cách di chuyển .2 Hình Xe ba bánh mặt phẳng .2 Hình Xe ba bánh lên Hình Xe ba bánh xuống Hình Hai bánh lên xuống linh động Hình Sơ đồ khối hệ thống robot Hình 2 Mơ hình robot sau lắp ráp Hình Pin .8 Hình Arduino Mega 2560 Hình Sơ đồ chân Arduino .12 Hình Trình biên dịch IDE 14 Hình Mạch điều khiển động robo shield .15 Hình Chân giao tiếp MPU6050 .17 Hình Động Encoder 18 Hình 10 Điều chỉnh độ rộng xung .19 Hình 11 Quan hệ vận tốc khơng tải động DC chu kỳ PWM cho động DC Hitachi 20 Hình 12 Kết nối Master Slave 21 Hình 13 Trạng thái lấy mẫu 1bit liệu SDA 22 Hình 14 Sơ đồ kết nối Arduino với cảm biến MPU6050 theo giao thức I2C 23 Hình 15 Nguyên lý điều khiển .24 Hình 16 Mơ hình lọc 26 Hình 17 Tín hiệu chưa lọc 26 Hình 18 Tín hiệu qua lọc Kalman .27 Hình 19 Quy trình lọc Kalman .29 Hình 20 Quy trình hồn chỉnh bọ lọc Kalman 30 Hình 21 Hệ thống điều khiển vịng kín 36 v Hình 22 Đồ thị biêu diễn hệ số PID theo thời gian 38 Hình 23 Sơ đồ giải thuật xe tự cân 40 vi DANH SÁCH BẢNG BIỂU Bảng Chi tiết cổng Serial 13 Bảng 2 Chức chân MPU6050 .18 Bảng Ảnh hưởng hệ số PID 37 Bảng Độ lợi hệ số PID 39 vii Đồ án tốt nghiệp SV: Bùi Duy Tin CHƢƠNG 1: TỔNG QUAN VỀ ROBOT HAI BÁNH TỰ CÂN BẰNG 1.1 Giới thiệu chung Cùng với phát triển khoa học kỹ thuật, ngày robot có khả thay người làm việc môi trường độc hại, sản xuất bắt chước người hình thức, hành vi suy nghĩ Hiện lĩnh vực robot phát triển nhanh nhờ vào phát triển liên tục công nghệ, robot chế tạo để phục vụ cho nhiều mục đích khác Với ý tưởng sử dụng robot thay người “Robot hai bánh tự cân bằng” đề tài nhiều tác giả quan tâm mơ hình robot có khả di chuyển linh hoạt lại không chiếm nhiều không gian 1.2 Thế robot hai bánh tự cân Hình 1 Mơ tả ngun lý cân Đối với xe hay robot ba hay bốn bánh, việc thăng ổn định chúng nhờ trọng tâm chúng nằm bề mặt chân đế bánh xe tạo Đối với xe bánh có cấu trúc xe đạp, việc thăng khơng di chuyển hồn tồn khơng thể, việc thăng xe dựa tính chất quay hồi chuyển hai bánh xe quay Còn xe hay robot hai bánh tự cân bằng, loại có hai bánh với trục hai bánh xe trùng nhau, robot cân bằng, trọng tâm cần giữ nằm bánh xe Điều giống ta giữ gậy dựng thẳng đứng cân lòng bàn tay Thực ra, trọng tâm tồn robot khơng biết nằm vị trí nào, khơng có cách tìm Do vậy, thay tìm cách Đồ án tốt nghiệp SV: Bùi Duy Tin 2.5.3.2 Xây dựng giải thuật PID cho robot cân Hình 22 Đồ thị biêu diễn hệ số PID theo thời gian * Thành phần tích phân: * Thành phần vi phân: Do lấy gần ∆t = є >0 thì: Tóm lại: 38 Đồ án tốt nghiệp SV: Bùi Duy Tin Đặt: Trong công thức (1) thời gian lấy mẫu ∆t nhỏ, nên bỏ qua ∆t Khi tìm hệ số Kp, Ki, Kd bao gồm ∆t Khi cơng thức lại viết lại sau: Điều khiện biên: u(0) = duty >  Tìm hệ số PID phương pháp thực nghiệm:  Cho hệ số Kp, Ki, Kd không  Tăng Kp từ nhỏ đến lớn ngõ đáp ứng với rung động dao động ổn định  Giảm Kp xuống nửa giá trị phản ứng  Tăng Ki từ nhỏ đến lớn, quan sát đáp ứng hệ thống thông số thay đổi Tăng dao động mức cho phép  Cuối tăng Kd hệ thống đạt tối ưu  Tìm hệ số PID phương pháp Ziegler-Nichols Độ lợi Ki Kd lúc đầu gán độ lợi P tăng P tiến tới độ lợi tới hạn Ku đầu vòng điều khiển bắt đầu dao động Ku thời gian dao động Pu dùng để gán độ lợi sau: Bảng Độ lợi hệ số PID 39 Đồ án tốt nghiệp SV: Bùi Duy Tin 2.5.4 Nguyên lý hoạt động  Đầu tiên, MPU6050 đọc liệu từ cảm biến gia tốc quay hồi chuyển liệu thông qua giao thức I2C  Thứ hai, lọc Kalman cụ thể tích hợp liệu từ máy đo gia tốc quay hồi chuyển để tiếp cận với góc độ thực tế độ nghiêng  Thứ ba, tính tốn tốc độ hướng bánh xe tương ứng.sử dụng giải thuật PID để tính tốn điều khiển chuyển động xe, để xe tiếp tục tự cân  Thứ tư, điều khiển động Các kết thuật toán PID biến thành lệnh để điều khiển chuyển động động giúp xe cân Hình 23 Sơ đồ giải thuật xe tự cân 40 Đồ án tốt nghiệp SV: Bùi Duy Tin CHƢƠNG 3: THỬ NGHIỆM VÀ KẾT LUẬN 3.1 Thử nghiệm Kết thực nghiệm mơ hình robot cho thấy robot có khả giữ cân vị trí đặt ban đầu Trong q trình robot giữ cân có lực tác động từ bên theo phương ngang làm robot nghiêng(khơng q mạnh) robot tiếp tục giữ cân di chuyển quay lại vị trí đặt ban đầu 3.2 Kết *Kết đạt đồ án:  Ơn lại tìm hiểu thêm nhiều kiến thức động lực học,điện tử  Sử dụng thành công lọc kalman mô hình robot để tính tốn góc  Xây dựng thành cơng giải thuật cân bám theo vị trí ban đầu dựa thuật toán PID  Làm quen với lĩnh vực lập trình chế tạo khí  Nghiên cứu bước đầu chế tạo thành công mơ hình robot đơn giản 3.3 Hƣớng phát triển Dựa kết robot chạy thành cơng,chúng em có ý tưởng phát triển thêm để chế tạo thành sản phẩm chở người.Ý tưởng đưa dựa vào số yêu cầu thực tế tạo phương tiện giúp người di chuyển thuận tiện nhỏ gọn di chuyển nhà,phục vụ việc giải trí dùng số công việc vận chuyển hàng hóa,đồ đạc.Bước đầu chúng em lắp đặt phần khung xe,xe di chuyển thời gian hạn chế nên sản phẩm chưa hoàn thiện.Hiện chúng em tiến hành thực phát triển thêm 41 Đồ án tốt nghiệp SV: Bùi Duy Tin TÀI LIỆU THAM KHẢO 1) http://www.Google.com 2) http://www.Arduino360.com 3) Alwafi Husein, Attitude and altitude control of two wheel trirotor hybrid robot,Master‟s thesis, 2013 4) Xe hai bánh tự cân di chuyển địa hình phẳng Mai Tuấn Đạt 5) Hệ thống điều khiển phi tuyến, Huỳnh Thái Hoàng 6) Lý thuyết điều khiển tự động, Huỳnh Thái Hồng 7) Hệ thống điều khiển thơng minh, Huỳnh Thái Hoàng 8) http://www.Arduino.vn 9) Arduino.cc 10) http://www.intructables.com 42 Đồ án tốt nghiệp SV: Bùi Duy Tin PHỤ LỤC Phụ lục 1: Chƣơng trình #include "I2Cdev.h" //Thu vien I2C #include "MPU6050.h" // Thu vien MPU #include "Kalman.h" // Thu vien Kalman #include "pwm01.h" // Thu vien xuat xung PWM #include // Thu vien PID cua arduino #include // Thu vien ngat timer cua Arduino #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 accelgyro; Kalman kalmanY; int16_t accX, accY, accZ; int16_t tempRaw; int16_t gyroX, gyroY, gyroZ; uint32_t timer; uint16_t PWM_L,PWM_R,PWM_T; bool dir = 0; bool play = 0; double CounterEncoder= 0; double Value; double accXangle, accYangle; // Goc cua cam bien gia toc double temp; double gyroXangle, gyroYangle; // Goc cam bien quay hoi chuyen double Setpoint, Input, Output; double Kp,Ki,Kd; // Thong so phan PID double kalAngleX, kalAngleY; // Goc tinh toan su dung bo loc Kalman long frequency = 15000; // Tan so PWM cho dong co 43 Đồ án tốt nghiệp SV: Bùi Duy Tin char PIN_PWM_L = 8; // Chan xuat xung PWM o dong co trai char PIN_DIR_L = 23; // Chan chieu cua dong co trai char PIN_PWM_R = 9; // Chan xuat xung PWM o dong co phai char PIN_DIR_R = 25; // Chan chieu cua dong co phai char PIN_BUTTON = 27; // nut nhan khoi dong pid #define LED_PIN 13 double Angle; //Goc tren robot double SetAngle = 88; // Goc robot o vi tri can bang PID myPID(&Input, &Output, &Setpoint,0,0,0, DIRECT); // Cat dat thong so PID #define OUTPUT_READABLE_ACCELGYRO void timerIsr() // Ham ngat timer ms { Angle = kalAngleY; if(Angle (SetAngle + 20)) Angle= SetAngle + 20; SetAngle=86-Value; Input = SetAngle-Angle; myPID.Compute(); // Tinh toan PID (Output >= 0)? dir=0:dir=1; // Xac dinh chieu de robot can bang digitalWrite(PIN_DIR_L,dir); digitalWrite(PIN_DIR_R,dir); PWM_T=4095-abs(Output); PWM_L= PWM_T; PWM_R= PWM_T*0.98; pwm_write_duty( PIN_PWM_R, PWM_R ); // Xuat xung PWM dieu khien dong co pwm_write_duty( PIN_PWM_L, PWM_L ); } void setup() { 44 Đồ án tốt nghiệp SV: Bùi Duy Tin // Cai dat I2C #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // Cat dat UARRT voi toc baud la 9600 Serial.begin(9600); // Cat dat thong so dau cho PMU accelgyro.initialize(); accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; kalmanY.setAngle(accYangle); gyroYangle = accYangle; timer = micros(); // Cac thong so PID Kp=90; Ki=8500; Kd=0.3; Setpoint = 0; // Thong so cat dat ban dau cua PID myPID.SetOutputLimits(-2000,2000); // Gioi han dau cua PID myPID.SetTunings(Kp,Ki,Kd); myPID.SetMode(AUTOMATIC); // PID chay o che Auto // Cai dat thong so xuat xung PWM pwm_set_resolution(12); // Xung PWM voi 12 bit pwm_setup( 6, frequency, 1); pwm_setup( 7, frequency, 2); pwm_setup( 8, frequency, 2); pwm_setup( 9, frequency, 2); pwm_write_duty( PIN_PWM_L, 4095); pwm_write_duty( PIN_PWM_R, 4095); // Cai dat che OUTPUT/INPUT 45 Đồ án tốt nghiệp SV: Bùi Duy Tin pinMode(LED_PIN, OUTPUT); pinMode(PIN_DIR_L, OUTPUT); pinMode(PIN_DIR_R, OUTPUT); attachInterrupt(39, Encoder, RISING); // Ngat ngoai canh xuong cho encoder pinMode(39, INPUT); pinMode(41, INPUT); pinMode(PIN_BUTTON, INPUT); } void loop() { accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); // Doc du lieu I2C tu cam bien IMU accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; // Tinh toan doi tu radian sang double gyroYrate = -((double)gyroY/131.0); gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Tinh toan thong so goc quay chua qua bo loc Kalman kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()timer)/1000000); // Tinh toan goc quay qua bo loc Kalman timer = micros(); temp = ((double)tempRaw + 12412.0) / 340.0; if(play==0) // Kiem tra nut de robot thuc hien can bang { if(digitalRead(PIN_BUTTON)) { play=1; Timer3.attachInterrupt(timerIsr); // Cat dat ngat timer Timer3.start(1000); // Cai dat thoi gian ngat la ms } } // Tinh toan sai so tren encoder 46 Đồ án tốt nghiệp SV: Bùi Duy Tin Value=(CounterEncoder/3300); Value = constrain(Value,-5,5); // Dua du lieu qua cong UART Serial.print(CounterEncoder); Serial.print("\t"); Serial.print(Value); Serial.print("\t"); Serial.print(SetAngle); Serial.print("\t"); Serial.println(kalAngleY); } void Encoder() // Ham ngat tinh toan encoder { if(((digitalRead(39)==0)&&(digitalRead(41)==1)) || ((digitalRead(39)==1) && (digitalRead(41)==0))) CounterEncoder ; else if(((digitalRead(39)==0)&&(digitalRead(41)==0)) || ((digitalRead(39)==1) && (digitalRead(41)==1))) CounterEncoder++; } Phụ lục 2: Chƣơng trình thƣ viện lọc Kalman / Dinh nghia bo loc Kalman #ifndef _Kalman_h #define _Kalman_h class Kalman { public: Kalman() { // Khai bao cac bien co dinh cho bo loc Kalman cac he so co the duoc thay doi Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03; bias = 0; // Reset bias // Thiet lap ma tran phuong sai loi P[0][0] = 0; 47 Đồ án tốt nghiệp SV: Bùi Duy Tin P[0][1] = 0; P[1][0] = 0; P[1][1] = 0; }; // dua cac thong so newAngle, newrate va delat time moi giay vao gia tri goc tinh toan double getAngle(double newAngle, double newRate, double dt) { // cap nhat thoi gian (uoc luong) // uoc luong gia tri moi rate = newRate - bias; angle += dt * rate; // Uoc luong ma tran covariance tiep theo P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt; // Cap nhat gia tri (hieu chinh) // Cap nhat gia tri Zk S = P[0][0] + R_measure; K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Tinh loi Kalam y = newAngle - angle; angle += K[0] * y; bias += K[1] * y; // Cap nhat lai ma tran covariance P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; P[1][1] -= K[1] * P[0][1]; return angle; }; 48 Đồ án tốt nghiệp SV: Bùi Duy Tin void setAngle(double newAngle) { angle = newAngle; }; / / su dung de thiet lap goc, co nghia la goc bat dau double getRate() { return rate; }; // tro lai muc rate / * Khai bao cac chuong trinh de dieu chinh bo loc Kalman * / void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; void setRmeasure(double newR_measure) { R_measure = newR_measure; }; private: / * lọc biến Kalman * / double Q_angle; // qua trinh nhieu phuong sai cho gia toc double Q_bias; // qua trinh nhieu phuong sai cho quay hoi chuyen double R_measure; // Do dac nhieu phuong sai double angle; // cac goc tinh boi bo loc Kalman – mot phan ma tran x double bias; // quay hoi chuyen tinh boi bo loc Kalman - bang x 1matrix double rate; // cap nhat ti le getAngle double P[2][2]; // Ma tran hiep phuong sai loi – day la ma tran x double K[2]; // loi Kalman – day la ma tran x double y; // goc khac ma tran x1 double S; // uoc luong sai so loi ma tran x }; #endif Phụ lục 3: Chƣơng trình thƣ viện PID #ifndef PID_v1_h #define PID_v1_h #define LIBRARY_VERSION 1.0.0 class PID { public: // Khai bao cac chuc nang su dung #define AUTOMATIC #define MANUAL #define DIRECT 49 Đồ án tốt nghiệp SV: Bùi Duy Tin #define REVERSE // Cac chuc nang su dung PID(double*, double*, double*, // * Lien ket PID ngo vao double, double, double, int); // Thiet lap cac thong so dieu chinh void SetMode(int Mode); // dieu khien he PID bang tay (0) tu dong (non-0) bool Compute(); // Thuc hien tinh toan PID, thiet lap tan so va thoi gian lay mau void SetOutputLimits(double, double); // Gia tri dau 0-255 doi voi 8-bit va tu - 65536 doi voi 16-bit void SetTunings(double, double, double); // Cai dat he so Kp, Ki,Kd void SetControllerDirection(int); // * Dieu khien truc tiep void SetSampleTime(int); // * Cai dat tan so theo mili giay // Hien thi chuc nang // Dat cac he so PID double GetKp(); double GetKi(); double GetKd(); int GetMode(); int GetDirection(); private: void Initialize(); double dispKp; double dispKi; double dispKd; double kp; double ki; double kd; int controllerDirection; double *myInput; // Lien ket cac bien dau vao, dau diem dat double *myOutput; // Lien ket bien ngo double *mySetpoint; // He so dat ban dau unsigned long lastTime; 50 Đồ án tốt nghiệp SV: Bùi Duy Tin double ITerm, lastInput; unsigned long SampleTime; double outMin, outMax; bool inAuto; }; #endif lastTime = now; } void SetTunings(double Kp, double Ki, double Kd) { kp = Kp; ki = Ki; kd = Kd; } 51 Đồ án tốt nghiệp SV: Bùi Duy Tin LỜI CẢM ƠN Trong thời gian đề tài nghiên cứu khoa học bước ngoặt cuối đánh dấu trưởng thành sinh viên giảng đường đại học.Để trở thành cử nhân hay kỹ sư đóng góp học cho phát triển đất nước Lời em xin chân thành cảm ơn hướng dẫn tận tình thầy ThS Nguyễn Ngọc Anh Tuấn, khoa Công nghệ Điện, trường Đại học Cơng Nghiệp Tp Hồ Chí Minh Trong suốt thời gian thực đề tài, thầy dành nhiều thời gian để hướng dẫn em thực đề tài Thầy hướng dẫn em tìm hiểu kiến thức cần thiết để thực đề tài, ln nhiệt tình hướng dẫn, sẵn sàng giúp đỡ chúng em gặp khó khăn việc lập trình, chế tạo robot Em xin chân thành cảm ơn thầy cô khoa Công nghệ Điện, thầy cô trường giảng dạy, giúp đỡ em suốt bốn năm học qua Chính thầy xây dựng cho em kiến thức tảng kiến thức chun mơn để em hồn thành đề tài TP Hồ Chí Minh, ngày 03 tháng 06 năm 2018 52 .. .Đồ án tốt nghiệp TP HCM, NĂM 2018 PHIẾU GIAO ĐỀ TÀI ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên/nhóm sinh viên đƣợc giao đề tài Bùi Duy Tin, MSSV: 14064861 Tên đề tài ĐIỀU KHIỂN XE CÂN BẰNG SỬ DỤNG ARDUINO. .. thăng xe dựa tính chất quay hồi chuyển hai bánh xe quay Còn xe hay robot hai bánh tự cân bằng, loại có hai bánh với trục hai bánh xe trùng nhau, robot cân bằng, trọng tâm cần giữ nằm bánh xe Điều. .. nghiêng xe Hình Mơ tả cách di chuyển 1.3 Tại phải thiết kế robot hai bánh tự cân Việc thiết kế robot hai bánh tự cân tảng để phát triển xe hai bánh tự cân sau ta cần so sánh xe hai bánh tự cân với