四旋翼和云台都离不开加速计和陀螺仪。加速计提供实时的重力加速度方向,进而可以计算出机体姿态角度。陀螺仪提供实时的旋转角度,通过积分也能换算成机体姿态角度。加速计对机体振动非常敏感,稍有扰动,加速度方向就会偏移(外力与重力的合力),但是长期运行时,经过平滑滤波算法之后的加速计数据就比较可信。相比而言,陀螺仪则对外部扰动不敏感,反而是长时间积分后会出现角度漂移。因此实际应用中,都是将二者采集的数据进行融合,相互取长补短。
下面这段代码是网上比较常见的6轴融合算法,原作者只添加了几句简单的英文注释,几处关键段落阅读起来非常费解。而网上的大多数引用页面对这段代码也都没有做进一步的分析,我怀疑大家也都是一知半解,拿来就用,用得顺手便没有再去深究。这个周末我仔细研究了一下,对各关键公式进行了理论推导,供大家参考。三个关键段落的推导过程详见后文Note1,Note2,Note3。
void IMUupdate() {
float norm;
float vx, vy, vz;
float ex, ey, ez;
float ax,ay,az,gx,gy,gz;
float q0,q1,q2,q3;
float q0temp,q1temp,q2temp,q3temp;
ax=Data.pSensor->accel[0];
ay=Data.pSenso