BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC NHA TRANG NGUYỄN NAM NGHIÊN CỨU PHƢƠNG PHÁP ĐIỀU KHIỂN CÂN BẰNG CHO ROBOT TỰ CÂN BẰNG LUẬN VĂN THẠC SĨ KHÁNH HÒA - 2015 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC NHA TRANG NGUYỄN NAM NGHIÊN CỨU PHƢƠNG PHÁP ĐIỀU KHIỂN CÂN BẰNG CHO ROBOT HAI BÁNH TỰ CÂN BẰNG LUẬN VĂN THẠC SĨ Ngành: Kỹ thuật khí Mã số: 60520103 Quyết định giao đề tài: 933/QĐ-ĐHNT ngày 26/9/2014 Quyết định thành lập HĐ: 1079/QĐ-ĐHNT ngày 19/11/2015 Ngày bảo vệ: 16/01/2016 Ngƣời hƣớng dẫn khoa học: TS ĐẶNG XUÂN PHƢƠNG Chủ tịch Hội đồng PGS.TS PHẠM HÙNG THẮNG Khoa sau đại học KHÁNH HÒA - 2015 LỜI CAM ĐOAN Tôi xin cam đoan kết đề tài: “NGHIÊN CỨU PHƢƠNG PHÁP ĐIỀU KHIỂN CÂN BẰNG CHO ROBOT HAI BÁNH TỰ CÂN BẰNG ” công trình nghiên cứu cá nhân chƣa đƣợc công bố công trình khoa học khác thời điểm Nha Trang, Ngày 19 tháng 10 năm 2015 Tác giả luận văn Nguyễn Nam i LỜI CẢM ƠN Trong suốt thời gian thực đề tài, nhận đƣợc giúp đỡ quý phòng ban trƣờng Đại học Nha Trang, tạo điều kiện tốt cho đƣợc hoàn thành đề tài Đặc biệt hƣớng dẫn tận tình TS Đặng Xuân Phƣơng giúp hoàn thành tốt đề tài Qua đây, xin gửi lời cảm ơn sâu sắc đến giúp đỡ Ngoài ra, xin chân thành cảm ơn TS Vũ Thăng Long, Th.S Trần Văn Hùng giảng viên môn Cơ điện tử nhiệt tình giúp đỡ mặt lý thuyết để hoàn thành phần điện điều khiển động Cuối xin gửi lời cảm ơn chân thành đến gia đình tất bạn bè giúp đỡ, động viên suốt trình học tập thực đề tài Tôi xin chân thành cảm ơn! Nha Trang, ngày 19 tháng 10 năm 2015 Tác giả luận văn Nguyễn Nam ii MỤC LỤC LỜI CẢM ƠN ii MỤC LỤC iii DANH MỤC BẢNG vi DANH MỤC HÌNH VẼ .vii TRÍCH YẾU LUẬN VĂN x Lời nói đầu Chƣơng - TỔNG QUAN 1.1 Nguyên lý hoạt động xe hai bánh tự cân 1.2 Ƣu nhƣợc điểm xe hai bánh tự cân 1.2.1 Ƣu điểm 1.2.2 Nhƣợc điểm 1.3 Khả ứng dụng 1.4 Nhu cầu thực tế 1.5 Một số dạng xe hai bánh tự cân 1.5.1.1 Segway 1.5.1.2 Xe Balancing 1.6 Tình hình nghiên cứu nƣớc 1.6.1 Quốc tế 1.6.1.1 JOE 1.6.1.2 nBot 1.6.1.3 BaliBot 10 1.6.1.4 Balancing robot (Bbot) 10 1.6.1.5 Equibot 10 1.6.1.6 Bender 11 1.6.1.7 ArduRoller balance bot 11 iii 1.6.1.8 Loại Robot hổ trợ ngƣời hãng TOYOTA 11 1.6.2 Trong nƣớc 12 1.6.2.1 Đại học Lạc Hồng 12 1.6.2.2 Đại học bách khóa TP.HCM 12 2.1 Phƣơng pháp nghiên cứu 14 2.2 Cơ sở lý thuyết động lực học 14 2.2.1 Nguyên lý hoạt động robot tự cân 15 2.2.2 Phân tích lực từ mô hình lắc ngƣợc 16 2.2.3 Mô hình động lực học 17 2.3 Lý thuyết điều khiển 23 2.3.1 Lý thuyết cổ điển 24 2.3.2 Lý thuyết đại 24 2.4 Giải thuật điều khiển PID 24 2.4.1 Bộ điều khiển PID 24 2.4.2 Giải thuật PID 25 2.4.3 Điều chỉnh hệ số PID 26 2.5 Các phƣơng pháp xử lý tín hiệu từ cảm biến 28 2.5.1 Độ xác cảm biến 28 2.5.2 Lọc bổ phụ thông tần (Complementary Filter) 29 2.5.3 Lọc thích nghi - Bộ lọc Kalman (Kalman Filter) 30 Chƣơng – CHẾ TẠO MÔ HÌNH XE BÁNH TỰ CÂN BẰNG 33 3.1 Chế tạo khí 33 3.1.1 Linh kiện điện tử 34 3.1.2.1 Cảm biến 34 3.1.2.2 Bánh xe 41 3.1.2.3 Động điện DC 41 iv 3.1.2.4 3.2 Điều khiển tốc độ động điện DC 44 Mạch điện tử 48 3.2.1 Nguồn điện 48 3.2.2 Mạch công suất điều khiển động 48 3.2.3 Cảm biến vị trí encoder 49 3.2.4 Bộ xử lý trung tâm - vi điều khiển Atmega 50 3.2.5 Mạch điều khiển trung tâm 51 3.3 Giải thuật - Lƣu đồ chƣơng trình 52 3.2.1 Chƣơng trình 53 3.2.2 Đọc tốc độ động 53 3.2.3 Chống nhiễu 54 3.4 Thử nghiệm 55 3.5 Kết 58 Chƣơng - KẾT LUẬN VÀ KHUYẾN NGHỊ 61 4.1 Những đóng góp tính đề tài 61 4.2 Hạn chế đề tài 61 4.3 Đề xuất hƣớng nghiên cứu tƣơng lai 62 DANH MỤC TÀI LIỆU THAM KHẢO 63 PHẦN THỦ TỤC 64 v DANH MỤC BẢNG Bảng Tác động việc tăng thông số độc lập 27 Bảng Phương pháp Ziegler–Nichols 27 Bảng Thông số thực nghiệm PID .60 vi DANH MỤC HÌNH VẼ Hình 1.1 Mô tả nguyên lý giữ thăng Hình 1.2 Segway HT kiểu i170 .6 Hình 1.3 Xe bánh tự cân Trevor Blackwell Hình 1.4 JOE Hình 1.5 nBot Hình 1.6 nBot Hình 1.7 Balibot 10 Hình 1.10 Robot Bender 10 Hình 1.8 Bbot 10 Hình 1.9 Equibot 11 Hình 1.11 ArduRoller balance bot 11 Hình 1.12 Loại robot, kiểu Rolling TOYOTA 11 Hình 1.13 Xe bánh tự cân (ĐHLH) 12 Hình 1.14 Xe bánh tự cân (ĐH TP.HCM) 12 Hình 2.1 Các trường hợp di chuyển robot hai bánh tự cân 15 Hình 2.2 Mô hình phân tích lực lắc ngược 16 Hình 2.3 Mô hình sơ đồ khối ngõ vào lắc ngược 16 Hình 2.4 Mô hình phân tích robot bánh tự cân 17 Hình 2.5 Sơ đồ khối điều khiển PID 25 Hình 2.6 Sơ đồ lọc tín hiệu từ cảm biến 29 Hình 2.7 Sơ đồ tín hiệu từ cảm biến lọc bổ phụ thông tần 30 Hình 2.8 Sơ đồ tín hiệu từ cảm biến lọc Kalman (Kalman Filter) 31 Hình 2.9 Từ hình 32 Hình 2.10 Từ liệu thu 32 Hình 3.1 Mô hình động 33 vii Hình 3.2 Bản vẽ cấu tạo khung mô hình .33 Hình 3.3 Sơ đồ khối Robot 34 Hình 3.4 Cảm biến MPU6050 34 Hình 3.5 Sơ đồ khối cấu tạo cảm biến MPU6050 35 Hình 3.6 Sơ đồ kết nối cảm biến MPU6050 37 Hình 3.7 Giản đồ xung giao tiếp I2C với MPU6050 37 Hình 3.8 Cách viết liệu lên MPU6050 38 Hình 3.9 Cách đọc liệu từ MPU6050 39 Hình 3.10 Thanh ghi có địa 1B .39 Hình 3.11 Các chế độ ghi 1B 39 Hình 3.12 Thanh ghi có địa 1C 39 Hình 3.13 Các chế độ ghi 1C .40 Hình 3.14 Thanh ghi có địa 24 .40 Hình 3.15 Các chế độ ghi 24 40 Hình 3.16 Thanh ghi có địa từ 59 đến 64 40 Hình 3.17 Thanh ghi có địa từ 67 đến 72 41 Hình 3.18 Bánh xe 41 Hình 3.19 Ba pha động điện chiều .42 Hình 3.20 Động DC giảm tốc Faulhaber 12V 44 Hình 3.21 Cách nối dây đọc tín hiệu từ Encoder 44 Hình 3.22 Đồ thị điều chế PWM 45 Hình 3.23 Tạo xung vuông phương pháp so sánh .47 Hình 3.24 Sơ đồ mạch nguồn 48 Hình 3.25 Sơ đồ mạch công suất 49 Hình 3.26 Encoder tương đối 49 Hình 3.27 Sơ đồ chân ATMEGA 51 viii Với tín hiệu có, ánh sáng chiếu qua, ngƣời ta ghi nhận đƣợc đèn led có chiếu qua lỗ hay không.Số xung đếm đƣợc tăng lên tính số lần ánh sáng bị cắt Nhƣ encoder tạo tín hiệu xung vuông tín hiệu xung vuông đƣợc cắt từ ánh sáng xuyên qua lỗ Nên tần số xung đầu phụ thuộc vào tốc độ quay tròn Đối với encoder dùng có tín hiệu lệch pha 90 Hai tín hiệu xác định đƣợc chiều quay động Đĩa cảm biến đƣợc tích hợp sẵn động với số 12, nói cách khác vòng quay, encoder trả 12 xung Bánh xe nhôm bọc cao su, có đƣờng kính 95 mm.Tính toán khoảng cách di chuyển theo số xung đếm đƣợc encoder Chu vi bánh xe: P = 2πd (cm) Số xung mâm encoder: 12 Tỉ số hộp giảm tốc: Góc mã xung: α = = (rad) Quãng đƣờng đƣợc xung: S = αR = 20 =1 (cm) Nếu cần di chuyển quãng đƣờng S số xung di chuyển cần phải có: n = (xung) 3.2.4 Bộ xử lý trung tâm - vi điều khiển Atmega - Tốc độ tối đa: 16MHz - Dung lƣợng nhớ chƣơng trình: KB - Bộ nhớ EEPROM: 512 Byte - Dung lƣợng nhớ RAM: KB Bộ nhớ chƣơng trình có khả ghi 10.000 lần, nhớ EEPROM ghi 100.000 lần Hỗ trợ bootloader, có khả tự ghi vào nhớ chƣơng trình, cập nhật chƣơng trình cho chip mà không cần mạch nạp (Hình 3.27) - Timer bit: - Timer 16 bit: - ADC: kênh, 10 bit - Giao tiếp: TWI (I2C), UART, SPI 50 - Điện áp hoạt động: Atmega8L: 2.7V – 5.5V Atmega8: 4.5V – 5.5V Hình 3.27 Sơ đồ chân ATMEGA Để chip hoạt động cần đƣợc FUSE Khi xuất xƣởng mặc định chip đƣợc FUSE sử dụng dao động nội với tần số 1MHz Nguồn RESET: Atmega8 có cách RESET - Reset cấp nguồn - Reset (thông qua chân RESET) - Watchdog RESET - Reset nguồn bị sụt áp 3.2.5 Mạch điều khiển trung tâm Bộ điều khiển sử dụng chân In5 In6 để kết nối với cảm biến góc, chân cho điều khiển hai động cơ, chân cho hai encoder, bên cạnh mạch tích hợp LED còi nhỏ để báo hiệu trạng thái hoạt động hệ thống.(Hình 3.28) 51 Các tín hiệu từ cảm biến góc đƣợc đọc lọc nhiễu tính toán trƣớc đƣa vào xử lý thuật toán Hai encoder trả xung điều khiển để từ tính toán vận tốc góc bánh xe vị trí bánh xe Chƣơng trình đƣợc nạp vào ATmega8, dựa vào thông số đầu vào nhƣ cảm biến góc, encoder điều khiển tính toán dựa thuật toán đƣợc mã hóa để đƣa tín hiệu PWM điều khiển động +5V R3 1K D1 RED Reset C3 4.7u +5V SU1 BUZZER R24 1K Buzz C1815 Q5 RxD TxD A12 A11 B11 B12 In4 In5 In6 Buzz 10 11 12 13 PC6/RESET PD0/RXD PD1/TXD PD2/INT0 PD3/INT1 PD4/XCK/T0 TOSC1/XTAL1/PB6 XTAL2/TOSC2/PB7 PD5/T1 PD6/AIN0 PD7/AIN1 22 R21 10k IC1 Reset AVCC VCC R1 10K 20 LED GND AGND +5V ADC5/SCL/PC5 ADC4/SDA/PC4 ADC3/PC3 ADC2/PC2 ADC1/PC1 ADC0/PC0 AREF SCK/PB5 MISO/PB4 MOSI/OC2/PB3 SS/OC1B/PB2 OC1A/PB1 ICP1/PB0 28 27 26 25 24 23 21 19 18 17 16 15 14 SCK LED MISO MOSI Motor1 Motor2 Dir1 ATmega8-DIL28small Hình 3.28 Sơ đồ mạch điều khiển trung tâm 3.3 Giải thuật - Lƣu đồ chƣơng trình Chƣơng trình điều khiển xe hai bánh tự cân đuợc chia làm phần nhƣ sau: chƣơng trình chính, tính tốc độ động cơ, tính góc vận tốc góc, thuật toán điều khiển động Việc thu nhận giá trị cảm biến đƣợc thực ngắt để đảm bảo không bị tín hiệu phản hồi từ encoder 52 3.2.1 Chƣơng trình Begin Begin Khởi động chế độ hoạt động biến Sai Tính toán vận tốc bánh xe Tính toán góc, vận tốc góc t=10ms? Đúng t=0 Thuật toán điều khiển PID Giải thuật End Cập nhật lại tham số báo LED, còi Hình 3.29 Giải thuật chương trình Thuật toán đƣợc lặp vô hạn nhằm giữ robot liên tục đƣợc giữ cân 3.2.2 Đọc tốc độ động Begin No Ch.B=1? pulse++ ; Yes pulse ; End interrupt void XINT1_ISR(void) { if(B==1) pulse_speed++; else pulse_speed ; } Hình 3.30 Giải thuật đọc tốc độ động 53 3.2.3 Chống nhiễu Lấy giá trị trung bình, so sánh giá trị với giá trị trung bình lớn bé loại bỏ sau tính lại giá trị trung bình từ giá trị có Begin 𝑛 𝑐𝑜𝑢𝑛𝑡𝑒𝑟𝑇(𝑖) 𝑛 result=0; slp=0; i=0; 𝑎𝑣𝑟 No i