标签：phi psi 欧拉角 矩阵 cos 旋转 theta sin

旋转矩阵与欧拉角

参考文献：
[Computing Euler angles from a rotation matrix ——Gregory G. Slabaugh]

三个主轴的旋转矩阵

右手坐标系，逆时针转动角度为正（右手螺旋定则确定）。

关于绕 \(x\) 轴旋转 \(\psi\) 弧度 的主动旋转定义为

\[ R_x(\psi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\psi & -\sin\psi \\ 0 & \sin\psi & \cos\psi \end{bmatrix} \]

关于绕 \(y\) 轴旋转 \(\theta\) 弧度 的主动旋转定义为

\[ R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \]

关于绕 \(z\) 轴旋转 \(\psi\) 弧度 的主动旋转定义为

\[ R_z(\phi) = \begin{bmatrix} \cos\phi & -\sin\phi & 0 \\ \sin\phi & \cos\phi & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

广义旋转矩阵

一般旋转矩阵具有如下形式，

\[ R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \]

旋转矩阵与转角

旋转矩阵可以认为是每个主轴都有一次旋转的共有三个旋转的序列，这也能与空间中的旋转是三个自由度对应。

将旋转矩阵拆解成 3 次旋转角度来做 3 次旋转连乘，有以下注意事项：

• 由于矩阵乘法是不可交换的，因此轴旋转先后的顺序将影响结果
• 旋转转轴也需要明确定义，是对“固定不动”的转轴旋转？还是对“转动的坐标系当下所在”的转轴旋转？

两种旋转转轴：

• 对方向“固定不动”的转轴旋转：Fixed angles
• 对“转动的坐标系当下所在”的转轴反向旋转：Euler angles

Fixed angles

Fixed angles 是以某个固定转轴来旋转坐标系的。

以绕固定轴X、Y、Z的顺序旋转为例，将其表示为旋转矩阵，

\[\begin{aligned} R & = R_z(\phi)R_y(\theta)R_x(\psi) \\ & = \begin{bmatrix} \cos\theta\cos\phi & \sin\psi\sin\theta\cos\phi − \cos\psi\sin\phi &\cos\psi\sin\theta\cos\phi + \sin\psi\sin\phi \\ \cos\theta\sin\phi & \sin\psi\sin\theta\sin\phi + \cos\psi\cos\phi & \cos\psi\sin\theta \sin\phi − \sin\psi\cos\phi \\ −\sin\theta & \sin\psi\cos\theta & \cos\psi\cos\theta \end{bmatrix} \end{aligned} \]

Euler angles

Fixed angles 是以自身坐标轴为转轴来旋转坐标系的。

求解角度

给定旋转矩阵 \(R\)，我们可以通过将 \(R\) 中的每个元素与矩阵乘积 \(R_z(\phi)R_y(\theta)R_x(\psi)\) 中的相应元素相等来计算欧拉角 \(\psi\)、\(\theta\) 和 \(\phi\)，这产生了九个可用于求欧拉角的方程。

求解 \(\theta\) 可能的两个角度

通过旋转矩阵的元素 \(r_{31}\) ，可以得到：

\[ r_{31} = −\sin\theta \]

再通过反三角函数就可以得出：

\[ \theta = −\arcsin{r_{31}} \tag{1} \]

若 \(r_{31} \not= {\pm1}\)，由于 \(\sin(\pi−\theta) = \sin\theta\)，实际上有两个不同的 \(\theta\) 值满足方程 \((1)\)；因此，这两个值都是能作为有效解的：

\[\begin{aligned} \theta_1 & = −\arcsin{r_{31}} \\ \theta_2 & = \pi − \theta_1 = \pi + \arcsin{r_{31}} \end{aligned} \]

若 \(r_{31} = {\pm1}\)，将有特殊处理的地方，这个在稍后处理。

因此，使用旋转矩阵的元素 \(r_{31}\)，我们能够确定 \(\theta\) 的两个可能值。

求解 \(\psi\) 的角度

为了找到 \(\psi\) 的值，观察旋转矩阵可以得到：

\[ \frac{r_{32}}{r_{33}} = \frac{\sin\psi\cos\theta}{\cos\psi\cos\theta} = \tan\psi \tag{2} \]

使用这个方程可以求解出 \(\psi\)：

\[ \psi = atan2(r_{32}, r_{33}) \]

其中 \(atan2(y, x)\) 是两个变量 \(x\) 和 \(y\) 的反正切。它类似于计算 \(y/x\) 的反正切，只是两个参数的正负符号用于确定结果的象限，该象限位于 \([-\pi，\pi]\) 范围内。函数 \(atan2(y, x)\) 在许多编程语言中都可用。

在解方程 \((2)\) 时必须小心：如果 \(\cos\theta > 0\)，则 \(\psi = atan2(r_{32}, r_{33})\)；然而，当 \(\cos\theta < 0\) 时，\(\psi = atan2(-r_{32}, -r_{33})\)。处理此问题的简单方法是使用以下式子求解出 \(\psi\)：

\[ \psi = atan2(\frac{r_{32}}{\cos\theta}, \frac{r_{33}}{\cos\theta}) \tag{3} \]

除 \(\cos\theta = 0\) 之外，方程 \((3)\) 适用于所有情况，这也在稍后处理这个特殊情况。对于每个 \(\theta\) 值，我们使用方程 \((3)\) 计算相应的 \(\psi\) 值，得出：

\[\begin{align} \psi_1 & = atan2(\frac{r_{32}}{\cos\theta_1}, \frac{r_{33}}{\cos\theta_1}) \tag{4} \\ \psi_2 & = atan2(\frac{r_{32}}{\cos\theta_2}, \frac{r_{33}}{\cos\theta_2}) \tag{5} \end{align} \]

求解 \(\phi\) 的角度

类似求解 \(\psi\) 的分析也适用于求解 \(\phi\)，观察旋转矩阵可以得到：

\[ \frac{r_{21}}{r_{11}} = \frac{\cos\theta\sin\phi}{\cos\theta\cos\phi } = \tan\phi \]

使用这个方程可以求解出 \(\phi\)：

\[ \phi = atan2(\frac{r_{21}}{\cos\theta}, \frac{r_{11}}{\cos\theta}) \tag{6} \]

同样，除了 \(\cos\theta = 0\) 之外，方程 \((6)\) 对所有情况都有效。这也在稍后处理这个特殊情况。对于每个 \(\theta\) 值，我们使用方程 \((3)\) 计算相应的 \(\psi\) 值，得出：

\[\begin{align} \phi_1 = atan2(\frac{r_{21}}{\cos\theta_1}, \frac{r_{11}}{\cos\theta_1}) \tag{7} \\ \phi_2 = atan2(\frac{r_{21}}{\cos\theta_2}, \frac{r_{11}}{\cos\theta_2}) \tag{8} \end{align} \]

当 \(\cos\theta \not= 0\) 的两组解

对于\(\cos\theta \not= 0\)的情况，通过上述我们可以得出两组欧拉角的解，它们再现了旋转矩阵，即：

\[ (\psi_1, \theta_1, \phi_1) \\ (\psi_2, \theta_2, \phi_2) \]

这两个解都是有效的。

处理特殊情况 \(\cos\theta = 0\)

如果旋转矩阵的元素 \(r_{31} = {\pm1}\)，也就是分别对应于 \(\theta = \pm \pi/2\)，也就是 \(\cos\theta = 0\)，上述方法将不起作用。

当我们尝试使用上述方法求解 \(\psi\) 和 \(\phi\) 的可能值时，会出现问题，因为旋转矩阵的元素 \(r_{11}\)、\(r_{21}\)、\(r_{32}\) 和 \(r_{33}\) 的值都将等于 \(0\)，也就是方程 \((3)\) 和 \((6)\) 将变为：

\[\begin{aligned} \psi & = atan2(\frac{0}{0}, \frac{0}{0}) \\ \phi & = atan2(\frac{0}{0}, \frac{0}{0}) \end{aligned} \]

在这种情况下，\(r_{11}\)、\(r_{21}\)、\(r_{32}\) 和 \(r_{33}\) 不限制 \(\psi\) 和 \(\phi\) 的值。因此，我们必须使用其它旋转矩阵的元素来计算 \(\psi\) 和 \(\phi\) 的值。

\(\theta = \pi/2\) 的情况：考虑 \(\theta = \pi/2\)，那么，

\[\begin{aligned} r_{12} & = \sin\psi\cos\phi − \cos\psi\sin\phi = \sin(\psi − \phi) \\ r_{13} & = \cos\psi\cos\phi + \sin\psi\sin\phi = \cos(\psi - \phi) \\ r_{22} & = \sin\psi\sin\phi + \cos\psi\cos\phi = \cos(\psi − \phi) = r_{13} \\ r_{23} & = \cos\psi\sin\phi − \sin\psi\cos\phi = −\sin(\psi − \phi) = -r_{12} \end{aligned} \]

使用 \(r_{12}\) 和 \(r_{13}\) 的方程，我们发现:

\[\begin{aligned} (\psi − \phi) & = atan2(r_{12}, r_{13}) \\ \psi & = \phi + atan2(r_{12}, r_{13}) \end{aligned} \]

\(\theta = -\pi/2\) 的情况：毫不奇怪，当 \(\theta = -\pi/2\)，类似的结果也成立，

\[\begin{aligned} r_{12} & = -\sin\psi\cos\phi − \cos\psi\sin\phi = -\sin(\psi + \phi) \\ r_{13} & = -\cos\psi\cos\phi + \sin\psi\sin\phi = -\cos(\psi + \phi) \\ r_{22} & = -\sin\psi\sin\phi + \cos\psi\cos\phi = \cos(\psi + \phi) = -r_{13} \\ r_{23} & = -\cos\psi\sin\phi − \sin\psi\cos\phi = −\sin(\psi + \phi) = r_{12} \end{aligned} \]

同样，使用 \(r_{12}\) 和 \(r_{13}\) 的方程，我们发现:

\[\begin{aligned} (\psi + \phi) & = atan2(-r_{12}, -r_{13}) \\ \psi & = -\phi + atan2(-r_{12}, -r_{13}) \end{aligned} \]

至此，就解决了从旋转矩阵求解所有情况欧拉角。

在 \(\theta = \pm \pi/2\) 的情况下，可以发现 \(\psi\) 和 \(\phi\) 是相连的，也就是 \(\psi\) 和 \(\phi\) 本应该有 2 个自由度，却降为了 1 个自由度，这种现象被称为万向锁。

尽管在这种情况下，\(\psi\) 和 \(\phi\) 有无限多组的解，但在实践中，人们往往对找到一个解的方案感兴趣。对此，可以设置 \(\phi = 0\) 并方便地如上所述计算 \(\psi\)。

这里只是对于矩阵乘积 \(R_z(\phi)R_y(\theta)R_x(\psi)\) 做出的例子，其它旋转顺序和旋转方式会有与其对应的矩阵乘积的表达式，要按照实际情况，类似地依上所述去求解欧拉角。

标签：phi,psi,欧拉角,矩阵,cos,旋转,theta,sin
From： https://www.cnblogs.com/champrin/p/17643082.html