Vì con quay hồi chuyển có xu hướng bị trơi theo thời gian nên để xác định được giá trị bị trôi trong một đơn vị thời gian làm phần bù cho giá trị cảm biến. Lúc này con quay hồi chuyển sẽ ổn định về mặt gía trị làm con quay hồi chuyển khộng bị trôi theo thời gian.
R = ∑2999𝑖=0 (𝑅 + 𝐺(𝑖)) ( 3.6 )
Offset = 𝑅
3000 ( 3.7 )
Gy = Gy - Offset ( 3.8 )
Trong đó R là tổng giá trị sau mỗi lần đọc G(i) là giá trị cảm biến tại thời điểm thứ i. Offset là giá trị trung bình sau 3000 lần đọc Gy là giá trị cảm biến đầu ra.
Bù được sử dụng để ước lượng góc quay dựa trên dữ liệu đọc được từ cảm biến của gia tốc và cảm biến con quay hồi chuyển. Bộ tích phân được dùng để tính góc
quay của IMU tại một thời điểm dựa trên sự thay đổi của vận tốc góc gyroData (0/s)
trong một khoảng thời gian dt (s) nhất định.
angle = angle + gyroData x dt ( 3.9 ) Khi kết hợp các thành phần của hệ thống trên ta có phương trình sau:
angle = a * (angle + gyroData x dt) + (1 - a) * accData ( 3.10 ) trong đó: angle - Góc cần xác định.
gyroData – Vận tốc góc đo được từ cảm biến con quay hồi chuyển (o/s). accData – Góc Dữ liệu đọc được từ cảm biến gia tốc (0).
dt - Chu kỳ lấy mẫu. a – Hệ số của bộ lọc.
Trong đó τ là hằng số thời gian. Hằng số thời gian (τ) được chọn sao cho phải lớn hơn khoảng thời gian của nhiễu điển hình trên cảm biến gia tốc, đồng thời giá trị này phải đảm bảo sao cho điểm gốc của cảm biến của cảm biến không bị lệch trong một khoảng chấp nhận được.
Đối với cảm biến IMU, thành phần đầu ra từ con quay hồi chuyển xảy ra hiện tượng trôi điểm khơng với vận tốc ±200/s trên tồn dải ±20000/s, như vậy hiện tượng trơi điểm khơng sẽ chiếm khoảng 1% tồn dải trong 1 giây. Để giảm hiện tượng trơi điểm khơng, chúng ta có thể chọn hằng số thời gian nhỏ hơn 1 giây. Chúng tôi chọn hằng số thời gian τ = 0.3s < 1s. Với tần số lấy mẫu là 250Hz, tương ứng với khoảng thời gian giữa 2 lần lấy mẫu là 0.004s. Hệ số của bộ lọc sẽ có giá trị như sau:
𝑎 = τ
τ+dt = 0.3
0.3+0.004 = 0.98 (3.11)
Khi đó cơng thức được viết lại như sau:
Sơ đồ nguyên lý bộ xử lý tín hiệu MPU – 6050
Nguồn cung cấp được lấy từ pin 3.7vDC, thông qua mạch sạc tăng áp cho điện áp ở ngõ ra là 5v với dịng tải là 1A. Nguồn cấp được đóng ngắt qua một cơng tắc gạt.
Các giá trị cảm biến MPU-6050 được xử lý và tính tốn thơng qua board Arduino Pro Mini, gửi tín hiệu điều khiển tới bộ xử lý tín hiệu điều khiển động cơ thơng qua module Bluetooth HC-05 với chuẩn giao tiếp UART.