TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN Đồ án I ĐỀ TÀI: TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN Giảng viên hướng dẫn Họ tên sinh viên Mã số sinh viên Mã lớp : : : : TS Vũ Vân Hà Nguyễn Việt Hoàng 20191856 717569 Hà Nội, 08/2022 LỜI NĨI ĐẦU Qua q trình thực đồ án I, em tìm hiểu học tập nhiều điều Mặc dù đồ án thực thời gian dài, nhiên cịn thiếu kinh nghiệm việc tìm kiếm thơng tin nên mong có sai sót mong thầy bỏ qua Em cố gắng thực đồ án môn học, tiếp thu kiến thức, kỹ kì học Em xin chân thành cảm ơn Hà Nội, ngày 12 tháng năm 2022 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 .8 2.2 Bộ lọc Kalman .9 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 toá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 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 thông số lấy từ Gyroscopes X (vận tốc góc), đồ thị lọc tương đối tốt cảm biến vị trí cố định hình 2.5 thay đổi vị trị hình 2.6 KẾT LUẬN Trong trình thực đồ án I, em thấy nhiều kiến thức mẻ, chuyên sâu so với việc học lý thuyết lớp Những công việc giúp 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 hoàn thiện thêm Cuối em xin cảm ơn thầy hướng dẫn em mơn đồ án 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()