目录
坐标系统建立
为了获得四旋翼飞行器控制系统的数学模型，首先建立两个基本坐标系，即惯性坐标系E（OXYZ）和飞行器坐标系B（OXYZ），如下图2.2.1所示。
通常在大地上要建立一个全局坐标系（Global Coordinate System，GCS），用来对整个多体系统提供一个统一的参照坐标系。在物体上要建立一个局部坐标系，称为BCS（Body Coordinate System），一方面用来描述物体在GCS内的位置和姿态，另一方面为物体上的点或其他坐标系提供局部的确定位置和姿态的标准。
分别定义欧拉角如下。
①航角：OX在OXY平面的投影与X轴夹角φ。
②仰角：OZ在OXZ平面的投影与Z轴夹角θ。
③滚角：OY在OYZ平面的投影与Y轴夹角φ。
飞行器坐标系到惯性坐标系的转换矩阵如下。
在建立上述坐标系的基础上，且无风低速飞行的情况下，忽略阻力系数，可以得到以下数学模型，即
其中，各字符代表含义如下表2.2.1所示。
四旋翼飞行器具有6个自由度，通过IMU惯性测量单元实时测量飞行器3个轴向的旋转角速度和加速度，进行姿态解算（即对数据进行处理融合后得到当前飞机的姿态信息），控制飞行器的4个输入控制量，飞行器即可达到平衡飞行的目的。
姿态解算
姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。
载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。
目前，描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法。三参数法也称欧拉角法，四参数法通常指四元数法，九参数法称为方向余弦法。
一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕3个不同的轴转动的3个角度来确定。
例如，载体坐标系作为动坐标系，导航坐标系作为参考坐标系，则姿态角即为一组欧拉角，按一定的转动顺序即可得到导航坐标系到载体坐标系的关系。
式中，γ、θ、ψ分别对应横滚（Roll）、俯仰（Pitch）、航向（Yaw）3个姿态角，根据欧拉角微分方程，由角速度可以求解3个姿态角。
欧拉角微分方程式只有3个，但每个方程都含有三角函数的运算，计算速度慢，且方程会出现“奇点”，使方程式退化，故不能全姿态工作。
奇点是指在某些特定的姿态下，欧拉角的计算结果变得不稳定或无法定义。这是因为欧拉角使用三个角度来描述姿态，而角度的范围有限，当物体的姿态接近某些特殊情况时，例如旋转轴与某个角度重合，就会导致计算结果出现问题。
因此，欧拉角法难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。
四元数算法
四元数法是捷联惯导中的一种算法，利用陀螺仪、加速度计（有条件的时候可以加上磁强计）的数据，将四轴机体坐标系与地球坐标系进行耦合，最后计算出正确的四轴姿态方位数据。
四元数的数学概念是1843年由哈密顿首先提出的，它是代数学中的内容之一。随着捷联式惯性导航技术的发展，为了更简便地描述刚体的角运动，采用了四元数这个数学工具，用它来弥补通常描述刚体角运动的3个欧拉角参数在设计控制系统时的不足。
四元数可以描述一个坐标系或一个矢量相对某一个坐标系的旋转，四元数的标量部分表示了转角的一半余弦值，而其矢量部分则表示瞬时转轴的方向、瞬时转动轴与参考坐标系轴间的方向余弦值。因此，一个四元数既表示了转轴的方向，又表示了转角的大小，往往称为转动四元数。
工程上一般运用范数为1的特征四元数，特征四元数的标量部分表示转角的一般余弦值，其矢量部分表示瞬时转轴n的方向。矢量R相对参考坐标系旋转一个转角θ，旋转轴n的方向由四元数的虚部确定，cosα、cosβ、cosγ表示旋转轴n与参考坐标系轴间的方向余弦值。
四元数姿态矩阵微分方程式只要解4个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。
四元数转欧拉角算法流程
运用机体上搭载的加速度计与陀螺的数据求出初始欧拉角。为了获取四旋翼飞行器当前准确、可靠的姿态信息（俯仰角、横滚角、偏航角），用于四轴的平衡及运动控制，应当首先进行数据融合。
1、单独使用加速度计测量姿态时
加速度计测量对象为比力，理想情况下想要得到的是重力加速度分量，但是任何机械系统不可避免存在的平动振动也会产生相当大的加速度，加速度计特别容易受飞行器机架振动的影响，振动振幅高达2个重力单位。而加速度计完全无法区分重力加速度和外力加速度这两种加速度。所以加速度计虽然能够精确测量出当前飞行器的各个加速度分量，且测量值随时间的变化相对较小，但是飞行器的线性振动给测量引入了强烈的噪声，使得这个数据不能直接使用。
2、单独使用陀螺仪测量姿态时
陀螺仪对振动不敏感，受外部影响弱，稳定性好，但输出量为角速度，需要积分才能得到姿态。短期内这个姿态数据具有很大的参考价值，但是由于陀螺仪器件本身误差的存在，会导致这个姿态数据随着长时间的积分逐渐偏离正确值，即测量值随时间变化比较大。
因此，可以看出，将这两种传感器的优缺点互补，将两者的数据融合起来才能得到更好的效果。
数据融合的方法有很多种，Madgwick的IMUupdate算法就是其中之一。IMUupdate算法中要求输入6个数据，gx、gy、gz分别为陀螺仪测量到的3个旋转角速度分量，ax、ay、az则为加速度计的3个加速度分量，IMUupdate函数体如下：
首先，将加速度计测量到的3个向量转换为单位向量，代码如下：
根据余弦矩阵和欧拉角的定义，将地理坐标系的重力向量转到机体坐标系，得到的υx、υy、υz，其实就是在当前的四元数机体坐标参照系上换算出来的重力单位向量，即
ax、ay、az是机体坐标参照系上加速度计测出来的重力向量，也就是实际测出来的重力向量。υx、υy、υz为用陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，ex、ey、ez就是两个重力向量的叉积，即
这个向量叉积仍然是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系上，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。用叉积误差ex、ey、ez来做PI调节，最后用得到的调节量对陀螺仪进行零偏修正，代码如下：
其中定义了Kp和Ki的值为
在上一步已经得到了修正过零偏的陀螺仪数据gx、gy、gz，在这里设机体坐标系相对平台坐标系的转动四元素为
然后使用一阶龙格—库塔法对陀螺仪角速度值进行处理更新得到新的四元数值。一阶龙格—库塔法计算式为
将角速度值代入上式展开，得到
具体的实现代码如下：
其中定义了halfT的值为0.001，代表四元数固定为2ms更新一次数据，halfT取值为周期的一半，即T/2。
在上面的四元数微分方程更新了四元数数据后，还需要将得到的四元数进行规范化处理，规范化的公式及过程代码如下：
至此已经获取到了稳定、有效的姿态四元数数据，但由于在设计控制规律时，欧拉角的直观性和几何意义仍然采用欧拉角描述，因此还需要将四元数转换为欧拉角，即
arctan的结果是，这并不能够覆盖所有的角度（对于角的取值范围已经满足），因此需要用arctan2来代替arctan，修正后的公式为
实际C语言代码如下：
至此姿态解算得到四旋翼飞行器稳定、可靠的姿态角度，可以用于四轴的飞行稳定控制中。