Thiết kế điều khiển con lắc ngược

Một trong các mô hình phổ biến được dùng để khảo sát các lý thuyết điều khiển được sử dụng ở hầu hết các trường đại học trên thế giới là mô hình con lắc ngược. Mặc dù, trên thế giới cũng có một số nhà cung cấp các thiết bị dạy học có chế tạo mô hình con lắc ngược với độ chính xác tin cậy cao. Tuy nhiên, với mong muốn tự xây dựng một mô hình thí nghiệm dựa trên những kiến thức đã có, dưới sự hướng dẫn của thầy Ths. Trần Thiện Dũng, em đã thực hiện đề tài Thiết kế điều khiển con lắc ngược ”.

TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP KHOA ĐIỆN TỬ BỘ MÔN ĐO LƯỜNG ĐIỀU KHIỂN ĐỒ ÁN TỐT NGHIỆP Sinh viên : Đỗ Thị Nguyệt Đỗ Văn Khánh Lớp : 50DDK Giáo viên hướng dẫn : Ths.Trần Thiện Dũng Thái Nguyên – 2019 NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN NHẬN XÉT CỦA GIÁO VIÊN CHẤM Thái Nguyên, ngày………tháng………năm 2019 GIÁO VIÊN CHẤM (Ký ghi rõ họ tên) Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược MỤC LỤC MỤC LỤC I DANH MỤC BẢNG BIỂU II DANH MỤC HÌNH ẢNH III CHƯƠNG TỔNG QUAN VỀ CON LẮC NGƯỢC 1.1 Tổng quan lắc ngược 1.2 Xây dựng mô hình động lực học lắc 1.2.1 Phương pháp Lagrange .3 1.2.2 Áp dụng phương pháp Lagrange xây dựng mơ hình lắc ngược 1.3 Tuyến tính hóa hệ thống xung quanh điểm làm việc 1.4 Kiểm tra mơ hình tốn học 10 1.5 Mơ hình động điện chiều 12 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN SWING –UP 15 2.1 Tổng quan 15 2.2 Phương pháp điều khiển lượng lắc .15 2.3 Mô điều khiển Swing-up 17 CHƯƠNG ĐIỀU KHIỂN ỔN ĐỊNH VỊ TRÍ CON LẮC 19 3.1 Thiết kế điều khiển dòng 19 3.2 Thiết kế điều khiển cân sử dụng phương pháp LQR .20 3.2.1 Giới thiệu phương pháp LQR .20 3.2.2 Thiết kế sử dụng Matlab .22 3.2.3 Mô điều khiển LQR 22 3.3 Kết hợp BĐK cân LQR 24 CHƯƠNG THIẾT KẾ, THI CÔNG VÀ THỰC NGHIỆM .26 4.1 Cấu trúc phần cứng hệ điều khiển 26 4.2 Thiết kế thi cơng phần khí .27 4.3 Thiết kế mạch Driver động 27 4.3.1 Thiết kế mạch nguyên lý 27 4.3.2 Thiết kế PCB 30 Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược 4.3 Thiết kế mạch điều khiển trung tâm .31 4.3.1 Giới thiệu chung 31 4.3.2 Cấu trúc 32 4.3.3 Thiết kế board chia .33 4.4 Lập trình điều khiển hệ thống .34 4.5 Kết thực nghiệm 41 CHƯƠNG KẾT LUẬN .43 5.1 Kết luận chung .43 5.2 Phương hướng phát triển 44 TÀI LIỆU THAM KHẢO 45 DANH MỤC BẢNG BIỂU Bảng 1 Bảng tham số hệ thống lắc ngược Bảng Bảng giá trị tham số hệ thống lắc ngược Bảng Bảng tham số hệ thống 14 Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược DANH MỤC HÌNH ẢNH Hình 1.1 Cầu trục Hình 1.2 Xe hai bánh tự cân Hình 1.3 Tên lửa Hình 1.4 Mơ hình lắc ngược Hình 1.5 Sơ đồ mơ hệ phi tuyến 11 Hình 1.6 Biểu đồ biểu thị góc quay lắc chưa có điều khiển .11 Hình 1.7 Biểu đồ biểu thị vị trí lắc chưa có điều khiển 12 Hình Sơ đồ nguyên lý động chiều .12 Hình 1.9 Mơ hình động điện chiều 14 Hình 2.1 Bộ điều khiển 17 Hình 2.2 Mơ điều khiển Swing-up .17 Hình 2.3 Mơ lượng lắc 17 Hình 2.4 Mô BĐK Swing-up .1 Hình 3.1 Mơ điều khiển dòng .19 Hình 3.2 Mơ điều khiển dòng rút gọn 19 Hình 3.3 Sơ đồ mơ điều kiển dòng với dòng đặt 20 Hình 3.4 Sơ đồ điều khiển LQR liên tục .21 Hình 3.5 Sơ đồ điều khiển phương pháp LQR 21 Hình 3.6 Sơ đồ mơ điều khiển LQR 23 Hình 3.7 Mơ tín hiệu góc lắc 23 Hình 3.8 Mơ tín hiệu vị trí xe .24 Hình 3.9 Mơ lực F tác động, vận tốc góc lắc, vận tốc xe 24 Hình 3.10 Mơ điều khiển kết hợp .25 Hình 3.11 Sơ đồ mô điều khiển kết hợp .25 Y Hình 4.1: Cấu trúc tổng quát 26 Hình 4.2: Tổng thể hệ thống 27 Hình 4.3: Sơ đồ mạch tổng thể 27 Hình 4.4: Khối nguồn 28 Hình 4.5: Khối cầu H IC kích .28 Hình 4.6: Khối cách ly tín hiệu điều khiển 29 Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược Hình 4.7: Khối NOR 30 hình 4.8: Khối đo dòng 30 hình 4.9: Mơ Altium mạch Driver 31 hình 4.10: Mạch thực Driver 31 hình 4.11: Arduino 2560 32 hình 4.12: Mạch nguyên lý 33 hình 4.13: Mạch PCB 34 hình 4.14: Tín hiệu góc  đưa tab Serial Plotter Arduino 41 Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược LỜI MỞ ĐẦU Thời đại ngày nay, với phát triển mạnh mẽ khoa học kỹ thuật đặc biệt lĩnh vực điều khiển tự động, ngày có thêm nhiều lý thuyết điều khiển đại bên cạnh lý thuyết kinh điển tồn thời gian qua Từ đó, có nhiều phương pháp điều khiển áp dụng cho đối tượng điều khiển phức tạp Tuy nhiên, hầu hết lý thuyết điều khiển phức tạp trừu tượng, đó, người học muốn hiểu cụ thể nội dung lý thuyết ngồi việc tìm hiểu kỹ lý thuyết, cần phải có mơ hình thực tế để kiểm nghiệm lại lý thuyết Một mơ hình phổ biến dùng để khảo sát lý thuyết điều khiển sử dụng hầu hết trường đại học giới mơ hình lắc ngược Mặc dù, giới có số nhà cung cấp thiết bị dạy học có chế tạo mơ hình lắc ngược với độ xác tin cậy cao Tuy nhiên, với mong muốn tự xây dựng mô hình thí nghiệm dựa kiến thức có, hướng dẫn thầy Ths Trần Thiện Dũng, em thực đề tài "Thiết kế điều khiển lắc ngược ” CHƯƠNG TỔNG QUAN VỀ CON LẮC NGƯỢC 1.1 Tổng quan lắc ngược Con lắc ngược loại robot ứng dụng vấn đề quan trọng lý thuyết điều khiển đề cập nhiều tài liệu điều khiển Mơ hình thực tế lắc ngược kiểm chứng lại lý thuyết điều khiển nhiên lắc ngược đặt nhiều thách thức lý thuyết điều khiển thiết bị điều khiển chúng Vì vậy, hệ thống phi tuyến nên vấn đề điều khiển lắc ổn định gặp nhiều khó khăn Hệ thống lắc ngược lĩnh vực quan trọng điện Được nghiên cứu sâu rộng thập kỷ gần đây, lĩnh vực có nhiều kết quan trọng lý thuyết thực tiễn Hệ thống lắc ngược vốn có hai điểm cân bằng, cân ổn định không ổn định Ở trạng thái cân ổn định lắc hướng xuống phía khơng có lực tác động đến hệ thống hệ thống trạng thái Trong trường hợp cân không ổn định vị trí lắc hướng lên cần lực tác động để trì trạng thái Vì việc điều khiển hệ thống lắc trì trạng thái đứng cân hướng lên lắc Hiện có nhiều nhà nghiên cứu sử dụng thuật toán điều khiển khác để điều khiển hệ thống lắc thuật toán điều khiển PID, điều khiển trượt, thuật toán điều khiển tối ưu LQR điều khiển logic mờ Fuzzy thu số thành công đáng kể Từ đề tài lắc ngược có đề tài nghiên cứu phát triển để nghiên cứu vấn đề như: Điều khiển cầu trục; Xe hai bánh tự cân ; Điều khiển ổn định góc tên lửa rời bệ phóng Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược Hình 1.1 Cầu trục Hình 1.2 Xe hai bánh tự cân bằng Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược Hình 1.3 Tên lửa 1.2 Xây dựng mơ hình động lực học lắc 1.2.1 Phương pháp Lagrange Để tìm phương trình động lực học mô tả hệ thống lắc ngược,phương pháp Lagrange sử dụng dựa cân lượng hệ thống Trong ngành tối ưu hóa, phương pháp nhân tử Lagrange (đặt theo tên nhà toán học Joseph Louis Lagrange) phương pháp để tìm cực tiểu cực đại địa phương hàm số chịu điều kiện giới hạn Khi gặp toán mà gặp điều kiện hàm f (x, y) với điều kiện ràng buộc g(x, y)  Để tìm cực trị hàm có điều kiện ràng buộc ta thiết lập hàm Lagrange: Z(x, y,  )  f (x, y)  g(x, y) (1.1) Trong  số chưa xác định, gọi nhân tử Lagrange.Điều kiện cần cực trị hệ phương trình sau: � Z ' (x, y,  )  f x' (x, y)   gx' (x, y)  �x Zy' (x, y,  )  fy' (x, y)  gy' (x, y)  � � Z (x, y,  )  g(x, y)  � ' (1.2) (x , y ,  ) Khi giải hệ phương trình ta số 0 nghiệm hệ điểm dừng Khi f (x0, y0 ) f (x1, y1 ) (x , y ) ta so sánh với - 1 số khác thỏa mãn điều Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược kiện g(x, y)  mà thông thường giá trị biên – để kiểm tra xem điểm dừng cực đại hay cực tiểu 1.2.2 Áp dụng phương pháp Lagrange xây dựng mơ hình lắc ngược Hình 1.4 Mơ hình lắc ngược Bảng 1 Bảng tham số hệ thống lắc ngược Ký hiệu Đơn vị  rad s m J1 kgm m1 kg Khối lượng lắc quy tâm m kg Khối lượng xe a1 g m Khoảng cách từ trục quay đến trọng tâm lắc m / s2 Gia tốc trọng trường d0 Nms Hệ số ma sát trượt d1 Nms Hệ số ma sát khớp quay Ru  Điện trở nội động Lu H Điện cảm cuộn dây động Km Wb Hằng số điện từ R m Bán kính pully Gọi Mơ tả Góc cánh tay trục thẳng đứng Vị trí xe Momen quán tính lắc c1 vị trí trọng tâm lắc: Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược Hình 4.5: Khối cầu H IC kích Để điều khiển mạch cầu H ta có nhiều cách điều khiển, ta chọn IC lái IR2184, IC hoạt động chân SD kéo lên mức cao suất đầu H0, L0 phụ thuộc vào giá trị sườn đầu vào chân số (IN) IC Với đầu H0 L0 với giá trị suất luôn ngược để đảm bảo không bị trùng dẫn phía Vì động loại tải cảm nên trình hoạt động tạo xung kim cao áp, xung gây nhiễu làm cho FET bị nóng giảm tuổi thọ FET Nên ta mắc song song động với mạch dập xung (mạch snubber) điện trở nối tiếp với tụ điện không phân cực mắc song song với chân D, S FET Dio1, Dio2 có tác dụng ngắt nhanh làm cho sườn xuống dốc thay qua điện trở R3, R4 dòng chảy qua Dioe giảm thiểu tượng trùng dẫn Dio5, Dio6 có tác dụng áp chân xuống âm tụ C22, C24 (Tụ bootstrap) nạp để kích cho nửa cao bên cầu  Khối cách ly tín hiệu điều khiển Hình 4.6: Khối cách ly tín hiệu điều khiển Khi tín hiệu đầu vi điều khiển để điều khiển tốc độ hướng cho động nhỏ, khả nhiễu cao nên cần phải cách li nhiễu cho tín hiệu điều khiển ổn định Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 31 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược Trong mạch ta sử dụng IC cách li quang PC817 6N137 để cách li tín hiệu điều khiển  Khối NOR Ở khối ta sử IC CD4001 tích hợp NOR đầu vào đầu Tín hiệu đầu vào DIR PWM lấy từ khối cách li quang qua hàm toán NOR để đảm bảo đầu điều khiển hướng PWM không rơi vào ngưỡng cấm Hình 4.7: Khối NOR  Khối đo dòng Để kiểm sốt dòng tải ln đảm bảo momen cần thiết ta phải liên tục đo dòng phản hồi về, khối sử dụng IC ACS712 30A hình 4.8: Khối đo dòng 4.3.2 Thiết kế PCB Việc thiết kế mạch nguyên lý mạch in PCB sử dụng phần mềm chuyên dụng Altium Designer 16, mạch sau thiết kế xong Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 32 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược hình 4.9: Mơ Altium mạch Driver Và mạch sau hồn thiện chạy thử… hình 4.10: Mạch thực Driver 4.3 Thiết kế mạch điều khiển trung tâm Mạch điều khiển trung tâm sử dụng Arduino mega2560 4.3.1 Giới thiệu chung Arduino board mạch vi xử lý, nhằm xây dựng ứng dụng tương tác với với môi trường thuận lợi Phần cứng bao gồm board mạch nguồn mở thiết kế tảng vi xử lý AVR Atmel 8bit, ARM Atmel 32-bit Những Model sử dụng nhiều ngoại vi, tương thích với nhiều board mở rộng khác Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 33 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược 4.3.2 Cấu trúc Một mạch Arduino bao gồm vi điều khiển AVR với nhiều linh kiện bổ sung giúp dễ dàng lập trình mở rộng với mạch khác Arduino thức thường sử dụng dòng chip megaAVR, đặc biệt ATmega8, ATmega168, ATmega328, ATmega1280, ATmega2560 Một vài vi xử lý khác sử dụng mạch Aquino tương thích Hầu hết mạch gồm điều chỉnh tuyến tính 5V thạch anh dao động 16 MHz Một vi điều khiển Arduino lập trình sẵn với boot loader cho phép đơn giản upload chương trình vào nhớ flash on-chip, so với thiết bị khác thường phải cần nạp bên Điều giúp cho việc sử dụng Arduino trực tiếp cách cho phép sử dụng máy tính gốc nạp chương trình Mạch lập trình đề tài Arduino MEGA 2560 vi điều khiển Atmega2560 Aduino MEGA 2560 bao gồm: - Chân GND - chân 5V - chân 3.3v - nút reset - 16 chân analog - chân UART - 54 Chân digital có 15 chân sử dụng PWM - Chân lập trình ISP Hình ảnh thực tế hình 4.11: Arduino 2560 4.3.3 Thiết kế board chia Việc xuất tín hiệu kết nối Arduino với thành phần khác hệ thống ko thể cắm dây vào board Arduino mà phải thông qua board chia từ việc kết nối trở nên dễ dàng đảm bảo tín hiệu thơng suốt  Mạch ngun lý Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 34 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược hình 4.12: Mạch nguyên lý  Mạch PCB hình 4.13: Mạch PCB 4.4 Lập trình điều khiển hệ thống Sử dụng trình IDE để lập trình cho Arduino #define ExA // Interrupt Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 35 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược #define ExB // Interrupt #define EphiA #define EphiB unsigned char dem=0, dem1=0,dem3=0; int current; int low,high; int Out; volatile int Sp=0; int e=0; float P,Kp=3.0,I=0,Ki=1.628,Kz=22; //P,Kp=0.2,I=0,Ki=0.1628; char inChar; int phi,x; int dem4=0; float Gphi,Gx,Tphi,Tx,phi_dot,x_dot,dE; boolean sEphiA,sEphiB,sExA,sExB; //============== Doc Encodeer =================// void Ephi(){ sEphiA=digitalRead(EphiA); sEphiB=digitalRead(EphiB); if(sEphiA){ if(sEphiB) phi++; else phi ;} else{ if(sEphiB) phi ; else phi++;} if(phi==-1001)phi=999; if(phi==1001)phi=-999; } Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 36 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược void Ex(){ sExA=digitalRead(ExA); sExB=digitalRead(ExB); if(sExA){ if(sExB) x ; else x++;} else{ if(sExB) x++; else x ;} } //================================================// //=============== BDK dong ========================// ISR(TIMER4_OVF_vect) { dem3++; dem1++; if(dem1==20) { dem1=0; Gphi=(phi*3.14)/1000; phi_dot=(Gphi-Tphi)*100; Tphi=Gphi; Gx=(x*0.117)/2000; x_dot=(Gx-Tx)*100; Tx=Gx; if((Gx-0.6)&(dem4==0)) { Sinh viên: Đỗ Thị Nguyệt;Đỗ Văn Khánh 37 Đồ án tốt nghiệp: Thiết kế điều khiển lắc ngược if((Gphi-0.2)) { Sp=(-600*Gphi)+(-50*phi_dot)+(200*Gx)+(180*x_dot); } else { dE=((phi_dot*phi_dot*0.007899535+(0.6622731*(cos(Gphi)+1)))1.33)*cos(Gphi)*phi_dot; if(dE>=0)Sp=-Kz; else if(dE0) { Sp=-10; } if((Gx