四轴项目总结之二-姿态，数据篇
本来想一篇博文写完的，不知怎的，卡死了，遂另开一篇，接下来还有一篇，分三篇吧，这张主要介绍飞行，飞控原理。
先给大家分享一个空间角度上的概念。
roll翻滚角
pitch俯仰角
yaw 航向角
飞行器能上下前后左右飞行,甚至一些高难度的动作都是依赖于这几个角度的精准控制，试着想象一下，前后左右飞，遥控器的各个遥感应该怎样打。你要实现的一些基本操作包括：
自稳
释放操纵杆之后飞机将会停止原有的飞行模式并悬停在一个稳定的位置。
电传操纵训练
某些飞行姿态可以直接通过飞控设定的，而避免了各种机械传动装置的复杂性。比如你可以自行设定横滚、航向和俯仰角度的最大最小值及其变化速率的快慢
自动飞行
模型可以按照你预先设定好的路径飞行，当然，你可以手动干预飞行路线。
自动返航（RTL
飞行器可以按照预设的路线自动返航至起始飞行点并保持在预设高度悬停，这个功能需要GPS支持。
悬停
如果不给飞行器任何的控制信号，那么飞行器将会尽可能保持原有的飞行姿态并保持原有的位置不变
自动启动和着陆
或许这是避免坠机的最好方式了。。。
利用一些外部模块或者采用其他飞控都可以实现更多不同的功能，采用什么飞控和外设取决于你需要什么功能
要实现上面的功能自然少不了各种各样的传感器，
基本的有AHRS采集加速度和角速度，超声波或气压计采集高度信息，GPS采集位置信息。其中前两种楼主有研究过，GPS暂时木有研究，先不予讨论。
AHRS
首先，它能采集到xyz三个轴的角速度和角加速度，至于xy轴和roll,pitch,yaw是怎样对应的，依赖于软件的实现，典型的有x型和+型，其中xy轴用的较多，z轴的数据主要用来防止飞行器发生自旋。有了这些参数之后，如果pid调的较好就能实现自稳了，那怎么来实现呢？要点在于怎么合成角度，有人说对角速度进行积分不就得到角度了嘛，对，也不对，原因在于只用角度误差太大了，于是就有各种滤波算法，典型的有卡尔曼滤波，互补滤波。下面给一个卡尔曼滤波的例子：
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
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;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}有时，考虑到有零飘，我们还要用磁力计来进行校准。关于算法之类的讲解，不在这里进行，感兴趣的可以自行推导，网上有很多的介绍和源码，当然了，还有相关参数的调试，如果不想自己进行，也可以在网上购买姿态结算模块，150左右，结算后的姿态uart或iic输出的都有，精度还蛮高的。
超声波，气压计
一般超声波模块适用于低空（<3m）,精度可达到cm级，气压计适用于高空，精度在0.5m左右，在后面的高度pid整定时，我们会提到，悬停时上下波动控制在20cm左右算是比较正常的范围。用定时器去采，或者直接用串口收就可以了，注意在后面数据的使用时需要加入姿态进行校准（×sin(theta)）,还有就是要对数据进行必要的滤波，将波动范围控制在一个合理的范围，以防出现异常。
摄像头
主要用来循迹，我主要参考的是飞思卡尔小车中用来循迹的算法，效果不是很理想，注意图像的数据量非常之大，要进行合理的压缩，二值化是必不可少的一个过程，同时，调摄像头也需要花费不少的功夫，建议一定要结合相应的可视调试助手进行调试。
自此，飞行器姿态结算和数据采集预处理算是有个小结了，姿态解算讲的不是很详细，请再参考其它博文和源码研究。后面主要进行pid的控制和调试。