微型四轴飞行器（5）九轴姿态融合算法B

方向余弦矩阵

飞行器在空中的运行姿态可以用平面和转动来表示，为了方便使用向量表示，需要建立两个空间直角坐标系。设R表示单位矢量在机体坐标系下的三个轴的投影，B表示单位矢量在地球坐标系下的三个轴的投影。我们通常对飞行器的偏航、俯仰、横滚了多少度的定义是参照地球坐标系而下得出的，也就是我们只需要知道矢量R相较于矢量B进行了怎样的运动就能获取所需的姿态信息。而向什么方向进行怎样的运动就是方向余弦矩阵。现设C_B^R表示矢量由坐标系B到坐标系R的方向余弦矩阵则R=C_B^R B。

直接把公式贴图了，word里面弄过来就变成上面文本的形状了

方向余弦矩阵就是表示将一个坐标系转化到另外一个坐标系需要进行的转动过程，由于坐标系是两两相交的，于是只需要进行三次转动变能得到新坐标系，先讨论单独对Z轴进行转动的情况。假设F=[r_x1 r_y1 r_z1 ]^T是以X1、Y1、Z1组成的坐标系下的一个向量，现在坐标系围绕Z1转动了α度,变成了 F_new=[r_x2 、r_y2 、r_z2 ]^T。如图


三角形OBD和三角形DFE相似，所以角ADB等于α，DE=cosα*DF = cos ⁡r_y1,并且在三角形AOD中，AD=sinα*OD=sin r_x1求得的。上面三个公式组合起来便是绕Z旋转的方向余弦矩阵:

同理可得绕其他轴旋转的方向余弦矩阵。而能同时表示绕三轴旋转的方向矩阵就是分别将绕单轴旋转的矩阵相乘，矩阵的乘法是不可交换的，也就是说旋转次序的不同也会导致矩阵的不同。这里规定矩阵先绕Y轴旋转β度，再绕X轴转动α度，最后绕Z轴转动γ度，得出由机体坐标系到地球坐标系的方余弦矩阵
公式 1

四元素

四元素是复数的多维度变化，正如复数可以表示平面坐标系下的旋转一样，四元素能够被用来表示空间直角坐标系下的旋转，他是由一个矢量和一个标量组成，标量表示旋转的角度，矢量表示旋转轴

为了减少运算量以及避免万向节死锁的情况，融合算法采用了四元素运算，他和方向余弦矩阵一样能够表示物体的旋转情况，并且方向余弦矩阵也有着与之对应的四元素矩阵，能通过四元素求得三个轴向的姿态角α、β、γ。
由于两个矩阵等效，于是我们在整个计算过程中都可以使用四元素进行，而在最后需要获取姿态角的时候，也就有四元素矩阵与方向余弦矩阵的关系得到姿态角。对于四元数的更新可以使用一阶龙格库塔公式，传入参数为三轴角速度和取样周期。公式的推导比较复杂，这里不是关注重点也就不进行讨论。最后推导出来。通过它也将姿态角连系上了原始数据。