IMU原理及姿态融合算法详解
IMU原理及姿态融合算法详解
一、组成
IMU全称是惯性导航系统，主要元件有陀螺仪、加速度计和磁力计。其中陀螺仪可以得到各个轴的加速度，而加速度计能得到x，y，z方向的加速度，而磁力计能获得周围磁场的信息。主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。
二、原理
a）陀螺仪
陀螺仪是通过测量科氏力来检测角速度的，科氏力在大学物理中提到过，如图
一个物体以固定的线速度v运动，同时受到一个角速度的影响，这时候在叉乘方向会有一个科氏力的作用，测量这个力便能直到角速度W的大小。
在实际的MEME传感器中，大致结构如图，在一个方向保持左右运动，若有旋转的角速度则会在垂直的方向产生科氏力，通过电容的变化来反应这个力的大小便能得到旋转速度的大小。
a）加速度计
加速度计的原理较为简单，就是通过牛顿第二定律来测量三轴的加速度，图中的质量块受到加速度的作用会左右运动，而两侧的电容能测量质量块的位置从而计算出加速度的大小。
a）磁力计
磁力计则是通过霍尔效应来测量磁场的强度，高中物理中学过霍尔效应也很简单，如图。一端通电，在磁场的作用下电子会往垂直的方向上跑从而在侧面产生电场，通过测量这个电场的强度及正负则能间接测量出场强的大小。
视频介绍如下链接
https://www.youtube.com/watch?v=eqZgxR6eRjo&feature=youtu.be
三、旋转的表达
a）欧拉角
对姿态的描述，最直观的便是欧拉角了。实际上一个动图便能十分直观的解释清楚了
最直观的感受就是分别绕三个轴进行旋转便是了。具体的可以参考知乎回答，如何通俗的解释欧拉角？
https://www.zhihu.com/question/47736315
b）旋转矩阵
在线性代数中已经学过，3*3的矩阵可以表达物体的旋转，如绕z轴的旋转
具体的对于绕其他轴的旋转的表达可以参考博客
https://blog.csdn.net/csxiaoshui/article/details/65446125
c）四元数
四元数其实是很奇怪的事情，但是却在数学上能优雅而完美的表达出三维空间中刚体的旋转。它很不直观但是却很有效，大部分效果较好的解算算法都是采用四元素解算的。
四元数的运算类似于矩阵运算，却有所不同。具体的运算有英文书籍《Understanding Quaternions》https://www.3dgep.com/understanding-quaternions/ ，当然国内也有翻译版本 https://www.qiujiawei.com/understanding-quaternions/
这里主要介绍如何通过四元数来更新姿态。
**已知当前姿态为四元数q1，在dt时间内角速度为w，求下一时刻的四元数 **
一般来说采用一阶龙格库塔法来更新四元数，主要的思路便是泰勒展开然后一阶近似。具体推导过程参考博客，具体的计算流程如下
https://blog.csdn.net/AASDSADAD/article/details/73080832
d）SO(3)及so(3)
对于这两种表达方法在主流陀螺仪的姿态解算中并不常用，但是在某些算法中需要对姿态进行求导数时便需要采用李群的方法去表达姿态。比如对旋转的求导如下
这部分内容较为复杂，如需深究可参考高翔博士的《视觉slam14讲》
博客：https://www.cnblogs.com/gaoxiang12/p/5137454.html
视频：https://www.bilibili.com/video/av51238804/?p=3
四、传感器的噪声及去除
传感器的噪声一般分为随机噪声，一般认为随机噪声是符合高斯分布的。另外一种则是由于传感器的测量原理导致的固定误差，这部分是去噪的重点。实际上由于制造和安装误差，会有许许多多的固定误差但是由于使用要求不高，且大部分校准需要高精度转台，只能做较为简单的校准工作。
a）陀螺仪
陀螺仪直接测量的是角速度而非角度，所以需要通过一次积分才能得到角度值。在积分的过程中若有固定的、某一个方向的数据则会在积分的过程中不断加大影响导致角度偏差。通常来说陀螺仪的温漂是比较严重的，基本上温漂是正比于芯片的价格，越贵的片子漂的越少。温漂的数据既与温度相关又与时间相关，也就是说不同温度下不一样，不同上电时间下也不一样。通常的简单做法就是在上电的时候静止一段时间计算出此时的零偏，然后每次减去零偏。更高级的方法需要标定温度与零偏的关系，然后线性插补，另一方面使用艾伦方差分析法得到零偏和时间的关系。（艾伦方差法见博客https://blog.csdn.net/yandld/article/details/81101984） 对于其他的误差比如三轴不相互垂直，以及尺度因子不一致等误差都可以忽略。当然更好的情况是在电路上做一个温度控制，维持温度在50度左右（必须要在常温以上）。
b）加速度计
对于加速度机同样会有零漂和尺度因子的误差，但是加速度计在静止时可直接得到角度不用积分，所以零漂的影响很小，但是尺度因子的影响较大。同样是重力加速度、各个面朝下时检测到的数值是不一样的，一般来说校准的方法有六面校准。就是各个面朝下然后记录重力的数值，计算得到尺度因子。目前mems传感器的精度已经比较高了，很多情况下只用正面朝上校准一次便可（仅适用于无人机）若要求不高，可不去校准加速度计，而对于云台有其他的校准思路。
c）磁力计
磁力计的数据误差较大，校准便显得很重要。一般可以导出数据到matlab中然后采用椭球校准的方法，但是这样比较麻烦，主要用于写论文。。。而大多数飞控的做法都是直接在单片机上处理的，步骤如下：先头朝上，水平旋转一周，然后头朝下，再水平旋转一周。若计算能力有限，可直接求最大最小数据的中值，得到偏差，然后计算幅值。
save.mag_offset[i] = 0.5f *(max_t[i] + min_t[i]);//中值校准
save.mag_gain[i] = safe_div(200.0f ,(0.5f *(max_t[i] - min_t[i])),0);//幅值校准
若计算能力较充裕，采用LM算法可计算出三维的偏差和三维的尺度因子，具体参考天穹飞控代码。
椭圆校准博客：https://blog.csdn.net/xian_z/article/details/78463905
五、姿态解算原理
IMU的算法紧紧的围绕着如何利用这三个元器件获得准确的姿态，基本要求有几点
- 滞后效应不明显
- 角度准确
- 静止时角度不漂
但很多时候都无法满足所有的要求，需要根据实际情况的需求来有所取舍。
a）陀螺仪
陀螺仪获得角度的方法很简单，直接积分就好了。但是直接积分却会带来巨大的误差！！！
第一个原因如图
解决方法如下，采用中值积分
另一个方面，陀螺仪得到的旋转数据是基于机体坐标系的而我们要求的是世界坐标系下的姿态，这中间必然有一个坐标变换的关系。
对于无人机来说pitch和roll都比较小，可以认为是零，则旋转矩阵变成了单位矩阵，但是对于云台和其他应用场景下都很难做这个近似，写代码的时候切勿直接积分了。
b）加速度计
加速度计的作用便是直接测量出pitch和roll角度，对积分得到的角度做一个矫正。基本的计算原理见博客http://www.starlino.com/imu_guide.html ，一般网上都采用互补滤波来综合加速度计角度不会漂但是噪声大而陀螺仪角度精确却会漂的特点。但是加速度计计算角度的公式是有前提的，便是物体静止，对于剧烈运动的物体则会引起巨大的误差。这个时候有一个很重要的工作便是做好加速度的补偿，一般分为线加速度的补偿和角加速度的校准。对于线加速度，很难得到有飞控源码中采用辅助姿态解算算法来做补偿（见天穹飞控源码），一般的处理方法可以计算角速度计的模长，若越大则加速度的置信越小。对于角加速度可以通过陀螺仪来计算得到，得到角加速度后可以计算出切向加速度和法向加速度分别补偿。
c）磁力计
磁力计可以看成一个指南针，可以对yaw轴角度进行一个矫正。具体的计算公式也比较简单，注意在套用公式前需要对数据进行倾斜补偿。具体见博客
https://blog.csdn.net/w8253497062015/article/details/79833321
但是实际的工程源码中都对公式进行了简化，这一点需要注意。
六、姿态解算算法
姿态解算算法五花八门，各种各样的都有，同一种算法还可以用不同的旋转表达方法实在是多种多样。所以要用好一个算法需要对原理深入的理解，看到算法的最根本的东西，虽然很难确实必须要做的。
a）互补滤波
互补滤波我就不多说了，百度下一堆博客。思想在于把陀螺仪的高频部分和磁力计或加速度计的低频部分叠加在一起得到更准确更稳定的姿态。
b）AHRS
AHRS一般有两种思路Madgwick和mahony，具体的各种google就能得到你想要的。这里只讲思想
首先是飞机较常用的mahony算法，算法应用在pix上较为成熟和经典
其次是20012年开源出来的madgwick算法，效果更好计算量也更大。
这两种算法需要仔细阅读其论文。
c）卡尔曼
卡尔曼不仅仅用在陀螺仪的姿态解算上，所以对卡尔曼滤波本身的理解便十分重要。推荐的网站有，https://nbviewer.jupyter.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb 可以通过直观的动画来理解卡尔曼。
对于卡尔曼算法可以建立不同的模型来估计当前姿态，其中一种较为直观简单的是互补滤波的推广
预测量：陀螺仪的积分角度
观测量：加速度计和磁力计分别计算出的roll、pitch和yaw
具体参考博客https://www.zhihu.com/question/52030383/answer/146476328
七、云台的特性及要求
1、roll轴角度几乎为零而pitch轴角度较大
2、安装位置不在中心，角加速度对加速度计影响大
3、机器人起停时有较大的线加速度，对加速度计影响大
4、yaw轴零点位置要求不严格，而pitch轴要求零点必须水平
对于云台的yaw轴我基本上采用来AHRS_madgwick 的思路，但是进一步的把互补滤波改成了卡尔曼滤波。但是由于当时对卡尔曼理解不够，反倒是按原论文互补滤波动态效果更好，但是静态效果依旧是卡尔曼更优，所以采取了不同情况下取不同算法的数据的方法。
陀螺仪的算法基本可以参考madgwick的算法思路但是最好能自己实现，改进思路可参考另外一个文档：陀螺仪算法的改进思路。
对于陀螺仪算法最最重要的点在于。
1、利用现有参数对算法进行较好的初始化
2、尽量根据使用场景情况在算法推导过程中做简化，再根据自己的推导写代码
3、误差的校准，加速度的补偿