4、欧拉角与旋转矩阵：从定义到工程实现
各位同学好，我是老张。今天咱们聊聊姿态解算里最基础、也最容易踩坑的一块——欧拉角与旋转矩阵。说实话，我当年刚入行时，被这俩东西绕得晕头转向。后来在飞控项目里摔了几次跟头，才算真正搞明白。今天我把这些经验掰开揉碎了讲给你听。
4.1 欧拉角定义与旋转顺序
欧拉角，说白了就是用三个角度来描述一个刚体在空间中的朝向。你想想看，一个物体要转成某个姿态，无非就是绕三个轴各转一次。但问题来了——先绕哪个轴转？
这就是旋转顺序。常见的顺序有：
- ZYX（航向-俯仰-横滚）：无人机、飞控最常用
- ZXY：某些惯性导航系统用
- XYZ：机器人学里常见
我个人习惯用ZYX顺序，也就是先绕Z轴转偏航角（ψ），再绕Y轴转俯仰角（θ），最后绕X轴转横滚角（φ）。为什么？因为这在工程上最直观——你先确定朝哪飞（偏航），再抬头低头（俯仰），最后调整左右倾斜（横滚）。
关键点：旋转顺序决定了欧拉角的定义域和奇异性。ZYX顺序下，俯仰角θ的范围是-90°到+90°，超出这个范围就会出现万向锁。
我在项目中遇到过一件事：有次调试四旋翼，发现姿态解算在俯仰角接近90°时突然跳变。查了半天，原来是旋转顺序没统一——传感器用的是ZYX，但姿态解算算法默认用了XYZ。嗯，这种低级错误，犯一次就记住了。
4.2 旋转矩阵推导
旋转矩阵，就是把欧拉角转换成数学上可计算的矩阵形式。每个基本旋转对应一个矩阵：
绕X轴旋转φ角：
R_x(φ) = [1 0 0 ]
[0 cosφ -sinφ ]
[0 sinφ cosφ ]绕Y轴旋转θ角：
R_y(θ) = [ cosθ 0 sinθ ]
[ 0 1 0 ]
[-sinθ 0 cosθ ]绕Z轴旋转ψ角：
R_z(ψ) = [cosψ -sinψ 0 ]
[sinψ cosψ 0 ]
[ 0 0 1 ]注意看符号方向。我刚开始学的时候，经常把sin的符号搞反。后来总结了个规律：右手定则，拇指指向旋转轴正方向，四指弯曲方向就是正角度方向。你对着右手比划一下，比死记硬背强多了。
4.3 欧拉角转旋转矩阵
有了上面的基础，ZYX顺序的旋转矩阵就是三个矩阵的乘积：
R = R_z(ψ) * R_y(θ) * R_x(φ)展开后得到：
R = [cosθcosψ sinφsinθcosψ - cosφsinψ cosφsinθcosψ + sinφsinψ]
[cosθsinψ sinφsinθsinψ + cosφcosψ cosφsinθsinψ - sinφcosψ]
[-sinθ sinφcosθ cosφcosθ ]这个矩阵看着复杂，但实际用起来很简单。你写代码时，直接把这9个元素按公式填进去就行。我建议你把这个矩阵存成常量，每次调用时直接赋值，别现场算——既慢又容易出错。
工程技巧：在嵌入式平台上，建议用查表法或预计算方式处理三角函数。我见过有人每次姿态更新都调用sin/cos，结果CPU占用率直接飙到30%以上。后来改成每10ms预计算一次，问题就解决了。
4.4 旋转矩阵转欧拉角
反过来，从旋转矩阵提取欧拉角，是姿态解算里更常见的需求。比如你从加速度计和磁力计算出了旋转矩阵，现在要把它转成欧拉角给飞控用。
以ZYX顺序为例，从上面的R矩阵可以反推：
θ = -arcsin(R[2][0]) // 俯仰角
φ = atan2(R[2][1], R[2][2]) // 横滚角
ψ = atan2(R[1][0], R[0][0]) // 偏航角这里有个坑：当θ接近±90°时，R[2][1]和R[2][2]同时趋近于0，atan2会变得不稳定。这就是万向锁问题。我曾经在无人机做特技飞行时遇到过，俯仰角拉到85°以上，横滚角突然乱跳，差点炸机。
避坑指南：如果你的应用场景可能接近万向锁，建议改用四元数。或者至少加一个阈值判断——当|θ| > 85°时，切换到另一种解算方式。我曾经在代码里加了这个保护，之后再没出过问题。
4.5 知识体系总览
下面这张图是我自己画的，把欧拉角和旋转矩阵的关系理清楚了。你照着这个思路学，不会乱。
4.6 代码实现要点
最后，我贴一段实际项目中用过的代码片段。这是从旋转矩阵提取欧拉角的C语言实现，我在STM32上跑过，稳定运行了两年多。
// 从旋转矩阵提取ZYX欧拉角
void matrix_to_euler(float R[3][3], float *phi, float *theta, float *psi) {
// 俯仰角
*theta = -asinf(R[2][0]);
// 检查万向锁
if (fabsf(*theta) > 1.56f) { // 约89.4°
// 进入万向锁区域，使用备用算法
*phi = 0.0f; // 强制横滚角为0
*psi = atan2f(-R[0][1], R[1][1]);
} else {
*phi = atan2f(R[2][1], R[2][2]);
*psi = atan2f(R[1][0], R[0][0]);
}
}注意看那个阈值判断。1.56弧度约等于89.4°，留了0.6°的余量。为什么？因为浮点运算有误差，卡死在90°边界上容易出问题。这是我炸过一次机后学到的教训。
个人建议：如果你刚开始做姿态解算，先用四元数。等把原理搞透了，再回来用欧拉角。四元数没有万向锁问题，而且计算量更小。但欧拉角直观，调试时看数据方便。两者各有优劣，看场景选。
好了，这一章就到这里。欧拉角和旋转矩阵是姿态解算的基石，搞懂了它们，后面的卡尔曼滤波、互补滤波才能用得顺手。记住我上面说的那些坑，能帮你少走不少弯路。
公众号:蓝海资料掘金营，微信deep3321