四轴飞行器制作之姿态解算

这块区域主要是很多重点概念的理解吧：姿态解算、四元数、欧拉角、PID算法、互补滤波、卡尔曼滤波。这些不论哪一个都需要好好理解，才能将整个四轴飞行器飞控部分大致流程理解下来

• 互补滤波、卡尔曼滤波算法：
  参考博客：https://blog.csdn.net/u013608300/article/details/52459515
  在四轴飞行器用到了MPU6050六轴传感器模块，里面包含加速度传感器、陀螺仪，通过它们都可以得到三轴的角度。本来仅通过一个陀螺仪就可以得到姿态角，但是有一定误差，就需要这两个算法进行调整误差。通过加速度计的数据对陀螺仪的数据进行校正。这里我采用的是互补滤波算法。
  互补滤波算法就是将两者数据进行一个叉乘，将会得到一个误差向量，再对这个误差向量通过一定方法补偿到陀螺仪数据中。这里就参考这个博客。

• 四元数：
  参考博客：https://www.cnblogs.com/hjlweilong/p/6018746.html
  四元数：类似于高中学习的复数，它是一种超复数，二元复数由一个实数a加上一个复数bi，表示为a+bi，i^2=-1，而四元数则是，a+bi+cj+dk，i^2=j^2=k^2=-1。
  
  一个重点是四元数可以表示一个旋转，通过一个点与四元数的四元数乘积可以得到一个新的四元数，这个转化后的四元数的后三项即为旋转后的x、y、z轴坐标。
  
  二元复数可以表示为一个平面，也就是二维空间，a和b表示二维空间上的坐标，可以表示为一个点进行二维旋转。
  而四元数表示的则是四维空间，a、b、c、d表示四维空间上的坐标，可以表示为一个点进行四维旋转。
  
  看上去好像是很难的样子，因为表示的是在四维空间上的运算，实际上的确是有一些复杂。
  但是回归到主题，四元数这个工具是用于我们四轴飞行器求姿态角的，应对的是三维空间，可以将四元数表示为一个在三维空间上的四维旋转，也就是将这个四维空间限制在设立的一个三维空间内将这个3维空间进行旋转，就像是在三维直角坐标系中选取一个平面进行旋转一样。
  
  举个例子理解了一下，从平面旋转来看，比如是你现在所在的平面是xoz，要旋转到yoz平面上就需要引入三维空间，xoz平面围绕z轴旋转到yoz平面。
  在四轴飞行器的姿态解算中也是需要完成一个旋转的过程，需要将一个空间进行旋转，这个空间就是四轴飞行器当前的参考空间，就是以四轴飞行器为参考，它的前方上方右方分别对应x轴y轴z轴，我们姿态调整的目的就是需要让四轴飞行器转到与地球的参考空间相同，那就需要进行一个四维空间上对一个三维空间的旋转。
  
  如果你想算一个点(wx,wy,wz)在这个旋转下新的坐标,需要进行如下操作，
  1.定义纯四元数
  qw = (0,wx,wy,wz) = 0 + wxi + wyj +wz*k
  2.进行四元数运算
  qw’ = q*qw*q^-1
  3.产生的一定是纯四元数，也就是说它的第一项为0，有如下形式：
  qw’ = (0,wx’,wy’,wz’) = 0 + wx’*i+wy’*j + wz’*k
  5.中的后三项(wx’,wy’,wz’)就是：
  W’ = (wx’,wy’,wz’)
  
  可以看到这个四元数就是q，坐标点是w，q与w以一定方式相乘后得到了一个四元数后三项就是转换后的坐标。
  接下来就需要在程序中实现对这个四元数的求解，参考博客：
  https://blog.csdn.net/qq_15063463/article/details/82455265
  程序中需要求出q，知道了q就相当于知道了这个物体是如何旋转的，q是一个单位旋转向量，也就是说它的模是1，也就是式子q0^2+q1^2+q2^2+q3^2=1，不会改变目标坐标的模。所以其中每次求得q后需要进行一个归一化的操作，就是要保证这个向量的模为一。

			norm=1.0f/sqrt(ax*ax+ay*ay+az*az);
            ax*=norm;
            ay*=norm;
            az*=norm;
            //对加速度数据进行归一化处理

            halfvx = q1q3 - q0q2;
            halfvy = q0q1 + q2q3;
            halfvz = q0q0 - 0.5f + q3q3;
            //利用四元数参数来估算重力分量 

            halfex = (ay * halfvz - az * halfvy);
            halfey = (az * halfvx - ax * halfvz);
            halfez = (ax * halfvy - ay * halfvx);
            //利用叉积来计算估算和实际重力的偏差

            exInt = exInt + halfex * Ki;
            eyInt = eyInt + halfey * Ki;
            ezInt = ezInt + halfez * Ki;
            //对重力差进行积分运算将积分后的结果累加到陀螺仪数据中，修正陀螺仪数据

            gx += Kp*halfex + exInt;
            gy += Kp*halfey + eyInt;
            gz += Kp*halfez + ezInt;
            //对重力差进行比例运算，将比例运算后的结果累加到陀螺仪数据中，用于修正陀螺仪数据

            q0 += (-q1*gx - q2*gy - q3*gz)*halfT;
            q1 += (q0*gx + q2*gz - q3*gy)*halfT;
            q2 += (q0*gy - q1*gz + q3*gx)*halfT;
            q3 += (q0*gz + q1*gy - q2*gx)*halfT;
            //四元数方程的求解，使用了一阶毕卡算法

            norm = 1.0f/sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 *= norm;
            q1 *= norm;
            q2 *= norm;
            q3 *= norm;
            //将四元数归一化

            AngleX=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰   pitch
            AngleY=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 横滚   roll

这样，就完成了一次四元数旋转运算。
最后就得到了四元数，经过一个换算公式即可得到欧拉角，俯仰角pitch和横滚角roll，而航向角yaw缺少地磁传感器的数据小四轴可以不采用，所以这里就不需要求。
AngleX=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰 pitch
AngleY=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 横滚 roll

• 欧拉角

  欧拉角是一种常用的描述方位的方法，是由欧拉提出的。基本思想就是将两个坐标系的变换分解为绕三个不同的坐标轴的三次连续转动组成的序列。欧拉角的旋转规定为连续两次旋转，必须绕着不同的转动轴旋转，所以一共有12种旋转顺规。比如我们选用Z-Y-X的旋转顺规来描述b系与n系的关系。Z-Y-X顺规就是指：绕Z轴旋转偏航角(YAW)，绕Y轴旋转横滚角(ROLL)，绕X轴旋转俯仰角(PITCH)。

所以通过互补滤波融合数据四元数解算飞行姿态，得到三轴欧拉角，再对这个过程引入PID算法，最后得到PID算法调整后的欧拉角，再与之前了解到的四轴飞行器飞行姿态转换与PWM波控制电机的知识，在油门控制的基础上，欧拉角乘以一定比例得到一个对电机转速进行微调的参数speed，再把这个值加到对应的电机上。

参考欧拉角的概念，分别对三个轴进行角度的转换就得到了目标的空间旋转，对x轴旋转是横滚运动，对2、4电机速度进行调整；对y轴旋转就是俯仰运动，改变1、3电机转速；对z轴旋转就是偏航运动，对1、3电机速度增加，对2、4电机速度减小。

接下来还需要理解PID算法，可以将整个飞控的大概轮廓理解了，不得不说这个四轴飞行器涵盖的知识点是真的挺多的，要真正理解是要花上一定功夫的。