Cảm biến vận tốc góc và gia tốc

Cảm biến MPU-6050 Cảm biến vận tốc góc gia tốc Người thực : Đoàn Hồng Trung Huỳnh Tấn Bảo Đặng Hữu Tiến Nguyễn Văn Bình Bộ mơn : Hệ thống điện tử Giảng viên : TS Lê Hoài Nam Tại phải quan tâm cảm biến ? - Vì phần quan trọng việc xây dựng robot bánh tự cân tới Ứng dụng Những thứ đời thường ngày máy bay, tơ, xe máy, tích hợp cảm biến tương tự MPU6050 Chip MPU-6050 Tích hợp hệ vi điện tử : - Gia tốc kế - Con quay hồi chuyển Có bậc tự Giới hạn đo : - Gia tốc : ±2g, ±4g, ±8g and ±16g - Vận tốc góc : ±250, ±500, ±1000, ±2000°/sec Ngồi có : + Bộ xử lý chuyển động số → Truy cập đến giá trị cảm biến, sử dụng thuật toán từ vi điều khiển bên ngồi để tính tốn, có quyền sử dụng đệm chân ngắt + Module I2C → Giao tiếp với bên + Cảm biến nhiệt độ → Tính sai số nhiệt độ + Bộ nhớ đệm FIFO → Tăng thời gian lấy liêu + Bộ lọc lập trình → Bộ lọc cho cảm biến gia tốc, gyro, nhiệt độ + Bo mạch GY521 - Rẻ - Dễ mua - Sử dụng thuận tiện Kết nối arduino - Nguồn: 5V, 3.3V SDA: SCL: INT: Giá trị đọc được: - Vận tốc góc theo trục x, y z - Gia tốc theo trục x, y z - Nhiệt độ bên chip Ví dụ : AcX = 9160 | AcY = -9556 | AcZ = -6648 | Tmp = 26.37 | GyX = 9077 | GyY = 7769 | GyZ = -28687 AcX = 13564| AcY = -2636 | AcZ = -9988 | Tmp = 26.55 | GyX = 9708 | GyY = 23384 | GyZ = -7700 AcX = 3840 | AcY = -1224 | AcZ = -10094 | Tmp = 26.41 | GyX = -2320 | GyY = 17385 | GyZ = -7 AcX = -1724 | AcY = 368 | AcZ = 10428 | Tmp = 26.46 | GyX = 3050 | GyY = -11115 | GyZ = -1357 AcX = 1724 | AcY = -752 | AcZ = 11888 | Tmp = 26.41 | GyX = -101 | GyY = -303 | GyZ = -133 Bản chất giá trị theo trục x, y, z Các bước lập trình - Kiểm tra kết nối vi điều khiển với MPU6050 đường truyền I2C → Gửi tín hiệu đánh thức MPU6050 trạng thái ngủ → Cài đặt cấu hình → Gửi yêu cầu lấy liệu ghi lưu trữ giá trị gốc cảm biến → Đọc giá trị đường truyền I2C - Có cách để có giá trị ổn định: ● Đọc giá trị gốc từ cảm biến vi điều khiển Sử dụng lọc trung bình, thơng thấp, thơng cao, bù, kalman, … để có giá trị ổn định ● Điều khiển lọc thông thấp bên cảm biến Khi tính tốn xong giá trị có ngắt xảy Sử dụng lọc bên cảm biến Ưu điểm : Tiết kiệm thời gian tính tốn cho vi điều khiển Dễ sử dụng dùng thư viện arduino Nhược điểm : Không hẳn xác Tốn chân ngắt ngồi Lập trình khó dùng vi điều khiển khơng có thư viện Thường xảy tượng treo ngắt Sử dụng lọc bên vi điều khiển - Bộ lọc thông thấp : Dễ hiểu, dễ sử dụng - Bộ lọc bù : Dễ sử dụng, xác lọc thơng thấp - Bộ lọc Kalman : Khó loại bỏ nhiễu điều thần kì Từ giá trị gốc góc nghiêng - Có thể tính góc nghiêng cảm biến từ cảm biến gyro accel Độ tin tưởng cảm biến lại phụ thuộc vào trạng thái chuyển động cảm biến Gia tốc/vận tốc góc = ( giá trị gốc – giá trị bù ) * hệ số tỉ lệ; Hệ số tỉ lệ = giới hạn đo / giới hạn lưu trữ Từ gia tốc ( trục y nằm ngang ) : góc = arctan ( -AcX / AcZ ) rad Từ vận tốc góc ( quanh trục y ) : góc = góc trước + Gy * dt độ Ví dụ : - Giới hạn đo gia tốc = 2g ( cài đặt cảm biến lúc lập trình ) - Giới hạn lưu trữ cảm biến = 32767 ( lưu trữ 16 bit ) - Giá trị gốc = 1944, giá trị bù = => Gia tốc = 1944 * ( / 32767 ) = 0.118 g - Gia tốc theo trục X = 0.118g - Gia tốc theo trục Z = 0.876g - Chọn Y nằm ngang => Góc = arctan (-0.118 / 0.876) * 180 / pi = - 15 độ - Giới hạn vận tốc góc = 250 d/s - Giá trị gốc = -34 => Vận tốc góc = -34 * ( 250 / 32767 ) = -0.26 độ/giây - Thời gian lần đọc giá trị từ cảm biến 0.01 s - Góc trước độ => góc = + ( - 0.26 ) = 7.74 độ Tính góc nghiêng sử dụng lọc - Bộ lọc thông thấp : Góc = (0.8)*(góc trước đó) + (1 - 0.8)*(gia tốc) - Bộ lọc bù : Góc = (0.966)*(góc trước + vận tốc góc* 0.0262) + (1 - 0.966)*(gia tốc) - 0.8 0.966 trọng số tính dựa vào thời gian đọc mẫu, 0.0262 thời gian đọc mẫu Trọng số = Biên thời gian / (Biên thời gian + thời gian đọc mẫu) Ví dụ : 0.966 = 0.75 / ( 0.75 + 0.0262 ) Trên biên thời gian giá trị cảm biến gia tốc tin cậy hơn, biên thời gian giá trị cảm biến quay hồi chuyển tin cậy Bộ lọc Kalman - Bản chất lọc Kalman dựa vào giá trị trước để dự đốn giá trị kết hợp với giá trị vừa đo cho kết xấp xỉ Phương trình toán học : - x: biến trạng thái u: biến đầu vào z: trạng thái đầu vào w v: Nhiễu trình nhiễu đo lường Lập trình lọc Kalman Các khái niệm liên quan : - Trạng thái: x ( góc ) - Hiệp phương sai: P ( véc tơ ) - Hệ số Kalman: K ( véc tơ ) - Phương sai nhiễu trình : Q - Phương sai nhiễu đo lường: R Đầu vào : Giá trị góc, vận tốc góc vừa đo thời gian đọc mẫu Bộ lọc : Trải qua bước tính tốn Đầu : Giá trị góc xấp xỉ Sử dụng C++ Ta tạo đối tượng tên Kalman Gồm thuộc tính Q_angle, Q_bias, R_measure, angle, bias, rate, P[2][2] Bước : rate = newRate - bias; angle += dt * rate; Bước : P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_gyroBias * dt; Bước Bước Bước y = newAngle - angle; angle += K[0] * y; bias += K[1] * y; Bước K[0] = P[0][0] / S; K[1] = P[1][0] / S; S = P[0][0] + R_measure; Bước float P00_temp = P[0][0]; float P01_temp = P[0][1]; P[0][0] -= K[0] * P00_temp; P[0][1] -= K[0] * P01_temp; P[1][0] -= K[1] * P00_temp; P[1][1] -= K[1] * P01_temp; Đối tượng Kalman có phương thức quan trọng setAngle getAngle sau: float getAngle(float newAngle, float newRate, float dt); // lấy giá trị sau lọc void setAngle(float angle); // truyền giá trị gốc chút PID robot cân Một chút cảm biến la bàn - Kết hợp với MPU6050 → bậc tự Chịu ảnh hướng từ trường trái đất Xác định phương hướng dựa vào hướng bắc Có thể bị nhiễu vật xung quanh có từ trường Hướng dẫn code robot bánh tự cân Thư viện có sẵn dành cho Arduino: - MPU6050 ( Jeff Rowberg ) : https://github.com/jrowberg/i2cdevlib - Kalman Filter Library ( TKJ Electronics ) : https://github.com/TKJElectronics/KalmanFilter Cài đặt : Sketch → Include Library → Manage Libraries → gõ tên thư viện → Install #include #include #include "MPU6050.h" void setup() { KhoiTaoMPU6050(); KhoiTaoDongCo(); } void loop() { DocCamBienMPU6050(); GiuCanBang(); } void GiuCanBang() { GiaTriDongCo = TinhToanPID(GocNghiengHienTai); HieuChinhGiaTriDongCo(); ThietLapGiaTriDongCo(); } Lập trình PID float error = RollAngle - BalanceAngle; IValue += KI * error; IValue = constrain(IValue, -255, 255); float dError = error - LastError; LastError = error; float output = KP * error + IValue + KD * dError; ... Từ giá trị gốc góc nghiêng - Có thể tính góc nghiêng cảm biến từ cảm biến gyro accel Độ tin tưởng cảm biến lại phụ thuộc vào trạng thái chuyển động cảm biến Gia tốc /vận tốc góc = ( giá trị gốc... lưu trữ Từ gia tốc ( trục y nằm ngang ) : góc = arctan ( -AcX / AcZ ) rad Từ vận tốc góc ( quanh trục y ) : góc = góc trước + Gy * dt độ Ví dụ : - Giới hạn đo gia tốc = 2g ( cài đặt cảm biến lúc... trước + vận tốc góc* 0.0262) + (1 - 0.966)* (gia tốc) - 0.8 0.966 trọng số tính dựa vào thời gian đọc mẫu, 0.0262 thời gian đọc mẫu Trọng số = Biên thời gian / (Biên thời gian + thời gian đọc