›
›

Đồ án Thiết kế và chế tạo xe hai bánh tự cân bằng (Phần 1)

pdf 74 trang phuongnguyen 3280
Download
Bạn đang xem 20 trang mẫu của tài liệu "Đồ án Thiết kế và chế tạo xe hai bánh tự cân bằng (Phần 1)", để tải tài liệu gốc về máy bạn click vào nút DOWNLOAD ở trên

Tài liệu đính kèm:

  • pdfdo_an_thiet_ke_va_che_tao_xe_hai_banh_tu_can_bang_phan_1.pdf

Nội dung text: Đồ án Thiết kế và chế tạo xe hai bánh tự cân bằng (Phần 1)

  1. BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HCM KHOA CƠ KHÍ CHẾ TẠO MÁY ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT CƠ ĐIỆN TỬ THIẾT KẾ VÀ CHẾ TẠO XE HAI BÁNH TỰ CÂN BẰNG GVHD: ThS. VÕ LÂM CHƯƠNG SVTH: HOÀNG MINH NHẬT MSSV: 11146076 SVTH: TRẦN ĐỨC TẤN MSSV: 11146102 S K L 0 0 3 8 8 0 Tp. Hồ Chí Minh, tháng 7/2015
  2. BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH BỘ MÔN CƠ ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP Đề tài: “THIẾT KẾ VÀ CHẾ TẠO XE HAI BÁNH TỰ CÂN BẰNG” Giảng viên hướng dẫn: ThS. VÕ LÂM CHƯƠNG Sinh viên thực hiện: HOÀNG MINH NHẬT MSSV: 11146076 TRẦN ĐỨC TẤN MSSV: 11146102 Lớp: 111461C Khoá: 2011 - 2015 Tp. Hồ Chí Minh, tháng 7 năm 2015
  3. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng TRƢỜNG ĐẠI HỌC SƢ PHẠM KỸ THUẬT TP. HCM CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA CƠ KHÍ CHẾ TẠO MÁY Độc lập - Tự do – Hạnh phúc Bộ môn Cơ Điện Tử NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Hoàng Minh Nhật MSSV: 11146076 Trần Đức Tấn 11146102 Lớp: 111461C Khoá: 2011 Ngành đào tạo: Kỹ thuật cơ điện tử Hệ: Chính quy 1. Tên đề tài: . . . . . . 2. Các số liệu, tài liệu ban đầu: . . . . . . . . 3. Nội dung chính của đồ án: . . . . . . . . . . 4. Ngày giao đồ án: 5. Ngày nộp đồ án: TRƢỞNG BỘ MÔN GIẢNG VIÊN HƢỚNG DẪN (Ký, ghi rõ họ tên) (Ký, ghi rõ họ tên)  Đƣợc phép bảo vệ (GVHD ký, ghi rõ họ tên) i
  4. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng LỜI CAM KẾT - Tên đề tài: Thiết kế và chế tạo xe hai bánh tự cân bằng - GVHD: Ths Võ Lâm Chƣơng - Hoàng Minh Nhật MSSV: 11146076 Trần Đức Tấn 11146102 - Lớp: 111461C - Địa chỉ sinh viên: - Số điện thoại liên lạc: - Email: - Ngày nộp khoá luận tốt nghiệp (ĐATN): - Lời cam kết: “Tôi xin cam đoan khoá luận tốt nghiệp (ĐATN) này là công trình do chính tôi nghiên cứu và thực hiện. Tôi không sao chép từ bất kỳ một bài viết nào đã được công bố mà không trích dẫn nguồn gốc. Nếu có bất kỳ một sự vi phạm nào, tôi xin chịu hoàn toàn trách nhiệm”. Tp. Hồ Chí Minh, ngày 15 tháng 07 Năm 2015 Ký tên Hoàng Minh Nhật Trần Đức Tấn ii
  5. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng LỜI CẢM ƠN Bài luận văn tốt nghiệp đánh dấu sự trƣởng thành của sinh viên sau 4 năm trên giảng đƣờng đại học. Đem những kiến thức áp dụng vào đời sống xã hội để xây dựng và phát triển đất nƣớc. Để thực hiện bài luận văn này ngoài sự nỗ lực cố gắng của chúng em, đó còn là sự trợ giúp to lớn của Ban Giám Hiệu Trƣờng Đại Học Sƣ Phạm Kỹ Thuật, thầy cô Khoa Cơ Khí Chế Tạo Máy, thầy cô Bộ môn Cơ Điện Tử. Chúng em muốn gửi lời cám ơn chân thành và sâu sắc nhất đến ngƣời thầy hƣớng dẫn thạc sỹ Võ Lâm Chƣơng. Thầy đã tận tình giúp đỡ dạy bảo, tạo mọi điều kiện thuân lợi, cho những lời khuyên bổ ích và động viên chúng em trong suốt quá trình thực hiện đồ án. Qua đây, tôi xin cám ơn tập thể Kỹ thuật Cơ điện tử khóa 11, đã không ngừng chia sẻ về kiến thức cùng giúp nhau tiến bộ mà còn động viên giúp đỡ nhau những lúc vui buồn trong cuộc sống Cuối cùng, chúng con xin gửi lời tri ân đến mọi ngƣời trong gia đình đặc biệt là công ơn sinh thành dƣỡng dục của ba mẹ. Ba mẹ không chỉ tạo mọi điều kiện giúp chúng con học hành đến nơi đến chốn dù kinh tế gia đình còn nhiều khó khăn và còn là nguồn động viên to lớn giúp chúng con vƣợt qua khó khăn. Xin chân thành cám ơn Sinh viên thực hiện iii
  6. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng TÓM TẮT ĐỒ ÁN XE HAI BÁNH TỰ CÂN BẰNG Đề tài “xe hai bánh tự cân bằng” đƣợc thực hiện nhằm mục đích áp dụng những kiến thức đã học để tạo ra đƣợc một phƣơng tiện di chuyển có kích thƣớc nhỏ gọn, tiết kiệm năng lƣợng, điều khiển dễ dàng phù hợp với mọi lứa tuổi và thân thiện với môi trƣờng. Đề tài đƣợc thực hiện từ việc thiết kế phần cơ khí dựa trên các định luật vật lý và sức bền vật liệu nhằm mục đích đáp ứng đƣợc các yêu cầu đặt ra nhƣ kích thƣớc nhỏ gọn, giảm đi lực ma sát và dễ điều khiển. Sau khi hoàn thành phần cơ khí thì sản phẩm đƣợc mô hình hóa và đƣợc mô phỏng trên simulink matlab để biết đƣợc các đặc tính và tìm phƣơng pháp điều khiển tốt nhất. Sau khi mô phỏng, nhóm đã quyết định sử dụng vi điều khiển DSPIC30F4011/4012 làm bộ xử lý trung tâm, sử dụng bộ lọc Kalman để lọc tín hiệu từ cảm biến đƣa về. Sau đó xuất tín hiệu điển khiển các bộ điều khiển động cơ để điều khiển vận tốc của các động cơ, từ đó giúp xe có thể cân bằng trên 2 bánh nằm ngang và chuyển động dựa trên độ nghiêng của thanh điều khiển. Sinh viên thực hiện ABSTRACT TWO WHEELS SELF BALANCING SCOOTER The project “two wheels self balancing scooter” is done with aim to apply the knowledge learned to create a moving vehicle with small size, energy saving, easy control fit for all ages and friendly with environment The project is done from design of mechanical based on the laws of physics and strength of materials with aim to response the specific requirements as compact size reduced friction and easy to handle. After completing mechanical components, the product is modeled and simulated in SIMULINK MATLAB to know the features and find the best method of control. After the simulation, the team decided to use the microcontroller dsPIC30F4011 / 4012 as the central processor, using the Kalman filter to filter the signal from the sensor. Then output control signal to motor control module which control speed of motor, vehicles can banlance on two wheels and moves based on the tilt of the control bar. iv
  7. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng MỤC LỤC Trang NHIỆM VỤ ĐỒ ÁN i LỜI CAM KẾT ii LỜI CÁM ƠN iii TÓM TẮT ĐỒ ÁN iv v vii viii x CHƢƠNG 1: GIỚI THIỆU 1 1.1 Tính cấp thiết của đề tài 1 1.2 Mục tiêu nghiên cứu của đề tài 1 1.3 Đối tƣợng và phạm vi nghiên cứu 1 1.3.1 Đối tƣợng nghiên cứu 1 1.3.2 Phạm vi nghiên cứu 1 1.4 Nhiệm vụ đề tài 1 1.5 Phƣơng pháp nghiên cứu 2 1.5.1 Cơ sở phƣơng pháp luận 2 1.5.2 Các phƣơng pháp nghiên cứu cụ thể 2 3 2 Tình hình nghiên cứu trong và ngoài nƣớc 3 2.1 Tình hình nghiên cứu trên thế giới 3 2.2 Tình hình nghiên cứu trong nƣớc 4 CHƢƠNG 3: CƠ SỞ LÝ THUYẾT 6 3.1 Thế nào là xe hai bánh tự cân bằng 6 3.1.1 Nguyên lý cân bằng 6 3.1.2 Nguyên lý di chuyển xe 7 3.1.3 Khả năng di chuyển của xe 7 3.2 Ƣu điểm và nhƣợc điểm của xe hai bánh tự cân bằng 9 3.2.1 Ƣu điểm 9 3.2.2 Nhƣợc điểm 9 3.3 Những tiềm năng của xe hai bánh tự cân bằng 9 3.4 Mô hình toán hệ thống 9 3.5 Lý thuyết bộ điều khiển PID 14 3.6 Lý thuyết bộ lọc Kalman 16 3.6.1 Giới thiệu chung 16 3.6.2 Lý thuyết bộ lọc Kalman 17 CHƢƠNG 4: PHƢƠNG HƢỚNG VÀ CÁC GIẢI PHÁP 20 4.1 Yêu cầu đề tài 20 4.2 Phƣơng hƣớng và giải pháp thực hiện 20 4.2.1 Phƣơng án bộ truyền động của xe 20 v
  8. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 4.2.2 Phƣơng án sử dụng động cơ của xe 22 4.2.3 Phƣơng án sử dụng thiết bị điều khiển 23 4.3 Lựa chọn giải pháp 23 CHƢƠNG 5: TÍNH TOÁN VÀ THIẾT KẾ 24 5.1 Thiết kế phần cơ khí 24 5.1.1 Tính toán công suất động cơ 24 5.1.2 Thiết kế cơ khí 25 5.1.3 Phân tích ứng suất, kiểm tra độ bền xe 29 5.2 Thiết kế phần điện 37 5.2.1 Sơ đồ khối hệ thống 37 5.2.2 Thiết kế mạch xử lý trung tâm 37 5.2.2.1 Giới thiệu vi điều khiển DSPIC 30F4011 37 5.2.2.2 Đặc điểm chung 38 5.2.3 Thiết kế driver điều khiển động cơ DC 40 5.2.3.1 Phần thiết kế 41 5.2.3.2 Các khối chính trong mạch 42 5.2.4 Cảm biến MPU6050 45 5.2.4.1 Giới thiệu 45 5.2.4.2 Sơ đồ chân và nguyên lý mạch cảm biến MPU6050 46 5.2.4.3 Cách thức đọc dữ liệu cảm biến MPU6050 47 5.2.4.4 Xử lý dữ liệu đọc từ cảm biến MPU-6050 48 5.2.4.5 Áp dụng bộ lọc Kalman cho cam biến MPU-6050 49 CHƢƠNG 6: THỰC NGHIỆM ĐÁNH GIÁ 52 6.1 Mô phỏng bằng Matlab và kết quả 52 6.1.1 Mô phỏng bằng Matlab 52 6.1.2 Kết quả mô phỏng 52 6.2 Độ chính xác cảm biến 54 6.3 Mô hình cơ khí thực tế 44 6.4 Giải thuật điều khiển xe 56 6.5 Lƣu đồ chƣơng trình điều khiển vận tốc động cơ DC 57 6.6 Lƣu đồ chƣơng trình chính cân bằng xe 58 6.7 Kết quả 59 - ĐỀ NGHỊ 60 61 vi
  9. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng DANH MỤC BẢNG BIỂU Trang Báng 3.1: Phƣơng pháp Ziegler-Nichols 16 Bảng 4.1: So sánh hai dòng vi điều khiển PIC 16F887 và DSPIC 30F4011 23 Bảng 5.1: Thông tin vật liệu part đỡ 29 Bảng 5.2: Thông tin ngoại lực tác dụng 30 Bảng 5.3: Kết quả phân tích part đỡ 31 Bảng 5.4: Kết quả phân tích của xe 34 vii
  10. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng DANH MỤC SƠ ĐỒ, HÌNH VẼ Trang Sơ đồ 5.1: Sơ đồ khối điện của xe 37 Sơ đồ 6.1: Lƣu đồ giải thuật điều khiển vận tốc động cơ DC 57 Sơ đồ 6.2: Lƣu đồ giải thuật mạch trung tâm 58 Hình 2.1: Xe Segway 3 Hình 2.2: Trevor Blackwell và scooter 4 Hình 2.3: Mô hình xe hai bánh tự cân bằng tác giả Nguyễn Gia Minh Thảo 5 Hình 2.4: Xe hai bánh tự cân bằng của Mai Tuấn Đạt 5 Hình 3.1: Nguyên lý cân bằng xe 6 Hình 3.2: Nguyên lý di chuyển xe hai bánh 7 Hình 3.3: Khả năng di chuyển xe ba hay bốn bánh trên mặt phẳng nghiêng 8 Hình 3.4: Xe hai bánh khi xuống và lên dốc 8 Hình 3.5: Phân tích lực bánh xe phải 10 Hình 3.6: Phân tích lực khung xe 12 Hình 3.7: Bộ điều khiển PID 15 Hình 3.8: Nguyên lý bộ lọc Kalman 17 Hình 4.1: Bộ truyền xích 20 Hình 4.2: Bộ truyền đai 21 Hình 4.3: Bộ truyền bánh răng 21 Hình 4.4: Động cơ AC 22 Hình 4.5: Động cơ DC 22 Hình 5.1: Phân tích lực bánh xe 24 Hình 5.2: Hộp giảm tốc 25 Hình 5.3: Bánh xe 26 Hình 5.4: Động cơ servo DC 200W 26 Hình 5.5: Sàn xe 27 Hình 5.6: Tấn đỡ 28 Hình 5.7: Hình 3D thiết kế xe 29 Hình 5.8: Part đỡ 30 Hình 5.9: Chọn mặt tác dụng lực để phân tích 31 viii
  11. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.10: Ứng suất khi tác dụng lực lên part đỡ 32 Hình 5.11: Độ dịch chuyển part đỡ 32 Hình 5.12: Hệ số an toàn 33 Hình 5.13: Hình ảnh tổng thể xe 33 Hình 5.14: Đặt lực tác dụng lên xe 34 Hình 5.15: Ứng suất khi tác dụng lực lên xe 35 Hình 5.16: Độ dịch chuyển của xe 35 Hình 5.17: Hệ số an toàn 36 Hình 5.18: Sơ đồ khối của xe 37 Hình 5.19: Mạch xử lý trung tâm 40 Hình 5.20: Mạch xử lý trung tâm thực tế 40 Hình 5.21: Mạch driver điều khiển động cơ DC 41 Hình 5.22: Khối nguồn driver 42 Hình 5.23: Khối điều khiển driver 42 Hình 5.24: Ngõ xuất ra PWM và CW vào cầu H 43 Hình 5.25: Ngõ nhận tín hiệu encoder 43 Hình 5.26: Khối cầu H 43 Hình 5.27: Khối nhận tín hiệu điều khiển 44 Hình 5.28: Khối hồi tiếp dòng 44 Hình 5.29: Mạch driver servo DC thực tế 45 Hình 5.30: Hình ảnh thực cảm biến MPU6050 45 Hình 5.31: Sơ đồ chân MPU-6050 46 Hình 5.32: Nguyên lý module GY-521 MPU-6050 47 Hình 5.33: Chuẩn giao tiếp I2C 47 Hình 5.34: Phƣơng thức truyền dữ liệu I2C 48 Hình 5.35: Tín hiệu đầu vào và ra bộ lọc Kalman 49 Hình 6.1: Mô phỏng hệ thống bằng MATLAB Simulink 52 Hình 6.2: Mô phỏng PID vào hệ thống 52 Hình 6.3: Đồ thị đáp ứng khi góc nghiêng ban đầu θ= 0.5 rad ≈ 30o 53 Hình 6.4: Đồ thị đáp ứng khi góc nghiêng ban đầu θ= 1 rad ≈ 60o 53 Hình 6.5: Đồ thị đáp ứng khi setpoint = 0.5rad ≈ 30o 54 Hình 6.6: So sánh kết quả thực nghiệm bộ lọc Kalman 54 Hình 6.7: Mô hình xe thực tê 56 Hình 6.8: Góc lệch so với vị trí cân bằng 59 Hình 6.9: Góc lệch so với vị trí cân bằng khi chịu lực tác động 59 ix
  12. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng DANH MỤC TỪ VIẾT TẮT ADC Analog To Digital Conversion CPU Central Processing Unit DC Direct Current DSC Digital Signal Controller DSP First In First Out EEPROM Electrically Erasable Programmable Read-Only Memory FIFO First In First Out I/O Input/Output I2C Inter Integrated Circuit MIPS Mega Instruction Per Second MCU Micro Controller Unit PID Proportional Integral Derivative PWM Pulse Width Modulation ROM Read Only Memory RAM Random Access Memory SPI Serial Peripheral Interface UART Universal Asynchronous Receiver/ Transmitter x
  13. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG I : GIỚI THIỆU CHUNG 1.1 Tính cấp thiết đề tài Theo tình hình thế giới hiện nay, việc di chuyển không còn là một vấn đề khó khăn. Có rất nhiều phƣơng tiện di chuyển khác nhau nhƣ: máy bay, tàu hỏa, ô tô, xe máy Nhƣng các phƣơng tiện di chuyển đó rất cồng kềnh nên không thể làm phƣơng tiện di chuyển trong các công ty nhà máy, không đƣợc sử dụng rộng rãi đối với mọi lứa tuổi và gây ô nhiễm môi trƣờng. Chính vì vậy, nhóm sinh viên đã quyết định thực hiện đề tài “xe hai bánh tự cân bằng” để góp phần tạo ra một phƣơng tiện di chuyển nhỏ gọn hữu ích trọng phạm vi hẹp, có thể sử dụng với mọi lứa tuổi, và ít gây ô nhiễm môi trƣờng. 1.2 Mục tiêu nghiên cứu của đề tài Tính toán và thiết kế đƣợc phần thân xe nhỏ gọn với chi phí thấp nhất mà vẫn đảm bảo độ cứng vững. Thiết kế đƣợc các mạch điện điều khiển chạy ổn định. Viết đƣợc chƣơng trình điều khiển để xe có thể di chuyển một cách ổn định. 1.3 Đối tƣợng và phạm vi nghiên cứu 1.3.1 Đối tƣợng nghiên cứu Tìm hiểu về phần mềm Matlab để có thể tạo mô hình toán sản phẩm, và từ đó có thể chọn đƣợc phƣơng pháp điều khiển thích hợp. Nghiên cứu về nguyên lý điều khiển động cơ servo DC để thiết kế mạch điều khiển và chƣơng trình điều khiển phù hợp. Tìm hiểu về vi điều khiển DSPIC30F4011/4012 để làm trung tâm điều khiển của xe. Tìm hiểu về cảm biến MPU6050 để làm cảm biến đo độ nghiêng của xe. Nghiên cứu bộ lọc Kalman để xử lý tín hiệu đo đƣợc từ cảm biến. 1.3.2 Phạm vi nghiên cứu Trọng tâm của đề tài là xây dựng giải thuật điều khiển xe dựa trên thuật toán PID. Xe đƣợc thiết kế chủ yếu di chuyển trên địa hình phẳng, không gồ ghề. Tải trọng thiết kế phù hợp với thể trạng ngƣời Việt Nam không quá 80kg 1.4 Nhiệm vụ đề tài Thiết kế đƣợc phần cơ khí của xe. Lập đƣợc mô hình toán của xe trên phần mềm Matlab 1
  14. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Sử dụng bộ điều khiển PID để điều khiển, và tìm thông số của bộ PID trên phần mềm Matlab. Thiết kế mạch điện điều khiển trung tâm, mạch điều khiển servo DC. Đọc và xử lý tín hiệu cảm biến sử dụng bộ lọc Kalman Xây dựng chƣơng trình và thuật toán điều khiển xe. 1.5 Phƣơng pháp nghiên cứu 1.5.1 Cơ sở phƣơng pháp luận Áp dụng lý thuyết vào đối tƣợng một cách cụ thể từ đó kiểm chứng, đánh giá những lý thuyết đã nghiên cứu Tổng hợp, phân tích những thông tin từ đó đƣa ra ƣu điểm, nhƣợc điểm và cách khắc phục nhƣợc điểm. 1.5.2 Các phƣơng pháp nghiên cứu cụ thể Tìm hiểu tài liệu trong và ngoài nƣớc liên quan đến đề tài. Xây dựng mô hình toán đối tƣợng Tiến hành thiết kế và gia công đáp ứng những yêu cầu đặt ra. Đƣa ra những thiếu xót và đƣa ra giải pháp khắc phục Nghiên cứu giải thuật và đƣa ra lƣu đồ từ đó viết chƣơng trình điều khiển đối tƣợng 2
  15. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG II : TỔNG QUAN NGHIÊN CỨU ĐỀ TÀI 2.1 Tình hình nghiên cứu trong và ngoài nƣớc 2.1.1 Tình hình nghiên cứu trên thế giới 2.1.1.1 Xe Segway Xe hai bánh tự cân bằng Segway đƣợc sản xuất bởi công ty Segway Inc, ở bang New Hampshire, Mỹ. Chiếc xe ra mắt lần đầu trƣớc công chúng vào ngày 03 tháng 12 năm 2001. Hiện nay loại xe này đƣợc bán rộng rãi trên toàn thế giới. Ở nƣớc phát triển xe Segway trở nên thông dụng và gần gũi với mọi ngƣời. Xe đƣợc phát triển từ dự án xe lăn tự leo cầu thang iBOT của Dean Kamen. Đặc điểm nổi bật của Segway là khả năng cân bằng nhờ hệ thống máy tính, động cơ và con quay hồi chuyển đặt bên trong xe, nó giúp cho xe dù chỉ có một trục chuyển động với hai bánh nhƣng luôn ở trạng thái cân bằng. Tất cả thông tin về trạng thái nghiêng truyền đến hai bảng mạch điều khiển bao gồm một bó vi xử lý. Segway có tổng cộng 10 bảng mạch vi xử lý với năng lực gấp 3 lần PC điển hình Hình 2.1 Xe Segway Thông số kỹ thuật: Tốc độ cao nhất : 12,5 dặm/giờ (20km/giờ), gấp 4 lần tốc độ đi bộ bình thƣờng. Trọng lƣợng không tải : 80 pound (36kg). Kích thƣớc : 19 x 25 inch (48 x 63,5 cm). Segway có chiều rộng đủ cho một ngƣời trƣởng thành. Tải trọng : ngƣời điều khiển nặng 250 pound (110kg) và hàng hóa nặng 75 pound (34kg). Phạm vi : di chuyển khoảng 17 dặm (28km) với mỗi lần sạc 3
  16. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Giao diện hiển thị chế độ hoạt động : Segway có màn hình LCD cung cấp thông tin về năng lƣợng, thông báo chế độ hoạt động và tình trạng lỗ nếu xe gặp sự cố. Giá thành : 6000 USD 2.1.1.2 Balancing Scooter Xe scooter đƣợc Trevor Blackwell chế tạo ra dựa theo Segway của Mỹ. Xe scooter tự cân bằng này đƣợc chế tạo từ động cơ xe lăn và pin dùng cho mô hình. Những bộ phận và module của xe có giá thành rẻ hơn một nửa so với xe Segway. Xe không cần phần mềm thực thi cao cấp hay phức tạp . Phiên bản đầu tiên đƣợc viết bằng Python Hình 2.2 Trevor Blackwell và scoot 2.1.2 Tình hình nghiên cứu trong nƣớc Tại Việt Nam, có nhiều tác giả thực hiện đề tài này: Luận văn thạc sỹ robot hai bánh tự cân bằng của tác giả Nguyễn Gia Minh Thảo, trƣờng Đại Học Bách Khoa Tp HCM. Đề tài này đã thực hiện thành công việc mô phỏng và thực nghiệm robot hai bánh tự cân bằng với bộ điều khiển Backstepping đáp ứng nhanh, khả năng giữ thăng bằng tốt 4
  17. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 2.3 Mô hình xe hai bánh tự cân bằng tác giả Nguyễn Gia Minh Thảo Năm 2005, sinh viên Mai Tuấn Đạt thiết kế thành công xe hai bánh tự cân bằng di chuyển trên mặt phẳng Hình 2.4 Xe hai bánh tự cân bằng của Mai Tuấn Đạt 5
  18. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG III : CƠ SỞ LÝ THUYẾT 3.1 Thế nào là xe hai bánh tự cân bằng 3.1.1 Nguyên lý cân bằng Đối với xe ba bánh hay bốn bánh, viêc giữ thăng bằng và ổn định của chúng là nhờ vào trọng tâm của chúng nằm trong bề mặt chân đế do các bánh xe tạo ra. Đối với xe hai bánh có cấu trúc nhƣ xe đạp, việc thăng bằng khi không di chuyển là hoàn toàn không thể, vì việc thăng bằng của xe đạp dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi đang quay. Còn đối với xe hai bánh tự cân bằng là loại xe hai bánh có trục hai bánh xe trùng nhau, để tự cân bằng thì trọng tâm của xe (bao gồm cả ngƣời lái) cần đƣợc giữ ngay giữa các bánh xe. Điều này giống nhƣ ta giữ một cây gậy thẳng đứng cân bằng trong lòng bàn tay. Thực ra, việc xác định trọng tâm của xe hai bánh tự cân bằng là rất khó khăn, có khi không xác định đƣợc, vì trọng tâm xe phụ thuộc nhiều yếu tố đƣợc đặt lên xe. Vì vậy việc giữ cho trọng tâm của xe luôn thăng bằng là rất khó khăn và không có động cơ nào có đủ khả năng quay với tốc độ cao để giữ thăng bằng cho trọng tâm xe khi chúng có độ lệch quá lớn. Về mặt kỹ thuật, góc giữa chiều trọng lực và sàn xe hai bánh tự cân bằng là có thể xác định đƣợc, thay vì cách xác định trọng tâm của xe thì thay vào đó có thể giữ cho tay lái thẳng đứng và luôn vuông góc với sàn xe cũng có thể giữ xe thăng bằng. Cân bằng Bị nghiêng Hình 3.1 Nguyên lý cân bằng của xe 6
  19. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 3.1.2 Nguyên lý di chuyển của xe Xe hai bánh tự cân bằng hoạt động theo nguyên lý sau: Khi thanh điều khiển đƣợc đẩy nghiêng về phía trƣớc trọng tâm xe sẽ bị lệch khỏi vị trí cân bằng, lúc này bánh xe yêu cầu phải chạy về phía trƣớc để cân bằng lại trọng tâm của xe nhằm giữ xe thăng bằng. Khi thanh điều khiển nghiêng về phía sau thì trọng tâm xe bị lệch về phía sau, lúc này bánh xe phải quay về phía sau để giữ thăng bằng lại trọng tâm của xe. Khi muốn dừng xe hoặc di chuyển chậm lại thì chỉ cần di chuyển thanh điều khiển ngƣợc hƣớng di chuyển. Qua đó có thể thấy rằng việc di chuyển xe hai bánh tự cân bằng rất dễ dàng chỉ phụ thuộc vào hƣớng di chuyển của thanh điều khiển 2 2 1 1 3 3 Đi lùi Đi tới 1: hƣớng di chuyển trọng tâm 2:hƣớng đẩy cần xe 3 hƣớng quay bánh xe Hình 3.2 Nguyên lý di chuyển của xe hai bánh xe 3.1.3 Khả năng di chuyển của của xe hai bánh tự cân bằng Đối với xe ba bánh hoặc bốn bánh thì việc giữ thăng bằng thì đơn giản dễ thực hiện hơn vì trọng lƣợng đƣợc phân bố đều trên các bánh lái và bánh đuôi. Tuy nhiên, nếu trọng lƣợng đƣợc dồn về bánh lái thì xe dễ bị ngã, còn nếu bị dồn về bánh đuôi hay những bộ phận đỡ trọng lƣợng của xe thì bánh lái sẽ dễ bị trƣợt, khả năng bám kém. Nhiều thiết kế xe có thể di chuyển tốt trên địa hình bằng phẳng, nhƣng không thể di chuyển trên các địa hình gồ ghề lên xuống (mặt phẳng nghiêng). Khi xe di chuyển lên mặt phẳng nghiêng thì trọng lƣợng bị dồn về phía đuôi xe làm cho bánh lái mất khả năng bám gây trƣợt và khó điều khiển. Khi xe di chuyển xuống mặt phẳng nghiêng thì trọng tâm bị dồn về bánh lái thì xe sẽ mất ổn định dễ bị ngã. 7
  20. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Xe đi xuống mặt phẳng nghiêng xe đi lên mặt phảng nghiêng Hình 3.3 Khả năng di chuyển xe ba(hay bốn) bánh trên mặt phảng nghiêng Đối với xe hai bánh đồng trục với nhau, tuy là bản thân nó là một hệ thống không ổn định do đó việc di chuyển trên địa hình bằng phẳng thì có kém hơn so với xe ba bánh hay bốn bánh. Tuy nhiên, nếu khi đi lên dốc hoặc xuống dốc thì trọng tâm của xe luôn luôn nằm trong khu vực của bánh xe (bộ phận đỡ toàn bộ khối lƣợng của xe) nên xe sẽ không bị trƣợt hay lật xe. Khi di chuyển lên dốc thì xe bị nghiêng về trƣớc trọng tâm đƣợc dồn về bánh xe do đó xe không bị lật hay mất thăng bằng. Khi xe xuống dốc thì xe bị nghiêng về phía sau, trọng lƣợng của xe vẫn dồn về hai bánh xe do đó xe không bị trƣợt Xe xuống dốc Xe lên dốc Hình 3.4. Xe hai bánh khi xuống và lên dốc 8
  21. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 3.2 Ƣu điểm và nhƣợc điểm của xe hai bánh tự cân bằng 3.2.1 Ƣu điểm Sử dụng năng lƣợng điện do đó không gây ô nhiễm môi trƣờng, có thể sạc điện để dùng Kích thƣớc gọn gàng do đó không gian hoạt động sẽ hiệu quả hơn, có thể di chuyển trong các không gian chật hẹp chỉ vừa đủ kích thƣớc của xe. Sử dụng tốt ở trong văn phòng Khả năng di chuyển linh hoạt ở các địa hình khác nhau. Có khả năng leo các dốc nhỏ, các bậc thang không quá cao Giá thành sản xuất không quá cao Cuốn hút ngƣời sử dụng cũng nhƣ mọi ngƣời xung quanh vì hình dạng kỳ lạ của nó, phá vỡ các hình ảnh thƣờng thấy về các phƣơng tiện giao thông con ngƣời 3.2.2 Nhƣợc điểm Việc điều khiển xe yêu cầu sự tập trung cao, không thể thƣ giãn một cách thoải mái khi đi xe hai bánh tự cân bằng. Không thể chở hai ngƣời cùng một lúc trên xe. Vận tốc di chuyển còn hạn chế, không thể đi trên một đoạn đƣờng dài vì không đủ năng lƣợng và ngƣời điều khiển khá mệt mỏi. 3.3 Những tiềm năng của xe hai bánh tự cân bằng Có thể dùng xe hai bánh tự cân bằng làm một phƣơng tiện di chuyển linh hoạt ở trong các văn phòng và trong các công ty hay xí nghiệp vì kích thƣớc nhỏ gọn và chỉ cần sạc điện. Có thể lập trình cho xe tự chạy tới các địa diểm mong muốn, từ đó có thể vận chuyển đƣợc các dụng cụ hàng hóa qua những nơi chật hẹp một cách dễ dàng. Có thể kết hợp với nhiều loại robot khác để tạo ra các robot có khả năng di chuyển linh hoạt. 3.4 Mô hình toán hệ thống Một số ký hiệu sử dụng x : Độ dịch chuyển θ : Góc nghiêng r : Bán kính bánh xe l : Khoảng cách từ trọng tâm thân xe đến trọng tâm bánh xe CCLR, : Momen quay từ động cơ truyền vào bánh xe 9
  22. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng HHPPLRLR,,, : Lực tƣơng tác giữa thân xe và hai bánh xe trái phải HH, : Lực ma sát giữa bánh xe trái, phải fL fR Va : Điện áp km :Hằng số momen quay động cơ ke : Hằng số sức phản điện động R : Điện trở danh định w P R C R H R M W H fR xxx,, Hình 3.5: Phân tích lực bánh xe phải Sử dụng định luật 2 Newton, tổng các lực theo phƣơng ngang x là Fx Ma (3.1) MW x H fR H R Tổng các lực quanh trọng tâm bánh xe: IW C R H fR r (3.2) Từ động học động cơ một chiều, momen quay của động cơ có thể đƣợc mô tả: d I (3.3) m Rdt a Thay thế các thông số từ phần vi phân động cơ, momen quay đầu ra động cơ d k k k CIVm e m (3.4) Rdt R w R a 10
  23. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Phƣơng trình (3.2) trở thành k k k Im e m V H r (3.5) W wRR w a fR Suy ra : k k k I HVm e m W (3.6) fRRr w Rr a r W Phƣơng trình (3.4) thay vào (3.1) để có phƣơng trình cho các bánh xe trái và phải Bánh xe bên trái k k k I M xm e m V W H (3.7) WRr w Rr a r W L Bánh xe bên phải k k k I M xm e m V W H (3.8) WRr w Rr a r W R Ta có: x rx WWr x rx WWr Phƣơng trình (3.7),(3.8) trở thành: Bánh xe bên trái k k k I M xm e x m V W x H (3.9) WRr22 Rr a r L Bánh xe bên phải k k k I M xm e x m V W x H (3.10) WRr22 Rr a r R Cộng (3.9) và (3.10) I22 k k k 2 MW x m e x m V H H (3.11) Wr22 Rr Rr a R L 11
  24. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Cấu hình xe mô hình nhƣ con lắc ngƣợc P Fxp CCLR x l Mg P l 2 HHLR PPLR Hình 3.6 Phân tích lực khung xe Tổng các lực theo phƣơng ngang FxP M x 2 HRLPPPPPP H M lcos M l sin M x (3.12) Suy ra 2 HRLPPPPPPP H M x M lcos M l sin (3.13) Thay (3.13) vào (3.11) I22 k k k 2MW x m e x m V M x M l cos M l 2 sin Wr22 Rr Rr a P P P P P P P (3.14) Tổng các lực vuông góc với con lắc Fxp M p xcos p 12
  25. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng HRLPRLPPPPPPP Hcos P P sin M g sin M l M x cos (3.15) Tổng momen quanh trọng tâm con lắc MIO HRLPRLPLRPP H lcos P P l sin C C I (3.16) Từ (3.4) tổng momen 2 bánh xe 22k k k C Cm e x m V R LRr R a Thay vào (3.16) 22k k k H H lcos P P l sin m e x m V I R L P R L PRr R a P P Suy ra: 22k k k H H lcos P P l sin Im e x m V (3.17) R L P R L P P PRr R a Nhân (3.15) với 2 HHlRLPRLPPPPPPcos PPl sin Mlg sin Ml Mlx cos (3.18) Thay (17) vào (18) 22k k k Im e x m V M lgsin M l2 M lx cos P PRr R a P P P p P P Suy ra 22k k k (I M l2 m e x m V M l gsin M lx cos P P PRr R a P P P P (3.19) Từ (3.14) và (3.19) ta có hệ phƣơng trình của hệ thống 22k I k k mV2 M W M x m e x M l cos M l 2 sin Rra W r22 P Rr p p p p p p 13
  26. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 22k k k I M l2 m e x m V M lgsin M lx cos p p pRr R a p p p p Hai phƣơng trình tuyến tính hóa bằng giả thiết p , với là góc nhỏ từ hƣớng đi lên thẳng đứng cosp 1;sin Hệ phương trình hệ thống: 22k k k (I M l2 )m e x m V M l g M lx p p pRr R a p p 22k I k k mV2 M W M x m e x M l Rra W r22 p Rr p Suy ra: M l22k k k M gl ppxm e x m V IMl2 RrIMl 2 RIMl 2a IMl 2 p p p p p p p p (3.20) 22k k k Ml xm V m e x p 2IIIa 2 2 Rr2 MWWW M Rr2 2 M M 2 M M Wr2 p W r 2 p W r 2 p (3.21) 3.5 Lý thuyết bộ điều khiển PID PID (Proportional Integral Derivative) là một cơ chế phản hồi vòng điều khiển tổng quát đƣợc sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp. Bộ điều khiển PID tính toán sai số là hiệu số giá trị đo và giá trị đặt mong muốn. Bộ điều khiển sẽ thực hiện giảm tối đa sai số bằng cách điều chỉnh giá trị điều khiển đầu vào. Giải thuật tính toán PID gồm 3 thông số riêng biệt là khâu tỉ lệ (P) , khâu tích phân (I) và khâu vi phân (D) 14
  27. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 3.7: Bộ điều khiển PID Thuật toán PID đƣợc biểu diễn theo biểu thức sau: dP Drive K* error K error K PID dt Trong đó: - error là sự sai lệch giữa giá trị đo hiện tại của hệ thống và giá trị cài đặt mong muốn. - error là tổng các sai số trƣớc đó. dP - là khoảng thời gian thay đổi biến điều khiển. dt - K P là thành phần tỉ lệ có tác dụng làm tăng tốc độ đáp ứng của hệ thống và làm giảm, chứ không triệt tiêu sai số xác lập của hệ. - KI là thành phần tích phân có tác dụng triệt tiêu sai số xác lập nhƣng có thể làm giảm tốc độ đáp ứng của hệ. - KD là thành phần vi phân làm tăng độ ổn định hệ thống, giảm độ vọt lố và cải thiện tốc độ đáp ứng của hệ. Tìm hệ số PID bằng phƣơng pháp thực nghiệm - Cho các hệ số , , bằng không - Tăng từ nhỏ đến lớn cho đến khi ngõ ra đáp ứng với sự rung động là dao động ổn định 15
  28. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Giảm K P xuống một nửa giá trị phản ứng ở trên - Tăng KI từ nhỏ đến lớn, quan sát đáp ứng của hệ thống khi thông số thay đổi. Tăng đến khi dao động ở mức cho phép - Cuối cùng tăng KD cho đến khi hệ thống tối ƣu Tìm hệ số PID bằng phƣơng pháp Ziegler-Nichols - Cho các hệ số , , bằng không - Tăng từ nhỏ đến lớn cho đến đạt giá trị tới hạn Kcrit để hệ rơi vào chế độ ổn định, đầu ra vòng điều khiển bắt đầu dao động - Xác định chu kỳ tới hạn Tcrit của dao động Bộ điều khiển Ti Td P 0.5 - - PI 0.45 0.83 - PID 0.6 0.5 0.125 Bảng 3.1: Phương pháp Ziegler-Nichols - Sau đó xác định , : K p Ki ; KKTd p d Ti 3.6 Lý thuyết bộ lọc Kalman 3.6.1 Giới thiệu chung Lý thuyết bộ lọc Kalman đƣợc Rudolf E. Kalman công bố năm 1960 thông qua một bài báo có tên đầy đủ “ A New Approach to Linear Filtering and Prediction Problems”. Đây là một đề tài nghiên cứu quan trọng nhằm giải quyết các bài toán lọc các thông tin rời rạc tuyến tính. Cùng với sự phát triển khoa học kỹ thuật, bộ lọc Kalman đã trở thành chủ đề nghiên cứu sôi nổi và đƣợc ứng dụng nhiều trong ngành kỹ thuật, công nghệ khác. Với công trình nghiên cứu trên, nhà toán học Mỹ gốc Hungary Kalman đƣợc tổng thống Hoa Kỳ Obama trao tặng National Medal of Science năm 2009. 16
  29. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 3.6.2 Lý thuyết về bộ lọc Kalman Bộ lọc Kalman là thuật toán sử dụng chuỗi các giá trị đo lƣờng. Những giá trị này chứa nhiễu gây ra sai số trong việc đo đạc. Bộ lọc Kalman sẽ ƣớc lƣợng trạng thái hệ thống dựa trên trạng thái hiện tại và trạng thái trƣớc đó của hệ thống nhằm tăng độ chính xác hơn so với việc đo độc lập. Phƣơng trình bộ lọc Kalman đƣợc chia thành 2 nhóm: phƣơng trình cập nhật thời gian và phƣơng trình cập nhật đo. Phƣơng trình cập nhật thời gian chịu trách nhiệm dự đoán trƣớc (theo thời gian) sử dụng giá trị hiện tại và hiệp biến số ƣớc lƣợng để dự đoán ƣớc lƣợng tiền nhiệm cho thời điểm kế tiếp. Phƣơng trình cập nhật giá trị đo chịu trách nhiệm đối với sự phản hồi, nghĩa là kết hợp giá trị mới với ƣớc lƣợng tiền nhiệm nhằm hiệu chỉnh ƣớc lƣợng hậu nghiệm Cập nhật thời gian Cập nhật phép đo (Dự đoán) (Hiệu chỉnh) Hình 3.8 Nguyên lý bộ lọc Kalman Ta xem trạng thái của hệ thống đã đƣợc mô hình hóa một cách rời rạc theo thời gian bằng một phƣơng trình ngẫu nhiên tuyến tính. Trạng thái của hệ thống tại thời điểm k đƣợc phát triển từ trạng thái của hệ thống tại thời điểm (k-1) theo cách tính sau: xk F k x k1 B k u k w k Trong đó: - Fk là ma trận chuyển trạng thái, nó đƣợc áp dụng cho trạng thái trƣớc đó xk 1 - Bk là ma trận kiểm soát đầu vào, nó đƣợc áp dụng cho vectơ điều khiển uk - wk là vectơ biến ngẫu nhiên đại diện cho nhiễu hệ thống và đƣợc giả định là tuân theo phân bố Gauss với trung bình là 0 và ma trận hiệp biến ( covariance) là Q , hay ngắn gọn wkk N(0, Q ) . 17
  30. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Ngoài ra, tại một thời điểm k bằng một phƣơng pháp nào đó ta có thể đo đạc, quan sát đƣợc chính xác trạng thái của hệ thống và nó có thể đƣợc biểu diễn bởi một phƣơng trình tuyến tính sau: zk H k x k v k Với - H là ma trận quan sát, nó chuyển không gian trạng thái đo đạc vào không gian trạng thái mà ta quan sát. Ma trận này sẽ lọc lại các giá trị đo đạc mà ta quan tâm trong chuỗi các gía trị đo đƣợc. - vk là vectơ biến ngẫu nhiên đại diện cho nhiễu khi đo đạc hay quan sát và đƣợc giả định tuân theo phân bố Gauss với trung bính là 0 và ma trận hiệp biến R , hay ngắn gọn vkk N(0, R ) Chú ý các điểm sau: - Trạng thái ban đầu của hệ thống và các vectơ nhiễu x0, w 1 , , wkk , v 1 , , v đƣợc xem là độc lập. - Ma trận QRkk, là các ma trận đối xứng nửa xác định dƣơng - Các ma trận Q,R,F,H có thể thay đổi theo thời gian ( từng bƣớc k), nhƣng ở đây chúng đƣợc giả sử không đổi để thuận tiện hơn trong quá trình tính toán. Bộ lọc Kalman đƣợc thực hiện theo các bƣớc sau: Quá trình dự đoán Trạng thái ƣớc đoán: xˆˆk| k 1 F k x k 1 B k u k (3.22) T Dự đoán, ƣớc tính hiệp phƣơng sai: PFPFQk| k 1 k k 1 k k (3.23) Quá trình điều chỉnh Độ lệch đo lƣờng: yk z k H k xˆ k|1 k (3.24) T Độ lệch phƣơng sai: SHPHRk k k|1 k k k (3.25) T 1 Tính độ lợi Kalman: KPHSk k|1 k k k (3.26) Cập nhật trạng thái ƣớc đoán đã điều chỉnh: xˆˆk x k|1 k K k y k (3.27) Cập nhật hiệp phƣơng sai ƣớc lƣợng đã điều chỉnh: PIKHPk() k k k|1 k (3.28) Trong hai quá trình trên: - xˆkk|1là ký hiệu ƣớc lƣợng dự đoán của trạng thái xk dựa trên quan sát ở thời điểm k-1. Nếu hiểu một cách đơn giản hơn thì đây là bƣớc dự đoán - xˆk là ƣớc lƣợng trạng thái xk dựa trên quan sát ở thời điểm k - Hiệp phƣơng sai ƣớc lƣợng dự đoán Pkk|1 là trung bình bình phƣơng sai số trong ƣớc lƣợng 18
  31. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Hiệp phƣơng sai ƣớc lƣợng dự đoán Pk là trung bình bình phƣơng sai số trong ƣớc lƣợng xˆk Nhìn vào hai quá trình thực hiện bộ lọc Kalman, ta có thể thấy mục đích chính là tìm đi tìm hệ số K ( độ lợi bộ lọc Kalman). Hệ số này sẽ thay đổi sau mỗi vòng lặp tính toán sao cho thỏa mãn phƣơng trình (3.27). Nghĩa là làm sao đƣa ra ƣớc lƣợng trạng thái hệ thống tại thời điểm k là sát với trạng thái thực của hệ thống nhất. 19
  32. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG IV : PHƢƠNG HƢỚNG VÀ CÁC GIẢI PHÁP 4.1 Yêu cầu của đề tài Thiết kế và chế tạo xe hai bánh tự cân bằng có thể chịu tải trọng cho một ngƣời trƣởng thành (thể trạng ngƣời Việt Nam 80kg). Vận tốc tối đa di chuyển trên mặt phẳng 20km/h. Thiết kế nhỏ gọn dễ di chuyển qua không gian hẹp. 4.2 Phƣơng hƣớng và giải pháp thực hiện 4.2.1 Phƣơng án bộ truyền động của xe Bộ truyền xích Hình 4.1 Bộ truyền xích Ƣu điểm: - Không có hiện tƣợng trƣợt, có thể làm việc khi quá tải - Lực tác động lên trục và ổ trục nhỏ - Hiệu suất cao Nhƣợc điểm: - Cần bôi trơn thƣờng xuyên - Điều chỉnh sức căng xích đều đặn - Có tiếng ồn khi làm việc do va đập - Tỷ số truyền không lớn <6 20
  33. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Bộ truyền đai Hình 4.2 Bộ truyền đai Ƣu điểm - Làm việc êm, không gây ồn - Đề phòng quá tải trên động cơ - Kết cấu, vận hành đơn giản Nhƣợc điểm - Kích thƣớc lớn so với bộ truyền khác - Tuổi thọ thấp - Tỷ số truyền có thể thay đổi do hiện tƣợng trƣợt trơn trừ đai răng Bộ truyền bánh răng ( hộp giảm tốc) Hình 4.3 Bộ truyền bánh răng Ƣu điểm - Hiệu suất cao - Tuổi thọ, độ tin cậy lớn 21
  34. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Có thể làm việc với vận tốc lớn - Kích thƣớc nhỏ, khả năng chịu tải lớn - Thuận tiện và đơn giản khi sử dụng Nhƣợc điểm - Chế tạo phức tạp - Có tiếng ồn khi vận tốc lớn 4.2.2 Phƣơng án sử dụng động cơ cho xe Động cơ AC Hình 4.4: Động cơ AC Đặc điểm - Giá thành cao - Công suất lớn - Khó điều khiển hơn động cơ DC Động cơ DC Hình 4.5 : Động cơ DC 22
  35. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Giá thành không quá cao - Công suất vừa và nhỏ - Điều khiển dễ dàng 4.2.3 Phƣơng án sử dụng thiết bị điều khiển Vi điều khiển trung tâm đƣợc đề xuất là dòng vi điều khiển 8 bit PIC (16F887) và dòng vi điều khiển 16 bit DSPIC (30F4011) So sánh khả năng ứng dụng vào đề tài của hai dòng vi điều khiển này: Vi điều khiển 8 bit PIC (16F887) Vi điều khiển 16 bit DSPIC (30F4011) Thông dụng với sinh viên. Dễ dàng cho Xa lạ với sinh viên.Cần thời gian để tìm việc lập trình sau này. Giá thành thấp. hiểu và ứng dụng nó. Giá thành cao Khả năng phát triển ứng dụng lớn do khả Hạn chế khả năng phát triển ứng dụng lớn năng hoạt động mạnh mẽ, nhanh hơn so với dòng 8 bit Bảng 4.1: Bảng so sánh hai dòng vi điều khiển PIC 16F887 và DSPIC 30F4011 4.3 Lựa chọn phƣơng án Theo phân tích của chúng tôi về ƣu và khuyết điểm và khả năng ứng dụng thành công vào đề tài cao. Chúng tôi đã quyết định lựa chọn các phƣơng án sau: Về bộ truyền động của xe: Bộ truyền bánh răng (hộp số) Về động cơ của xe: Động co DC Về thiết bị điều khiển: vi điều khiển 16 bit DSPIC 30F4011/4012 Về cảm biến: MPU-6050 vì tính thông dụng và phù hợp với đề tài 23
  36. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG V : TÍNH TOÁN VÀ THIẾT KẾ 5.1 Thiết kế phần cơ khí 5.1.1 Tính toán công suất động cơ Đặt vấn đề : Xe chịu tải trọng 100kg, chạy với vận tốc tối đa 20km/h . Với đƣờng kính bánh xe 0.28m và hộp giảm tốc có tỉ số truyền 1/15 - Phân tích lực bánh xe Hình 5.1 Phân tích lực bánh xe P : Trọng lực của xe F : Lực tƣơng tác giữa thân xe và bánh xe N : Phản lực Fms : Lực ma sát M : Momen bánh xe - Tính toán các giá trị Xe chịu tải trọng 100kg ( thân xe và ngƣời trên xe), P F 100 9. 8 980N Chia đều cho 2 bánh xe, vậy mỗi bánh xe chịu lực 490N Chiếu lực theo phƣơng y, chọn chiều dƣơng hƣớng lên PFN0 PF N490N 24
  37. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Fms k N 0 03 490 14 7N Với k là hệ số ma sát lăn Để bánh xe chuyển động thì MFRms ( với R là bán kinh bánh xe ) M 14 (.)(.) 7 0 14 2 1 N m 2100 N mm Bánh xe đi qua hộp giảm tốc có tỉ số truyền 1 : 15 Mdc 2100:(.) 15 140 N mm Bánh xe có đƣờng kinh 0.28 m , chu vi của nó là C 2 R 2 0 () 14 0 88 m Xe đi với vận tốc 20 km/h . Tốc độ quay của bánh xe v 20 1000 n 380(/) v p C 0. 88 60 Tốc độ của động cơ ndc n 15 380 15 5700(/) v p Công suât động cơ M n 140 5700 Pdc dc 0.() 083 kW 9 55 1066 9 55 10 Chọn động cơ công suất: 83W 5.1.2 Thiết kế cơ khí a. Hộp giảm tốc Hình 5.2 Hộp giảm tốc Hộp giảm tốc TSUBAKI có tỉ số truyền 1 : 15 đƣợc sử dụng cho cơ cấu truyền động của xe. Loại hộp giảm tốc này đƣợc dùng phổ biến trong công nghiệp có độ 25
  38. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng bền rất cao, thuận lợi cho việc sửa chữa và thay thế, phù hợp với yêu cầu thiết kế của xe. b. Bánh xe Hình 5.3 Bánh xe Khả năng chịu tải của bánh xe lớn với đƣờng kính bánh xe 28 cm. c. Động cơ DC Hình 5.4 Động cơ servo DC 200W Động cơ DC servo của công ty TAMAGAWA SEIKI Công suất 200W Điên áp 40V Gắn nội Encoder 250 xung/ vòng 26
  39. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng d. Sàn xe Hình 5.5 Sàn xe Đƣợc làm bằng vật liệu gỗ, vững chắc e. Miếng đỡ Đƣợc làm bẳng vật liệu thép CT3 - Môđun đàn hồi khi kéo: E = 2,1.106 kG/cm2 - Môđun đàn hồi trƣợt: G = 0,81. 106 kG/cm2 - Giới hạn chảy: = 240 – 280 MPa - Giới hạn bền: = 380 – 420 Mpa - Độ dai va đập: ak = 50 – 100 J/ cm2 - Khối lƣợng riêng: = 7,83 T/ m3 - Độ dãn dài khi đứt: = 21% - Ứng suất cho phép lớn nhất: 180 MPa 27
  40. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.6 Tấm đỡ f. Tổng thể toàn bộ xe Việc thiết kế và sắp xếp các thiết bị và bô phận trong xe phải đảm bảo độ cứng vững và cân đối về khối lƣợng. 28
  41. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.7 Hình 3D thiết kế xe 5.1.3 Phân tích ứng suất, kiểm tra độ bền xe a. Part đỡ trên: - Thông tin vật liệu Material Steel, Carbon Density 7.87 g/cm^3 Mass 0.180092 kg Area 15853 mm^2 Volume 22883.4 mm^3 x=10.6744 mm Center of Gravity y=10.5431 mm z=50 mm Bảng 5.1: Thông tin part đỡ 29
  42. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Hình ảnh Hình 5.8 Part đỡ trên - Tác dụng ngoại lực: Load Type Force Magnitude 500.000 N Vector X 0.000 N Vector Y -500.000 N Vector Z 0.000 N Bảng 5.2: Thông tin ngoại lực tác dụng 30
  43. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Chọn mặt tác dụng lực để phân tích Hình 5.9 Chọn mặt tác dụng lực để phân tích Kết quả phân tích - Sơ lược kết quả: Name Minimum Maximum Volume 22883.8 mm^3 Mass 0.180095 kg Von Mises Stress 0.0000039673 Mpa 47.9734 Mpa 1st Principal Stress -4.64311 Mpa 56.5613 Mpa 3rd Principal Stress -49.6127 Mpa 8.33043 Mpa Displacement 0 mm 0.0556568 mm Safety Factor 7.29571 ul 15 ul Bảng 5.3: Kết quả phân tích part đỡ 31
  44. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Ứng suất giới hạn chảy của thép CT3 là 240 Mpa. Vì vậy đủ bền và không bị chảy dẻo trong quá trình xe chạy - Hình ảnh kết quả Von Mises Stress (ứng suất khi bị tác dụng lực) Hình 5.10 Ứng suất khi bị tác dụng lực part đỡ Displacement (độ dịch chuyển ) Hình 5.11 Độ dịch chuyển part đỡ 32
  45. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Safety Factor (hệ số an toàn ) Hình 5.12 Hệ số an toàn Độ an toàn thấp nhất 7.4ul ứng với trọng lƣợng 100kg, nhƣ vậy có thể thấy độ an toàn là phù hợp b. Tổng thể: - Hình ảnh: Hình 5.13 Hình ảnh tổng thể xe 33
  46. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Đặt lực tác dụng : lực đặt lên 1000N ( tải trọng 100kg) Hình 5.14 Đặt lực lên xe Kết quả phân tích : - Sơ lƣợc kết quả Name Minimum Maximum Volume 22883.8 mm^3 Mass 0.180095 kg Von Mises Stress 0 Mpa 0.007769 Mpa 1st Principal Stress -0.003788 MPa 0.008301 Mpa 3rd Principal Stress -0.01176Mpa 6.60748 Mpa Displacement 0 mm 1.902e-006 mm Safety Factor 14.0034 ul 15 ul Bảng 5.4: Kết quả phân tích tổng thể xe 34
  47. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Hình ảnh kết quả: Von Mises Stress (ứng suất khi bị tác dụng lực) Hình 5.15 Ứng suất xe khi bị tác dụng lực Displacement (độ dịch chuyển ) Hình 5.16 Độ dịch chuyển của xe 35
  48. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Safety Factor (hệ số an toàn ) Hình 5.17 Hệ số an toàn Kết luận: Khi phân tích ứng suất tổng thể mô hình ta thấy hệ số an toàn rất cao chính vì thế nên các chi tiết trong mô hình rất bền vững có thể chịu đƣợc tải trọng 100kg. 36
  49. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 5.2 Thiết kế phần điện 5.2.1 Sơ đồ khối hệ thống Hình 5.18 Sơ đồ khối của xe 5.2.2 Thiết kế mạch xử lý trung tâm Yêu cầu về mạch : dùng IC trung tâm là vi điều khiển DSPIC30F4011, mạch phải xuất các chân IO để điều khiển mạch driver DC, mạch phải có khối hiển thị giao tiếp với ngƣời dùng. 5.2.2.1 Giới thiệu vi điều khiển DSPIC 30F4011 Họ vi điều khiển 16 bit DSPic do công ty công nghệ Microchip Technology Inc. sản xuất, đƣợc phát triển trên nền họ vi điều khiển 8 bit Pic. 37
  50. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Vi điều khiển DSPic là một chip xử lý mạnh với bộ xử lý 16 bit (có khả năng xử lý dữ liệu có độ dài 16 bit). Với tốc độ tính toán cao dựa trên kiến trúc RISC, kết hợp các chức năng điều khiển tiện ích của một bộ vi điều khiển hiệu năng cao 16-bit (highperformance 16-bit microcontroller), có thể thực hiện chức năng của một bộ xử lý tín hiệu số (DSP) nên DSPIC còn có thể đƣợc xem là một bộ điều khiển tín hiệu số (Digital Signal Controller – DSC). Họ vi điều khiển DSPIC có thể đạt tới tốc độ xử lý 40 MIPS (Mega Instruction Per Second - triệu lệnh trên một giây). Ngoài ra DSPIC còn đƣợc trang bị bộ nhớ Flash, bộ nhớ dữ liệu EEPROM và các ngoại vi hiệu năng cao và rất đa dạng các thƣ viện phần mềm cho phép thực hiện các giải thuật nhúng với hiệu suất cao một cách dễ dàng trong một khoảng thời gian ngắn. Chính vì vậy DSPIC đƣợc ứng dụng rất rộng rãi trong các ứng dụng xử lý tín hiệu số, đo lƣờng và điều khiển tự động, .v v Họ vi điều khiển DSPIC đƣợc chia ra làm ba loại tùy theo mục đích của ngƣời sử dụng : Bộ điều khiển số cho điều khiển motor và biến đổi nguồn (DSC Motor Control & Power Conversion Family) Bộ điều khiển số cho sensor (DSC Sensor Family) Bộ điều khiển số đa mục đích (DSC General Purpose Family) 5.2.2.2 Đặc điểm chung a. Khối xử lý trung tâm CPU Tập lệnh cơ bản gồm 84 lệnh. Chế độ định địa chỉ linh hoạt. Độ dài lệnh 24-bit, độ dài dữ liệu 16-bit. Bộ nhớ chƣơng trình Flash 24 Kbytes. Bộ nhớ RAM độ lớn 1Kbytes. Bộ nhớ EEPROM. Mảng 16 thanh ghi làm việc 16-bit Tốc độ làm việc lên tới 40 MIPS b. Bộ chuyển đổi tƣơng tự số ADC Bộ chuyển đổi tƣơng tự - số (ADC) 10-bit:. + Tốc độ lấy mẫu tối đa 1 Msps (Mega samples per second). + Tối đa 10 kênh lối vào ADC. 38
  51. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng + Thực hiện biến đổi cả trong chế độ Sleep và Idle. Chế độ nhận biết điện thế thấp khả lập trình. Tạo Reset bằng nhận diện điện áp khả lập trình. c. Các cổng vào ra I/O Port và các ngoại vi Dòng ra, vào ở các chân I/O lớn: 25 mA 3 Timer 16-bit, có thể ghép 2 Timer 16-bit thành Timer 32-bit Chức năng Capture 16-bit Các bộ so sánh/PWM 16-bit Module SPI 3 dây (hỗ trợ chế độ Frame) Module I2C, hỗ trợ chế độ đa chủ tớ, địa chỉ từ 7-bit đến 10-bit UART có khả năng địa chỉ hoá, hỗ trợ bộ đệm FIFO d. Bộ xử lý tín hiệu số Nạp dữ liệu song song Hai thanh chứa 40-bit có hỗ trợ bão hoà logic Thực hiện phép nhân 2 số 17-bit trong một chu kì máy Tất cả các lệnh DSP đều thực hiện trong một chu kì máy Dịch trái hoặc phải 16 bit trong một chu kì máy e. Một số đặc điểm khác Bộ nhớ Flash: ghi/xoá lên tới 10.000 lần (điều kiện công nghiệp) và trên dƣới 100.000 lần (thông thƣờng) Bộ nhớ EEPROM: ghi/xoá lên tới 100.000 lần (điều kiện công nghiệp) và trên dƣới 1.000.000 lần (thông thƣờng) Khả năng tự nạp trình dƣới điều khiển của software Watch Dog Timer mềm dẻo với bộ dao động RC nguồn thấp trên chip. Chế độ bảo vệ firmware khả lập trình Khả năng tự lập trình nối tiếp trên mạch điện ( In Circuit Serial Programming – ICSP) Có thể lựa chọn các chế độ quản lí nguồn: Sleep hoặc Idle Mạch đƣợc thiết kế 39
  52. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.19: Mạch xử lý trung tâm Mạch đƣợc thi công ngoài thực tế Hình 5.20: Mạch xử lý trung tâm thực tế 40
  53. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 5.2.3 Thiết kế mạch driver điều khiển động cơ DC 5.2.3.1 Phần thiết kế Hình 5.21 Mạch driver điều khiển động cơ DC 41
  54. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 5.2.3.2 Các khối chính trong mạch - Khối nguồn Hình 5.22: Khối nguồn driver Mạch sử dụng 2 IC nguồn LM2596. Nguồn điện đầu vào là 24VDC, IC LM2596ADJ xuất nguồn đầu ra là 12V (cho mạch cầu H) và IC LM2596 5V xuất ra 5V (cho các IC). - Khối điều khiển Hình 5.23: Khối điều khiển driver MCU đƣợc sử dụng là DSPIC30F4011. Sử dụng bộ QEI để nhận tín hiệu từ Encorder(chân 5,6,7). Sử dụng kênh PWM1L(chân 26) để xuất PWM điều khiển động cơ, chân RE1(chân 25) làm chân CW(điều khiển xuất ra chiều cho đông cơ)chân RE8 (chân 16) làm chân Enable, chân RD0 (chân 15) làm chân Dir(chiều đƣợc nhận vào). 42
  55. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.24: Ngõ xuất ra PWM và CW vào cầu H Hình 5.25: Ngõ nhận tín hiệu encoder - Khối cầu H Hình 5.26: Khối cầu H Sử dụng FET IRF3205 và 2 IC đệm. Sử dụng IC IR2148 nhằm làm IC công suất nhằm khuếch dòng vào FET và chống trùng dẫn 43
  56. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng - Khối nhận tín hiệu điều khiển Hình 5.27: Khối nhận tín hiệu điều khiển Chân PUL sử dụng OPTO 6N137. Chân DIR và EN dùng OPTO 4N35 - Khối hồi tiếp dòng Hình 5.28: Khối hồi tiếp dòng - Dùng IC ACS712. - Khi muốn dùng hồi tiếp dòng thì nối chân 1 với chân 2, chân 3 với chân 4 của DOMINO4. Khi không muốn dùng thì nối chân 1 với 4. - Chân 1 của DOMINO4 đƣớc lấy từ mạch cầu H 44
  57. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.29 Mạch Driver servo DC thực tế 5.2.4 Cảm biến MPU6050 5.2.4.1 Giới thiệu MPU-6050 là cảm biến của hãng InvenSense. MPU-6050 là một trong những giải pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 (mở rộng 9 ) trục cảm biến tích hợp trong 1 chip duy nhất MPU-6050 sử dụng công nghệ MotionFusion của InvenSense có thể chạy trên các thiết bị di động, tay điều khiển. Tín hiệu ra của module GY-521 MPU-6050 là tín hiệu đƣợc truyền qua I2C với tốc độ 400KHz và SPI 1MHz 45
  58. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.30 Hình ảnh thực cảm biến MPU6050 Vcc : 5V / 3.3V GND: 0V SCL: chân SCL trong giao tiếp I2C SDA: chân SDA trong giao tiếp I2C XDA: chân dữ liệu (kết nối với cảm biến khác) XCL: chân xung (kết nối với cảm biến khác) AD0: Bit 0 của địa chỉ I2C INT: chân ngắt 5.2.4.2 Sơ đồ chân và mạch nguyên lý cảm biến MPU6050 Hình 5.31: Sơ đồ chân MPU6050 46
  59. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.32: Mạch nguyên lý module GY-521 MPU-6050 5.2.4.3 Cách thức đọc dữ liệu cảm biến MPU6050 Để đọc dữ liệu từ cảm biến phải dùng phƣơng thức giao tiếp I2C hoặc SPI. Ở đây nhóm chọn phƣơng thức giao tiếp I2C Hình 5.33 Chuẩn giao tiếp I2C MCU mạch xử lý trung tâm(vi điều khiển DSPIC30F4011) là Master, cảm biến MPU6050 là Slave Địa chỉ giao tiếp của cảm biến MPU6050 : #define MPU6050_ADDRESS 0XD0 Truyền dữ liệu 47
  60. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 5.34 Phương thức truyền dữ liệu I2C 5.2.4.4 Xử lý dữ liệu đọc từ cảm biến MPU-6050 Cảm biến MPU-6050 sẽ trả về hai giá trị: - Dữ liệu lấy về từ Gyro : + Theo tài liệu do nhà sản xuất cung cấp “MPU-6000 and MPU-6050 Register Map and Description Revision 4.0 (03/09/2012) ”, ta sẽ tính đƣợc vận tốc góc theo công thức sau: gyroRate=gyroRead/[LBS Sensitivity] Trong đó: gyroRate: vận tốc góc (độ/giây) gyroRead : dữ liệu 16 bit đọc về từ Gyro của MPU-6050 [LBS Sensitivity]: hệ số dùng để suy ra vận tốc góc, nó phụ thuộc vào thiết lập chế độ hoạt động của Gyro trong MPU-6050 + Từ vận tốc góc ta suy ra giá trị góc xoay nhƣ sau: gyroAnglek = gyroAngle k -1 +gyroRate k Δt Trong đó: gyroAnglek : Giá trị góc xoay cần tìm gyroAnglek -1 : Giá trị góc xoay ngay trƣớc thời điểm k gyroRatek : Giá trị vận tốc góc tính đƣợc tại thời điểm k Δt : Khoảng thời gia tính từ trạng thái k-1 đến k Nhận xét : Việc đo góc bằng gyro rất dễ dàng mà không dễ bị nhiễu bởi tác động bên ngoài. Tuy nhiên việc tính giá trị góc từ vận tốc góc bằng tích phân dẫn đến việc sai số cộng dồn theo thời gian, nên giá trị có xu hƣớng trôi dạt và không về zero khi hệ thống 48
  61. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng đã trở lại vị trí ban đầu. Nhƣ vậy kết quả đo góc xoay bằng gyro chỉ đáng tin cậy trong khoảng thời gian ngắn - Dữ liệu lấy từ Accelerometer, suy ra đƣợc góc quay sau: roll=atan2(accY,accZ)*RAD_TO_DEG pitch=atan(-accX/sqrt(accY accY+accZ)) RAD_TO_DEG Trong đó roll : góc xoay quanh trục X pitch : góc xoay quanh trục Y accX,accY,accZ: Giá trị gia tốc lần lƣợt theo các trục X,Y,Z RAD_TO_DEG : Hệ số chuyển đổi đơn vị góc radian sang độ Nhận xét: Với đặc điểm dễ bị nhiễu với mọi lực tác động, dù chỉ một lƣợng nhỏ cũng đủ làm kết quả đo góc bằng Accelerometer bị nhiễu. Do đó việc đo góc xoay bằng gia tốc chỉ đáng tin cậy trong khoảng thời gian tƣơng đối dài hoặc trong môi trƣờng không có các lực gây nhiễu. 5.2.4.5 Áp dụng bộ lọc Kalman cho cảm biến MPU-6050 từ Ac cel erom eter Lọc Kalman Góc yro từ G Hình 5.35: Tín hiệu đầu vào và đầu ra bộ lọc Kalman Trong đó: . từ Accelerometer: góc xoay đo đƣợc từ cảm biến gia tốc . từ Gyro: giá trị vận tốc lấy từ cảm biến gyro . Angle : góc xoay sau khi qua bộ lọc Kalman Trong chuyển động tròn ta có: dt t ()b t k k11 k k 1 bbkk1 49
  62. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Trong đó: . k : là góc tại thời điểm k. . k 1 : là góc tại thời điểm k. . k 1 : vận tốc góc của tại thời điểm k-1. . : là góc lệch của tại thời điểm k. bk Trƣớc tiên ta sẽ thiết lập các ma trận và các vector cần thiết cho bộ lọc Kalman. Góc và vận tốc góc đƣợc mô tả bởi không gian trạng thái tuyến tính: k Trạng thái của hệ thống vào thời điểm k đƣợc phát triển từ trạng thái hệ thống tại thời điểm k-1 kF k1 Bu k w k 1 t Với F là ma trận trạng thái F 01 Không có tín hiệu đầu vào nên Buk đƣợc bỏ qua Các ma trận Qk , R k là ma trận đối xứng nửa xác định dƣơng đại diện cho nhiễu quá trình và nhiễu đo lƣờng. Vì các nhiễu này rất khó để xác định nên chúng ta tạm thiết lập sau: Q 0 Q t 0 Q R = [R] Các thông số trong ma trận này do ngƣời dùng tự gán gia trị tùy thuộc vào mức độ nhiễu thu đƣợc từ đồ thị tính góc xoay. 00PP 01 Ma trận hiệp phƣơng sai P 10PP 11 Ma trận quan sát H 10 Áp dụng chƣơng trình tính toán của bộ lọc Kalman ˆˆ Bƣớc 1: Trạng thái ƣớc đoán: k| k 1F k k 1 B k u k kk|1 1 t k 1 kk11t kk|1 01 k 1 0 k 1 T Bƣớc 2 : Dự đoán, ƣớc hiệp phƣơng sai: PFPFQk| k 1 k k 1 k k 00 01 1t PPkk11 1 0 PQk|1 k10 11 k 0 1PPkk11t 1 50
  63. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 00 10 01 11 Pk1 t P k 1 P k 1 t P k 1 10 10 11 Qk PPkk11t 1 00P t 10 P t 01 P t 11 P 01 P t 11 P Q 0 k1 k 1 k 1 k 1 k 1 k 1 . t 10 11 11 0 Q Pk1 t. P k 1 P k 1 00P t 11 P 01 P 10 P Q 01 P t 11 P k1 k 1 k 1 k 1 k 1 k 1 10 11 11 Pk1 t P k 1 P k 1 t Q ˆ Bƣớc 3: Đo lệch đo lƣờng: yk z k H k k|1 k Độ lệch đo lƣờng = Giá trị trạng thái đo – Giá trị dự đoán sẽ đo đƣợc trƣớc đó kk| -1 yk z k- 1 0 z k k| k -1 kk| -1 T Bƣớc 4: Đo lệch hiệp phƣơng sai: SHPHRk k k|1 k k k 00 01 PPk| k 1 k | k 1 1 SRkk10 10 11 PPk| k 1 k | k 1 0 1 00PPRPRS 01 00 k| k 1 k | k 10 k k | k 1 T 1 Bƣớc 5: Tính độ lợi Kalman: KPHSk k|1 k k k 00 01 00 K PP1 11Pkk|1 0 k| k 1 k | k 1 10PP 11 10 K1 k| k 1 k | k 1 0 SSPkk|1 ˆˆ Bƣớc 6: Cập nhật trạng thái ƣớc đoán đã điều chỉnh: k k|1 kKy k k Ky k kk|1 K0 k| k 1 0 k .yk k k|1 k K1 k| k 1Ky 1 k Bƣớc 7: Cập nhật hiệp phƣơng sai ƣớc lƣợng đã điều chỉnh: PIKHPk() k k k|1 k 00 01 10 K0 10K0 PPk| k 1 k | k 1 PPk10 k|1 k 10 11 01 K1 K1 1 PPk| k 1 k | k 1 00 01 1KPKP0 .k | k 1 1 0 . k | k 1 Pk 10 00 11 01 PKPPKPk| k 1 1 k | k 1 k | k 1 1 k | k 1 51
  64. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng CHƢƠNG VI : KẾT QUẢ THỰC NGHIỆM 6.1 Mô phỏng bằng Matlab và kết quả 6.1.1 Mô phỏng bằng Matlab Từ phƣơng trình (3.20) và (3.21) đƣa vào Matlab. Ta có hệ thống sau: Hình 6.1 : Mô phỏng hệ thống bằng MATLAB Simulink Mô phỏng PID vào hệ thống Hỉnh 6.2 : Mô phỏng PID vào hệ thống 6.1.2 Kết quả mô phỏng Đặt góc nghiêng ban đầu θ= 0.5 rad≈ 30o 52
  65. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Đồ thị góc nghiêng : Goc nghieng xe 0.6 0.5 0.4 0.3 0.2 0.1 0 -0.1 -0.2 -0.3 -0.4 0 1 2 3 4 5 6 7 8 9 10 Hình 6.3: Đồ thị đáp ứng khi góc nghiêng ban đầu θ= 0.5 rad ≈ 30o Nhận xét: đồ thị đạt giá trị đáp ứng sau 2s Đặt góc nghiêng ban đầu θ = 1 rad ≈ 60o Đồ thị góc nghiêng : Goc nghieng xe 1 0.8 0.6 0.4 0.2 0 -0.2 -0.4 0 1 2 3 4 5 6 7 8 9 10 Hình 6.4: Đồ thị đáp ứng khi góc nghiêng ban đầu θ= 1 rad ≈ 60o 53
  66. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Nhận xét: Đồ thị đạt giá trị đáp ứng sao 2s Đặt góc nghiêng ban đầu θ=0 độ, setpoint = 0.5rad ≈ 30o Đồ thị góc Goc nghieng xe 0.5 0.4 0.3 0.2 0.1 0 -0.1 0 1 2 3 4 5 6 7 8 9 10 Hình 6.5: Đồ thị đáp ứng khi setpoint = 0.5rad ≈ 30o Nhận xét: Đồ thị đạt giá trị đáp ứng sau 4s 6.2 Độ chính xác cảm biến Để có thể điều khiển xe giữ cân bằng và di chuyển một cách ổn định thì thông tin về góc nghiêng của xe rất quan trọng. Góc nghiêng của xe đƣợc tính toán dựa vào giá trị trả về từ cảm biến MPU6050. Cảm biến này tích hợp gồm cảm biến gia tốc và con quay hòi chuyển. Hai cảm biến này là dạng cảm biến vi cơ điện tử (MEMS) nên rất nhạy với rung động. Để có thể đo đạc góc nghiêng của xe thì các giá trị thô này phải đƣợc xử lý loại bỏ nhiễu. Kết quả thực nghiệm so sánh giá trị góc nghiêng của xe thu đƣợc từ cảm biến gia tốc , con quay hồi chuyển và giá trị sau khi lọc Kalman 54
  67. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 6.6: So sánh kết quả thực nghiệm bộ lọc Kalman Qua đồ thị có thể thấy đƣợc, đối với tín hiệu góc thô đƣợc đo bằng cảm biến gia tốc (đƣờng màu đỏ) thì rất dễ bị nhiễu do tác động bên ngoài, làm sai kết quả. Giá trị góc thô đƣợc đo bằng con quay hồi chuyển gyro ( đƣờng màu đen) thì ổn định hơn rất nhiều nhƣng có xu hƣớng trôi theo thời gian do sai số cộng dồn. Từ hai phƣơng pháp đo góc trên, ta thấy rõ tầm quan trọng của bộ lọc Kalman. Nhìn vào đồ thị ta thấy góc sau khi lọc Kalman ( đƣờng màu xanh) vừa có tính ổn định không chịu ảnh hƣởng nhiều từ nhiễu vừa không bị trôi theo thời gian. 6.3 Mô hình cơ khí Mô hình xe hai bánh tự cân bằng đƣợc thiết kế và thi công có khả năng chịu tải trọng một ngƣời trƣởng thành 80kg. Xe có thể di chuyển với vận tốc 20km/h. Xe có màn hình LCD hiển thị vận tốc, góc nghiêng của xe. Xe sử dụng bình acquy làm nguồn nuôi, khi cạn nguồn có thể sạc dễ dàng. 55
  68. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Hình 6.7: Mô hình xe thực tê 6.4 Giải thuât điều khiển xe Mạch điều khiển trung tâm sẽ đọc cảm biên MPU6050 đo góc nghiêng của xe. Số liệu này sẽ đƣợc xử lý qua bộ lọc Kalman. Góc nghiêng đã qua xử lý sẽ đƣợc so sánh với góc đặt ( góc tại vị trí cân bằng ). Dựa vào sai số này mà xe sẽ di chuyển theo tốc độ và hƣớng tƣơng ứng nhờ vào bộ điều khiển PID. Bộ điều khiển này sẽ xuất ra số xung và chiều cho driver. Từ đó driver sẽ điều khiển tốc độ động cơ sao cho trọng tâm ngƣời điều khiển và xe luôn đặt lên trục hai bánh xe. 56
  69. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 6.5 Lƣu đồ chƣơng trình diều khiển vận tốc động cơ DC Start Cấu hình DSPIC 30F4012 Ngắt timer 1 Đọc số xung encoder Tính toán vân tốc hiện tại Tính toán PID Xuất PWM Sơ đồ 6.1: Giải thuật điều khiển vận tốc động cơ DC 57
  70. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng 6.6 Lƣu đồ chƣơng trình chính cân bằng xe Start Cấu hình DSPIC30F4011 I/O I2C LCD Cấu hình MPU6050 Đọc ID cảm biến MPU6050 và hiển thị LCD Sai ID=0x68h Đúng Đọc giá trị gia tốc và gyro từ MPU6050 Tính toán góc nghiêng từ giá trị đọc đƣợc Tính toán góc Hiển thị nghiêng bằng bộ lọc LCD Kalman Tính toán PID Xuất số xung, chiều điều khiển vận tốc và chiều động cơ 58
  71. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng Sơ đồ 6.2: Lưu đồ giải thuật mạch trung tâm 6.7 Kết quả Hình 6.8: Góc lệch so với vị trí cân bằng Nhận xét: Đáp ứng chƣa đƣợc tốt do thông số PID chƣa thật sự phù hợp Hình 6.9: Góc lệch so với trị cân bằng khi chịu lực tác động Nhận xét: Khi góc lệch càng cao độ vọt lố lớn, xe khó về vị trí cân bằng 59
  72. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng KẾT LUẬN- KIẾN NGHỊ Những kết quả đạt đƣợc Sau quá trình học tập nghiên cứu, chúng em thu đƣợc những kết quả nhất định.  Thiết kế và chế tạo hoàn chỉnh cơ khí và điện cho xe  Kiểm nghiệm bộ lọc Kalman trên vi điều khiển DSPIC30F4011  Mô hình hóa xe hai bánh tự cân bằng  Mô phỏng điều khiển xe bằng thuật toán PID trên phần mềm MATLAB  Sử dụng thuật toán PID điều khiển vận tốc động cơ DC  Xe có thể cân bằng khi chƣa có tải Những điều chƣa đạt đƣợc Xe cân bằng chƣa ổn định Chƣa khắc phục tình trạng treo cảm biến khi bị nhiễu Kết luận Đồ án tốt nghiệp là cơ hôi để giúp cho chúng em áp dụng những kiến thức đã học và đi sâu vào thực tiễn. Đây là giai đoạn giúp chúng em hoàn thiện kiến thức, kỹ năng của bản thân để trang bị cho mình sau khi ra trƣờng Tuy nhiên quá trình thực hiện đồ án, nhóm thực hiện còn nhiều hạn chế về kiến thức, thiết bị, thời gian nên đồ án còn nhiều sai sót. Rất mong sự đóng góp ý kiến của thầy cô và các bạn để nhóm đồ án ngày càng hoàn thiện. Kiến nghị Để bắt kịp quá trình phát triển công nghệ kỹ thuật. Nhóm mong muốn nhà trƣờng tạo điều kiện thuận lợi cho sinh viên khóa sau tiếp tục phát triển đề tài. 60
  73. ĐATN: Hoàng Minh Nhật & Trần Đức Tấn GVHD: Ths Võ Lâm Chƣơng TÀI LIỆU THAM KHẢO Tiếng Việt [1] Mai Tuấn Đạt (Tháng 7 năm 2005). Luận văn tốt nghiệp đại học Xe hai bánh tự cân bằng di chuyến trên địa hình phẳng. Trƣờng Đại Học Bách Khoa Thảnh Phố Hồ Chí Minh. [2] Nguyễn Gia Minh Thảo ( năm 2010). Luận văn tốt nghiệp thạc sĩ Robot hai bánh tự cân bằng. Trƣờng Đại Học Bách Khoa Thành Phố Hồ Chí Minh [3] Nguyễn Thế Hùng (năm 2006).Điều khiển tự động. Trƣờng Đại Học Sƣ Phạm Kỹ Thuật TP Hồ Chí Minh. Tiếng Anh [1] dsPIC30F4011/4012Data Sheet, High-Performance,16-Bit Digital Signal Controllers (năm 2010). [2] MPU-6000 and MPU-6050 Register Map And Descriptions Revision 4.0 (03/09/2012) [3] Gyroscope [4] Accelerometer [5] Rich Chi Ooi, Balancing a Two Wheeled Autonomous Robot. The University of Western Australia School of Mechanical Engineering [6] Greg Welch and Gary Bishop (July 24, 2006). An Introduction the Kalman Filter 61
  74. S K L 0 0 2 1 5 4