Mô hình hóa và điều khiển robot tự cân bằng trên một quả bóng

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH LUẬN VĂN THẠC SĨ CÙ MINH PHUỚC MƠ HÌNH HĨA VÀ ÐIỀU KHIỂN ROBOT TỰ CÂN BẰNG TRÊN MỘT QUẢ BÓNG NGÀNH: KỸ THUẬT ĐIỆN TỬ - 60520203 S K C0 0 Tp Hồ Chí Minh, tháng 04/2016 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH LUẬN VĂN THẠC SĨ CÙ MINH PHƯỚC MƠ HÌNH HĨA VÀ ĐIỀU KHIỂN ROBOT TỰ CÂN BẰNG TRÊN MỘT QUẢ BÓNG NGÀNH: KỸ THUẬT ĐIỆN TỬ - 60520203 Hướng dẫn khoa học: TS NGUYỄN MINH TÂM Tp Hồ Chí Minh, tháng 4/2016 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH LUẬN VĂN THẠC SĨ CÙ MINH PHƯỚC MƠ HÌNH HĨA VÀ ĐIỀU KHIỂN ROBOT TỰ CÂN BẰNG TRÊN MỘT QUẢ BÓNG NGÀNH: KỸ THUẬT ĐIỆN TỬ - 60520203 Hướng dẫn khoa học: TS NGUYỄN MINH TÂM Tp Hồ Chí Minh, tháng 4/2016 LÝ LỊCH KHOA HỌC I LÝ LỊCH SƠ LƯỢC: Họ & tên: CÙ MINH PHƯỚC Giới tính: Nam Ngày, tháng, năm sinh: 15/10/1991 Nơi sinh: Bình Định Quê quán: An Nhơn, Bình Định Dân tộc: Kinh Chỗ riêng địa liên lạc: 203/8, Đường 11, Khu phố 4, Linh Xuân, Thủ Đức, HCM Điện thoại quan: Điện thoại nhà riêng: 01682813763 Fax: E-mail: minhphuocbinhdinh@gmail.com II QUÁ TRÌNH ĐÀO TẠO: Trung học chuyên nghiệp: Hệ đào tạo: Nơi học (trường, thành phố): Ngành học: Thời gian đào tạo từ ……/…… đến ……/ …… Đại học: Hệ đào tạo: Chính qui Thời gian đào tạo từ 9/2009 đến 12/ 2013 Nơi học (trường, thành phố): Trường ĐH Sư Phạm Kỹ Thuật Tp Hồ Chí Minh Ngành học: Công nghệ Điện tự động Tên đồ án, luận án mơn thi tốt nghiệp: Mơ hình hóa điều khiển Robot hai bánh tự cân Ngày & nơi bảo vệ đồ án, luận án thi tốt nghiệp: ĐH Sư Phạm Kỹ Thuật Tp Hồ Chí Minh Người hướng dẫn: TS Nguyễn Minh Tâm III Q TRÌNH CƠNG TÁC CHUYÊN MÔN KỂ TỪ KHI TỐT NGHIỆP ĐẠI HỌC: Thời gian Nơi công tác Công việc đảm nhiệm 12/2014 – Trường Cao Đẳng Kỹ Thuật Cao Giảng viên Thắng i LỜI CAM ĐOAN Tôi cam đoan cơng trình nghiên cứu tơi Các số liệu, kết nêu luận văn trung thực chưa cơng bố cơng trình khác Tp Hồ Chí Minh, ngày … tháng năm 2016 (Ký tên ghi rõ họ tên) ii LỜI CẢM ƠN Lời cảm ơn đầu tiên, xin gửi đến Thầy Nguyễn Minh Tâm người tận tình hướng dẫn truyền đạt kiến thức, kinh nghiệm giúp tơi hồn thành luận văn 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, Cơ Khí hỗ trợ giúp tơi làm mơ hình cho học hỏi kiến thức bổ ích trình học tập để ứng dụng lý thuyết vào nghiên cứu phát triển đề tài Tôi xin chân thành gửi lời cảm ơn đến tất bạn cao học Kỹ thuật điện tử khóa 2014A động viên, giúp đỡ, trao đổi kiến thức với suốt khóa học Tp.HCM, ngày tháng 04 năm 2016 Tác giả CÙ MINH PHƯỚC iii TÓM TẮT LUẬN VĂN CAO HỌC Robot tự cân bóng hệ thống khó có tính phi tuyến cao, nghiên cứu phát triển số nước thành cơng Mơ hình sử dụng để kiểm chứng tính xác thực giải thuật tuyến tính lẫn phi tuyến, cổ điển đại Trong luận văn xem xét giải vấn đề liên quan đến Robot tự cân bóng mơ khảo nghiệm mơ hình Robot, thiết kế điều khiển tự cân Trong luận văn học viên dựa giải thuật tối ưu hóa tồn phương tuyến tính LQR, điều khiển cân Robot, làm tảng để phát triển Robot điểu khiển theo vi trí linh hoạt sau Autonomous balancing on a ball is a difficult system because of high nonlinearity, is research and development in some countries succeed The model above can be used to verify the authenticity of the linear algorithm and nonlinear, as well as a modern classic In this essay will examine and resolve issues related to self-balancing robot on a ball like Robot simulation models, design controls balance In this thesis students will be based on sustainable LQR algorithm, balancing robot controller, the foundation for the development of robot control placement is flexible in the future iv PHỤ LỤC TRANG TỰA TRANG LÝ LỊCH KHOA HỌC i LỜI CAM ĐOAN ii LỜI CẢM ƠN iii TÓM TẮT LUẬN VĂN CAO HỌC iv PHỤ LỤC v DANH SÁCH CÁC HÌNH ẢNH ix DANH MỤC TỪ VIẾT TẮT VÀ BẢNG SỐ LIỆU xii Chương 13 GIỚI THIỆU TỔNG QUAN 13 1.1 Giới thiệu Robot tự cân bóng 13 1.2 Tình hình phát triển Robot tự cân bóng giới 14 1.2.1 Robot tự cân bóng đại học Carnegie Mellon 14 1.2.2 Robot tự cân bóng đại học Tohoku Gakuin 15 1.2.3 Robot tự cân bóng Đại học Adelaide (UA) 15 1.2.4 Robot tự cân bóng Đại học General Hsing 16 1.2.5 Robot tự cân bóng Đại học Zurich 16 1.3 Lý chọn đề tài 17 1.4 Mục tiêu đề tài 17 1.5 Nội dung đề tài 18 1.6 Giới hạn nội dung đề tài 18 Chương 19 THI CÔNG THIẾT KẾ MẠCH ĐIỆN VÀ PHẦN CỨNG 19 2.1 Tổng quan thiết kế hệ thống điều khiển 19 2.2 Những phần cứng robot tự cân bóng 19 2.2.1 Bánh Omni 19 2.2.2 Vi điều khiển Arduino Mega 2560 21 2.2.3 Cảm biến IMU DOF 22 2.2.4 Nguồn DC 12V 23 2.2.5 Driver PID cho động DC Servo 24V 23 2.2.6 Động DC Servo 24 v 2.2.7 Module thu phát, truyền nhận liệu 24 2.3 Sơ đồ tổng quan mạch điện điều khiển robot 25 2.4 Thi cơng mơ hình thực tế Robot tự cân bóng 25 Chương 27 KHẢO SÁT LÝ THUYẾT 27 3.1 Bộ điều khiển PID 27 3.1.1 Cấu trúc điều khiển PID 27 3.1.2 Bộ điều khiển PID 28 3.2 Bộ điều khiển tồn phương tuyến tính (LQR) 28 3.2.1 Bài toán LQR liên tục cách tìm lời giải tối ưu 28 3.2.2 Bài tốn LQR liên tục thời gian vơ hạn 29 3.2.3 Bài tốn LQR rời rạc thời gian vơ hạn 30 3.3 Bộ lọc Kalman 30 3.3.1 Quá trình ước lượng 31 3.3.2 Bản chất xác suất lọc 33 3.3.3 Thuật toán Kalman rời rạc 33 3.4 Bộ lọc bổ sung Complementary filter 35 Chương 37 MÔ PHỎNG VÀ KHẢO NGHIỆM MÔ HÌNH ROBOT 37 4.1 Phân tích Robot tự cân bóng theo hệ qui chiếu 2D 37 4.1.1 Các điều kiện giả thiết ban đầu xây dựng mơ hình 38 4.1.2 Chọn hệ tọa độ qui chiếu 38 4.1.3 Chọn hệ tọa độ Descartes theo mặt phẳng yz (hoặc xz) 39 4.1.4 Chọn hệ tọa độ Descartes theo mặt phẳng xy 40 4.2 Phương trình động lực học theo mặt phẳng tọa độ yz/xz 40 4.2.1 Động hệ thống 40 4.2.2 Moment xoắn ngoại lực 41 4.2.3 Thành lập công thức Euler-Lagrange 42 4.2.4 Tuyến tính hóa, thành lập phương trình biến trạng thái 43 4.3 Phương trình động lực học theo mặt phẳng tọa độ xy 45 4.3.1 Động hệ thống 45 4.3.2 Moment xoắn ngoại lực 45 4.3.3 Thành lập phương trình Euler-Lagrang 46 vi kalmanX.setAngle(roll); // Set starting angle kalmanY.setAngle(pitch); gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch; timer = micros(); //delay(5000); beep(1); speed_feedback_1 = 0; speed_feedback_2 = 0; speed_feedback_3 = 0; } void loop() { /* Update all the values */ while (i2cRead(0x3B, i2cData, 14)); accX = ((i2cData[0] POSITION_LIMIT) position_Y = POSITION_LIMIT; speed_Y = K_angle_Y * angle_Y + K_angular_dot_Y * angular_dot_Y + K_position_Y * position_Y + K_velocity_Y * velocity_filter_Y; // -// speed_feedback_1 = 0; speed_feedback_2 = 0; speed_feedback_3 = 0; } void PWM_output(long int pwm_1, long int pwm_2, long int pwm_3) { 78 if(pwm_1>=PWM_LIMIT) pwm_1=PWM_LIMIT; if(pwm_1=PWM_LIMIT) pwm_2=PWM_LIMIT; if(pwm_2=PWM_LIMIT) pwm_3=PWM_LIMIT; if(pwm_3=0) { int tram = (int) a/100; int chuc = (int) (a/10)%10; int donvi= (int) a%10; Serial.write('+'); 80 Serial.write(tram+48); Serial.write(chuc+48); Serial.write(donvi+48); Serial.write(' '); } else if(a=0) { int tram = 0;//(int) compAngleX/100; int chuc = 0;//(int) (compAngleX/10)%10; int donvi= 0;//(int) compAngleX%10; Serial.write('+'); Serial.write(tram+48); Serial.write(chuc+48); Serial.write(donvi+48); Serial.write(' '); } else if(b Xác định góc lăn vận tốc góc lăn bóng theo Ox/Oy Bộ điều khiển LQR moment lực theo trục Ox/Oy Phân phối thành tín hiệu điều khiển động DC1, DC2, DC3 end #INT1 #INT2 #INT3 Tính xung DC Pulse1 Tính xung DC Pulse2 Tính xung DC Pulse3 Return Return Return Hình 13: Cấu trúc điều khiển LQR giữ thăng Robot Hệ thống điều khiển sử dụng nguồn cung cấp chủ yếu từ Pin Li-po 24VDC, xử lý trung tâm vi điều khiển Arduino Mega 2560 Cảm biến IMU hồi tiếp giá trị góc nghiêng thân Robot, vận tốc góc, encoder giúp xác định tốc độ Hình 15: Lưu đồ giải thuật chương trình cấp vào điều khiển robot Mơ hình robot tự cân bóng thi cơng hồn thiện: Hình 19 Mơ hình robot tự cân bóng chạy thực nghiệm Hình 16: Mơ hình robot tự cân bóng thực tế Với thư viện hỗ trợ hiển thị đồ thị chạy Visual Basic 6.0, tác giả thực q trình điều khiển thực mơ hình khí lấy tín hiệu thơng qua sử dụng truyền thông RS232 truyền không dây qua thiết bị Bluetooth, thu liệu đáp ứng góc nghiêng hệ thống: Hình 17: Đáp ứng ngõ góc nghiêng thân robot so với điểm cân Đáp ứng vận tốc góc nghiêng hệ thống thực: Hình 18: Đáp ứng ngõ vận tốc góc nghiêng thân robot so với điểm cân Ta thấy điều khiển LQR cho kết tốt Tuy nhiên, với điều khiển LQR này, hệ thống chưa tối ưu hóa, góc lệch thân Robot cịn dao động quanh vị trí cân cao Thành cơng bước đầu tảng cho việc áp dụng thành công giải thuật LQR giải thuật khác cao VIII KẾT LUẬN Tác giả xây dựng mơ hình tốn học, hàm trạng thái mơ mơ hình Matlab Simulink Bên cạnh đó, tác giả thiết kế, mơ khảo sát số điều khiển PID, LQR mơ hình robot tự cân bóng Chất lượng lọc nhiễu Kalman, Complementary việc xử lý tín hiệu lấy từ cảm biến IMU mơ đánh giá, yếu tố quan trọng cần quan tâm điều khiển có thực tốt tin cậy hay khơng phụ thuộc nhiều vào độ ổn định giá trị hồi tiếp Do đó, việc hiệu chỉnh đánh giá lọc, không dừng lại việc mô mà tác giả thực thực tế cách sử dụng vi điều khiển giao tiếp trực tiếp với cảm biến IMU Chương trình điều khiển robot tự cân bóng theo giải thuật LQR tác giả viết ngôn ngữ C, sử dụng trình biên dịch Arduino IDE, tồn hệ thống chạy vi điều khiển Mega 2560 thông dụng phổ biến TÀI LIỆU THAM KHẢO [1] Koos van der Blonk Modeling and Control of a BallBalancing Robot, Internship & Master thesis at ALTEN Mechatronics, pp.2-25, July 2014 [2] Peter Fankhauser and Corsin Gwerder Modeling and Control of a Ballbot, Bachelor Thesis, pp.22-33, Spring Term 2010 [3] Magnus Jonason Bjärenstam and Michael Lennartsson Development of a ball balancing robot with omni wheels, Department of Automatic Control, pp.5-34, March 2012 [4] T B Lauwers, G A Kantor, and R L Hollis The Robotics Institute Carnegie Mellon University Pittsburgh, Pennsylvania, USA, A Dynamically Stable Single-Wheeled Mobile Robot with Inverse Mouse-Ball Drive, Conf on Robotics and Automation, Orlando, FL, pp.8-16, May 15-19, 2006 [5] Kumagai, M and Ochiai, T Development of a Robot Balanced on a Ball, pp5-16, March 3, 2010 [6] Huỳnh Thái Hồng Hệ thống điều khiển thơng minh , Nhà xuất ĐH Quốc Gia, Tp.HCM, 2006 Thực tế, đề tài điều khiển Robot tự giữ cân tốt bóng giải thuật LQR kết hợp với lọc bổ sung Complementary xử lý tín hiệu hồi tiếp Tuy nhiên hạn chế điểm Robot thiết kế khí nặng cồng kềnh dẫn đến moment lực đổ xuống lớn, khiến Robot không tự giữ Bên cạnh loại bánh Omni sử dụng đề tài chưa phải giải pháp tốt nhất, bánh chạy không mượt độ bám tiếp xúc bánh vệ tinh với bóng chưa tốt Robot khơng thể giữ thăng lâu cho bóng trơi tự khoảng dài, lực hãm động phụ thuộc nhiều vào ma sát bánh với mặt bóng Mơ hình Robot tự cân bóng đề tài không đặt nhiều thách thức nghiên cứu chế tạo Để thiết kế mơ hình robot hồn chỉnh, linh hoạt cần phải tính tốn thật xác kết cấu khí, phải có thơng số thật xác động cơ, encoder, cảm biến… điều khiển tối ưu Tp.HCM, ngày … tháng … năm 20… Tp.HCM, ngày … tháng … năm 20… Giảng viên hướng dẫn Học viên thực TS Nguyễn Minh Tâm Cù Minh Phước S K L 0 ... 5.1.2 Mô đáp ứng điều khiển PID (SISO) mơ hình Robot 55 5.2 Bộ điều khiển LQR Robot tự cân bóng 56 5.2.1 Thiết kế điều khiển LQR cho mơ hình Robot 57 5.2.2 Mô đáp ứng điều khiển LQR mơ hình. .. triển Robot tự cân bóng giới 14 1.2.1 Robot tự cân bóng đại học Carnegie Mellon 14 1.2.2 Robot tự cân bóng đại học Tohoku Gakuin 15 1.2.3 Robot tự cân bóng Đại học Adelaide (UA) 15 1.2.4 Robot. .. [1] 16 Hình 1.5 : Robot tự cân bóng đại học General Hsing [1] 16 Hình 1.6 : Robot tự cân bóng đại học General Hsing [1] 17 Hình 2.1 : Tổng quan bố trí hệ thống điều khiển robot tự cân bóng [2]