(TIỂU LUẬN) 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 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 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Ẽ CHƯƠNG ENCODER – CẢM BIẾN ĐO VỊ TRÍ GĨC, TỐC ĐỘ ĐỘNG CƠ 1.1Cấu tạo: 1.2Phân loại: 1.2.1Incremental encoder (T 1.2.2 Absolute encoder (Tuyệt đối) 1.3Thông số encoder cần quan tâm 1.4Cách đọc encoder tương đối 1.5Thực nghiệm 1.5.1 Thiết bị phần cứng 1.5.2Sơ đồ đấu dây 1.5.3Kết thực nghiệm CHƯƠNG MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC KALMAN 2.1Nguyên lý hoạt động 2.2Bộ lọc Kalman 2.2.1Bản chất lọc Kalma 2.2.2Bộ lọc Kalman cho hệ 2.3Bài toán cảm biến MPU6050: 2.4Kết thực nghiệm: KẾT LUẬN TÀI LIỆU THAM KHẢO PHỤ LỤC 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 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 toán Kalman Chú ý k- giá trị dự đoán cập nhật từ giá trị ước lượng k-1 Thuật tố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 11 Với giá trị thu đươc z ∈ R zk = Hxk + vk = xk + v k 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”: − K= k 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 12 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 13 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 hồ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! 14 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-chuoithoi-gian/3625 15 PHỤ LỤC Chương trình lấy liệu MPU6050: #include /* #define ACCEL_XOUT_H #define ACCEL_XOUT_L #define ACCEL_YOUT_H #define ACCEL_YOUT_L #define ACCEL_ZOUT_H #define ACCEL_ZOUT_L #define TEMP_OUT_H #define TEMP_OUT_L #define GYRO_XOUT_H #define GYRO_XOUT_L #define GYRO_YOUT_H #define GYRO_YOUT_L #define GYRO_ZOUT_H #define GYRO_ZOUT_L #define ADDR_MPU6050 */ const int MPU=0x68; // I2C address of the MPU6050 int AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; void setup(){ Wire.begin(); Wire.beginTransmission(MPU); Wire.write(0x6B); // PWR_MGMT_1 register 16 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()