导航及无人机姿态检测融合算法很难?用sensor fusion来搞定!
时间:08-05
来源:意法半导体AMG
点击:
在后的后计算相乘的公式如下图:
至此初始化四元数完成。
第三步就是使用AHRSUpdate算法了,用完以后再根据公式计算出欧拉角,此公式跟旋转顺序和旋转使用的轴向有关,我们的旋转顺序是Z-X-Y,且绕Z是Yaw,绕X是Pitch,绕Y是Roll,推到过程如下图:
首先得出3个方向余旋矩阵:
下图绕Z轴Yaw:
下图绕X轴pitch:
下图绕Y轴Roll:
然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw,如下图:
将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比,即可求出欧拉角,注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的,而下图的四元数姿态矩阵是固定的:
最后一步就是求出欧拉角,公式如下图:
以下是静止时,刚上电时的数据:
以下是上电半个小时以后的数据:
可以看出正常的静止状态下,数据的波动范围是不超过1度的,也不会有漂移。具体效果还需要上机架飞起来以后再做调整。
最后总结一下:
我的代码中,0° 注意跑108Mhz时,需要修改延迟函数和串口函数,适当增加一下延迟函数的数值避免I2C通信失败,修改串口函数是为了避免108Mhz下串口乱码问题
地磁传感器 运动传感器 3轴加速度计 3轴陀螺仪 意法半导体 相关文章:
- 详解地磁感应器原理/应用及优缺点(09-09)
- 飞思卡尔MMA955xL运动传感器大比拼(09-20)
- MEMS传感器种类及国内外生产厂商(10-02)
- 意法半导体:混合动力及电动汽车解决方案(02-02)
- 立足整个信号链需求 意法半导体音频解决方案(02-07)
- 意法半导体数字电视和机顶盒解决方案(02-07)