基礎(chǔ)理解，參照：https://www.cnblogs.com/Estranged-Tech/p/16903025.html

歐拉角、萬向節(jié)死鎖（鎖死）理解

一、歐拉角理解

舉例講解

歐拉角用三次獨(dú)立的繞確定的軸旋轉(zhuǎn)角度來表示姿態(tài)。如下圖所示

經(jīng)過三次旋轉(zhuǎn)，旋轉(zhuǎn)角度分別為𝛼、𝛽和 𝛾，由初始的𝑥𝑦𝑧坐標(biāo)系得到了最終的𝑥?𝑦?𝑧?
坐標(biāo)系。這就是歐拉角來表示姿態(tài)的方法。
如圖所示為航空航天中常用的歐拉角，圖中的𝜓、𝜃和𝜙
對應(yīng)于上圖中的𝛼、𝛽和 𝛾。

順規(guī)、內(nèi)旋與外旋

上面所舉的例子中，旋轉(zhuǎn)的順序是按照 𝑧?𝑦?𝑥 的順序來進(jìn)行旋轉(zhuǎn)的，并且每一次旋轉(zhuǎn)都是繞自身軸（運(yùn)動(dòng)軸）進(jìn)行的， 這只是歐拉角的一種表示方式。歐拉角共有12種表示方式。

• 順規(guī)
  順規(guī)即歐拉角三次轉(zhuǎn)動(dòng)的順序規(guī)定，歐拉角一共有12種順規(guī)

三個(gè)軸只用兩個(gè)的：Proper Euler angles (z?x?z, x?y?x, y?z?y, z?y?z, x?z?x, y?x?y）
三個(gè)軸全都用的：Tait-Bryan angles (x?y?z, y?z?x, z?x?y, x?z?y, z?y?x, y?x?z）

上文舉例所用的 𝑧?𝑦?𝑥是三個(gè)軸都用的一種順規(guī)，即 先繞𝑧軸旋轉(zhuǎn)，再繞𝑦軸旋轉(zhuǎn)，最后繞𝑥軸旋轉(zhuǎn)。、

• 內(nèi)旋與外旋
  根據(jù)繞旋轉(zhuǎn)軸的不同，可以分為內(nèi)旋和外旋。

內(nèi)旋 Intrinsic rotations：繞運(yùn)動(dòng)軸
外旋 Extrinsic rotations：繞固定軸

上文所舉的例子每次旋轉(zhuǎn)都是繞上一次旋轉(zhuǎn)所新產(chǎn)生的坐標(biāo)軸旋轉(zhuǎn)的（這句話有點(diǎn)繞），比如第二次旋轉(zhuǎn)繞的是𝑦″而不是固定的坐標(biāo)軸𝑦。所以是繞運(yùn)動(dòng)軸，即內(nèi)旋。

如下圖所示，每一次旋轉(zhuǎn)都是繞藍(lán)色的固定軸旋轉(zhuǎn)

如下圖所示，每一次旋轉(zhuǎn)都是繞紅色（第一張圖）、綠色（第二張圖）、紫色（第三張圖）的運(yùn)動(dòng)軸旋轉(zhuǎn)：


在繞軸旋轉(zhuǎn)的時(shí)候，順規(guī)有12種，內(nèi)外旋有2種，但是一般情況下，歐拉角都是說的繞自身軸（運(yùn)動(dòng)軸）旋轉(zhuǎn)，即 內(nèi)旋 。所以歐拉角共有 1×12=12種表示方式。

小結(jié)

• 首先說一個(gè)很有意思卻很不直觀的結(jié)論：三次繞固定軸旋轉(zhuǎn)的最終姿態(tài)和以相反順序三次繞運(yùn)動(dòng)軸旋轉(zhuǎn)的最終姿態(tài)相同。

• 一般情況下，每一個(gè)領(lǐng)域有自己默認(rèn)的歐拉角定義，也就是24種的其中之一。比如經(jīng)典力學(xué)中使用𝑧𝑥𝑧，量子力學(xué)使用的是𝑧𝑦𝑧，航空航天使用𝑧𝑦𝑥或𝑧𝑥𝑦。所以在跨行業(yè)或者跨模塊協(xié)作的時(shí)候，一定要問清楚對方是哪一種歐拉角。

二、數(shù)學(xué)公式

三、萬向節(jié)死鎖理解

• 說明
  歐拉角表示姿態(tài)的時(shí)候，會出現(xiàn)萬向節(jié)死鎖的情況，當(dāng)我使用傳感器（維特智能WT901c-485）讀取角度的時(shí)候，在𝑍𝑌𝑋的順規(guī)下，當(dāng)pitch角度為90或-90的時(shí)候，roll和yaw角度會亂飄，這就是遇到了萬向節(jié)的死鎖。不同的順規(guī)死鎖的情況不一樣，本文僅以𝑍𝑌𝑋說明死鎖的情況。
• 形象表示
  我們以手機(jī)為例，說明一下萬向節(jié)死鎖的情況
  注意，歐拉角表示姿態(tài)時(shí)，只能旋轉(zhuǎn)三次，這是理解死鎖的前提！
  首先這是一個(gè)手機(jī)，我們對其建立坐標(biāo)系，平行于手機(jī)長邊是 𝑋軸，平行于手機(jī)短邊是 𝑌軸，垂直于桌面是 𝑍軸。我們按照 𝑍-𝑌-𝑋的順規(guī)，對手機(jī)進(jìn)行旋轉(zhuǎn)：
  
  1.首先，先繞 𝑍軸旋轉(zhuǎn)得到 𝑋′𝑌′𝑍′坐標(biāo)系，如圖中綠色所示：
  
  2.然后對 𝑋′𝑌′𝑍′ 繞 𝑌′軸旋轉(zhuǎn)-90°，得到 𝑋″𝑌″𝑍″ ，如圖中藍(lán)色所示（此時(shí)旋轉(zhuǎn)后的 𝑋″ 軸軸與旋轉(zhuǎn)之前的 𝑍′軸重合）
  
  3.最后將 𝑋″𝑌″𝑍″ 繞 𝑋″軸旋轉(zhuǎn)得到 𝑋?𝑌?𝑍? 坐標(biāo)系，如圖中黃色所示。
  
• 這時(shí)候重點(diǎn)來了！！！

我們發(fā)現(xiàn)，這樣旋轉(zhuǎn)過后，手機(jī)的長邊是與桌面（地面）垂直的，手機(jī)的短邊是與桌面（地面）平行的。并且，無論第一步轉(zhuǎn) 𝑍軸轉(zhuǎn)了多少度，第三步轉(zhuǎn) 𝑋 軸轉(zhuǎn)了多少度，手機(jī)長邊與短邊都是這個(gè)狀態(tài)。
這就是歐拉角表示角度時(shí)的“萬向節(jié)死鎖”現(xiàn)象，按理說歐拉角可以表示三個(gè)自由度，即三個(gè)方向的旋轉(zhuǎn)。但是在這種情況下，只要𝛽=±90°，無論 𝛼與𝛾怎么取值，手機(jī)最終的長邊都會與地面垂直，這就好像少了一個(gè)自由度，只有2個(gè)自由度，所以稱之為“萬向節(jié)死鎖”。

• 那么為什么會出現(xiàn)這種情況呢？

一種感性的理解是這樣：我們在第二步旋轉(zhuǎn)的時(shí)候，將旋轉(zhuǎn)后的 𝑋″ 軸與旋轉(zhuǎn)之前的 𝑍′ 軸重合（這個(gè)我在上面旋轉(zhuǎn)演示的時(shí)候加粗說明了）。而第一步旋轉(zhuǎn)是繞 𝑍′ 軸旋轉(zhuǎn)的，第三步旋轉(zhuǎn)是繞 𝑋″軸旋轉(zhuǎn)的，所以兩軸重合意味著兩次旋轉(zhuǎn)繞的是同一個(gè)軸，所以說，三個(gè)自由度變?yōu)榱藘蓚€(gè)自由度。

• 數(shù)學(xué)解釋：
  

歐拉角與旋轉(zhuǎn)矩陣轉(zhuǎn)換中的相關(guān)問題：

• 先出結(jié)論：

# ea(人為給定的)轉(zhuǎn)rotmat 再轉(zhuǎn)ea后兩個(gè)ea的值存在不一致，且含義不一致（主要出現(xiàn)在y角度在一定范圍時(shí)時(shí)），但兩個(gè)不同的ea可以得到相同的旋轉(zhuǎn)矩陣。
# rotmat轉(zhuǎn)ea再轉(zhuǎn)rotmat（前后rotmat一致）再轉(zhuǎn)ea 前后ea含義是一致的，說明相同的rotmat可以獲得一致的ea
# rotmat轉(zhuǎn)ea再轉(zhuǎn)rotmat 前后rotmat是一致的，# 以上說明rotmat到ea是多射，但使用函數(shù)進(jìn)行rotmat轉(zhuǎn)換ea時(shí)可以獲得一致的ea結(jié)果；ea到rotmat是單射。以上說明雖存在多設(shè)情況但不影響我們場景的使用。（再驗(yàn)證下）

import torch
import mathdef euler_to_rotation_matrix(euler_angles):"""將歐拉角轉(zhuǎn)為旋轉(zhuǎn)矩陣。歐拉角采用 Z-Y-X順序 的內(nèi)旋旋轉(zhuǎn)（與X-Y-Z 的外旋一致）參數(shù)：euler_angles：歐拉角，形狀為 (batch_size, 3)返回：rotation_matrix：旋轉(zhuǎn)矩陣，形狀為 (batch_size, 3, 3)"""# 將歐拉角轉(zhuǎn)為弧度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# 計(jì)算旋轉(zhuǎn)矩陣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_xreturn rotation_matrixdef rotation_matrix_to_euler(rotation_matrix):"""將旋轉(zhuǎn)矩陣轉(zhuǎn)為歐拉角, 歐拉角采用 Z-Y-X順序 的內(nèi)旋旋轉(zhuǎn)（與X-Y-Z 的外旋一致）參數(shù)：rotation_matrix：旋轉(zhuǎn)矩陣，形狀為 (batch_size, 3, 3)返回：euler_angles：歐拉角，形狀為 (batch_size, 3)"""# 計(jì)算歐拉角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])# 將弧度轉(zhuǎn)為角度x, y, z = x * 180 / torch.pi, y * 180 / torch.pi, z * 180 / torch.pi# 組合ea = torch.stack((x, y, z), dim=1)return eaea1=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) # 有falserm2=euler_to_rotation_matrix(ea2)
print("全true")
print(rm1-rm2 <0.0001) # q全true