一种常见的四轴飞行器姿态解算方法分析 - 智能车制作

　全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。实际上，近年四轴飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。越来越多的人开始关注并研究四轴飞行器。

　　本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。

　　把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)

　　这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。

　　在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。

　　向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

　　用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度

　　四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨

　　实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。

　　姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转轴的次序不同，公式也不同。以下给出其中一种公式：

　　读完程序，深刻的意识到了理论基础的重要性。Mahony的互补滤波函数，确实很巧妙，利用叉乘误差来修正四轴的姿态，姿态解算速度越快，则解算的精度越高。在许多国内开源程序中，也是用到了这种方法。在解四元数微分方程时，该程序用到了一阶毕卡解法。同样可用于解四元数微分方程的还有龙格-库塔法，由于篇幅有限，此处就不介绍龙格-库塔法了，有兴趣的网友请自行查阅相关资料。