互补滤波算法在姿态测量中的应用

一、加速度计测量角度原理

以xOz平面为例，当姿态传感器发生倾斜，传感器重力作用在X轴与Z轴的分量分别为ax与az，俯仰角

加速度计测量三轴角度存在的问题，当物体存在运动加速度时，使得姿态角测量不准确，角度会存在偏移。

二、陀螺仪测量角度原理

陀螺仪测量角速度，系统每隔时间T采样一次绕X、Y轴旋转的角速度，通过积分计算得到角度。假设初始俯仰角为

存在问题：陀螺仪测量的角度是通过积分获取，通过存在零偏，如果不加入修正，其测量的角度误差会逐渐增大，此外，陀螺仪测量的是相对值，因此通常需要装订初始角度。

三、互补滤波算法

互补滤波以陀螺仪为主，加速度计为辅，通过参考加速度计角度来修正陀螺仪的漂移，为陀螺仪提供绝对参考，互补滤波后的角度去加速度计测量角度和陀螺仪角度的加权平均值：

四、MPU6050测量角度计算及互补滤波算法代码

/*获取陀螺仪加速度计原始数据*/ MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ); /*陀螺仪数据校准*/ GY_Cali = GY + GY_Offset; /*计算加速度计角度*/ AngleAcc = -atan2(AX, AZ) / 3.1415926535 * 180; /*计算陀螺仪角度增量*/ AngleDelta = GY_Cali / 32768.0 * 2000.0 * (ANGLE_T / 1000.0); /*中心角度校准*/ AngleAcc_Cali = AngleAcc + AngleAcc_Offset; /*对加速度计角度进行一阶低通滤波，使其更平滑*/ float Alpha0 = 0.8; AngleAcc_Filter = Alpha0 * AngleAcc_Filter + (1 - Alpha0) * AngleAcc_Cali; /*角度累加陀螺仪角度增量，得到新的角度值*/ Angle += AngleDelta; /*新的角度值与加速度计角度进行互补滤波，抑制漂移*/ float Alpha1 = fabs(DifSpeed) / 5.0 * 0.02 + 0.005; //根据差速动态调整滤波参数 if (Alpha1 > 0.02) {Alpha1 = 0.02;} //参数限幅 Angle = Alpha1 * AngleAcc_Filter + (1 - Alpha1) * Angle;//互补滤波得到角度值