基础理解，参照：https://www.cnblogs.com/Estranged-Tech/p/16903025.html

欧拉角、万向节死锁（锁死）理解

一、欧拉角理解

举例讲解

欧拉角用三次独立的绕确定的轴旋转角度来表示姿态。如下图所示

经过三次旋转，旋转角度分别为𝛼、𝛽和 𝛾，由初始的𝑥𝑦𝑧坐标系得到了最终的𝑥‴𝑦‴𝑧‴
坐标系。这就是欧拉角来表示姿态的方法。
如图所示为航空航天中常用的欧拉角，图中的𝜓、𝜃和𝜙
对应于上图中的𝛼、𝛽和 𝛾。

顺规、内旋与外旋

上面所举的例子中，旋转的顺序是按照 𝑧−𝑦−𝑥 的顺序来进行旋转的，并且每一次旋转都是绕自身轴（运动轴）进行的， 这只是欧拉角的一种表示方式。欧拉角共有12种表示方式。

• 顺规
  顺规即欧拉角三次转动的顺序规定，欧拉角一共有12种顺规

三个轴只用两个的：Proper Euler angles (z−x−z, x−y−x, y−z−y, z−y−z, x−z−x, y−x−y）
三个轴全都用的：Tait-Bryan angles (x−y−z, y−z−x, z−x−y, x−z−y, z−y−x, y−x−z）

上文举例所用的 𝑧−𝑦−𝑥是三个轴都用的一种顺规，即 先绕𝑧轴旋转，再绕𝑦轴旋转，最后绕𝑥轴旋转。、

• 内旋与外旋
  根据绕旋转轴的不同，可以分为内旋和外旋。

内旋 Intrinsic rotations：绕运动轴
外旋 Extrinsic rotations：绕固定轴

上文所举的例子每次旋转都是绕上一次旋转所新产生的坐标轴旋转的（这句话有点绕），比如第二次旋转绕的是𝑦″而不是固定的坐标轴𝑦。所以是绕运动轴，即内旋。

如下图所示，每一次旋转都是绕蓝色的固定轴旋转

如下图所示，每一次旋转都是绕红色（第一张图）、绿色（第二张图）、紫色（第三张图）的运动轴旋转：


在绕轴旋转的时候，顺规有12种，内外旋有2种，但是一般情况下，欧拉角都是说的绕自身轴（运动轴）旋转，即 内旋 。所以欧拉角共有 1×12=12种表示方式。

小结

• 首先说一个很有意思却很不直观的结论：三次绕固定轴旋转的最终姿态和以相反顺序三次绕运动轴旋转的最终姿态相同。

• 一般情况下，每一个领域有自己默认的欧拉角定义，也就是24种的其中之一。比如经典力学中使用𝑧𝑥𝑧，量子力学使用的是𝑧𝑦𝑧，航空航天使用𝑧𝑦𝑥或𝑧𝑥𝑦。所以在跨行业或者跨模块协作的时候，一定要问清楚对方是哪一种欧拉角。

二、数学公式

三、万向节死锁理解

• 说明
  欧拉角表示姿态的时候，会出现万向节死锁的情况，当我使用传感器（维特智能WT901c-485）读取角度的时候，在𝑍𝑌𝑋的顺规下，当pitch角度为90或-90的时候，roll和yaw角度会乱飘，这就是遇到了万向节的死锁。不同的顺规死锁的情况不一样，本文仅以𝑍𝑌𝑋说明死锁的情况。
• 形象表示
  我们以手机为例，说明一下万向节死锁的情况
  注意，欧拉角表示姿态时，只能旋转三次，这是理解死锁的前提！
  首先这是一个手机，我们对其建立坐标系，平行于手机长边是 𝑋轴，平行于手机短边是 𝑌轴，垂直于桌面是 𝑍轴。我们按照 𝑍-𝑌-𝑋的顺规，对手机进行旋转：
  
  1.首先，先绕 𝑍轴旋转得到 𝑋′𝑌′𝑍′坐标系，如图中绿色所示：
  
  2.然后对 𝑋′𝑌′𝑍′ 绕 𝑌′轴旋转-90°，得到 𝑋″𝑌″𝑍″ ，如图中蓝色所示（此时旋转后的 𝑋″ 轴轴与旋转之前的 𝑍′轴重合）
  
  3.最后将 𝑋″𝑌″𝑍″ 绕 𝑋″轴旋转得到 𝑋‴𝑌‴𝑍‴ 坐标系，如图中黄色所示。
  
• 这时候重点来了！！！

我们发现，这样旋转过后，手机的长边是与桌面（地面）垂直的，手机的短边是与桌面（地面）平行的。并且，无论第一步转 𝑍轴转了多少度，第三步转 𝑋 轴转了多少度，手机长边与短边都是这个状态。
这就是欧拉角表示角度时的“万向节死锁”现象，按理说欧拉角可以表示三个自由度，即三个方向的旋转。但是在这种情况下，只要𝛽=±90°，无论 𝛼与𝛾怎么取值，手机最终的长边都会与地面垂直，这就好像少了一个自由度，只有2个自由度，所以称之为“万向节死锁”。

• 那么为什么会出现这种情况呢？

一种感性的理解是这样：我们在第二步旋转的时候，将旋转后的 𝑋″ 轴与旋转之前的 𝑍′ 轴重合（这个我在上面旋转演示的时候加粗说明了）。而第一步旋转是绕 𝑍′ 轴旋转的，第三步旋转是绕 𝑋″轴旋转的，所以两轴重合意味着两次旋转绕的是同一个轴，所以说，三个自由度变为了两个自由度。

• 数学解释：
  

欧拉角与旋转矩阵转换中的相关问题：

• 先出结论：

# ea(人为给定的)转rotmat 再转ea后两个ea的值存在不一致，且含义不一致（主要出现在y角度在一定范围时时），但两个不同的ea可以得到相同的旋转矩阵。
# rotmat转ea再转rotmat（前后rotmat一致）再转ea 前后ea含义是一致的，说明相同的rotmat可以获得一致的ea
# rotmat转ea再转rotmat 前后rotmat是一致的，

# 以上说明rotmat到ea是多射，但使用函数进行rotmat转换ea时可以获得一致的ea结果；ea到rotmat是单射。以上说明虽存在多设情况但不影响我们场景的使用。（再验证下）

import torch
import math


def euler_to_rotation_matrix(euler_angles):
    """
    将欧拉角转为旋转矩阵。欧拉角采用 Z-Y-X顺序 的内旋旋转（与X-Y-Z 的外旋一致）
    参数：
        euler_angles：欧拉角，形状为 (batch_size, 3)
    返回：
        rotation_matrix：旋转矩阵，形状为 (batch_size, 3, 3)
    """
    # 将欧拉角转为弧度
    x, y, z = euler_angles[:, 0], euler_angles[:, 1], euler_angles[:, 2]
    x, y, z = x * torch.pi / 180, y * torch.pi / 180, z * torch.pi / 180
    # 计算旋转矩阵
    c_x, s_x = torch.cos(x), torch.sin(x)
    c_y, s_y = torch.cos(y), torch.sin(y)
    c_z, s_z = torch.cos(z), torch.sin(z)
    rotation_matrix_x = torch.stack([torch.ones_like(c_x), torch.zeros_like(c_x), torch.zeros_like(c_x),
                                     torch.zeros_like(c_x), c_x, -s_x,
                                     torch.zeros_like(c_x), s_x, c_x], dim=1).reshape(-1, 3, 3)
    rotation_matrix_y = torch.stack([c_y, torch.zeros_like(c_y), s_y,
                                     torch.zeros_like(c_y), torch.ones_like(c_y), torch.zeros_like(c_y),
                                     -s_y, torch.zeros_like(c_y), c_y], dim=1).reshape(-1, 3, 3)
    rotation_matrix_z = torch.stack([c_z, -s_z, torch.zeros_like(c_z),
                                     s_z, c_z, torch.zeros_like(c_z),
                                     torch.zeros_like(c_z), torch.zeros_like(c_z), torch.ones_like(c_z)], dim=1).reshape(-1, 3, 3)
    rotation_matrix = rotation_matrix_z @ rotation_matrix_y @ rotation_matrix_x
    return rotation_matrix


def rotation_matrix_to_euler(rotation_matrix):
    """
    将旋转矩阵转为欧拉角, 欧拉角采用 Z-Y-X顺序 的内旋旋转（与X-Y-Z 的外旋一致）
    参数：
        rotation_matrix：旋转矩阵，形状为 (batch_size, 3, 3)
    返回：
        euler_angles：欧拉角，形状为 (batch_size, 3)
    """
    # 计算欧拉角
    sy = torch.sqrt(rotation_matrix[:, 0, 0] ** 2 + rotation_matrix[:, 1, 0] ** 2)
    x = torch.atan2(rotation_matrix[:, 2, 1], rotation_matrix[:, 2, 2])
    y = torch.atan2(-rotation_matrix[:, 2, 0], sy)
    z = torch.atan2(rotation_matrix[:, 1, 0], rotation_matrix[:, 0, 0])
    # 将弧度转为角度
    x, y, z = x * 180 / torch.pi, y * 180 / torch.pi, z * 180 / torch.pi
    # 组合
    ea = torch.stack((x, y, z), dim=1)
    return ea


ea1=torch.tensor([[   0.,   75.,   90.],
        [   0.,   78.,   90.],
        [   0.,  105.,   90.],
        [   0.,  102.,   90.],
        [   0.,  -75.,   90.],
        [   0., -105.,   90.],
        [   0.,  -65.,   90.],
        [   0., -125.,   90.]])
rm1=euler_to_rotation_matrix(ea1)


ea2=rotation_matrix_to_euler(rm1)

print("有false")
print(ea1==ea2) # 有false

rm2=euler_to_rotation_matrix(ea2)
print("全true")
print(rm1-rm2 <0.0001) # q全true