TÌM HIỂU NGUEYEN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỌP BỘ LỌC KALMAN MỤC LỤC DANH MỤC HÌNH VẼ.............................................................................................................i CHƯƠNG 1. ENCODER – CẢM BIẾN ĐO VỊ TRÍ GÓC, TỐC ĐỘ ĐỘNG CƠ ............2 1.1 Cấu tạo: ...........................................................................................................................2 1.2 Phân loại:........................................................................................................................2 1.2.1 Incremental encoder (Tương đối) ............................................................................2 1.2.2 Absolute encoder (Tuyệt đối)...................................................................................3 1.3 Thông số encoder cần quan tâm ....................................................................................4 1.4 Cách đọc encoder tương đối...........................................................................................4 1.5 Thực nghiệm ...................................................................................................................5 1.5.1 Thiết bị phần cứng ...................................................................................................5 1.5.2 Sơ đồ đấu dây...........................................................................................................7 1.5.3 Kết quả thực nghiệm................................................................................................7 CHƯƠNG 2. MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC KALMAN .......................8 2.1 Nguyên lý hoạt động .......................................................................................................8 2.2 Bộ lọc Kalman.................................................................................................................9 2.2.1 Bản chất bộ lọc Kalman...........................................................................................9 2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc ...............................................................10 2.3 Bài toán cảm biến MPU6050: ...................................................................................... 11 2.4 Kết quả thực nghiệm: ...................................................................................................13 KẾT LUẬN .............................................................................................................................14 TÀI LIỆU THAM KHẢO......................................................................................................15 PHỤ LỤC ................................................................................................................................16
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN Đồ án I ĐỀ TÀI: TÌM HIỂU NGUEYEN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỌP BỘ LỌC KALMAN Giáo viên hướng dẫn: Sinh viên thực hiện: TS Nguyễn Hoài Nam Trần Đức Long 20181627 Phạm Thị Hương Ly 20181638 Vi Hồng Nam 20181671 Nguyễn Khánh Châu 20186310 Trần Văn Huy 20181538 Lê Vũ Việt Long 20181605 Nguyễn Thế Long 20181616 Hà Nội, 6-2021 LỜI NÓI ĐẦU Qua q trình nhiều thực đồ án I, nhóm sinh viên chúng em thực phần yêu cầu thầy giao Mặc dù đồ án thực thời gian dịch bệnh, tất trao đổi qua tin nhắn, mail, khiến cho việc tương tác khó khăn Tuy nhiên, chúng em cảm ơn thầy TS Nguyễn Hồi Nam mơn Điều khiển tự động, người trực tiếp hướng dẫn, tạo điều kiện, giúp đỡ chúng em trình làm đồ án, để chúng em hồn thiện đồ án cách tốt Chúng em cố gắng thực đồ án môn học, tiếp thu kiến thức, kỹ kì học đặc biệt Tuy nhiên, sinh viên chúng em khơng thể tránh việc thiếu sót q trình thực hiện, báo cáo mơn học Em xin chân thành cảm ơn Hà Nội, ngày 20 tháng năm 2021 LỜI CAM ĐOAN Em xin cam đoan đồ án I nhóm em thực hướng dẫn thầy TS Nguyễn Hoài Nam Các số liệu kết hoàn toàn với thực tế Để hoàn thành đồ án em sử dụng tài liệu ghi danh mục tài liệu tham khảo không chép hay sử dụng tài liệu khác Hà Nội, ngày 20 tháng năm 2021 MỤC LỤC DANH MỤC HÌNH VẼ i CHƯƠNG ENCODER – CẢM BIẾN ĐO VỊ TRÍ GĨC, TỐC ĐỘ ĐỘNG CƠ 1.1 Cấu tạo: 1.2 Phân loại: 1.2.1 Incremental encoder (Tương đối) 1.2.2 Absolute encoder (Tuyệt đối) 1.3 Thông số encoder cần quan tâm 1.4 Cách đọc encoder tương đối 1.5 Thực nghiệm 1.5.1 Thiết bị phần cứng 1.5.2 Sơ đồ đấu dây 1.5.3 Kết thực nghiệm CHƯƠNG MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC KALMAN 2.1 Nguyên lý hoạt động 2.2 Bộ lọc Kalman 2.2.1 Bản chất lọc Kalman 2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc 10 2.3 Bài toán cảm biến MPU6050: 11 2.4 Kết thực nghiệm: 13 KẾT LUẬN 14 TÀI LIỆU THAM KHẢO 15 PHỤ LỤC 16 DANH MỤC HÌNH VẼ Hình 1.1 Cấu tạo encoder Hình 1.2 Encoder tương đối Hình 1.3 Encoder tuyệt đối Hình 1.4 Tín hiệu kênh A B Hình 1.5 Encoder 334 Hình 1.6 Module L298 Hình 1.7 Sơ đồ dây Hình 1.8 Vị trí góc vận tốc Hình 2.1 MPU6050 Hình 2.2 Mơ hình Kalman .10 Hình 2.3 Thuật tốn Kalman 11 Hình 2.4 Mơ hình lọc Kalman cho hệ tuyến tính 11 Hình 2.5 Vị trí khơng đổi .13 Hình 2.6 Vị trí dao động 13 i CHƯƠNG ENCODER – CẢM BIẾN ĐO VỊ TRÍ GĨC, TỐC ĐỘ ĐỘNG CƠ 1.1 Cấu tạo: Encoder cấu tạo gồm: • Đĩa quang trịn có rảnh nhỏ quay quanh trục: Trên đĩa đục lỗ (rãnh), đĩa quay chiếu đèn led lên mặt đĩa có ngắt quãng xảy Các rãnh đĩa chia vịng trịn 360o thành góc Và đĩa có nhiều dãy rãnh tính từ tâm trịn • Bộ cảm biến thu (photosensor) • Nguồn sáng (Light source) Hình 1.1 Cấu tạo encoder 1.2 Phân loại: 1.2.1 Incremental encoder (Tương đối) Tương ứng đĩa 2bit, cho ngõ dạng xung vuông pha AB, ABZ ABZA|B|Z| (đọc A đảo, B đảo, Z đảo) Loại Encoder có tín hiệu xung A B phổ biến Tín hiệu khe Z tín hiệu để xác định động quay vòng Hình 2.1 MPU6050 2.2 Bộ lọc Kalman Bộ lọc trình xử lý nhằm loại bỏ khơng có giá trị khơng quan tâm đến giữ lại có giá trị sử dụng Trong xử lý tín hiệu, lọc thiết kế để lọc tín hiệu (cần tìm) từ tín hiệu trộn lẫn tín hiệu nhieeuc tín hiệu bẩn ( không cần thiết ) Vậy lọc Kalman, cách khái quát tập hợp phương trình tốn học mơ tả phương pháp tính toán truy hồi hiệu cho phép ước đoán trạng thái trình cho trung bình phương sai độ lệch ( giá trị thực giá trị ước đoán) nhỏ Bộ lọc Kalman hiệu việc ước đoán trạng thái khứ, tương lai chí xác hệ thống mơ khơng khẳng định 2.2.1 Bản chất lọc Kalman Để mơ hình hóa hoạt động mạch lọc Kalman, có tín hiệu đo được, có mơ hình tín hiệu đo ( địi hỏi tuyến tính) sau áp dụng vào hệ thống phương trình mạch lọc để ước lượng trạng thái quan tâm Để ứng dụng cách hiệu mạch lọc Kalman phải mơ hình hóa cách tuyến tính thay đổi trạng thái cần ước lượng (estimate) ước đốn(predict) Hình 2.2 Mơ hình Kalman 2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc Mơ hình đối tượng tuyến tính rời rạc biểu diễn thơng qua hệ phương trình trạng thái sau: xk = Axk-1 + Buk-1 + wk-1 zk = Hxk + vk Trong : − x : biến trạng thái − u : biến đầu vào − z : trạng thái đầu vào ( đo lường ) − w, v : nhiễu trình nhiễu đo lường tương ứng Giả sử nhiễu trình w(t) nhiễu đo lường v(t) nhiễu trắng , có phương sai tương ứng Q R p(w) ~ N (0, Q) p(v) ~ N (0, R) Với hệ thống lọc Kalman tuyến tính cho phép ta ước lượng cách tốt biến trạng thái xk, kí hiệu 𝑥̂ k , cho hiệp phương sai chúng Pk đạt giá trị nhỏ Pk = E[ekekT] Ek = xk - 𝑥̂ k Thuật toán Kalman cho hệ rời rạc sau: Như ta thấy thuật tốn gồm hai bước : bước ước lượng dự đoán ( time update) bước làm xác đự dốn ( mcasurement update ) dựa thông tin đầu vào đo zk Hình 2.3 Thuật tốn Kalman Chú ý 𝑥̂ k- giá trị dự đoán cập nhật từ giá trị ước lượng 𝑥̂ k-1 Thuật toán đc trình bày sơ đồ khối sau: Hình 2.4 Mơ hình lọc Kalman cho hệ tuyến tính 2.3 Bài tốn cảm biến MPU6050: Có thơng số đưa từ cảm biến, nhiên thông số chịu tác động nhiễu, ta thiết kế lọc Kalman để ước lượng giá trị tốt giá trị cần đo Mơ hình tốn : xk = Axk-1 + Buk-1 + wk-1 = xk-1 + wk Với giá trị thu đươc z ∈ R zk = Hxk + vk = xk + vk Với : − Giá trị cần đo x(t) − Tín hiệu thu từ cảm biến z(t) − Nhiễu trình w(t) − Nhiễu đo lường v(t) Các phương trình cho lọc Kalman : Phương trình trình “Time update”: 𝑥̂ k- = 𝑥̂ k-1 Pk- = Pk-1 + Q Phương trình trình “easurement update”: Kk = 𝑝𝑘− 𝑃𝑘− +𝑅 𝑥̂ k = 𝑥̂ k- + Kk(zk - 𝑥̂ k- ) Pk = (1- Kk) 𝑃𝑘− Một số giả thiết: Tín hiệu cần đo x(t) ~ (0,50) w(t) ~ (0,1e-3) Q = 1e-3 nhiễu đo lường tốn mơ lấy giá trị từ cảm biến ta lấy khoảng 10% giá trị thật Ta có Kk tỉ lệ nghịch với R, R lớn tốc độ ước lượng chậm, giá trị ước lượng tin tưởng giá trị đo ngược lại Do việc chọn R quan trọng ảnh hưởng đến tốc độ ước lượng hệ thống Chọn thông số ước lượng ban đầu : x(0) = 0, P0- = Giá trị x(0) khơng quan trọng thích nghi tưng bước để đạt đến giá trị cần xác định P0- khác 2.4 Kết thực nghiệm: Ứng dụng lọc Kalman việc lấy góc từ cảm biến MPU6050: Hình 2.5 Vị trí khơng đổi Hình 2.6 Vị trí dao động KẾT LUẬN Trong q trình thực đồ án I, nhóm sinh viên chúng em thấy nhiều kiến thức, kỹ so với việc học lý thuyết lớp Những phần công việc giao báo cáo hàng tuần khiến cho chúng em ngày tiếp thu phát triển Việc tìm hiểu lý thuyết xong bước mô phỏng, thực hành, việc lựa chọn thiết bị cần thiết để kiểm tra kết thực nghiệm mơ hình, phần cịn sai sót, phát triển hồn thiện thêm Nếu có kết tốt, cơng việc ứng dụng thêm vào học phần đồ án việc thiết kế mơ hình xe cân bằng, xây dựng điều khiển tối ưu, điều khiển phi tuyến cho đối tượng Trong q trình thực hiện, có sai sót, chúng em cố gắng tiếp thu hoàn thiện theo yêu cầu TS Nguyễn Hoài Nam đề tài trau chuốt Em xin chân thành cảm ơn! TÀI LIỆU THAM KHẢO [1] Y Kim and H Bang, Introduction to Kalman Filter and Its Applications [2] Sangtae K and SangJoo K (2015) Dynamic Modeling of a Two-wheeled Inverted Pendulum Balancing Mobile Robot In- 350 ternational Journal of Control, Automation, and Systems, 13 (4) pp 926- 933 [3] Ming Y., Shuang W and Jian-Zhong S (2016) Simultaneous balancing and trajectory tracking control for two-wheeled in- 310 verted pendulum vehicles: A composite control approach Neurocomputing 191, pp.44-5 [4] Kim H W and Jung S (2016) Control of a two-wheel robotic vehicle for personal transportation Robotica, 34 (5), pp 1186-1208 [5] Z Li, C Yang, and L Fan, Advanced Control of Wheeled Inverted Pendulum Systems, Springer, Berlin, Germany, 2013 [6] K G Tran, T T Nguyen, T Q Pham, P D Nguyen, D V Nguyen, P A Nguyen, H M Pham, P D Nguyen and N H Nguyen, Control of TWIR using LQR Controller and Compound Disturbance Observer [7] https://forum.machinelearningcoban.com/t/kalman-filter-va-bai-toan-chuoi-thoigian/3625 PHỤ LỤC Chương trình lấy liệu MPU6050: #include /* #define ACCEL_XOUT_H 0x3B #define ACCEL_XOUT_L 0x3C #define ACCEL_YOUT_H 0x3D #define ACCEL_YOUT_L 0x3E #define ACCEL_ZOUT_H 0x3F #define ACCEL_ZOUT_L 0x40 #define TEMP_OUT_H 0x41 #define TEMP_OUT_L 0x42 #define GYRO_XOUT_H 0x43 #define GYRO_XOUT_L 0x44 #define GYRO_YOUT_H 0x45 #define GYRO_YOUT_L 0x46 #define GYRO_ZOUT_H 0x47 #define GYRO_ZOUT_L 0x48 #define ADDR_MPU6050 0x68 */ const int MPU=0x68; // I2C address of the MPU-6050 int AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; void setup(){ Wire.begin(); Wire.beginTransmission(MPU); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); Serial.begin(9600); } void loop(){ Wire.beginTransmission(MPU); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU,14,true); // request a total of 14 registers AcX=Wire.read()