Ứng dụng ARM STM32F4 discovery và matlab simulink trong thiết kế các bộ điều khiển đồ án tốt nghiệp ngành công nghệ kỹ thuật ô tô

MỤC LỤC LỜI CẢM ƠN i TÓM TẮT ii MỤC LỤC iii DANH MỤC CÁC CHỮ VIẾT TẮT VÀ KÝ HIỆU vi DANH MỤC CÁC HÌNH vii DANH MỤC CÁC BẢNG x CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI .1 1.1 Tình hình nghiên cứu nước 1.1.1 Tổng quan tình hình nghiên thuộc lĩnh vực đề tài 1.1.2 Danh mục cơng trình liên quan 1.2 Tính cấp thiết đề tài 1.3 Mục tiêu đề tài 1.4 Phương pháp phạm vi nghiên cứu CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 Lịch sử phát triển lý thuyết điều khiển 2.1.1 Điều khiển kinh điển (classical control) .3 2.1.2 Điều khiển đại (modern control) 2.1.3 Điều khiển thông minh (intelligent control) 2.2 Thành phần hệ thống điều khiển .4 2.3 Thiết kế điều khiển 2.3.1 Phần loại thiết kế điều khiển 2.3.2 Bộ điều khiển PID CHƯƠNG 3: GIỚI THIỆU PHẦN MỀM VÀ PHẦN CỨNG .9 3.1 Giới thiệu MATLAB/SIMULINK 3.2 Giới thiệu board mạch ARM STM32F407 DISCOVERY .9 3.3 Các công cụ cần chuẩn bị để làm thí nghiệm 10 3.3.1 Phần cứng 10 3.3.2 Phần mềm 10 CHƯƠNG 4: HƯỚNG DẪN SỬ DỤNG THƯ VIỆN WAIJUNG TRONG MATLAB/SIMLINK 12 4.1 Chuyển đổi tín hiệu Analog sang Digital Digital sang Analog 12 4.1.1 Giới thiệu chân ADC, DAC 12 4.1.1.1 Số lượng chân 12 4.1.1.2 Đặc tính điện áp 12 4.1.2 Thí nghiệm sử dụng khối ADC 13 4.1.2.1 Cấu hình khối Regular ADC 13 4.1.2.2 Điều khiển LED biến trở 15 4.1.3 Thí nghiệm sử dụng khối DAC 17 4.1.3.1 Cấu hình khối Regular DAC 17 4.1.3.2 Tạo tín hiệu tuần hồn 18 4.2 Sử dụng Digital Input/Output 19 4.2.1 Giới thiệu Digital Input/Output 19 iii 4.2.1.1 Sơ đồ mạch đầu vào 20 4.2.1.2 Sơ đồ mạch đầu 20 4.2.2 Thí nghiệm sử dụng khối Digital Output 22 4.2.2.1 Cấu hình khối Digital Output 22 4.2.2.2 Điều chỉnh thời gian sáng LED 23 4.2.3 Thí nghiệm sử dụng khối Digital Input 25 4.2.3.1 Cấu hình khối Digital Input 25 4.2.3.2 Điều chỉnh LED nút nhấn 26 4.3 Sử dụng Timer/Counter 27 4.3.1 Giới thiệu Timer/Counter 27 4.3.2 Thí nghiệm sử dụng khối Basic PWM 29 4.3.2.1 Khái quát PWM 29 4.3.2.2 Cấu hình khối Basic PWM 30 4.3.2.3 Điều chỉnh độ sáng LED dùng PWM 31 4.3.3 Thí nghiệm sử dụng khối Encoder Read 32 4.3.3.1 Giới thiệu khối Encoder Read 32 4.3.3.2 Cấu hình khối Encoder Read 34 4.3.3.3 Đọc tốc độ động DC dùng khối Encoder Read 35 4.3.4 Thí nghiệm sử dụng khối Timer IRQ 36 4.3.4.1 Khái quát ngắt 36 4.3.4.2 Cấu hình khối Timer IRQ 36 4.3.4.3 Điểu khiển LED dùng khối Timer IRQ 37 4.4 Sử dụng cổng giao tiếp UART 38 4.4.1 Giới thiệu UART 38 4.4.2 Thí nghiệm sử dụng khối UART Setup UART Tx 39 4.4.2.1 Cấu hình khối UART Setup UART Tx 39 4.4.2.2 Truyền giá trị từ STM32F407 DISCOVERY lên máy tính 41 4.4.3 Thí nghiệm sử dụng khối Host Serial 41 4.4.3.1 Cấu hình khối Host Serial 41 4.4.3.2 Truyền giá trị xuống board STM32F407 DISCOVERY 43 4.5 Sử dụng cổng giao tiếp I2C 45 4.5.1 Giới thiệu I2C 45 4.5.2 Giới thiệu cảm biến GY-521 6DOF MPU6050 46 4.5.3 Thí nghiệm sử dụng khối I2C 47 4.5.3.1 Cấu hình khối I2C 47 4.5.3.2 Thí nghiệm đọc cảm biến GY-521 6DOF MPU 6050 50 CHƯƠNG 5: MƠ HÌNH DC MOTOR 53 5.1 Giới thiệu phần cứng mơ hình DC motor 53 5.1.1 DC motor 53 5.1.2 Mô-đun điều khiển motor 53 5.2 Xây dựng phương trình động học 54 5.2.1 Thông số DC motor 54 5.2.2 Thiết lập phương trình vi phân 55 5.2.3 Thiết lập hàm truyền 55 5.2.4 Thiết lập phương trình khơng gian trạng thái 56 5.3 Bộ điều khiển DC mtor 57 iv 5.3.1 .Mơ hình sơ đồ mạch điện 57 5.3.2 Giải thuật kết thực nghiệm 58 5.3.3 So sánh, đánh giá với điều khiển Arduino 61 CHƯƠNG 6: MÔ HÌNH CON LẮC NGƯỢC 67 6.1 Giới thiệu phần cứng mơ hình lắc ngược 67 6.1.1 DC motor 67 6.1.2 Cảm biến góc quay 68 6.2 Xây dựng phương trình động học 68 6.2.1 Thông số lắc 68 6.2.2 Thiết lập phương trình vi phân 68 6.2.3 Thiết lập hàm truyền 71 6.3 Bộ điều khiển lắc ngược 71 6.3.1 Mơ hình sơ đồ mạch điện 71 6.3.2 Giải thuật kết thực nghiệm 72 6.3.3 So sánh đánh giá kết với Arduino 73 CHƯƠNG 7: MÔ HÌNH ROBOT TỰ CÂN BẰNG 75 7.1 Giới thiệu phần cứng mơ hình robot cân 75 7.1.1 DC motor 75 7.1.2 Module điều khiển động 75 7.1.3 Cảm biến gia tốc MPU 6050 76 7.2 Xây dựng phương trình động lực học 76 7.2.1 Thông số Robot cân 76 7.2.2 Thiết lập phương trình 77 7.3 Bộ điều khiển robot cân bằngMơ hình sơ đồ mạch điện 81 7.3.1 Giải thuật 81 CHƯƠNG 8: KẾT LUẬN VÀ ĐỀ NGHỊ 82 8.1 Kết thực 82 8.2 Hướng phát triển đề tài 82 DANH MỤC TÀI LIỆU THAM KHẢO 83 PHỤ LỤC 84 v DANH MỤC CÁC CHỮ VIẾT TẮT VÀ KÝ HIỆU PID Proportional Intergral Derivative SISO Single-Input/Single-Output MIMO Multi-Input/Multi-Ouput TPHCM Thành phố Hồ Chí Minh DC motor Direct Current motor LQR Liner Quadratic Regulator QĐNS Qũy đạo nghiệm số POT Percent of Overshoot PC Personal Computer MATLAB Maxtric Laboratory Kit Kitten LED Light Emitting Diode ADC Analog to Digital Converter DAC Digital to Analog Coverter Bit Binary Digit PWM Pulse Width Modulation LCD Liquid Crystal Display IC Integrated Cricuit SDA Serial Date Line SCL Serial Clock Line I2C Inter-Intergrated Cỉcuit vi DANH MỤC CÁC HÌNH Hình 2.1 Sơ đồ khối hệ thống điều khiển Hình 2.2 Sơ đồ khối hiệu chỉnh nối tiếp Hình 2.3 Sơ đồ khối hồi tiếp trạng thái Hình 2.4 Sơ đồ khối PID Hình 2.5 Sơ đồ khối điều khiển P Hình 2.6 Sơ đồ điều khiển D Hình 3.1 Phần mềm MATLAB & SIMULINK Hình 3.2 Board mạch ARM STM32F407 DISCOVERY 10 Hình 4.1 Khối Regular ADC 14 Hình 4.2 Hộp thoại Regular ADC 14 Hình 4.3 Code Simulink “Chớp LED điện trở” 16 Hình 4.4 Kết lắp mạch “biến trở điều khiển LED” 16 Hình 4.5 Kết đồ thị văn biến trở 17 Hình 4.6 Khối Regular DAC 17 Hình 4.7 Hộp thoại Regular DAC 17 Hình 4.8 Code Simulink “Tạo tín hiệu tuần hoàn” 19 Hình 4.9 Khối tạo tần số cao 19 Hình 4.10 Đồ thị xuất xuất xung tam giác sin 19 Hình 4.11 Sơ đồ mạch Pull-Up Input 20 Hình 4.12 Sơ đồ mạch Pull-Down Input 20 Hình 4.13 Sơ đồ mạch Push-Pull Output P-MOS mở 21 Hình 4.14 Sơ đồ mạch Push-Pull Output P-MOS đóng 21 Hình 4.15 Khối Digital Output 22 Hình 4.16 Hộp thoại Digital Output 22 Hình 4.17 Code Simulink “Điều chỉnh thời gian sáng LED” 23 Hình 4.18 Cách Run mơ hình Simulink 24 Hình 4.19 Thiết lập khối Pulse Generator 24 Hình 4.20 Kết lắp mạch “Điều chỉnh thời gian sáng LED” 25 Hình 4.21 Đồ thị tín hiệu sáng LED 25 Hình 4.22 Khối Digital Input 25 Hình 4.23 Hộp thoại Digital Input 26 Hình 4.24 Code Simulink “Điều khiển LED nút nhấn” 26 Hình 4.25 Kết lắp mạch “Điều khiển LED nút nhấn” 27 Hình 4.26 Sự hoạt động Timer/Counter Prescaler thay đổi từ đến 28 Hình 4.27 Cách điều chỉnh độ rộng xung 29 Hình 4.28 Điều chỉnh độ rộng xung với cạnh lên 29 Hình 4.29 Điều chỉnh độ rộng xung với cạnh lên xuống 30 Hình 4.30 Khối Basic PWM 30 Hình 4.31 Khối hộp thoại Basic PWM 30 Hình 4.32 Code Simulink “điều chỉnh độ sáng Led dùng PWM” 31 Hình 4.33 Kết lắp mạch “Điều chỉnh độ sáng LED dùng PWM 32 Hình 4.34 Kết đồ thị “Điều chỉnh độ sáng LED dùng PWM” 32 Hình 4.35 Cấu trúc mã hóa quay 33 vii Hình 4.36 Tín hiệu từ kênh A, kênh B lệch pha 90 độ 33 Hình 4.37 Giao diện tín hiệu xung vng 34 Hình 4.38 Khối Encoder read 34 Hình 4.39 Khối hộp thoại Encoder Read 35 Hình 4.40 Hộp thoại khối Timer IRQ 36 Hình 4.41 Khối Timer IRQ 36 Hình 4.42 Code simulink “điều khiển LED dùng khối Timer IRQ” 37 Hình 4.43 Chương trình đọc LED 38 Hình 4.44 Mạch điều khiển LED dùng Timer IRQ 38 Hình 4.45 Khối UART Setup 39 Hình 4.46 Khối UART Tx 39 Hình 4.47 Khối hộp thoại UART Tx 40 Hình 4.48 Code simulink “truyền giá trị từ Board mạch lên máy tính 41 Hình 4.49 Kết lắp mạch “Truyền giá trị từ Board lên máy tính” 41 Hình 4.50 Kết gửi lên máy tính qua phần mềm Terminal 41 Hình 4.51 Hộp thoại Host Serial Setup 42 Hình 4.52 Hộp thoại Host Serial Tx 42 Hình 4.53 Hộp thoại Host Serial Rx 43 Hình 4.54 Mơ hình code nạp xuống board 43 Hình 4.55 Mơ hình chạy chế độ inf máy tính 44 Hình 4.56 Kết lắp mạch “Thí nghiệm truyền giá trị xuống board” 44 Hình 4.57 Đồ thị điều chỉnh Slider gửi giá trị xuống board 44 Hình 4.58 Sơ đồ kết nối thiết bị I2C 45 Hình 4.59 Cảm biến GY-521 6DOF MPU6050 46 Hình 4.60 Khối I2C Master Setup 48 Hình 4.61 Khối hộp thoại I2C Master Setup 48 Hình 4.62 Khối I2C Master Read/Write 49 Hình 4.63 Khối hộp thoại I2C Master Read/Write 49 Hình 4.64 Code Simulink “Đọc cảm biến MPU6050” (1) 50 Hình 4.65 Code Simulink “Đọc cảm biến MPU6050” (2) 51 Hình 4.66 Giá trị gia tốc đọc lúc cân (đỏ - trục x; vàng – trục y; xanh – trục z) 51 Hình 4.67 Đọc giá trị góc lúc đặt cân (đỏ - trục x; vàng – trục y; xanh – trục z) 52 Hình 5.1 Động DC 53 Hình 5.2 Module cầu H 54 Hình 5.3 Mạch điện phần ứng sơ đồ chuyển động rotor 54 Hình 5.4 Mơ hình DC motor 57 Hình 5.5 Sơ đồ mạch điện điều khiển tốc độ DC motor 58 Hình 5.6 Giải thuật điều khiển tốc độ động sử dụng STM32F407VG DISCOVERY 59 Hình 5.7 Khối đọc tốc độ động “Encoder Read” 59 Hình 5.8 Chương trình hiển thị kết máy tính (real-time) 60 Hình 5.9 Kết điều khiển tốc độ DC motor 60 Hình 5.10 Giải thuật điều khiển tốc độ động sử dụng Arduino Mega 2560 61 Hình 5.11 Kết đồ thị sử dụng Arduino Mega 2560 – thời gian lấy mẫu ms 61 Hình 5.12 Kết điều khiển STM32F407 DISCOVERY – xung vuông, chu kỳ 10ms (đỏ - mong muốn; đen – thực tế) 62 viii Hình 5.13 Kết điều khiển ARDUINO MEGA 2560 – xung vuông, chu kỳ 10ms (đen - mong muốn; đỏ – thực tế) 62 Hình 5.14 Kết điều khiển STM32F407 DISCOVERY – xung vuông, chu kỳ 1ms (đỏ - mong muốn; đen – thực tế) 63 Hình 5.15 Kết điều khiển ARDUINO MEGA 2560 – xung vuông, chu kỳ 1ms (đỏ - mong muốn; đen – thực tế) 63 Hình 5.16 Kết điều khiển STM32F407 DISCOVERY – xung sin, tần số pi (đỏ - mong muốn; đen – thực tế) 64 Hình 5.17 Kết điều khiển ARDUINO MEGA 2560 – xung sin, tần số pi (đỏ mong muốn; đen – thực tế) 64 Hình 5.18 Kết điều khiển STM32F407 DISCOVERY – xung sin, tần số 4*pi (đỏ - mong muốn; đen – thực tế) 65 Hình 5.19 Kết điều khiển ARDUINO MEGA 2560 – xung sin, tần số pi (đỏ mong muốn; đen – thực tế) 65 Hình 6.1 Sơ đồ vật thể tự hệ lắc 68 Hình 6.2 Hình lắc theo phương chiếu thẳng 69 Hình 6.3 Hình lắc theo phương chiếu thẳng đứng 69 Hình 6.4 Mơ hình lắc ngược 71 Hình 6.5 Sơ đồ mạch điện điều khiển lắc ngược 72 Hình 6.6 Giải thuật điều khiển lắc ngược 72 Hình 6.7 Kết thực nghiệm dùng STM32F407 DISCOVERY 73 Hình 6.8 Kết thực nghiệm lắc ngược 73 Hình 6.9 Kết lắc ngược dùng Arduino 73 Hình 7.1 DC motor 25GA370 75 Hình 7.2 Cầu H L298 76 Hình 7.3 MPU 6050 76 Hình 7.4 Sơ đồ tính tốn robot cân 77 Hình 7.6 Cẩm biến MPU6050 gá robot 81 Hình 7.5 Mơ hình robot cân 81 Hình 7.7 Sơ đồ mạch điện robot cân 81 Hình 7.8 Giải thuật điều khiển robot cân 81 Hình 9.1 Giao diện Website Waijung Blockset (1) 83 Hình 9.2 Giao diện Website Waijung Blockset (2) 83 Hình 9.3 Biểu tượng phần mềm Matlab/Simulink 84 Hình 9.4 Thư viện phần mềm Simulink (1) 84 Hình 9.5 Thư viện phần mềm Simulink (2) 85 Hình 9.6 Demo Simulink 85 Hình 9.7 Giao diện sau nạp thành công 86 Hình 9.8 Run file setup 87 Hình 9.9 Chọn Next để tiếp tục cài đặt 88 Hình 9.10 Qúa trình cài đặt 89 Hình 9.11 Hai cửa sổ chọn Next Finish 89 Hình 9.12 Mơ-đun UART CP2102 89 Hình 9.13 Kết nối UART với máy tính 89 Hình 9.14 Giao diện thiết lập UART Setup (trái) giao diện UART Tx (phải) 90 Hình 9.15 Giải nén phần mềm 90 ix DANH MỤC CÁC BẢNG Bảng 4.1 Số lượng chân mô-đun ADC 12 Bảng 4.2 Đặc tính điện mô-đun ADC 13 Bảng 4.3 Đặc tính điện mơ-đun DAC 13 Bảng 4.4 Mô tả chức khối ADC 15 Bảng 4.5 Nối dây biến biến trở 16 Bảng 4.6 Mô tả khối Regular DAC 18 Bảng 4.7 Đặc tính dịng điện 21 Bảng 4.8 Các khối Simulink cho điều chỉnh thời gian sáng LED 23 Bảng 4.9 Mô tả khối Pulse Genertor 24 Bảng 4.10 Các khối Simulink cho thí nghiệm “Điều khiển LED nút nhấn” 27 Bảng 4.11 Cấu hình mơ-đun Timer 28 Bảng 4.12 Mô tả khối hộp thoại Encoder Read 35 Bảng 4.13 Mô tả khối hộp thoại Timer IRQ 37 Bảng 4.14 Mô tả khối hộp thoại UART Setup 39 Bảng 4.15 Mô tả khối hộp thoại UART Tx 40 Bảng 4.16 Bảng mô tả khối hộp thoại I2C Setup 48 Bảng 4.17 Mô tả Khối hộp thoại I2C Master Read/Write 49 Bảng 5.1 Thông số mơ hình hóa DC motor 54 Bảng 7.1 đại lượng, kí hiệu, đơn vị phương trình 76 x CHƯƠNG : TỔNG QUAN ĐỀ TÀI 1.1 Tình hình nghiên cứu ngồi nước 1.1.1 Tổng quan tình hình nghiên thuộc lĩnh vực đề tài Trong nước: Hiện có nhiều đề tài nghiên cứu dùng lý thuyết điều khiển khác nhau: PID, Fuzzy, LQR, đề điều khiển robot cân bằng, lắc ngược thông qua điều khiền tốc độ vị trí DC motor Tuy nhiên lắc ngược đặc biệt robot cân đặt nhiều vấn đề khó khăn lý thuyết điều khiển thiết bị điều khiển chung hệ phi tuyến Ngồi nước: Cơng trình nghiên cứu chủ yếu thiết kế hoàn chỉnh module điều khiển từ phần xử lý tín hiệu, xử lý tính tốn trung tâm, cơng suất 1.1.2 Danh mục cơng trình liên quan • Nghiên cứu Mai Tuấn Đạt (Đại học Bách Khoa -thành phố Hồ Chí Minh) với đề tài “Xe hai bánh tự cân di chuyển địa hình phẳng” • Trương Lê Hữu Phát – Lê Phước Vạn, Đồ án tốt nghiệp “Robot hai bánh tự cân dùng LQR”, Tp Hồ Chí Minh, 2016 • Nguyễn Tùng Lâm – Nguyễn Tường Duy, Đồ án tốt nghiệp “Thiết kế chế tạo KIT thí nghiệm phục vụ mơn học điều khiển tự động”, Tp Hồ Chí Minh, 2016 1.2 Tính cấp thiết đề tài Mơn học ứng dụng điều khiển tự động ô tô môn học áp dụng cho sinh viên năm ngành công nghệ kỹ thuật ô tô trường Đại học Sư phạm Kỹ thuật TPHCM Môn học trang bị cho sinh viên kiến thức hệ thống điều khiển tự động Mặc dù có số ví dụ minh họa mơ hình giảng dạy qua thời gian số mơ hình trở nên hư hỏng cần phải phục chế lại Thêm vào với việc dùng Board Arduino Mega 2560 để Kit mô hình DC motor lắc ngược có số hạn chế số thời điểm cần đáp ứng nhanh chưa thực tốc độ xử lí Arduino chưa cao Từ vấn đề chúng em thấy cần thiết phải nghiên cứu ứng dụng Board mạch khác có nhớ tốc độ xử lí nhanh để đáp ứng cho vào mơ hình mà cụ thể Board ARM STM32F407 DISCOVERY 1.3 Mục tiêu đề tài Tìm hiểu thiết bị ngoại vi hướng dẫn sử dụng Board ARM STM32F407 DISCOVERY nhúng vào Matlab/Simulink Đọc tín hiệu cảm biến tốc độ vị trí, cảm biến gia tốc để điều khiển mơ hình điều khiển Điều khiển tốc độ vị trí DC motor thay đổi tín hiệu đầu vào (hằng số, xung vng sóng sin) tăng dần thời gian đáp ứng Dựa vào toán vị trí DC motor điều khiển lắc ngược robot cân Khơi phục cải tiến lại mơ hình DC motor lắc ngược Thiết kế, chế tạo robot cân Điều khiển, lấy kết thực nghiệm mơ hình so sánh kết điều khiển dùng Board Arduino Mega 2560 1.4 Phương pháp phạm vi nghiên cứu Phương pháp nghiên cứu kết hợp lý thuyết thực nghiệm, nghiên cứu tài liệu lý thuyết điều khiển tự động tài liệu có liên quan, dựa vào phần tính tốn mơ đề tài trước để hiểu lý thuyết thuật tốn mơ Tìm hiểu thiết bị ngoại vi bảng liệu Board ARM STM32F407 để thiết lập mơ hình điều khiển Phạm vi nghiên cứu gồm điều khiển vị trí, tốc độ DC motor, cân lắc ngược chuyển động quay robot cân Từ đó, chúng em đánh giá kết lý thuyết kết thực nghiệm dựa mô thực nghiệm đồng thời đưa so sánh tốc độ xử lí board mạch Arduino ARM STM32F407 Cuối chúng em đưa nhận xét đề xuất hướng phát triển đề tài Hệ số ma sát bánh xe với mặt phẳng fw Hệ số ma sát động với Robot fm Mơ mem qn tính động Jm Hệ số EMF động Kb Mô mem xoắn động Kt kg.m^2 V sec/rad Nm/A Điện trở động rm Ohm Tỷ số giảm tốc n Gia tốc trọng trường g Góc trung bình bánh trái phải 𝜃 Kg/ m^2 rad 𝜃𝑙,𝑟 rad Góc nghiêng phần thân robot 𝜓 rad Góc xoay robot 𝜙 rad Tọa độ bánh trái 𝑥𝑙 , 𝑦𝑙 , 𝑧𝑙 m Tọa độ bánh phải 𝑥𝑟 , 𝑦𝑟 , 𝑧𝑟 m Tọa độ trung bình 𝑥𝑚 , 𝑦𝑚 , 𝑧𝑚 m Moment phát động theo phương khác 𝐹𝜃 , 𝐹ψ , 𝐹𝜙 Nm Moment phát động động bánh trái, phải 𝐹𝑙,𝑟 Nm Dòng điện động bánh trái, phải 𝑖𝑙 ,𝑖𝑟 A 𝑣𝑙 , 𝑣𝑟 V Góc bánh trái phải Điện áp động bánh trái, phải 7.2.2 Thiết lập phương trình Xây dựng hệ phương trình trạng thái mơ tả hệ thống robot hai bánh tự cân bằng: Hình 7.4 Sơ đồ tính tốn robot cân 77 Sử dụng phương pháp Euler-Lagrange để xây dựng mơ hình động học Giả sử thời điểm t = 0, robot di chuyển theo chiều dương trục x, ta có phương trình sau: Góc tịnh tiến trung bình hai bánh xe góc xoay robot xác định (𝜃𝑙 +𝜃𝑟 ) 𝜃 [ ]=[ ] 𝜙 𝑅 (𝜃 −𝜃 ) 𝑊 𝑙 𝑟 (7-1) Trong tọa độ trung bình Robot hệ qui chiếu: ∫ 𝑥̇ 𝑚 𝑥𝑚 [𝑦𝑚 ] = ∫ 𝑦̇𝑚 𝑧𝑚 [ 𝑅 ] 𝑥̇ 𝑅𝜃̇𝑐𝑜𝑠𝜙 Hay [ 𝑚 ] = [ ] 𝑦̇𝑚 𝑅𝜃̇𝑠𝑖𝑛𝜙 (7-2) Tọa độ bánh trái hệ qui chiếu : 𝑊 𝑥𝑚 − 𝑠𝑖𝑛𝜙 𝑥𝑙 [𝑦𝑙 ] = 𝑊 𝑦𝑚 + 𝑐𝑜𝑠𝜙 𝑧𝑙 𝑧𝑚 [ ] (7-3) Tọa độ bánh phải hệ qui chiếu : 𝑊 𝑥𝑚 + 𝑠𝑖𝑛𝜙 𝑥𝑟 [𝑦𝑟 ] = 𝑊 𝑦𝑚 − 𝑐𝑜𝑠𝜙 𝑧𝑟 𝑧𝑚 [ ] (7-4) Tọa độ tâm đối xứng hai động hệ qui chiếu : 𝑥𝑏 𝑥𝑚 + 𝐿𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜙 𝑦 [ 𝑏 ] = [ 𝑦𝑚 𝐿𝑠𝑖𝑛𝜓𝑠𝑖𝑛𝜙 ] 𝑧𝑏 𝑧𝑚 + 𝐿𝑐𝑜𝑠𝜓 (7-5) Phương trình động chuyển động tịnh tiến: 1 𝑇1 = 𝑚(𝑥̇ 𝑙 + 𝑦̇ 𝑙 + 𝑧̇𝑙 ) + 𝑚(𝑥̇ 𝑟 + 𝑦̇𝑟 + 𝑧̇𝑟 ) + 𝑚(𝑥̇ 𝑏 + 𝑦̇ 𝑏 + 𝑧̇𝑏 ) 2 (7-6) Phương trình động chuyển động quay : 78 1 1 2 𝑇2 = 𝐽𝑤 𝜃̇𝑙 + 𝐽𝑤 𝜃̇𝑟 + 𝐽𝜓 𝜓̇ + 𝐽𝜙 𝜙̇ + 𝑛2 𝐽𝑚 (𝜃̇𝑙 − 𝜓̇)2 2 2 + 𝑛2 𝐽𝑚 (𝜃̇𝑟 − 𝜓̇)2 (7-7) Với 𝑛 𝐽𝑚 (𝜃̇𝑙 − 𝜓̇)2 ; 𝑛2 𝐽𝑚 (𝜃̇𝑟 − 𝜓̇)2 2 (7-8) động quay phần ứng động trái phải Phương trình năng: 𝑈 = 𝑚𝑔𝑧𝑙 + 𝑚𝑔𝑧𝑟 + 𝑚𝑔𝑧𝑏 (7-9) Phương trình Lagrange : 𝐿 = 𝑇1 + 𝑇2 − 𝑈 (7-10) 𝑑 𝜕𝐿 𝜕𝐿 ( )− = 𝐹𝜃 𝑑𝑡 𝜕𝜃̇ 𝜕𝜃 (7-11) 𝑑 𝜕𝐿 𝜕𝐿 ( )− = 𝐹𝜓 𝑑𝑡 𝜕𝜓̇ 𝜕𝜓 (7-12) 𝑑 𝜕𝐿 𝜕𝐿 ( )− = 𝐹𝜙 𝑑𝑡 𝜕𝜙̇ 𝜕𝜙 (7-13) Lấy đạo hàm L theo biến ta được: [(2𝑚 + 𝑀)𝑅2 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝐿𝑅𝜓̇ 𝑠𝑖𝑛𝜓 = 𝐹𝜃 (7-14) (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜃̈ + (𝑀𝐿2 + 𝐽𝜓 + 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝑔𝐿𝑠𝑖𝑛𝜓 − 𝑀𝐿2 𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = 𝐹𝜓 (7-15) 𝑊2 [ 𝑚𝑊 + 𝐽𝜙 + (𝐽𝑤 + 𝑛2 𝐽𝑚 ) + 𝑀𝐿2 𝑠𝑖𝑛2 𝜓] 𝜙̈ + 2𝑀𝐿2 𝜓̇𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = 𝐹𝜙 (7-16) 2𝑅 Momen động lực động DC sinh ra: 𝐹𝑙 + 𝐹𝑟 𝐹𝜃 𝐹𝜓 [𝐹𝜓 ] = [ ] 𝑊 𝐹𝜙 (𝐹 − 𝐹𝑟 ) 2𝑅 𝑙 (7-17) 𝐹𝑙 = 𝑛𝐾𝑡 𝑖𝑙 + 𝑓𝑚 (𝜓̇ − 𝜃̇𝑙 ) − 𝑓𝑤 𝜃̇𝑙 (7-18) Với 79 𝐹𝑟 = 𝑛𝐾𝑡 𝑖𝑟 + 𝑓𝑚 (𝜓̇ − 𝜃̇𝑟 ) − 𝑓𝑤 𝜃̇𝑟 𝐹𝜓 = −𝑛𝐾𝑡 𝑖𝑙 − 𝑛𝐾𝑡 𝑖𝑟 − 𝑓𝑚 (𝜓̇ − 𝜃̇𝑙 ) − 𝑓𝑚 (𝜓̇ − 𝜃̇𝑟 ) (7-19) (7-20) Sử dụng phương pháp PWM để điều khiển động nên chuyển từ dòng điện sang điện áp động cơ: ̇ = 𝑣𝑙,𝑟 + 𝐾𝑏 (𝜓̇ − 𝜃̇𝑙,𝑟 ) − 𝑅𝑚 𝑖𝑙,𝑟 𝐿𝑚 𝑖̇𝑙,𝑟 (7-21) Xem điện cảm phần ứng tương đối nhỏ (gần 0), bỏ qua, suy ra: 𝑖𝑙,𝑟 = 𝑣𝑙,𝑟 + 𝐾𝑏 (𝜓̇ − 𝜃̇𝑙,𝑟 ) 𝑅𝑚 (7-22) Từ đó, moment lực sinh ra: 𝐹𝜃 = 𝛼 (𝑣𝑙 + 𝑣𝑟 ) − 2(𝛽 + 𝑓𝑤 )𝜃̇ + 2𝛽𝜓̇ (7-23) 𝐹𝜓 = −𝛼 (𝑣𝑙 + 𝑣𝑟 ) + 2𝛽𝜃̇ − 2𝛽𝜓̇ (7-24) 𝑛𝐾𝑡 𝑛𝐾𝑡 𝐾𝑏 𝛽 = + 𝑓𝑚 𝑅𝑚 𝑅𝑚 (7-25) 𝑊 𝑊2 𝛼(𝑣𝑟 − 𝑣𝑙 ) − (𝛽 + 𝑓𝑤 )𝜙̇ 2𝑅 2𝑅 (7-26) Với 𝛼 = 𝐹𝜙 = Thu phương trình động lực học mơ tả chuyển động robot sau: [(2𝑚 + 𝑀)𝑅2 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝐿𝑅𝜓̇ 𝑠𝑖𝑛𝜓 = 𝛼 (𝑣𝑙 + 𝑣𝑟 ) − 2(𝛽 + 𝑓𝑤 )𝜃̇ + 2𝛽𝜓̇ (𝑀𝐿𝑅𝑐𝑜𝑠𝜓 − 2𝑛2 𝐽𝑚 )𝜃̈ + (𝑀𝐿2 + 𝐽𝜓 + 2𝑛2 𝐽𝑚 )𝜓̈ − 𝑀𝑔𝐿𝑠𝑖𝑛𝜓 − 𝑀𝐿2 𝜙̇ 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = −𝛼(𝑣𝑙 + 𝑣𝑟 ) + 2𝛽𝜃̇ − 2𝛽𝜓̇ 𝑊2 [ 𝑚𝑊 + 𝐽𝜙 + (𝐽𝑤 + 𝑛2 𝐽𝑚 ) + 𝑀𝐿2 𝑠𝑖𝑛2 𝜓] 𝜙̈ 2 2𝑅 𝑊 𝑊2 ̇ ̇ + 2𝑀𝐿 𝜓𝜙 𝑠𝑖𝑛𝜓𝑐𝑜𝑠𝜓 = 𝛼(𝑣𝑟 − 𝑣𝑙 ) − (𝛽 + 𝑓𝑤 )𝜙̇ 2𝑅 2𝑅 (7-27) (7-28) (7-29) 80 7.3 Bộ điều khiển robot cân bằngMơ hình sơ đồ mạch điện Hình 7.6 Mơ hình robot cân Hình 7.5 Cẩm biến MPU6050 gá robot Hình 7.7 Sơ đồ mạch điện robot cân 7.3.1 Giải thuật Hình 7.8 Giải thuật điều khiển robot cân 81 CHƯƠNG : KẾT LUẬN VÀ ĐỀ NGHỊ 8.1 Kết thực • Đề tài hồn thành số cơng việc sau: - Tìm hiểu biết cách sử dụng board ARM STM32F4 DISCOVERY nhúng vào Matlab/Simulink - Biết cách sử dụng phần mềm hiển thị liệu lên máy tính Terminal phần mềm máy xung Hantek - Đọc giá trị cảm biến gia tốc, vị trí tốc độ - Nghiên cứu DC motor điều khiển tốc độ vị trí - Nghiên cứu lắc ngược điều khiển cân lắc - Nghiên cứu robot cân đồng thời thiết kế thi cơng mơ hình thực nghiệm • Tuy nhiên đề tài cịn nhiều hạn chế như: - Mơ hình lắc ngược cịn vướng dây cảm biến encoder hoạt động nên hạn chế số vịng quay - Mơ hình lắc ngược dừng lại mức điều khiển SISO nên chưa có điều khiển Swing-up - Tín hiệu cảm biến gia tốc MPU6050 bị nhiễu chưa dùng lọc Kalman 8.2 Hướng phát triển đề tài • Phát triển thêm điều khiển có Swing-up • Tìm hiểu để thêm lọc Kalman vào xử lí tín hiệu cảm biến gia tốc • Cải thiệt phần cứng, xây dựng ứng dụng giải thuật đai khác vào hệ thống 82 DANH MỤC TÀI LIỆU THAM KHẢO • Tài liệu nước [1] Nguyễn Thị Phương Hà – Huỳnh Thái Hoàng, Lý thuyết điều khiển tự động, NXB Đại học Quốc Tp Hồ Chí Minh, 2005 [2] Nguyễn Phung Quang, Giáo trình Matlab Simulink, NXB Khoa học Kỹ thuật, Hà Nội, 2004 [3] Mai Tuấn Đạt, Luận văn tốt nghiệp đại học “Xe hai bánh tự cân di chuyển địa hình phẳng”, Tp Hồ Chí Minh, 2005 [4] Trương Lê Hữu Phát – Lê Phước Vạn, Đồ án tốt nghiệp “Robot hai bánh tự cân dùng LQR”, Tp Hồ Chí Minh, 2016 [5] Nguyễn Tùng Lâm – Nguyễn Tường Duy, Đồ án tốt nghiệp “Thiết kế chế tạo KIT thí nghiệm phục vụ mơn học điều khiển tự động”, Tp Hồ Chí Minh, 2016 [6] Nguyễn Trung Hiếu, Bài giảng “Automatic Control for Automotive Application”, Đại học Sư phạm Kỹ thuật Tp Hồ Chí Minh, 2018 • Tài liệu nước ngồi [7] Karl J.Astrom – Tore Hagglund, PID Controller: Theory, Design and Tuning – 2nd editio, Instrument Society of America, 1995 • Tài liệu Internet [8] STMicroelectronics, “RM0090: STM32F40xxx Reference Manual [Online]”, 2013 [9] STMicroelectronics, “STM32 F407xx Datasheet-production data [Online]”, 2013 [10] http://aimagin.com/blog/waijung-tutorials/ [11] http://waijung.aimagin.com/ [12] http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&se ction=SystemModeling [13] https://github.com/kriswiner/MPU6050/blob/master/MPU6050BasicExample.i no [14] https://www.i2cdevlib.com/forums/topic/217-acceleration-in-ms2/ [15] https://archive.cnx.org/contents/0a48120e-a4c0-41c7-a0bf-02b8f4623418@1/tng-quan-v-h-th-ng-i-u-khi-n [16] https://www.alldatasheet.com/ [17] https://www.youtube.com/channel/UCRWxAN2CmlDdiY_WhGibhNQ [18] http://ctms.engin.umich.edu/CTMS/index.php?aux=Activities_DCmotorB [19] https://www.mathworks.com/help/simulink/examples/anti-windup-controlusing-a-pid-controller.html 83 PHỤ LỤC ❖ Hướng dẫn cài đặt thư viện WaijungBlockset vào Matlab/Simulink Các bước thực để thêm thư viện Waijungblockset vào thư viện MATLAB/SIMULINK ➢ Bước 1: Truy cập vào địa web: http://waijung.aimagin.com/ Hình 9.1 Giao diện Website Waijung Blockset (1) Để cài đặt thư viện Waijung Blockset vào thư viện Simulink ta vào địa trang web Sau nhấp vào biểu tượng Download Soflware (vị trí khoanh trịn) hình… Sau nhấp chuột ta giao diện web hình… Tiếp sau ta nhấp chuột tiếp vào biểu tượng Dowload Now (vị trí khoanh trịn trịn hình) để download thư viện Waijung Blockset máy tính Hình 9.2 Giao diện Website Waijung Blockset (2) Khi hồn tất q trình tải ta tệp có tên “waijung17_03a” Để cài đặt thư viện ta cần phải giải nén tệp vào vị trí máy tính, mà thấy thuận tiện 84 ➢ Bước 2: Cài đặt thư viện waijungblockset vào thư viện Matlab/Simulink Để cài đặt ta cần chạy phần mềm Matlab/Simulink cách đúp chuột phải vào biểu tượng Matlab, kích vào chạy Run as administrator hình vẽ để tránh tình trạng xảy lỗi trình làm việc Hình 9.3 Biểu tượng phần mềm Matlab/Simulink Sau chạy phần mềm Matlab/Simulink xong ta hình… Tiếp theo ta cần phải nhấp vào Curent Directory Matlab để tìm đường dẫn đến thư mục “waijung17_3a” giải nén lúc trước hình… (vị trí khoanh trịn thứ 1) Hình 9.4 Thư viện phần mềm Simulink (1) Tiếp theo nhấp đúp chuột vào install_waijung.m (vị trí khoanh trịn thứ 2) để cài đặt thư viện 85 Hình 9.5 Thư viện phần mềm Simulink (2) Khi q trình cài đặt hồn tất Ta mở thư viện Simulink xuất thêm thư viện Waijung Blockset hình … (vị trí khoanh trịn hình trên) việc cài đặt thư viện Waijung Blockset thành cơng ➢ Bước 3: Nạp thử chương trình vào bo mạch STM32F4 Trong thư mục “waijung17_3a” nhà cung cấp cho người dùng phần mềm ví dụ Ta dùng thử demo cho đèn led bo STM32 chớp tắt cách vào đường dẫn sau: waijung17_03a\targets\stm32f4_target\stm32f4\demo\ digital_output_demo tool Current Directoty phần mềm Matlab Hình 9.6 Demo Simulink Để nạp code vào bo STM32 ta cần kết nối board dây cáp qua công USB máy tính Tiếp theo nhấp vào biểu tượng Incremental buid cơng cụ 86 khối demo (vị trí khoanh trịn màu đỏ Hình 3.8) để nạp dịng code vào board Khi hình máy tính xuất Hình 3.9 Và khối hình chuyển sang màu xanh việc nạp code vào board STM32F4 thành cơng Hình 9.7 Giao diện sau nạp thành cơng 87 ❖ Hướng dẫn cài ST-LINK ➢ Bước 1: Truy cập vào địa web: https://www.st.com/en/development-tools/stlink-v2.html ➢ Bước 2: Tải cài đặt bước theo hình Hình 9.8 Run file setup Hình 9.9 Chọn Next để tiếp tục cài đặt 88 Hình 9.10 Qúa trình cài đặt Hình 9.11 Hai cửa sổ chọn Next Finish 89 ❖ Hướng dẫn kết nối USB UART CP2102 Để thu thập liệu từ thực tế hiển thị hình máy tính ta sử dụng mơ đun UART CP2102 Hình 9.12 Mơ-đun UART CP2102 Hình Mơ-đun UART CP2102 cho biết kết nối truyền liệu nối tiếp (Tx) máy phát với chân liệu nhận (Rx) máy thu để gửi nhận liệu Máy phát máy thu phải nối mát với Thông qua mô-đun UART CP2102 ta truyền nhận liệu từ phần cứng hiển thị máy tính Hình 9.13 Kết nối UART với máy tính Để nhận tín hiệu thơng qua mô đun UART từ bo STM32 ta cần phải cài đặt khối blockset thư viện Các khối (UART Tx UART Setup) lấy từ Simulink library >> Waijung Blockset >> STM32F4 Target >> On Peripheral Chip >> UART consisting of UART setup Nếu muốn sử dụng UART STM32, ta cần phải cài đặt UART Block Simulink: • Thiết lập chân PIN Tx, Rx, RTS, CTS STM32F4 • Thiết lập UART 90 Trong mơ đun ta sử dụng chân Tx chân D8 chân Rx chân D9 Mô đun UART sử dụng Mơ-đun Tốc độ truyền 115200 (bps) Hình 9.14 Giao diện thiết lập UART Setup (trái) giao diện UART Tx (phải) ➢ Bước 1: Truy cập vào địa web: http://hocdientu.vn/threads/download-terminalv1-9b.39/ ➢ Bước 2: Tải cài đặt bước theo hình Hình 9.15 Giải nén phần mềm 91 ... dạng, sơ đồ khối cấu hình hệ thống điều khiển thường gặp 2.3 Thiết kế điều khiển 2.3.1 Phần loại thiết kế điều khiển Thiết kế tồn q trình bổ sung thiết bi phần cứng thuật toán, phần mềm vào hệ... án tốt nghiệp ? ?Thiết kế chế tạo KIT thí nghiệm phục vụ mơn học điều khiển tự động”, Tp Hồ Chí Minh, 2016 1.2 Tính cấp thiết đề tài Môn học ứng dụng điều khiển tự động ô tô môn học áp dụng cho... tượng điều khiển Thiết bị đo lường có chức thu nhập thông tin, điều khiển thực chức xử lý thông tin, định điều khiển đối tượng điều khiển chịu tác động tín hiệu điều khiển Hệ thống điều khiển