全国各地现已连续敞开低空操控，北京也将在2015年全面敞开低空范畴，这对低空飞翔器将是一个非常严重的好消息!低空飞翔器也将迎来一个新的开展春天。实际上，近年四轴飞翔器开展适当敏捷，国内的航拍水平越来越高，顺丰及亚马逊已在测验将无人机用于快递职业。越来越多的人开端重视并研讨四轴飞翔器。
本文将剖析一种常见的四轴飞翔器姿势解算办法，Mahony的互补滤波法。此法简略有用，期望能给学习四轴飞翔器的朋友们带来协助。关于姿势解算和滤波的理论知识，引荐秦永元的两本书，一是《惯性导航》，现在已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论根底，可在书中寻觅。
一起欢迎到论坛发帖沟通：http://forum.eepw.com.cn/forum/368/1
下面开端进入正题：
先界说Kp，Ki，以及halfT 。
Kp，Ki，操控加速度计批改陀螺仪积分姿势的速度
halfT ，姿势解算时刻的一半。此处解算姿势速度为500HZ，因而halfT 为0.001
#define Kp 2.0f
#define Ki 0.002f
#define halfT 0.001f
初始化四元数
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
界说姿势解算差错的积分
float exInt = 0, eyInt = 0, ezInt = 0;
以下为姿势解算函数。
参数gx，gy，gz别离对应三个轴的角速度，单位是弧度/秒;
参数ax，ay，az别离对应三个轴的加速度原始数据
因为加速度的噪声较大，此处应选用滤波后的数据
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
将加速度的原始数据，归一化，得到单位加速度
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
把四元数换算成“方向余弦矩阵”中的第三列的三个元素。依据余弦矩阵和欧拉角的界说，地舆坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这儿的vx、vy、vz，其实便是当时的机体坐标参照系上，换算出来的重力单位向量。(用表明机体姿势的四元数进行换算)
vx = 2*(q1*q3 – q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 – q1*q1 – q2*q2 + q3*q3;
这儿阐明一点，加速度计因为噪声比较大，并且在飞翔过程中，受机体振荡影响比陀螺仪显着，短时刻内的可靠性不高。陀螺仪噪声小，可是因为积分是离散的，长时刻的积分会呈现漂移的状况，因而需求将用加速度计求得的姿势来纠正陀螺仪积分姿势的漂移。
在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿势来推算出的重力向量是vx、vy、vz;它们之间的差错向量，便是陀螺积分后的姿势和加速度计测出来的姿势之间的差错。
向量间的差错，能够用向量积(也叫外积、叉乘)来表明，ex、ey、ez便是两个重力向量的叉积。这个叉积向量仍旧是坐落机体坐标系上的，而陀螺积分差错也是在机体坐标系，并且叉积的巨细与陀螺积分差错成正比，正好拿来纠正陀螺。因为陀螺是对机体直接积分，所以对陀螺的纠正量会直接表现在对机体坐标系的纠正。
叉乘是数学根底，百度百科里有具体解说。
ex = (ay*vz – az*vy);
ey = (az*vx – ax*vz);
ez = (ax*vy – ay*vx);
将叉乘差错进行积分
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
用叉乘差错来做PI批改陀螺零偏，经过调理Kp，Ki两个参数，能够操控加速度计批改陀螺仪积分姿势的速度
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
四元数微分方程，没啥好说的了，看上面引荐的书吧，都是理论的东西，自个揣摩揣摩
真实揣摩不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最终转化成欧拉角即可了。不过主张仍是把理论弄清楚一点。
q0 = q0 + (-q1*gx – q2*gy – q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz – q3*gy)*halfT;
q2 = q2 + (q0*gy – q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy – q2*gx)*halfT;
四元数单位化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
姿势解算后，就得到了表明姿势的四元数。但四元数不行直观，一般将其转化为欧拉角。转化时依据旋转轴的次第不同，公式也不同。以下给出其间一种公式：
读完程序，深入的认识到了理论根底的重要性。Mahony的互补滤波函数，的确很奇妙，使用叉乘差错来批改四轴的姿势，姿势解算速度越快，则解算的精度越高。在许多国内开源程序中，也是用到了这种办法。在解四元数微分方程时，该程序用到了一阶毕卡解法。相同可用于解四元数微分方程的还有龙格-库塔法，因为篇幅有限，此处就不介绍龙格-库塔法了，有爱好的网友请自行查阅相关材料。
