互补滤波公式

我的四轴飞行器经验总结(四)--互补滤波法进行姿态解算详解

2017年02月26日 09:00:20 阅读数：3419

消除误差的方法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料，本设计采用互补滤波法，达到的效果也比较好。

算法核心思想：利用加速度计来补偿陀螺仪，希望能最大限度消除误差，进行准确解算机体姿态。

首先明确两个概念：四元数转欧拉角，四元数与旋转矩阵变换。http://blog.csdn.net/xoyojank/article/details/5682944

四元数转欧拉角公式：

  arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角的取值范围已经满足)，因此需要用atan2来代替arctan。

 

四元数与旋转矩阵变换：

 

这里表述可能有一些让人不是很容易理解的地方，其实就是在理想条件下，我们是只需要陀螺仪来做姿态解算就可以了，如果没有任何干扰，芯片性能也优秀，那么用陀螺仪解算出的角速度积分就可以得到角度，不需要加速度计。但是这是不可能的，传感器的测量误差（主要是陀螺仪的积分误差和振动引起的加速度计误差），会造成测量值的不准确，如果刚开始陀螺仪误差为0.1，那么积分一段时间后可能变成10，所以我们要进行修正和补偿，陀螺仪具有良好的短时间测量准确度（希望得到高频值），加速度计有良好的长时间测量准确度（希望得到低频值），那么正好合并使用，简称数据融合。

介于资料和难度，那么一般选用互补滤波，即A=B*K+C*(1-K)。

由于两个传感器都有误差，那么我们算出他们之间的误差向量，再将误差向量补偿给陀螺仪，这里使用PI控制器来补偿，在准确性和时效性上可能更好一些，但是道理是一样的，最后用一阶比卡算法更新四元数，将算好的四元数转化为欧拉角计算。有一些地方需要良好的数学基础知识，我不能百分之百理解，但解算思路没问题。