消除误差的方法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料,本设计采用互补滤波法,达到的效果也比较好。
算法核心思想:利用加速度计来补偿陀螺仪,希望能最大限度消除误差,进行准确解算机体姿态。
首先明确两个概念:四元数转欧拉角,四元数与旋转矩阵变换。http://blog.csdn.net/xoyojank/article/details/5682944
四元数转欧拉角公式:
arctan和arcsin的结果是,这并不能覆盖所有朝向(对于
角
的取值范围已经满足),因此需要用atan2来代替arctan。
四元数与旋转矩阵变换:
这里表述可能有一些让人不是很容易理解的地方,其实就是在理想条件下,我们是只需要陀螺仪来做姿态解算就可以了,如果没有任何干扰,芯片性能也优秀,那么用陀螺仪解算出的角速度积分就可以得到角度,不需要加速度计。但是这是不可能的,传感器的测量误差(主要是陀螺仪的积分误差和振动引起的加速度计误差),会造成测量值的不准确,如果刚开始陀螺仪误差为0.1,那么积分一段时间后可能变成10,所以我们要进行修正和补偿,陀螺仪具有良好的短时间测量准确度(希望得到高频值),加速度计有良好的长时间测量准确度(希望得到低频值),那么正好合并使用,简称数据融合。
介于资料和难度,那么一般选用互补滤波,即A=B*K+C*(1-K)。
由于两个传感器都有误差,那么我们算出他们之间的误差向量,再将误差向量补偿给陀螺仪,这里使用PI控制器来补偿,在准确性和时效性上可能更好一些,但是道理是一样的,最后用一阶比卡算法更新四元数,将算好的四元数转化为欧拉角计算。有一些地方需要良好的数学基础知识,我不能百分之百理解,但解算思路没问题。