0

クワッドコプターを作ろうとしていて、MPU6050 を使用して PID アルゴリズムの角度データを取得しています。ただし、補完フィルターを配置しても、時間の経過とともにジャイロスコープ、特に y 軸で多くのドリフトが発生します。上記のドリフトを最小限に抑えるためにコードの改善を誰かが提案できれば、それは大歓迎です。

'''

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)

'''

4

2 に答える 2

0

私がしたことの 1 つは、ファイラー定数を 0.8 に変更したことです。ジャイロドリフトが大きくなりすぎることなく、十分に正確な答えが得られることがわかりました。多くの場合、サイトでは 0.97 が見つかりますが、それははるかに高品質の機器用だと思います。

于 2020-07-20T08:42:09.400 に答える