四元数转欧拉角代码解析
本文的内容就是解析正点原子MPU6050的mpu_dmp_get_data()函数中，三个欧拉角的由来，即如何将MPU6050输出的四元数转化为姿态解算所需要的欧拉角。
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
其实上述三个公式的核心就是将一次的姿态变换分别用四元数矩阵和欧拉角矩阵表示出来，由于这两个矩阵是等价的即对应元素都相等，通过简单的对比运算就可以得到上述的三个公式。
因此，我将从1.四元数矩阵的得到；2.欧拉角矩阵的得到；3.两个矩阵的等价运算三个部分进行说明。
1.四元数矩阵的得到
三重矢量计算公式：
AX(BXC)=B(A·C)-C(A·B)
这个公式很好记，右边部分就是BACK-CAB(后面的出租车)
2.欧拉角矩阵的得到
q02+q12+q22+q32=1
从9.2.33到9.2.34的化简，其实就是利用
进行化简，把1去掉即可。
将右侧的矩阵乘开，可得到一个3x1矩阵，
与左边3x1矩阵对应元素相等，这个相等的
关系，就是上个框框中求出的三个等式。
各轴上的单位1，就是图1.2.2矩阵任意
行与列各个元素的平方和为1。
到这里，用欧拉角表示描述一次旋转变换已经结束了。
然而，上述的姿态矩阵C n b仅仅是《惯性导航》这本书先Z,再X,最后Y旋转变换而形成的姿态矩阵，这样的旋转顺序其实是和很多大家实际使用的飞控代码不一样的（同样的，关于θφγ的实际意义其实也没有明确的规定）。
此文目的就是解析“正点原子”飞控代码中四元数转欧拉角部分，因此，接下来，
我将采用此代码所使用的旋转顺序，先Z,再Y ,最后X 的旋转变换重新计算旋转矩阵C n b。
C n
b
=[cosφ−sinφ0sinφ
cosφ0001][cosγ0−sinγ010sinγ0cosγ][10
00cosθsinθ0−sinθ
cosθ
] =[cosφcosγ−sinφcosθ+sinθcosφsinγ−sinφsinθ−cosφsinγcosθsinφcosγ
cosφcosθ+sinγsinθsinφ
cosφsinθ−sinγsinφcosθsinγ−sinθcosγcosγcosθ
]
3. 两个矩阵的等价运算
至此，我们已经分别用四元数法与欧拉角法共同描述了一次旋转变换。
这一节，将利用前两节最后得到的结论，进行等价运算。
为了观察方便，这里将两个矩阵再次列出。
[cosφcosγ
−sinφcosθ+sinθcosφsinγ−sinφsinθ−cosφsinγcosθsinφcosγ
cosφcosθ+sinγsinθsinφ
cosφsinθ−sinγsinφcosθsinγ
−sinθcosγ
cosγcosθ
]
为了运算方便，只需比对两个矩阵的第三行与第一列即可得出结论。
γ=sin −1（2(q 1q 3−q 0q 2)）——俯仰角pitcℎ
θ=tan −1
（−2(q 2q 3+q 0q 1)q 02−q 12−q 22+q 3
2）——横滚角roll φ=tan −1（2(q 1q 2+q 0q 3)
q 02+q 12−q 22−q 3
2）——航向角yaw 在最终得到实际运算代码前，说明两个小细节：
1.三个欧拉角θφγ最终在代码中体现时，会有正负号的区别，这个很好理解，四轴向上，向右，顺时针滚动为正还是负呢？你喜欢怎样，只要不离谱，代码是自己的，可以自行调整。
可以看到正点原子的代码的方向选取与惯性导航这本书的方向选取，在pitch 角与roll 角上是相反的。
2.计算横滚角θ时，上述计算式子分母和实际代码有些许差异，是因为进行了一次 q 02+q 12+q 22+q 32
的等价代换。
下面，我们再来看看这段四元数转欧拉角的代码，是不是很清晰了呢？ *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // γ
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // θ *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // φ
4. 结语
刚刚接触stm32控制下的四旋翼设计，才疏学浅，如有错误，欢迎大家指正交流！
本文参考书目秦永元的《惯性导航》。
作者：朱一尘 2016.5.30。