Đồ Án Tốt Nghiệp robot xe hai bánh tự cân bằng

Khoa: Điện tử viễn thông robot cân SVTH : Nhóm TRƯỜNG ĐẠI HỌC ĐIỆN LỰC KHOA ĐIỆN TỬ VIỄN THÔNG THIẾT KẾ, CHẾ TẠO ROBOT HAI BÁNH TỰ CÂN BẰNG GIẢNG VIÊN HƯỚNG DẪN : THS TRẦN TRỌNG THẮNG Hà SINH VIÊN THỰC HIỆN : TRẦN ĐÌNH PHÚ : BÙI ĐỨC CƯỜNG CHUYÊN NGÀNH : ĐIỆN TỬ VIỄN THÔNG LỚP : Đ6-ĐTVT2 KHÓA : 2011-2016 Nội, năm 2016 TRƯỜNG ĐẠI HỌC ĐIỆN LỰC KHOA ĐIỆN TỬ VIỄN THÔNG Hà Nội, năm 2016 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ở GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm thành cử nhân hay kỹ sư đóng góp học cho phát triển đất nước Lời chúng em xin chân thành cảm ơn hướng dẫn tận tình thầy ThS Trần Trọng Thắng, khoa Điện tử-Viễn thông, trường Đại học Điện lực Trong suốt thời gian thực đề tài, thầy dành nhiều thời gian để hướng dẫn chúng em thực đề tài Thầy hướng dẫn chúng em tìm hiểu kiến thức cần thiết để thực đề tài Bên cạnh chúng em xin gởi lời cảm ơn chân thành đến thầy ThS Hoàng Xuân Đơng, thầy 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 Điện tử-Viễn thông, thầy cô trường giảng dạy, giúp đỡ chúng em suốt bốn năm học qua Chính thầy cô xây dựng cho chúng em kiến thức tảng kiến thức chuyên môn để em hồn thành đề tài Hà nội, ngày tháng năm 2016 SINH VIÊN THỰC HIỆN Trần Đình Phú Bùi Đức Cường MỤC LỤC GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm Danh mục hình ảnh CHƯƠNG 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 GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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.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 xác định trọng tâm nằm bánh xe ta cho xe di chuyển nhằm triệt tiêu góc nghiêng xe GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm Hình 1.2: 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 thể loại ba bánh hay bốn bánh Những mobile robot xây dựng hầu hết robot robot di chuyển ba bánh xe, với hai bánh lái lắp ráp đồng trục, bánh đuôi nhỏ Có nhiều kiểu khác nhau, kiểu thơng dụng Còn xe bánh, thường đầu xe có hai bánh truyền động đầu xe lại gắn hai bánh lái Hình 1.3: Xe ba bánh mặt phẳng Việc thiết kế ba hay bốn bánh làm cho xe/mobile robot thăng ổn định nhờ trọng lượng chia cho hai bánh lái bánh đi, hay khác để đỡ trọng lượng xe Nếu trọng lượng đặt nhiều vào bánh lái xe/robot khơng ổn định GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm dễ bị ngã, đặt nhiều vào bánh hai bánh khả bám Nhiều thiết kế xe/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 xuống đồi, trọng lượng xe/robot dồn vào đầu xe làm bánh lái khả bám trượt ngã, bậc thang, chí dừng hoạt động quay tròn bánh xe Hình 1.4: Xe bánh lên Khi di chuyển lên đồi, việc tệ hơn, trọng tâm thay đổi phía sau chí làm xe/robot bị lật úp di chuyển bậc thang.Việc bố trí bốn bánh xe, giống xe đồ chơi hay loại xe bốn bánh sử dụng giao thông không gặp vấn đề điều làm xe/robot khơng gọn Hình 1.5: Xe bánh xuống GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm Ngược lại, xe dạng hai bánh đồng trục lại thăng linh động di chuyển địa hình dốc, thân hệ thống khơng ổn định Khi leo sườn dốc, tự động nghiêng trước giữ cho trọng lượng dồn hai bánh lái Tương tự vậy, bước xuống dốc, nghiêng sau giữ trọng tâm rơi vào bánh lái Chính vậy, khơng có tượng trọng tâm xe rơi vùng đỡ bánh xe để gây lật úp Hình 1.6: Hai bánh lên xuống linh động 1.4 Mục tiêu đồ án 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.Trong khuôn khổ tháng thực luận văn tốt nghiệp, mục tiêu đề sau: • Tìm hiểu loại robot cân bằng, nguyên lý cân • Thiết kế mạch điện tử kết hợp cảm biến thực chức đo góc (phần cứng) GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm • Giải thuật cho vi điều khiển kết hợp bù trừ cảm biến để có giá trị đo góc xác • Xây dựng thuật tốn điều khiển cho động cơ, giữ thăng • Lập trình điều khiển 1.5 Phương pháp nghiên cứu Đề tài tiếp cận dựa phương pháp sau: • Phương pháp khảo sát tài liệu, tìm hiểu tài liệu liên quan đến đến đề tài như: cấu trúc robot hai bánh tự cân bằng, cảm biến MPU6050, mạch điều khiển động • Phương pháp khảo sát thuật toán lọc nhiễu cho cảm biến như: lọc Kalman thuật tốn điều khiển PID • Phương pháp thực nghiệm tiến hành xây dựng thuật toán mơ hình 1.6 Giới hạn đồ án Đề tài tập trung vào việc xây dựng mơ hình phần cứng robot như: kết cấu mơ hình, mạch điều khiển động cơ, thuật toán vi điều khiển lọc Kalman giải thuật cân PID Robot cân có khả điều khiển để di chuyển chưa có nhiều ứng dụng GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm CHƯƠNG THIẾT KẾ CHẾ TẠO 2.1 Sơ đồ khối Cảm biến MPU6050 2.1 i2c Arduino UNO R3I2C Mạch điều khiển PWM động DC 2.2 Khối nguồn Hình 2.1: Sơ đồ khối hệ thống robot GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 2.2 Thiết kế phần cứng Đồ án sử dụng Arduino Uno R3 điều khiển trung tâm Khung robot dc chế tạo từ miếng nhựa cứng liên kết với vít đồng với hai động đặt đồng trục cho phép robot di chuyển theo hai hướng trước sau Hai động điều khiển robo shield, sử dụng cảm biến MPU6050 gián vào mặt phẳng khung nhựa để xác định góc nghiêng robot Hai bánh xe bao bọc cao su có nhiều rãnh để ăng độ bám cao, giúp robot điều khiển cân tốt Hình 2.2: Mơ hình robot sau lắp ráp 2.3 Mạch điện tử 2.3.1 Nguồn điện GVHD : ThS Trần Trọng Thắng Page 10 Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm 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 Hình 2.25: Mơ hình xe chế tạo TÀI LIỆU THAM KHẢO 1) http://www.Google.com 2) http://www.Arduino360.com GVHD : ThS Trần Trọng Thắng Page 52 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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 5) 6) 7) 8) Tuấn Đạt Arduino.cc 123doc.org http://www.instructables.com http://www.Arduino.vn PHỤ LỤC Phụ lục 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 GVHD : ThS Trần Trọng Thắng Page 53 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm #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 GVHD : ThS Trần Trọng Thắng Page 54 Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm long frequency = 15000; // Tan so PWM cho dong co 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); GVHD : ThS Trần Trọng Thắng Page 55 Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm 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() { // 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; GVHD : ThS Trần Trọng Thắng Page 56 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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 pinMode(LED_PIN, OUTPUT); GVHD : ThS Trần Trọng Thắng Page 57 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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(); GVHD : ThS Trần Trọng Thắng Page 58 Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm 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 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 { GVHD : ThS Trần Trọng Thắng Page 59 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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 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 GVHD : ThS Trần Trọng Thắng Page 60 Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm // Thiet lap ma tran phuong sai loi P[0][0] = 0; 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; GVHD : ThS Trần Trọng Thắng Page 61 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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; }; 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; }; GVHD : ThS Trần Trọng Thắng Page 62 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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 Chương trình thư viện PID #ifndef PID_v1_h #define PID_v1_h #define LIBRARY_VERSION 1.0.0 class PID { GVHD : ThS Trần Trọng Thắng Page 63 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm public: // Khai bao cac chuc nang su dung #define AUTOMATIC #define MANUAL #define DIRECT #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 (non0) 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 8bit 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(); GVHD : ThS Trần Trọng Thắng Page 64 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm 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; double ITerm, lastInput; unsigned long SampleTime; double outMin, outMax; bool inAuto; }; GVHD : ThS Trần Trọng Thắng Page 65 Khoa: Điện tử viễn thông robot cân SVTH : Nhóm #endif lastTime = now; } void SetTunings(double Kp, double Ki, double Kd) { kp = Kp; ki = Ki; kd = Kd; } GVHD : ThS Trần Trọng Thắng Page 66 ... 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... 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 thể loại ba bánh hay bốn bánh Những mobile robot xây dựng hầu hết robot robot di chuyển ba bánh xe, với hai bánh lái lắp... xe GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thơng robot cân SVTH : Nhóm Hình 1.2: 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