Xsens MTi-20传感器互补滤波器角度漂移问题求助

我使用互补滤波器融合Xsens MTi-20传感器的陀螺仪与加速度计数据，计算出的角度动态表现看似正常，但传感器静止时横滚角（roll）和俯仰角（pitch）会持续匀速增大。数据输出频率为100Hz，dt取值0.01；陀螺仪数据单位为rad/sec，加速度计数据单位为m/s²。相关代码片段如下：

import math
import time

DEG_TO_RAD = math.pi / 180
RAD_TO_DEG = 180 / math.pi
start_time = time.time()

accumulated_gyr_pitch = 0
accumulated_gyr_roll = 0

# 互补滤波器
def complimentary_filter(acc, gyr, dt, alpha):
    global accumulated_gyr_pitch
    global accumulated_gyr_roll

    angle_acc_pitch = math.atan2(acc[0], math.sqrt(acc[1]**2 + acc[2]**2)) * RAD_TO_DEG
    angle_acc_roll = math.atan2(acc[1], math.sqrt(acc[0]**2 + acc[2]**2)) * RAD_TO_DEG

    accumulated_gyr_pitch += gyr[1] * dt 
    accumulated_gyr_roll += gyr[0] * dt

    # 弧度转角度
    accumulated_gyr_pitch_deg = accumulated_gyr_pitch * RAD_TO_DEG
    accumulated_gyr_roll_deg = accumulated_gyr_roll * RAD_TO_DEG

    pitch = (1 - alpha) * (accumulated_gyr_pitch_deg) + (alpha * angle_acc_pitch)
    roll = (1 - alpha) * (accumulated_gyr_roll_deg) + (alpha * angle_acc_roll)
    return roll, pitch

def apply_filters(data_buffer):
    alpha = 0.25
    dt = 0.01
    current_time = time.time()
    elapsed_time = current_time - start_time

    with open('filtered_data.txt', 'w') as filtered_file:
        while keep_reading_data:
            if len(data_buffer) > 0:
                acc, gyr, mag, elapsed_time = data_buffer.pop(0)
                roll, pitch= complimentary_filter(acc, gyr, dt, alpha)
                print(f'Roll: {roll:.2f}, Pitch: {pitch:.2f}, Time: {elapsed_time:.2f}')
                filtered_file.write(f'Roll: {roll:.2f}, Pitch: {pitch:.2f}, Time: {elapsed_time:.2f}\n')
                filtered_file.flush()
            time.sleep(0.01)

静止时角度持续线性增大，横滚角和俯仰角随时间呈匀速上升趋势。

问题原因分析 #

• 陀螺仪零偏未校准：传感器静止时陀螺仪输出存在微小偏移（零偏），积分后会不断累积导致角度漂移。
• 滤波器逻辑错误：当前代码中陀螺仪积分是全局累加原始数据，未将融合后的角度作为下一次积分的基准，加速度计的修正作用无法有效抵消陀螺仪的累积漂移。
• alpha参数不合理：0.25的权重让加速度计仅占25%的修正比例，对漂移的抑制能力不足。
• dt固定值误差：用固定0.01s作为时间间隔，实际线程休眠和数据读取的误差会导致积分累积偏差。

解决方案 #

1. 校准陀螺仪零偏

   • 将传感器静止放置，采集10~30秒的陀螺仪数据，计算各轴数据的平均值作为零偏值。
   • 使用陀螺仪数据前，减去对应轴的零偏：

     # 示例：替换为实际校准得到的零偏值
     gyr_bias = [roll_bias, pitch_bias, yaw_bias]
     corrected_gyr_roll = gyr[0] - gyr_bias[0]
     corrected_gyr_pitch = gyr[1] - gyr_bias[1]
     

2. 修正互补滤波器逻辑
   改为基于融合后的角度进行陀螺仪积分，让加速度计持续修正漂移：

   import math
   import time
   
   DEG_TO_RAD = math.pi / 180
   RAD_TO_DEG = 180 / math.pi
   
   # 初始化融合角度，可从静止时的加速度计角度初始化
   fused_roll = 0.0
   fused_pitch = 0.0
   # 预校准的陀螺仪零偏
   gyr_bias = [0.0, 0.0, 0.0]  # 替换为实际校准值
   
   def complimentary_filter(acc, gyr, dt, alpha):
       global fused_roll, fused_pitch
   
       # 加速度计计算角度
       angle_acc_pitch = math.atan2(acc[0], math.sqrt(acc[1]**2 + acc[2]**2)) * RAD_TO_DEG
       angle_acc_roll = math.atan2(acc[1], math.sqrt(acc[0]**2 + acc[2]**2)) * RAD_TO_DEG
   
       # 陀螺仪零偏修正后积分更新角度
       gyro_roll_delta = (gyr[0] - gyr_bias[0]) * dt * RAD_TO_DEG
       gyro_pitch_delta = (gyr[1] - gyr_bias[1]) * dt * RAD_TO_DEG
       fused_roll += gyro_roll_delta
       fused_pitch += gyro_pitch_delta
   
       # 互补融合，用加速度计修正陀螺仪漂移
       fused_roll = (1 - alpha) * fused_roll + alpha * angle_acc_roll
       fused_pitch = (1 - alpha) * fused_pitch + alpha * angle_acc_pitch
   
       return fused_roll, fused_pitch
   

3. 调整alpha参数
   对于100Hz采样率，建议将alpha设为0.05~0.1之间（加速度计权重），增强对漂移的修正能力。可根据实际测试动态调整，平衡动态响应和漂移抑制的需求。

4. 使用真实时间间隔计算dt
   不要固定使用0.01s，而是根据相邻两次数据的时间戳差值计算真实dt：

   def apply_filters(data_buffer):
       alpha = 0.08
       last_time = time.time()
   
       with open('filtered_data.txt', 'w') as filtered_file:
           while keep_reading_data:
               if len(data_buffer) > 0:
                   acc, gyr, mag, elapsed_time = data_buffer.pop(0)
                   current_time = time.time()
                   dt = current_time - last_time
                   last_time = current_time
                   roll, pitch = complimentary_filter(acc, gyr, dt, alpha)
                   print(f'Roll: {roll:.2f}, Pitch: {pitch:.2f}, Time: {elapsed_time:.2f}')
                   filtered_file.write(f'Roll: {roll:.2f}, Pitch: {pitch:.2f}, Time: {elapsed_time:.2f}\n')
                   filtered_file.flush()
               time.sleep(0.001)
   

内容的提问来源于stack exchange，提问作者Chris_xx