下面是我对匿名源码修改整理后得到的代码,去掉了冗余便于讲解,此外给出了一些优化建议。
一、函数调用
二、Drv_Icm20602_Read()
读取16位ADC采样值
三、imu_origDataPrepare()
因为传感器安装角度和载体坐标系可能不一致,因此需要一个基础的坐标转换,最终得到x前、y右、z下的载体坐标系。
因为加速度计对高频抖动敏感,因此需要一个低通滤波器。陀螺仪加一个低通滤波无伤大雅。
当传感器不在载体中心时,载体旋转时传感器会有一个向心加速度,该加速度会被传感器测量出来,因此要去掉。
将16位ADC转换为物理单位值,对于该例程为±16g的加速度,单位是cm/s^2,就是说重力加速度测出来的值是980;角速度为±2000dps(degree per second)。
注:
之后使用时建议根据场景降低量程,提高精度
四、imu_postureSolution()
姿态解算的基本思路就是更新四元数,并且利用四元数到旋转矩阵的公式更新旋转矩阵。
下面是具体的实现:
初始时要求使模长为1,然后通过迭代得到正确的四元数。
这个是旋转矩阵的定义,最终求解欧拉角是利用该旋转矩阵。
读取载体坐标系下的加速度,并单位化。
利用四元数求旋转矩阵,同时也获得了世界坐标系下重力加速度(0,0,1)T在载体坐标系下的各轴分量。
求PI积分值。这里使用测量的角速度加上误差。由于误差是叉积,无量纲,因此可以理解为与角速度同一量纲,然后乘上时间就是更新的角度量。
说明:
1.初始四元数随意给定,因此刚开始会有一个较长的迭代时间将四元数收敛到正确的值。解决办法是刚开始(或判断无人机停止飞行时),将PI参数调大,使之快速收敛;
2.为了得到更好的效果,应当在初始开机后对加速度计、角速度计求零偏,即使用较长的时间求测量值的平均值,对之后的数据减去这个零偏。这个的好处包括:增大PI响应速度,在没有磁力计的情况下减小yaw漂移。
注:向量积无法区分0-90度和90-180度:
因此使用向量积作为误差,姿态角roll和pitch取值范围为±90度。
yaw之所以是±180度,是因为还增加了点乘判断向量方向:
五、imu_getEuler()
通过旋转矩阵求解欧拉角:
atan2相关知识汇总_weixin_42142612的博客-CSDN博客
/weixin_42142612/article/details/80972768
欧拉角和旋转矩阵的相互转换 - summer91 - 博客园
/larry-xia/p/11926121.html
如果觉得《匿名mahony互补滤波代码详解》对你有帮助,请点赞、收藏,并留下你的观点哦!