6轴陀螺仪姿态解算

之前看过学长姿态解算相关代码,因为要做平衡车的项目,希望陀螺仪处理数据能够达到很好的效果,大概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函数的用法,都没用,最后我还是放弃了,把陀螺仪位置改了。

你可能感兴趣的:(嵌入式硬件)