陀螺仪和加速度传感器角度融合算法

加速度计和陀螺仪角度融合

一、预期结果

图中A-Angle：通过加速度计解算出的角度值，G-Angle: 通过加速度计解算出的角度数据，M-Angle：融合过后的角度值。

1、传感器在某一固定点绕Z轴进行范围为[ -90°, 90°]的转动

图1

从图1可看出，当传感器在静止（或做匀速运动）状态下进行角度变化时，通过加速度计解算出的角度是可靠的，由于系统刚开始运行（时间很短），因而通过陀螺仪解算出的角度也是可靠的。

2、传感器保持固定角度(0°)不变进行变速运动

图2

从图2可看出，在变速运动情况下，通过加速度计解算出的角度不可靠，陀螺仪的误差随着时间的增长而变大，亦不再可靠。而融合后的角度基本没有变化，可以得到融合模块对加速度计和陀螺仪的数据进行了有效的融合。

算法并不是直接融合通过加速度计和陀螺仪解算得到的角度，而是先用通过加速度计得

到的角度和上一次融合后的角度对陀螺仪的角速度进行了修改后再积分，有效地抑制了加速度计和陀螺仪的偏差。

融合模块的调用采用中断方式，中断时间为5ms。中断服务函数内容如下：

Angle_new=Angle_last+((θ-angle)×n +(w-offset))×t;

Angle_last=Angle_new;