(Đồ án hcmute) robot hai bánh tự cân bằng sử dụng bộ điều khiển fuzzy pid

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN - TỰ ĐỘNG HÓA ROBOT HAI BÁNH TỰ CÂN BẰNG SỬ DỤNG BỘ ĐIỀU KHIỂN FUZZY PID GVHD: ThS NGUYỄN TRẦN MINH NGUYỆT SVTH: NGUYỄN TÂN AN MSSV: 12151157 LÊ MINH TRÍ MSSV: 12151086 SKL 0 Tp Hồ Chí Minh, tháng năm 2016 an TÓM TẮT ĐỀ TÀI Robot hai bánh tự cân đối tượng có tính phi tuyến cao tương đối khơng ổn định Vì vậy, ta phải có phương pháp điều khiển để đưa hệ thống robot hai bánh vị trí cân mong muốn Một phương pháp ứng dụng vào điều khiển Fuzzy-PID Với ưu điểm hệ thống khí khơng q phức tạp, robot hai bánh tự cân sử dụng rộng rãi việc giảng dạy nghiên cứu lý thuyết điều khiển tự động cũng xây dựng điều khiển số Hệ thống bao gồm: hai bánh xe gắn vào bệ robot kết nối với hai động DC thông qua truyền đai, bệ robot gắn với thân robot tượng trưng cho quay lắc ngược Cảm biến Gyro-86 sử dụng để tính góc xoay để đảm bảo thân robot xoay trục động Ta phải điều khiển để thân robot cân vị trí cân cho phép Mục tiêu đề tài xây dựng điều khiển FuzzyPID với luật điều khiển Max-Min để điều khiển đối tượng có độ phi tuyến ổn định Luật điều khiển mô Matlab Simulink kiểm nghiệm vi điều khiển CortexM4F Ưu điểm sử dụng Cortex-M4F để thực thi điều khiển là: giá thành rẻ, sử dụng linh hoạt dễ dàng ứng dụng vào thực tế thương mại hóa Cảm biến sử dụng Gyro giá rẻ đáp ứng yêu cầu đưa i an ABSTRACT Two-wheeled self-balancing robot is an object of high nonlinearity and relatively unstable Therefore, we must have a new control method to give two-wheeled robotic system for the location of the desired balance One of the methods to be applied to control Fuzzy-PID that is With mechanical advantage system is not too complicated, two-wheeled self-balancing robot is widely used in teaching and research control theory as well as the construction of digital controllers The system includes: two wheels are attached to the base of the robot and connected to two DC motors through belt driven, the base of the robot is attached to the body of the robot's rotation represents the inverted pendulum rod Gyro-86 sensor is used to calculate the rotation angel of the robot body to ensure the rotation of the motor shaft Thus the body of the robot will orbit relative around the motor shaft We have to control the robot to be able to balance itself in a position to balance allows The objective of the project is to build Fuzzy-PID controller with Max-Min control law to control objects nonlinearity is stable This control law will be simulated by matlab simulink and tested on the Cortex-M4F microcontroller Advantages when using the Cortex-M4F to execute the controller are: low cost, flexible use and easy application into commercial reality when variables used Gyro sensor price cheap but still meet the request made ii an Lời cảm ơn Nhóm khơng thể theo đuổi hồn thành đề tài luận văn tốt nghiệp vịng 16 tuần khơng có giúp đỡ người thân bạn bè xung quanh Do vậy, với trân trọng cảm kích, nhóm xin gửi lời cảm ơn đến thầy cơ, người thân gia đình bạn bè xung quanh chăm sóc, an ủi gặp trở ngại động viên nhóm để thực luận văn Lời cảm ơn đầu tiên, xin chân thành gửi đến Cơ Nguyễn Trần Minh Nguyệt người tận tình hướng dẫn truyền đạt kiến thức giúp tơi hồn thành luận văn Điều quan trọng Th.S Nguyễn Trần Minh Nguyệt hướng nhóm tìm đường nghiên cứu thuật tốn có tính ứng dụng cao thực tiễn, phù hợp với sở thích niềm đam mê tơi Bên cạnh đó, xin chân thành gửi lời cảm ơn đến quý thầy cô môn Điều Khiển Tự Động: thầy Nguyễn Minh Tâm, cô Nguyễn Trần Minh Nguyệt, thầy Lê Chí Kiên, thầy Trương Đình Nhơn thầy Nguyễn Đình Phú,… cho nhóm kiến thức bổ ích quý giá trình học tập để ứng dụng vào nghiên cứu phát triển đề tài cũng ứng dụng vào công việc sau Tôi cũng xin chân thành gửi lời cảm ơn đến tất bạn đại học khóa 2012 động viên, giúp đỡ, trao đổi kiến thức với suốt khóa học Cuối cùng, tơi xin chân thành cảm ơn quan tâm, hỗ trợ, tạo điều kiện động viên vật chất lẫn tinh thần thành viên gia đình suốt thời gian qua Tp.HCM, ngày 19 tháng 07 năm 2016 Nhóm thực đề tài Nguyễn Tấn An Lê Minh Trí iii an DANH MỤC CÁC TỪ VIẾT TẮT PID: Proportional-Integral-Derivative NE: Negative ZE: Zero PO: Positive NB: Negative Big NS:Negative Small ZE: Zero PS: Positive Small PB: Positive Big iv an DANH MỤC CÁC BẢNG BIỂU Bảng 3.1 Luật suy luận mờ Bảng 3.2 Chọn giá trị cho biến mờ sai số Bảng 3.3 Chọn giá trị cho biến đạo hàm sai số v an DANH SÁCH HÌNH ẢNH VÀ BIỂU MẪU Hình 1.1 Nbot Hình 1.2 Giải thuật điều khiển Nbot Hình 1.3 Robot điều khiển logic mờ Hình 1.4 Balance bot I Hình 1.5 Segway Hình 1.6 Xe hai bánh tự cân HTV nhóm thực Hình 2.1 Mơ hình Ngũn lý giữ thăng Hình 2.2 Mơ tả cách bắt đầu di chuyển Hình 2.3 Trạng thái xe ba bánh di chuyển địa hình phẳng dốc Hình 2.4 Trạng thái xe hai bánh di chuyển địa hình phẳng, dốc Hình 2.5 Miền xác định miền tin cậy tập mờ Hình 2.6 Một số dạng hàm thuộc (Gaussian, PI-shape, S-shape, Sigmoidal, Z-shape) Hình 2.7 Phép giao hai tập mờ Phép hợp Hình 2.8 Phép hợp hai tập mờ Phép bù Hình 2.9 Phép bù tập mờ Hình 2.10 Giải mờ theo phương pháp cực đại Phương pháp trọng tâm Hình 2.11 Sơ đồ cấu trúc điều khiển mờ Hình 2.12 Sơ đồ khối cấu trúc điều khiển Fuzzy-PID Hình 2.13 Bus I2C thiết bị ngoại vi Hình 2.14 Kết nối thiết bị vào bus I2C chế độ chuẩn (Standard chế độ nhanh (Fast mode) Hình 2.15 Truyền nhận thiết bị chủ tớ ( Master – Slave ) Hình 2.16 Điều kiện Start Stop bus I2C Hình 2.17 Quá trình truyền tải bit liệu Hình 2.18 Dữ liệu truyền bus I2C Hình 2.19 Bit ACK bus I2C Hình 2.20 Lưu đồ thuật tốn truyền nhận liệu Hình 2.21 Cấu trúc byte liệu Hình 2.22 Quá trình truyền liệu Hình 2.23 Ghi liệu từ chủ đến tớ Hình 2.24 Đọc liệu từ thiết bị tớ Hình 2.25 Quá trình đọc ghi phối hợp liệu Hình 2.26 Sơ đồ tín hiều đầu vào lọc Kalman mode) vi an Hình 2.27 Cảm biến GY-86 Hình 2.28 Sơ đồ nguyên lý cảm biến MPU6050 Hình 2.29 Bộ KIT Tiva C Hình 3.1 Nguyên lý hoạt động robot hai bánh tự cân Hình 3.2 Hình chiếu cạnh lắc ngược bánh Bảng 3.3 Chọn giá trị cho biến mờ đạo hàm sai số Hình 3.4 Ngõ vào sai số hàm mờ Hình 3.5 Ngõ vào đạo hàm sai số theo thời gian Hình 3.6 Mối quan hệ đầu vào Hình 3.7 Mơ hình mơ Fuzzy-PID Hình 3.8 Đáp ứng đầu dùng hệ số Kp Hình 3.9 Đáp ứng đầu dùng hệ số Kp fuzzy Hình 3.10 Đáp ứng đầu dùng PD Hình 3.11 Đáp ứng đầu dùng PD Fuzzy Hình 3.12 Tín hiệu điều khiển PD PD Fuzzy Hình 3.13 Đáp ứng đầu PD PD Fuzzy Hình 4.1 Lưu đồ giải thuật Hình 4.2 Lưu đồ khối điều khiển cân Hình 4.3 PIN K-POWER 3S 11.1V 2200mAh 25C Hình 4.4 Đơng DC 12V Hình 4.5 Mạch nguồn LD 1085 Hình 4.6 Hình ảnh thực tế Hình 4.7 Driver cầu H Hình 4.8 Sơ đồ kết nối cầu H Hình 4.9 Kết nối cảm biến vào vi điều khiển Hình 4.10 Hình robot nhìn từ xuống Hình 4.11 Robot nhìn trực diện vii an Mục Lục TĨM TẮT ĐỀ TÀI i Lời cảm ơn iii DANH MỤC CÁC TỪ VIẾT TẮT iv DANH MỤC CÁC BẢNG BIỂU v DANH SÁCH HÌNH ẢNH VÀ BIỂU MẪU vi CHƯƠNG 1: TỔNG QUAN 1.1 Lời nói đầu 1.2 Các cơng trình nghiên cứu liên quan 1.2.1 Nbot 1.2.2 Two-wheeled inverted pendulum 1.2.3 Balance bot I 1.2.4 Segway 1.2.5 HTV 1.3 Mục tiêu đề tài 1.4 Giới hạn đề tài 1.5 Phương pháp nghiên cứu CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 10 2.1 Thế xe hai bánh tự cân 10 2.2 Tại phải thiết kế xe hai bánh cân 11 2.3 Ưu nhược điểm xe hai bánh tự cân 12 2.3.1 Ưu điểm xe scooter tự cân hai bánh 12 2.3.2 Nhược điểm xe 13 2.4 Khả ứng dụng 13 ` an 2.5 Giới thiệu thuật toán PID 13 2.5.1 Thành phần tỉ lệ (P) 14 2.5.2 Thành phần tích phân (I) 14 2.5.3 Thành phần vi phân (D) 14 2.5.4 Chỉnh định tham số điều khiển PID 15 2.6 Lý thuyết điều khiển mờ (Fuzzy Logic Control) 16 2.6.1 Định nghĩa tập mờ 16 2.6.2 Các phép toán tập hợp mờ 17 2.6.3 Tính chất tập hợp mờ 19 2.6.4 Biến mờ biến ngôn ngữ 20 2.6.5 Mệnh đề mờ mệnh đề hợp thành 21 2.6.6 Các luật hợp thành 22 2.6.7 Giải mờ 22 2.6.8 Hệ quy tắc mờ 23 2.6.9 Xây dựng điều khiển Fuzzy 24 2.7 Cấu trúc điều khiển Fuzzy-PID 26 2.8 Giới thiệu I2C 26 2.8.1 Đặc điểm giao tiếp I2C 27 2.8.2 START and STOP conditions 29 2.8.3 Định dạng liệu truyền 30 2.8.4 Định dạng địa thiết bị 32 2.8.5 Truyền liệu bus I2C, chế độ Master‐Slave 32 2.9 Giới thiệu Kalman 34 2.9.1 Giới thiệu lý thuyết lọc Kalman 34 2.9.2 Xử lý liệu đọc từ cảm biến MPU6050 36 ` an CHƯƠNG 5: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI an Chương Kết Luận Và Hướng Phát Triển Đề Tài 5.1 Các kết đạt  Hiểu phương pháp Fuzzy-PID sử dụng luật điều khiển kết hợp với trọng số Kp, Ki, Kd  Ứng dụng điểu khiển thành công cho đối tượng robot hai bánh tự cân  Hiểu luật điều khiển Sugeno với biến ngôn ngữ mờ  Nắm cách thức hoạt động luật điều khiẻn để từ áp dụng phương thức điều khiển vào ứng dụng đối tượng thực tế 5.2 Các kết chưa đạt  Thời gian đáp ứng luật điều khiển chậm nên đáp ứng chưa nhanh mong muốn  Cân hệ thống phải thời gian để ổn định  Hệ thống dừng lại việc cân chưa phát triển việc di chuyển  Phần khí hệ thống chưa vững chắc, dây điện hệ thống chưa tối ưu 5.3 Hướng phát triển đề tài  Nghiên cứu sâu giải thuật Fuzzy-PID giải thuật cân để ứng dụng vào đối tượng khó điều khiển hơn: humannod robot, auto-pilot…  Tìm hiểu nghiên cứu phương pháp Fuzzy khác cách sử dụng phương pháp suy diễn Max-Min để đưa luật điều khiển khác phương pháp suy diễn  Chế tạo xe hai bánh tự cân di chuyển điều khiển người Đồ án tốt nghiệp 63 an Tài liệu tham khảo Sách điều khiển thông minh – Nguyễn Thi Phương Hà Sách hệ thống điều khiển thông minh– Huỳnh Thái Hoàng Movement control using zero dynamics of two-wheeled inverted pendulum robot Balancing a two-wheeled autonomous robot Modelling and control of a balancing robot using a digital state space approach Two-wheeled balancing LEGO robot Fuzzy auto-tuning PID control of multiple joint robot driven by ultrasonic motors www.segway.com http://www.cs.hmc.edu/~dodds/projects/RobS03/Mapping/oldindex.html 10 http://www.mathworks.com/help/fuzzy/examples/using-lookup-table-insimulink-to-implement-fuzzy-pidcontroller.html?requestedDomain=www.mathworks.com Đồ án tốt nghiệp 64 an Phụ lục Code: PID: #include "myinc/reenyle_pid.h" #include "myinc/fuzzy.h" #include "math.h" void pid_setup(float p, float i, float d, pid *set_pid,float Max_Limit, float Min_Limit, float db) { set_pid->deadband= db; set_pid->Err = 0; set_pid->Err1 = 0; set_pid->Err2 = 0; set_pid->P_Factor = p; set_pid->I_Factor = i; set_pid->D_Factor = d; set_pid->Kc_Factor = 0.85; set_pid->OutMin = Min_Limit; set_pid->OutMax = Max_Limit; set_pid->OutPreSat = 0; } float I_Sat(float Ui, float limitMax, float limitMin) { return (Ui > limitMax ? limitMax : (Ui < limitMin ? limitMin : Ui)); } float my_abs(float inval) { return (inval >= ? inval : (-inval)); } float a; float Reenyle1_pid_controller(pid *pid_get, float sys_error) { if (my_abs(sys_error) > pid_get->deadband) Đồ án tốt nghiệp 65 an { a=1; pid_get->Err = sys_error; pid_get->Up = a*(pid_get->P_Factor*pid_get->Err); pid_get->Ui += pid_get->I_Factor*pid_get->Err ; pid_get->Ud = (pid_get->D_Factor*(pid_get->Err - pid_get->Err1))*a; pid_get->OutPreSat = pid_get->Up + pid_get->Ui + pid_get->Ud; } else { pid_get->OutPreSat = pid_get->Ui; } pid_get->Out = (pid_get->OutPreSat > pid_get->OutMax ? pid_get->OutMax : (pid_get->OutPreSat < pid_get->OutMin ? pid_get->OutMin : pid_get>OutPreSat)); pid_get->SatErr = pid_get->Out - pid_get->OutPreSat; pid_get->Err1 = pid_get->Err; // add fuzzy a = fuzzy(pid_get->Err, (pid_get->Err - pid_get->Err1)); return pid_get->Out; } Fuzzy: #include #include #include "inc/hw_memmap.h" #include "inc/hw_types.h" #include "driverlib/debug.h" #include "driverlib/gpio.h" #include "driverlib/pwm.h" #include "driverlib/sysctl.h" #define f_and(a,b) ( (a) < (b) ? (a) : (b) ) #define f_or(a,b) ( (a) > (b) ? (a) : (b) ) int x; Đồ án tốt nghiệp 66 an char array[3]; #define a1 #define b1 #define c1 -0.4 0.4 #define a2 -1.8 #define b2 -1 #define c2 #define d2 #define e2 1.8 char Error_LNegative (int CRISP) // NE { { if (CRISP NS1 array[0] = Error_normal (error); array[1] = SumError_LNegative(sumerror); array[2] = f_and(array[0],array[1]); x = f_or(x,NS1(array[2],0)); //ZE and ZE > ZE1 array[0] = Error_normal (error); array[1] = SumError_normal (sumerror); array[2] = f_and(array[0],array[1]); x = f_or(x,ZE1(array[2],0)); //ZE and PO > PS array[0] = Error_normal (error); array[1] = SumError_LPositive (sumerror); array[2] = f_and(array[0],array[1]); x = f_or(x,PS(array[2],0)); //PO and NE > ZE2 array[0] = Error_LPositive (error); array[1] = SumError_LNegative(sumerror); array[2] = f_and(array[0],array[1]); x = f_or(x,ZE2(array[2])); //PO and ZE > PS1 array[0] = Error_LPositive (error); array[1] = SumError_normal (sumerror); array[2] = f_and(array[0],array[1]); x = f_or(x,PS1(array[2])); //PO and PO > PB array[0] = Error_LPositive (error); array[1] = SumError_LPositive (sumerror); array[2] = f_and(array[0],array[1]); Đồ án tốt nghiệp 72 an x = f_or(x,PB(array[2])); return x; } Main: #include #include #include "stdio.h" #include "inc/hw_types.h" #include "driverlib/fpu.h" #include "driverlib/sysctl.h" #include "driverlib/rom.h" #include "myinc/io_led.h" #include "myinc/myadc.h" #include "myinc/i2c_imu.h" #include "myinc/kalman.h" #include "myinc/systemtick.h" #include "myinc/ledpwm.h" #include "myinc/reenyle_pid.h" #include "myinc/fuzzy.h" #define Numcount 265 float Kp = 55.1; float Ki = 0.2555; float Kd = 2.025; float DesiredSpeed = -0.4; pid motorPID; extern unsigned int crccal(unsigned char *ptr,unsigned char len); float Dist_in = 0, Dist_out_i=0, Dist_out_i_1=0, Roll = 0, Pitch = 0, Yaw = 0; char LCD_character[]="REENYLE Co., Ltd"; char Dist_str[17]={0}; unsigned char sample_count=0; float ActualSpeed,duty_cycle,ek,esum,ek_1,ek_2,uk,uk_1,proportional,integral,differenti al; Đồ án tốt nghiệp 73 an float er_control_value; int countme=0; int main() { SysCtlClockSet(SYSCTL_USE_PLL | SYSCTL_SYSDIV_4 SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ); I2C_Imu_Init(); kalman_init(); Systick_Init(); PWM_Init(); pid_setup(Kp, Ki, Kd, &motorPID, 1250, -1200, 0.1); Update_Motor_Duty(0,0,0,0); esum = 0; while(1) { if(Get_TimeOut()) { kalman_run(); Roll = Get_angle(0); Pitch = Get_angle(1); Yaw = Get_angle(2); countme=0; ActualSpeed = -kalmanPitch(); ek=(DesiredSpeed-ActualSpeed); motorPID.P_Factor = Kp; motorPID.I_Factor = Ki; motorPID.D_Factor = Kd; uk = Reenyle1_pid_controller(&motorPID,ek); if (uk>=0) Update_Motor_Duty(0,uk,uk,0); else Update_Motor_Duty(-uk,0,0,-uk); Clear_TimeOut(); } else Đồ án tốt nghiệp | 74 an { ++countme; if(countme=0) Update_Motor_Duty(0,uk,uk,0); else Update_Motor_Duty(-uk,0,0,-uk); } else if(countme>=Numcount) { Update_Motor_Duty(0,0,0,0); } else { } } } } Đồ án tốt nghiệp 75 an S an K L 0 ... hồi chuyển hai bánh xe quay Còn xe hai bánh tự cân bằng, loại xe có hai bánh với trục hai bánh trùng nhau, xe cân bằng, trọng tâm xe (bao gồm người sử dụng chúng) cần nằm bánh xe Điều giống việc... trúc điều khiển Fuzzy- PID Sơ đồ khối điều khiển Fuzzy- PID Hình 2.12 Sơ đồ khối cấu trúc điều khiển Fuzzy- PID Ý tưởng PID - Mờ sử dụng Logic mờ để tối ưu giá trị tham số điều khiển PID Bộ điều khiển. .. 10 2.1 Thế xe hai bánh tự cân 10 2.2 Tại phải thiết kế xe hai bánh cân 11 2.3 Ưu nhược điểm xe hai bánh tự cân 12 2.3.1 Ưu điểm xe scooter tự cân hai bánh 12 2.3.2 Nhược