基础概念
欧拉角---->旋转矩阵
转换公式推导
旋转矩阵就是交接确认绕各个轴旋转所得的旋转矩阵相乘之后得到的 的一个3*3的矩阵而已。
我们要推导的案例是 ZYX欧拉角(内旋) 的转换公式：
-
旋转顺序：先绕Z轴转
rz得到旋转矩阵R_z，再绕Y轴转ry旋转矩阵R_y，最后绕X轴转rx旋转矩阵R_x -
对应旋转矩阵：
R = R_z(rz) * R_y(ry) * R_x(rx) -
已知旋转矩阵元素
r11, r12, ..., r33，求rx, ry, rz
基本旋转矩阵:
# 绕X轴旋转rx
R_x(rx) = [1, 0, 0;
0, cos(rx), -sin(rx);
0, sin(rx), cos(rx)]
# 绕Y轴旋转ry
R_y(ry) = [cos(ry), 0, sin(ry);
0, 1, 0;
-sin(ry), 0, cos(ry)]
# 绕Z轴旋转rz
R_z(rz) = [cos(rz), -sin(rz), 0;
sin(rz), cos(rz), 0;
0, 0, 1]组合旋转矩阵:
R = R_z(rz) * R_y(ry) * R_x(rx)
让我们一步步计算这个乘积。
得到完整的旋转矩阵:
R = [r11, r12, r13;
r21, r22, r23;
r31, r32, r33]
其中：
r11 = cos(rz)*cos(ry)
r12 = cos(rz)*sin(ry)*sin(rx) - sin(rz)*cos(rx)
r13 = cos(rz)*sin(ry)*cos(rx) + sin(rz)*sin(rx)
r21 = sin(rz)*cos(ry)
r22 = sin(rz)*sin(ry)*sin(rx) + cos(rz)*cos(rx)
r23 = sin(rz)*sin(ry)*cos(rx) - cos(rz)*sin(rx)
r31 = -sin(ry)
r32 = cos(ry)*sin(rx)
r33 = cos(ry)*cos(rx)旋转矩阵--->欧拉角
转换公式推导
旋转矩阵就是交接确认绕各个轴旋转所得的旋转矩阵相乘之后得到的 的一个3*3的矩阵而已，所以得到这个矩阵之后，只需要逆运算就可以算的三个欧拉角。
反向求解欧拉角:
现在我们从矩阵元素反向求解 rx, ry, rz。
1. 求解 ry
从 r31 = -sin(ry) 得：
sin(ry) = -r31但是仅凭这个无法唯一确定 ry，因为 sin(ry) = -r31 有两个解（ry 和 π-ry）。
我们需要更多信息。观察 r11 和 r21：
r11 = cos(rz)*cos(ry)
r21 = sin(rz)*cos(ry)于是：
r11² + r21² = cos²(rz)*cos²(ry) + sin²(rz)*cos²(ry)
= cos²(ry)*(cos²(rz) + sin²(rz))
= cos²(ry)所以：
cos(ry) = ±√(r11² + r21²)结合 sin(ry) = -r31，我们可以用 atan2 函数：
ry = atan2(-r31, √(r11² + r21²))这里用 atan2(y, x) 同时考虑了正弦和余弦，给出了正确的象限。