6轴陀螺仪姿态解算_python 根据陀螺仪输出数据计算轨迹

之前看过学长姿态解算相关代码，因为要做平衡车的项目，希望陀螺仪处理数据能够达到很好的效果，大概2个星期前，看的学长代码，当时把大部分代码看懂是用来干什么的，但原理还是一窍不通，没办法，太高深了hhhhh。用学长的代码很顺利就完成了基本工作，但当时调pid看上位机时看到波形是一堆毛刺，把我的强迫症激起来了，但当时急着调pid，学长的代码就将就着用吧。

最近手头的急事都没了，静下心来，想好好姿态解算完善一下，比如加个滤波，我这才发现我都不知道应该在哪加滤波的。之前是用的mahony互补滤波+四元数求解角度。学长用的姿态解算好像都是祖传的，今天仔细找找网上的代码，发现祖传代码还有点细节上的小问题，直立组学长用的滤波我也认不出来名字，其他有的学长用的是一阶低通滤波。

好吧，这个姿态解算是第二个问题，第一个问题是怎么处理陀螺仪原始数据，我就从小小小小小白的层面来讲，因为我现在也是一个小小小小白。首先陀螺仪读取三轴角速度数据，加速度计读取三轴加速度数据，理想情况下，三轴角速度均为0，2轴加速度为0，其中竖直方向始终有个重力加速度。所以竖直方向的加速度对角速度没有互补作用，导致绕竖直方向旋转的角度（偏航角）不准，和陀螺仪的x，y，z轴没关系。（即使陀螺仪竖直放，x或y轴到竖直面上，绕竖直方向旋转的角度依然不准）。

1.滤波
输入级使用算数平均滤波就可以，取十次求平均一般比较稳定。如果要使用加速度，可以再加一个系数比较小的低通滤波。
角速度记得去掉零点偏移值,角速度不用去除零点漂移。

加速度计的噪声较大，所以最好用个滤波处理，查到的有8层窗口滑动平均滤波，效果似乎不错，我用的是16层滑窗感觉效果更好，还有FIR、IIR等的滤波算法，我还没开始用，用滤波器的话有一点要注意：滤波器是会造成延迟的，延迟对控制系统的影响是很大的，所以千万不能为了追求滤波效果，造成大的延迟，通常不要超过一个周期最好。一阶低通效果就足够了，当然也可以设计IIR、FIR之类的。

我之前找滤波算法，花了几天时间，真浪费时间，其实这个原始数据不做滤波应该也是可以的，pid调好就行了，不过因为我的强迫症，非想找个滤波，那毛刺信号看的真的烦人，结果卡尔曼滤波又看不懂，只能用滑动窗口了。

然后就是网上遍地流传的四元数姿态解算，关键是有的代码都不一样，大家对比一下，整理一下。这部分不可避免要看一大堆的博客。

2.角度融合
角速度积分得到的角度比较稳，但会随时间慢慢漂移且只反映变化量；加速度取反三角函数得到的角度不稳，但跟随真实角度，且是对地的绝对角度。
平衡车整个过程都要用到角度这个变量，单纯角速度积分带来的漂移偏差不可忽视，而且参考点不好确定，所以需要用角度融合，将两者得出的角度进行滤波融合，取长补短，就能得到比较可靠的角度。

角度融合滤波的方法有：一阶互补滤波，卡尔曼滤波、四元数mahony滤波。

很重要的一点是，我一开始陀螺仪水平放，用学长的代码完全问题不大，然后我陀螺仪竖直放，结果原来代码用不了了，因为会出现角度跳变问题，然后我捣鼓了半天代码改不了，甚至去找了asin，atan2函数的用法，都没用，最后我还是放弃了，把陀螺仪位置改了。