Filter 陀螺仪漂移太大

我正在尝试制作一架四旋翼直升机，我正在使用MPU6050为我的PID算法获取角度数据。然而，即使有了互补滤波器，随着时间的推移，我仍然会在陀螺仪上产生很大的漂移，尤其是在y轴上。如果有人能建议对代码进行改进，以最大限度地减少上述偏差，我们将不胜感激

'''

从mpu6050导入mpu6050
导入时间
输入数学
imu=mpu6050.mpu6050（0x68）
陀螺角x，陀螺角y，陀螺角z=0,0,0
加速计角度x，加速计角度y=0,0
comp_filtered_x，comp_filtered_y=0,0
过滤器常数=0.9
pi=3.14159265358979323846
弧度=57.29578
回转常数=131
acc_const=4096.0
陀螺误差x=0
陀螺误差y=0
imu.设置陀螺仪量程（250）
imu.设置加速范围（8）
dt，时间当前，时间当前=0,0,0
time\u curr=time.time（）
i=0
当我<200时：
陀螺误差=imu.read\uI2C\u字（imu.gyro\uXOUT0）/gyr\u常数
陀螺误差y+=imu读取i2c字（imu陀螺YOUT0）/gyr常数
i+=1
陀螺误差=陀螺误差/200
陀螺误差=陀螺误差/200
尽管如此：
时间\上一次=时间\当前
time\u curr=time.time（）
dt=（当前时间-当前时间）
陀螺仪原始值x=（imu.read\u i2c\u字（imu.gyro\u XOUT0）/gyr\u常量）-陀螺仪误差x
陀螺原始y=（imu.读取i2c字（imu.陀螺YOUT0）/陀螺常数）-陀螺误差y
gyro_raw_z=imu.read_i2c_word（imu.gyro_ZOUT0）/gyr_const
陀螺仪角度=陀螺仪原始角度*dt
陀螺_角度_y+=陀螺_原始_y*dt
陀螺仪角度=陀螺仪原始角度*dt
加速计\u原始\u x=imu.读取\u i2c\u字（imu.加速度\u XOUT0）/acc\u常数
加速计原始y=imu.读取i2c字（imu.加速度YOUT0）/acc\常数
加速计原始z=imu.读取i2c字（imu.加速计ZOUT0）/加速度常数
加速计角度=math.atan（（加速计原始值）/math.sqrt（功率（（加速计原始值），2）+功率（（加速计原始值），2））*rad_deg
加速计角度y=数学atan（-1*（加速计原始x）/数学sqrt（功率（（加速计原始y），2）+功率（（加速计原始z），2））*弧度
comp_filtered_x=（滤波器常数*陀螺角）+（1-滤波器常数）*加速计角
comp_filtered_y=（滤波器常数*陀螺角）+（（1-滤波器常数）*加速计角）
打印（comp_filtered_x，“，comp_filtered_y）

---

''

我认为你对此无能为力。MPU6050中的廉价陀螺仪不够精确，无法防止漂移。为了获得可靠的定位，您需要至少有一个用于绝对定位的磁性传感器，例如BNO055

我认为你对此无能为力。MPU6050中的廉价陀螺仪不够精确，无法防止漂移。为了获得可靠的定位，您需要至少有一个用于绝对定位的磁性传感器，例如BNO055

我所做的一件有帮助的事情是将文件管理器常数更改为0.8。我发现这在不考虑太多陀螺漂移的情况下给出了足够准确的答案。通常在网站上你会发现0.97，但我认为这是为了更高质量的设备。

我做了一件有帮助的事情，就是将文件管理器常数更改为0.8。我发现这在不考虑太多陀螺漂移的情况下给出了足够准确的答案。通常在网站上你会发现0.97，但我认为这是为了更好的质量设备

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)