Гироскоп дрейфует слишком сильно

Question

Я пытаюсь создать квадрокоптер и использую MPU6050 для получения данных об углах для моего алгоритма ПИД. Тем не менее, даже с установленным дополнительным фильтром, я все еще чувствую большой дрейф на гироскопе, особенно на оси у. Если бы кто-нибудь мог предложить усовершенствования кода, чтобы минимизировать упомянутый дрейф, это было бы очень полезно.

'' '

from mpu6050 import mpu6050
import time
import math

imu = mpu6050.mpu6050(0x68)

gyro_angle_x,gyro_angle_y,gyro_angle_z = 0,0,0     
accelerometer_angle_x, accelerometer_angle_y = 0,0
comp_filtered_x,comp_filtered_y = 0,0

filter_const = 0.9
pi = 3.14159265358979323846
rad_deg = 57.29578
gyr_const = 131
acc_const = 4096.0
gyro_error_x = 0
gyro_error_y = 0

imu.set_gyro_range(250)
imu.set_accel_range(8)

dt, time_curr , time_prev = 0,0,0

time_curr = time.time()

i = 0
while i < 200:
    gyro_error_x += imu.read_i2c_word(imu.GYRO_XOUT0) / gyr_const
    gyro_error_y += imu.read_i2c_word(imu.GYRO_YOUT0) / gyr_const
    i +=1

gyro_error_x = gyro_error_x / 200
gyro_error_y = gyro_error_y / 200
while True:
    time_prev = time_curr
    time_curr = time.time()
    dt = (time_curr - time_prev)

    gyro_raw_x = (imu.read_i2c_word(imu.GYRO_XOUT0) / gyr_const) - gyro_error_x
    gyro_raw_y = (imu.read_i2c_word(imu.GYRO_YOUT0) / gyr_const) - gyro_error_y
    gyro_raw_z = imu.read_i2c_word(imu.GYRO_ZOUT0) / gyr_const

    gyro_angle_x += gyro_raw_x * dt
    gyro_angle_y += gyro_raw_y * dt
    gyro_angle_z += gyro_raw_z * dt

    accelerometer_raw_x = imu.read_i2c_word(imu.ACCEL_XOUT0) / acc_const
    accelerometer_raw_y = imu.read_i2c_word(imu.ACCEL_YOUT0) / acc_const
    accelerometer_raw_z = imu.read_i2c_word(imu.ACCEL_ZOUT0) / acc_const

    accelerometer_angle_x = math.atan((accelerometer_raw_y)/math.sqrt(pow((accelerometer_raw_x),2) + pow((accelerometer_raw_z),2)))*rad_deg
    accelerometer_angle_y = math.atan(-1*(accelerometer_raw_x)/math.sqrt(pow((accelerometer_raw_y),2) + pow((accelerometer_raw_z),2)))*rad_deg

    comp_filtered_x = (filter_const * gyro_angle_x) + ((1-filter_const) * accelerometer_angle_x)
    comp_filtered_y = (filter_const * gyro_angle_y) + ((1-filter_const) * accelerometer_angle_y)

    print(comp_filtered_x, "   ", comp_filtered_y)

' ''

Answer 1

Не думаю, что с этим можно что-то сделать. Дешевые гироскопы в MPU6050 просто недостаточно точны, чтобы предотвратить смещение. Для надежной ориентации вам понадобится что-то, по крайней мере, с датчиком magneti c для абсолютной ориентации, например, BNO055.