旋转几何 (Rotation Geometry)

约 4639 个字 9 张图片 预计阅读时间 15 分钟

旋转几何 (Rotation Geometry)

在机器人学中，描述姿态的核心是旋转。我们使用 旋转矩阵 (Rotation Matrix) 来精确描述一个坐标系相对于另一个坐标系的旋转状态。

下面是绕三个主轴（Z, Y, X）旋转的基础旋转矩阵：

绕 Z 轴旋转角度 \(\alpha\)
$$
\begin{bmatrix} \cos \alpha & -\sin \alpha & 0 \ \sin \alpha & \cos \alpha & 0 \ 0 & 0 & 1 \end{bmatrix}
$$
绕 Y 轴旋转角度 \(\beta\)
$$
\begin{bmatrix} \cos \beta & 0 & \sin \beta \ 0 & 1 & 0 \ -\sin \beta & 0 & \cos \beta \end{bmatrix}
$$
绕 X 轴旋转角度 \(\gamma\)
$$
\begin{bmatrix} 1 & 0 & 0 \ 0 & \cos \gamma & -\sin \gamma \ 0 & \sin \gamma & \cos \gamma \end{bmatrix}
$$

2.1 位置与姿态 (Position & Orientation)

• 为了在空间中操纵一个物体，我们必须能够描述末端执行器 (end-effector) 或被操纵物体的姿态 (orientation)、位置 (position)，以及它们的时间导数（即速度）。
• 对运动的分析被称为运动学 (kinematics)。它是一门几何科学，仅限于对运动进行纯粹的几何描述。

坐标系 (Coordinate Frame)

• 由于机械臂的连杆之间可以相互旋转或平移，我们需要为每个连杆建立一个本体坐标系 (body-attached coordinate frames) \(\Sigma_1, \Sigma_2, \Sigma_3, \dots\) 来确定它们的相对配置。
• 同时，还需要一个全局固定的参考坐标系 (global fixed reference frame) \(\Sigma_0\)。（世界坐标系）

旋转 (Rotation)

• 每个机器臂的刚性连杆都附着一个坐标系 \(\Sigma\)。
• 连杆相对于其相邻连杆进行旋转。
• 核心问题: 一个点 P 固定在一个连杆上，它在连杆的本体坐标系 o-xyz 中的坐标为 \((x_p, y_p, z_p)\)。当这个连杆相对于一个固定的参考坐标系 O-XYZ 旋转后，我们如何确定点 P 的新位置？

不同坐标系中的向量表示

决定刚体位姿的向量在不同坐标系中有不同的表示。

• 在世界坐标系 O-XYZ 中: \(^0\mathbf{r} = X\mathbf{I} + Y\mathbf{J} + Z\mathbf{K}\)
• 在本体坐标系 o-xyz 中: \(^1\mathbf{r} = x\mathbf{i} + y\mathbf{j} + z\mathbf{k}\)

旋转变换矩阵 (Rotation Transformation Matrix)

点 P 在 OXYZ 中的坐标，是向量 \(\mathbf{r}\) 在 OXYZ 各个轴上的投影。利用内积 (inner product) 的定义，我们有：
$$
X = \mathbf{I} \cdot \mathbf{r} = \mathbf{I} \cdot (x\mathbf{i} + y\mathbf{j} + z\mathbf{k}) $$
$$
Y = \mathbf{J} \cdot \mathbf{r} = \mathbf{J} \cdot (x\mathbf{i} + y\mathbf{j} + z\mathbf{k})$$
$$
Z = \mathbf{K} \cdot \mathbf{r} = \mathbf{K} \cdot (x\mathbf{i} + y\mathbf{j} + z\mathbf{k})
$$
整理成矩阵形式：
$$
^0\mathbf{r} =
\begin{bmatrix} X \ Y \ Z \end{bmatrix} =
\begin{bmatrix}
\mathbf{I} \cdot \mathbf{i} & \mathbf{I} \cdot \mathbf{j} & \mathbf{I} \cdot \mathbf{k} \
\mathbf{J} \cdot \mathbf{i} & \mathbf{J} \cdot \mathbf{j} & \mathbf{J} \cdot \mathbf{k} \
\mathbf{K} \cdot \mathbf{i} & \mathbf{K} \cdot \mathbf{j} & \mathbf{K} \cdot \mathbf{k}
\end{bmatrix}
\begin{bmatrix} x \ y \ z \end{bmatrix} =
^0R_1 \ ^1\mathbf{r}
$$
其中 \(^0R_1\) 被称为 旋转变换矩阵 (rotation transformation matrix)。

旋转变换矩阵的核心功能

• \(^0R_1\) 将点 P 在坐标系 \(\Sigma_1\) 中的坐标，转换为其在坐标系 \(\Sigma_0\) 中的坐标。
  $$
  ^0R_1 =
  \begin{bmatrix}
  \mathbf{I} \cdot \mathbf{i} & \mathbf{I} \cdot \mathbf{j} & \mathbf{I} \cdot \mathbf{k} \
  \mathbf{J} \cdot \mathbf{i} & \mathbf{J} \cdot \mathbf{j} & \mathbf{J} \cdot \mathbf{k} \
  \mathbf{K} \cdot \mathbf{i} & \mathbf{K} \cdot \mathbf{j} & \mathbf{K} \cdot \mathbf{k}
  \end{bmatrix}
  $$

• 同样地，我们可以将 P 在 \(\Sigma_0\) 中的坐标转换为在 \(\Sigma_1\) 中的坐标：
  $$
  ^1\mathbf{r} =
  \begin{bmatrix} x \ y \ z \end{bmatrix} =
  \begin{bmatrix}
  \mathbf{i} \cdot \mathbf{I} & \mathbf{i} \cdot \mathbf{J} & \mathbf{i} \cdot \mathbf{K} \
  \mathbf{j} \cdot \mathbf{I} & \mathbf{j} \cdot \mathbf{J} & \mathbf{j} \cdot \mathbf{K} \
  \mathbf{k} \cdot \mathbf{I} & \mathbf{k} \cdot \mathbf{J} & \mathbf{k} \cdot \mathbf{K}
  \end{bmatrix}
  \begin{bmatrix} X \ Y \ Z \end{bmatrix} =
  ^1R_0 \ ^0\mathbf{r}
  $$

• 一个至关重要的性质是，这两个矩阵互为转置和逆：
  $$
  ^1R_0 = {^0R_1}^T = {^0R_1}^{-1}
  $$

旋转矩阵的性质 (Properties)

1. \(R\) 是一个正交矩阵 (orthogonal matrix)，因为：
   $$
   {^0R_1}^T = {^0R_1}^{-1}
   $$

2. 旋转变换矩阵也称为方向余弦矩阵 (matrix of direction cosines)，因为它的元素都是向量 \(\mathbf{r}\) (原文如此，但更准确地说是坐标系1的基向量) 相对于坐标系0各轴的方向余弦。
   $$
   ^1R_0 =
   \begin{bmatrix}
   \mathbf{i} \cdot \mathbf{I} & \mathbf{i} \cdot \mathbf{J} & \mathbf{i} \cdot \mathbf{K} \
   \mathbf{j} \cdot \mathbf{I} & \mathbf{j} \cdot \mathbf{J} & \mathbf{j} \cdot \mathbf{K} \
   \mathbf{k} \cdot \mathbf{I} & \mathbf{k} \cdot \mathbf{J} & \mathbf{k} \cdot \mathbf{K}
   \end{bmatrix} =
   \begin{bmatrix}
   \cos(\mathbf{I}, \mathbf{i}) & \cos(\mathbf{I}, \mathbf{j}) & \cos(\mathbf{I}, \mathbf{k}) \
   \cos(\mathbf{J}, \mathbf{i}) & \cos(\mathbf{J}, \mathbf{j}) & \cos(\mathbf{J}, \mathbf{k}) \
   \cos(\mathbf{K}, \mathbf{i}) & \cos(\mathbf{K}, \mathbf{j}) & \cos(\mathbf{K}, \mathbf{k})
   \end{bmatrix}
   $$

3. 矩阵 \(R\) 的三列是本体坐标系 (frame 1) 的三个主轴在世界坐标系 (frame 0) 中的表示，分别记为 \(\mathbf{n}, \mathbf{s}, \mathbf{a}\)。
   $$
   \mathbf{n} = \begin{bmatrix} n_x \ n_y \ n_z \end{bmatrix}, \
   \mathbf{s} = \begin{bmatrix} s_x \ s_y \ s_z \end{bmatrix}, \
   \mathbf{a} = \begin{bmatrix} a_x \ a_y \ a_z \end{bmatrix}
   $$
   $$
   ^0R_1 \triangleq [\mathbf{n} \ \mathbf{s} \ \mathbf{a}] = \begin{bmatrix}
   \mathbf{I} \cdot \mathbf{i} & \mathbf{I} \cdot \mathbf{j} & \mathbf{I} \cdot \mathbf{k} \
   \mathbf{J} \cdot \mathbf{i} & \mathbf{J} \cdot \mathbf{j} & \mathbf{J} \cdot \mathbf{k} \
   \mathbf{K} \cdot \mathbf{i} & \mathbf{K} \cdot \mathbf{j} & \mathbf{K} \cdot \mathbf{k}
   \end{bmatrix}
   $$

4. 矩阵中只有 3 个独立变量，因为它包含 6 个约束条件。

   • 单位向量约束 (3个):
     $$ |\mathbf{n}| = |\mathbf{s}| = |\mathbf{a}| = 1 $$

   • 正交约束 (3个):
     $$ \mathbf{n} \times \mathbf{s} = \mathbf{a}, \quad \mathbf{s} \times \mathbf{a} = \mathbf{n}, \quad \mathbf{a} \times \mathbf{n} = \mathbf{s} $$

2.2 坐标变换实例

绕世界坐标轴旋转 (Rotation about Global Axes)

绕 Z 轴旋转

一个点 \((x,y,z)\) 绕世界坐标系 Z 轴旋转 \(\alpha\) 角后，其在世界坐标系中的新坐标 \((X,Y,Z)\) 为：
$$\begin{align}
X &= x \cos \alpha - y \sin \alpha \
Y &= x \sin \alpha + y \cos \alpha \
Z &= z
\end{align}
$$
矩阵形式为：
$$
^0\mathbf{r} =
\begin{bmatrix} X \ Y \ Z \end{bmatrix} =
\begin{bmatrix} \cos \alpha & -\sin \alpha & 0 \ \sin \alpha & \cos \alpha & 0 \ 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} x \ y \ z \end{bmatrix} = R_z(\alpha) \ ^1\mathbf{r}
$$
这个结果也可以通过方向余弦的定义得到。

三个基本旋转矩阵

• 绕 X 轴旋转 \(\alpha\) 角
  $$
  R_x(\alpha) = \begin{bmatrix} 1 & 0 & 0 \ 0 & \cos \alpha & -\sin \alpha \ 0 & \sin \alpha & \cos \alpha \end{bmatrix}
  $$

• 绕 Y 轴旋转 \(\beta\) 角
  $$
  R_y(\beta) = \begin{bmatrix} \cos \beta & 0 & \sin \beta \ 0 & 1 & 0 \ -\sin \beta & 0 & \cos \beta \end{bmatrix}
  $$

• 绕 Z 轴旋转 \(\gamma\) 角
  $$
  R_z(\gamma) = \begin{bmatrix} \cos \gamma & -\sin \gamma & 0 \ \sin \gamma & \cos \gamma & 0 \ 0 & 0 & 1 \end{bmatrix}
  $$

示例 EX 2-1-1

问题

一个在 frame 0 中的向量 \(^1\mathbf{r} =[1,3,2]^T\)。当 frame 1 相对于 frame 0 绕 Z 轴旋转了 60 度后，求其在世界坐标系 (frame 0) 中的位置 \(^0\mathbf{r}\)。

解:
应该是 \(^0\mathbf{r} = R_z(60^\circ) \ ^1\mathbf{r}\)。
$$\begin{align}
^0\mathbf{r} &=
\begin{bmatrix} \cos 60^\circ & -\sin 60^\circ & 0 \ \sin 60^\circ & \cos 60^\circ & 0 \ 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} 1 \ 3 \ 2 \end{bmatrix}\
&=
\begin{bmatrix} 0.5 & -0.8660 & 0 \ 0.8660 & 0.5 & 0 \ 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} 1 \ 3 \ 2 \end{bmatrix}\
&=
\begin{bmatrix} 0.5 - 3 \times 0.8660 \ 0.8660 + 3 \times 0.5 \ 2 \end{bmatrix}\
&=
\begin{bmatrix} -2.098 \ 2.366 \ 2 \end{bmatrix}
\end{align}\
$$

示例 EX 2-1-2

问题

考虑一个刚体 B，它以 0.3 rad/s 的速率持续绕世界坐标系 G 的 Y 轴转动。求刚体上一点 P 在 G 中的坐标及其速度。

解:

1. 旋转变换矩阵: 角度 \(\beta\) 是时间的函数，\(\beta = 0.3t\)。
   $$
   Q_{Y,\beta} =
   \begin{bmatrix} \cos 0.3t & 0 & \sin 0.3t \ 0 & 1 & 0 \ -\sin 0.3t & 0 & \cos 0.3t \end{bmatrix}
   $$

2. 坐标关系: 点 P 在世界坐标系 G 中的坐标 \(^G\mathbf{r}_P\) 和在本体坐标系 B 中的坐标 \(^B\mathbf{r}_P\) 的关系为 \(^G\mathbf{r}_P = Q_{Y,\beta} \ ^B\mathbf{r}_P\)。
   $$
   \begin{bmatrix} X \ Y \ Z \end{bmatrix} =
   \begin{bmatrix} \cos 0.3t & 0 & \sin 0.3t \ 0 & 1 & 0 \ -\sin 0.3t & 0 & \cos 0.3t \end{bmatrix}
   \begin{bmatrix} x \ y \ z \end{bmatrix} =
   \begin{bmatrix} x \cos 0.3t + z \sin 0.3t \ y \ z \cos 0.3t - x \sin 0.3t \end{bmatrix}
   $$

3. 速度: 对坐标关系式关于时间 \(t\) 求导，得到点 P 的速度 \(^G\mathbf{v}_P\)。
   $$
   ^G\mathbf{v}P = \dot{Q}} \ ^B\mathbf{rP =
   \frac{d}{dt} \left( Q} \right) \ ^B\mathbf{rP
   $$
   $$
   \dot{Q}} = 0.3 \begin{bmatrix} -\sin 0.3t & 0 & \cos 0.3t \ 0 & 0 & 0 \ -\cos 0.3t & 0 & -\sin 0.3t \end{bmatrix
   $$
   因此速度为：
   $$
   ^G\mathbf{v}_P = 0.3
   \begin{bmatrix} z \cos 0.3t - x \sin 0.3t \ 0 \ -x \cos 0.3t - z \sin 0.3t \end{bmatrix}
   $$

2.3 连续旋转 (Successive Rotation)

1. 绕世界坐标系旋转 (Rotation About Global Axes)

如果 \(^0R_1\) 和 \(^0R_2\) 是两次连续的、绕世界坐标系轴的旋转。
* 第一次旋转后: \(^0\mathbf{r}_1 = ^0R_1 \mathbf{r}\)
* 第二次旋转后: \(^0\mathbf{r}_2 = ^0R_2 \ ^0\mathbf{r}_1\)

将两者结合，我们得到：
$$
^0\mathbf{r}_2 = ^0R_2 (^0R_1 \mathbf{r}) = (^0R_2 \ ^0R_1) \mathbf{r}
$$
总的旋转矩阵 \(^0R\) 是通过前乘 (pre-multiplied) 得到的：
$$
^0R = ^0R_2 \ ^0R_1
$$
对于 \(n\) 次旋转，总矩阵为：
$$
^0R = ^0R_n \cdots ^0R_2 \ ^0R_1
$$

核心规则：世界坐标系旋转

如果旋转是绕固定的世界坐标系进行的，新的旋转矩阵要乘在左边 (pre-multipied)。矩阵相乘的顺序与旋转发生的顺序相反。

示例 EX 2-1-3

问题

求一个物体先绕世界坐标系 X 轴旋转 90°(\(\pi/2\))，再绕世界坐标系 Y 轴旋转 -90°(\(-\pi/2\)) 后的总旋转矩阵。

解:
根据前乘规则，总旋转 \(^0R_1 = R_y(-\pi/2) \ R_x(\pi/2)\)。
$$
^0R_1 =
\underbrace{
\begin{bmatrix} \cos(-\frac{\pi}{2}) & 0 & \sin(-\frac{\pi}{2}) \ 0 & 1 & 0 \ -\sin(-\frac{\pi}{2}) & 0 & \cos(-\frac{\pi}{2}) \end{bmatrix}
}{R_y(-\pi/2)}
\underbrace{
\begin{bmatrix} 1 & 0 & 0 \ 0 & \cos(\frac{\pi}{2}) & -\sin(\frac{\pi}{2}) \ 0 & \sin(\frac{\pi}{2}) & \cos(\frac{\pi}{2}) \end{bmatrix}
}
$$
$$
^0R_1 =
\begin{bmatrix} 0 & 0 & -1 \ 0 & 1 & 0 \ 1 & 0 & 0 \end{bmatrix}
\begin{bmatrix} 1 & 0 & 0 \ 0 & 0 & -1 \ 0 & 1 & 0 \end{bmatrix}
=
\begin{bmatrix} 0 & -1 & 0 \ 0 & 0 & -1 \ 1 & 0 & 0 \end{bmatrix}
$$

2. 绕本体坐标系旋转 (Rotation About Local Axes)

假设第一次旋转是绕 frame 0 的主轴，第二次旋转是绕旋转后的 frame 1 的主轴。
* 第一次旋转: \(^0\mathbf{r} = ^0R_1 \ ^1\mathbf{r}\)
- 第二次旋转(在 frame 1 中描述): \(^1\mathbf{r} = ^1R_2 \ ^2\mathbf{r}\)

结合两者，我们得到最终点在 frame 0 中的坐标：
$$
^0\mathbf{r} = ^0R_1 \ (^1R_2 \ ^2\mathbf{r}) = (^0R_1 \ ^1R_2) \ ^2\mathbf{r}
$$
总的旋转矩阵 \(^0R_2\) 是通过后乘 (post-multiplied) 得到的：
$$
^0R_2 = ^0R_1 \ ^1R_2
$$

核心规则：本体坐标系旋转

如果旋转是绕移动的本体坐标系进行的，新的旋转矩阵要乘在右边 (后乘)。矩阵相乘的顺序与旋转发生的顺序相同。

总结：旋转的复合 (Composition of Rotations)

• 如果一次旋转是相对于本体坐标系 (local frame)，则该旋转矩阵是相对的，应该被后乘 (post-multiplied)。
• 如果一次旋转是相对于世界坐标系 (global frame)，则该旋转矩阵是绝对的，应该被前乘 (pre-multiplied)。

示例 EX 2-1-4

问题

求一个物体先绕其自身 X 轴旋转 90°(\(\pi/2\))，再绕其新的 y 轴旋转 -90°(\(-\pi/2\)) 后的总旋转矩阵。

解:
根据后乘规则，总旋转 \(^0R_1 = R_x(\pi/2) \ R_y(-\pi/2)\)。
$$
^0R_1 =
\underbrace{
\begin{bmatrix} 1 & 0 & 0 \ 0 & \cos(\frac{\pi}{2}) & -\sin(\frac{\pi}{2}) \ 0 & \sin(\frac{\pi}{2}) & \cos(\frac{\pi}{2}) \end{bmatrix}
}{R_x(\pi/2)}
\underbrace{
\begin{bmatrix} \cos(-\frac{\pi}{2}) & 0 & \sin(-\frac{\pi}{2}) \ 0 & 1 & 0 \ -\sin(-\frac{\pi}{2}) & 0 & \cos(-\frac{\pi}{2}) \end{bmatrix}
}
$$
$$
^0R_1 =
\begin{bmatrix} 1 & 0 & 0 \ 0 & 0 & -1 \ 0 & 1 & 0 \end{bmatrix}
\begin{bmatrix} 0 & 0 & -1 \ 0 & 1 & 0 \ 1 & 0 & 0 \end{bmatrix}
=
\begin{bmatrix} 0 & 0 & -1 \ -1 & 0 & 0 \ 0 & 1 & 0 \end{bmatrix}
$$

2.4 姿态的表示 (Representation of Orientation)

• 旋转矩阵是表示刚体姿态的多种模型之一。
• 数学上，任何一个给定的姿态都可以通过三次连续的、绕坐标系轴的旋转来表示。这意味着任何方向余弦矩阵都可以是三个基本旋转矩阵的乘积。
• 姿态可以由与基本旋转矩阵相关的三个角度来定义。
• 三角度表示法可以使用绕 (固定的) 世界坐标系 的角度，和/或绕 (移动的) 本体坐标系 的角度来完成。

1. 固定角表示法 (Fixed-angle Representation)

* 如果三次旋转都是绕世界坐标系的主轴进行的，我们可以使用三个固定角 (Fixed Angle) 表示法来描述一个姿态。
* 通常有12种不同的独立组合。

2. RPY 角

• 关于固定轴的三角度可用于==确定刚体的姿态==，它们也被称为 【世界坐标系】的滚转-俯仰-偏航角 (global roll-pitch-yaw angles)。
• \(\alpha, \beta, \gamma\) 被称为 X-Y-Z RPY 角。
• 计算公式（前乘）：

\[ ^0R_1 = R_z(\gamma) R_y(\beta) R_x(\alpha) \]

$$

\mathbf{R} =
\begin{bmatrix}
\cos\gamma & -\sin\gamma & 0 \
\sin\gamma & \cos\gamma & 0 \
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
\cos\beta & 0 & \sin\beta \
0 & 1 & 0 \
-\sin\beta & 0 & \cos\beta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \
0 & \cos\alpha & -\sin\alpha \
0 & \sin\alpha & \cos\alpha
\end{bmatrix}
$$

• 12种固定角表示：
  • ABC 型 (如 ZYX, XYZ): ZXY, ZYX, XYZ, XZY, YXZ, YZX
  • ABA 型 (如 ZXZ, ZYZ): ZXZ, ZYZ, XYX, XZX, YXY, YZY

考虑方法：同一个轴不能连续旋转两次。

示例 EX 2-2-1

问题

一个本体坐标系依次绕世界坐标系的 Z-Y-X 轴旋转 30°, 45°, 90°。用三个固定角确定其姿态。

解:
这是一个 Z-Y-X 固定角 表示。旋转顺序是Z, Y, X，角度分别是 \(\alpha=30^\circ, \beta=45^\circ, \gamma=90^\circ\)。
根据前乘规则，总旋转矩阵：
$$

\begin{align*}
{}^0R_1 &= R_x(\gamma) R_y(\beta) R_z(\alpha) \

&=R_x(90^\circ) R_y(45^\circ) R_z(30^\circ)\
&= \begin{bmatrix}
c\gamma & -s\gamma & 0 \
s\gamma & c\gamma & 0 \
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
c\beta & 0 & s\beta \
0 & 1 & 0 \
-s\beta & 0 & c\beta
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \
0 & c\alpha & -s\alpha \
0 & s\alpha & c\alpha
\end{bmatrix} \
&= \begin{bmatrix}
c\alpha c\beta & -s\alpha c\beta & s\beta \
s\alpha c\gamma + c\alpha s\beta s\gamma & c\alpha c\gamma - s\alpha s\beta s\gamma & -c\beta s\gamma \
s\alpha s\gamma - c\alpha s\beta c\gamma & c\alpha s\gamma + s\alpha s\beta c\gamma & c\beta c\gamma
\end{bmatrix} \
&= \begin{bmatrix}
0.6124 & -0.3536 & 0.7071 \
0.6124 & -0.3536 & -0.7071 \
0.5000 & 0.8660 & 0
\end{bmatrix}
\end{align*}
$$

（Tips：也可以带入）

3. 欧拉角表示法 (Euler-angle Representation)

欧拉证明，任何两个共原点的独立正交坐标系，都可以通过一系列三次绕【本体坐标系轴】的旋转联系起来，其中没有两次连续的旋转是绕同一个轴的。这种绕本体坐标系的三角度旋转被称为欧拉角 (Euler Angles)。

• 同样有12种不同的组合。
  • ABC 型: Zxy, Zyx, Xyz, Xzy, Yxz, Yzx
  • ABA 型: Zxz, Zyz, Xyx, Xzx, Yxy, Yzy

Z-x-z欧拉角

这是一个特殊的例子，它执行：

1. Step1: 绕 Z 轴旋转 \(\alpha\) (Precession/进动)
2. Step2: 绕新的 x 轴旋转 \(\beta\) (Nutation/章动)
3. Step3: 绕更新后的 z 轴旋转 \(\gamma\) (Spin/自旋)

计算公式（后乘）：
$$
\begin{align}
^0R_1 &= R_z(\alpha) R_x(\beta) R_z(\gamma) \
&= \begin{bmatrix} c\alpha & -s\alpha & 0 \ s\alpha & c\alpha & 0 \ 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} 1 & 0 & 0 \ 0 & c\beta & -s\beta \ 0 & s\beta & c\beta \end{bmatrix}
\begin{bmatrix} c\gamma & -s\gamma & 0 \ s\gamma & c\gamma & 0 \ 0 & 0 & 1 \end{bmatrix} \
&= \begin{bmatrix}
c\alpha c\gamma - s\alpha c\beta s\gamma & -c\alpha s\gamma - s\alpha c\beta c\gamma & s\alpha s\beta \
s\alpha c\gamma + c\alpha c\beta s\gamma & -s\alpha s\gamma + c\alpha c\beta c\gamma & -c\alpha s\beta \
s\beta s\gamma & s\beta c\gamma & c\beta
\end{bmatrix}
\end{align}
$$

示例 EX 2-2-2

问题

一个本体坐标系依次绕其自身的 X-y-z 轴旋转 90°, 45°, 30°。用三个欧拉角确定其姿态。

解:
这是一个 X-y-z 欧拉角 表示。旋转顺序是X, y, z，角度分别是 \(\alpha=90^\circ, \beta=45^\circ, \gamma=30^\circ\)。
根据后乘规则，总旋转矩阵 \(R = R_x(\alpha) R_y(\beta) R_z(\gamma)\)。
$$
R = R_x(90^\circ) R_y(45^\circ) R_z(30^\circ)
$$

观察

这个结果与 EX 2-2-1 的结果完全相同！

欧拉角 V.S. 固定角

ABC 欧拉角表示与CBA 固定角表示具有相同的数学形式。

• ABC 欧拉角 (e.g. ZYX-Euler): \(R_{ZYX} = R_z(\alpha)R_y(\beta)R_x(\gamma)\)
• CBA 固定角 (e.g. XYZ-Fixed): \(R_{XYZ} = R_z(\gamma)R_y(\beta)R_x(\alpha)\)

ABA 欧拉角表示与ABA 固定角表示具有相同的数学形式。

• ABA 欧拉角 (e.g. ZXZ-Euler): \(R_{ZXZ} = R_z(\alpha)R_x(\beta)R_z(\gamma)\)
• ABA 固定角 (e.g. ZXZ-Fixed): \(R_{ZXZ} = R_z(\gamma)R_x(\beta)R_z(\alpha)\)

2.5 三角度表示法的问题

1. 表示不唯一 (Non-unique)

由一个旋转矩阵定义的姿态，可能对应不同的三角度组合（欧拉角或固定角）。

例如：对于 XYZ RPY角，可以证明
$$
R_z(\gamma \pm \pi) R_y(-\beta \pm \pi) R_x(\alpha \pm \pi) = R_z(\gamma) R_y(\beta) R_x(\alpha)
$$

Proof

\[ \begin{align*} & R_z(\pm\pi + \gamma) R_y(\pm\pi - \beta) R_x(\pm\pi + \alpha) \\ &= \begin{bmatrix} \cos(\pm\pi + \gamma) & -\sin(\pm\pi + \gamma) & 0 \\ \sin(\pm\pi + \gamma) & \cos(\pm\pi + \gamma) & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos(\pm\pi - \beta) & 0 & \sin(\pm\pi - \beta) \\ 0 & 1 & 0 \\ -\sin(\pm\pi - \beta) & 0 & \cos(\pm\pi - \beta) \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\pm\pi + \alpha) & -\sin(\pm\pi + \alpha) \\ 0 & \sin(\pm\pi + \alpha) & \cos(\pm\pi + \alpha) \end{bmatrix} \\ &= \begin{bmatrix} -\cos\gamma & \sin\gamma & 0 \\ -\sin\gamma & -\cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} -\cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & -\cos\beta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & \sin\alpha \\ 0 & -\sin\alpha & -\cos\alpha \end{bmatrix} \\ &= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} -1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} -\cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & -\cos\beta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{bmatrix} \\ &= R_z(\gamma) R_y(\beta) R_x(\alpha) \end{align*} \]

解决方案：
为了获得唯一的解，通常会对中间的俯仰角 \(\beta\) 的范围进行限制。
* 对于 ABC 或 CBA 类型：
- 限制 \(\beta \in (-\pi/2, \pi/2)\)。
- 此时 \((\alpha, \beta, \gamma)\) 的唯一解域为 \((-\pi, \pi] \times (-\pi/2, \pi/2) \times (-\pi, \pi]\)。
- 对应的 Euler 或 RPY 角度为 \((\alpha, \beta, \gamma) \Leftrightarrow (\alpha \pm \pi, -\beta \pm \pi, \gamma \pm \pi)\)。

• 对于 ABA 类型：
• 限制 \(\beta \in (0, \pi)\)。
• 此时 \((\alpha, \beta, \gamma)\) 的唯一解域为 \((-\pi, \pi] \times (0, \pi) \times (-\pi, \pi]\)。
• 对应的 Euler 或 RPY 角度为 \((\alpha, \beta, \gamma) \Leftrightarrow (\alpha \pm \pi, -\beta, \gamma \pm \pi)\)。

2. 旋转矩阵反解

当给定一个旋转矩阵时，可以找到等效的 RPY 角。

\[ ^0R_1 = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} = \begin{bmatrix} c\beta c\gamma & s\alpha s\beta c\gamma - c\alpha s\gamma & c\alpha s\beta c\gamma + s\alpha s\gamma \\ c\beta s\gamma & s\alpha s\beta s\gamma + c\alpha c\gamma & c\alpha s\beta s\gamma - s\alpha c\gamma \\ -s\beta & s\alpha c\beta & c\alpha c\beta \end{bmatrix} \]

如果 \(\boxed{\beta \ne \pm \pi/2}\)（即 \(\cos\beta > 0\)），解是唯一的：

\[ \beta = -\arcsin(r_{31}) $$ $$ \alpha = \mathrm{atan2}(r_{32}, r_{33}) $$ $$ \gamma = \mathrm{atan2}(r_{21}, r_{11}) \]

其中 \(\mathrm{atan2}(y, x)\) 函数定义如下：

\[ \mathrm{arctan2}(y, x) = \begin{cases} \arctan(y/x) & x > 0 \\ \arctan(y/x) + \pi & x < 0, y \ge 0 \\ \arctan(y/x) - \pi & x < 0, y < 0 \\ \pi/2 & x = 0, y > 0 \\ -\pi/2 & x = 0, y < 0 \\ \text{Undefined} & x=0, y=0 \end{cases} \]

为什么要用 \(\mathrm{arctan2}\)？

1. 标准 \(\arctan\) 的局限性:
   我们知道 \(tan(θ) = y/x\)，所以很自然地会想到用 \(θ = \arctan(y/x)\) 来求角度。但标准 \(\arctan\) 函数的返回值范围是 \((-\pi/2, \pi/2)\)，也就是-90°到+90°。这意味着它只能区分第一和第四象限。

   • 例如，点 (1, 1) 和点 (-1, -1)，它们的 y/x 都是 1。\(\arctan(1)\) 会返回 π/4 (45°)，它无法告诉你点到底是在第一象限还是第三象限。

2. \(\arctan 2\) 的优势:
   \(\arctan 2(y,x)\)是一个双变量反正切函数。它同时考虑 y 和 x 的符号，从而能够唯一确定**一个角度 \(\theta\)，其范围是 \((-\pi, \pi]\)，即覆盖了完整的360°。

   • 正如批注中所说：“给定y,x，唯一确定\(\theta\)”。例如，如果x是负的，y是正的（第二象限），\(\arctan 2\) 的返回值会自动落在正确的区间里（π/2 到 π 之间）。

3. 万向节死锁 (Gimbal Lock Problem)

这是一个由特定俯仰角引起的奇异性问题，会导致三角度表示法出现无穷多解。

• 条件：当 \(\beta = \pm \pi/2\) 时，\(\sin\beta = \pm 1, \cos\beta = 0\)。

• 后果：此时，第一次旋转的轴和第三次旋转的轴发生重合，导致系统失去一个自由度 (DOF)。

• 数学表现：对于 X-Y-Z RPY角，当 \(\beta = \pm\pi/2\) 时，旋转矩阵变为：

\[ \begin{align*} ^0R_1 &= \begin{bmatrix} 0 & r_{12} & r_{13} \\ 0 & r_{22} & r_{23} \\ \mp 1 & 0 & 0 \end{bmatrix} \\ &= \begin{bmatrix} 0 & \pm s\alpha c\gamma - c\alpha s\gamma & \pm c\alpha c\gamma + s\alpha s\gamma \\ 0 & \dots & \dots \\ \mp 1 & 0 & 0 \end{bmatrix} \\ &= \begin{bmatrix} 0 & -\sin(\gamma \mp \alpha) & \pm\cos(\gamma \mp \alpha) \\ 0 & \cos(\gamma \mp \alpha) & \sin(\gamma \mp \alpha) \\ \mp 1 & 0 & 0 \end{bmatrix} \end{align*} \]

我们只能求解 \(\alpha\) 和 \(\gamma\) 的和或差，而无法唯一确定它们各自的值。

\[ \alpha \mp \gamma = \mathrm{atan2}(-r_{23}, r_{22}) \]

这种奇异性经常出现在机器人的手腕部分。

示例 EX 2-2-3

问题

从给定的旋转矩阵 \(R\) 中，求解 Z-x-z 欧拉角。
$$
R = \begin{bmatrix} 0.1268 & -0.9268 & 0.3536 \ 0.7803 & -0.1268 & -0.6124 \ 0.6124 & 0.3536 & 0.7071 \end{bmatrix}
$$

解:
将 \(R\) 与 Z-x-z 欧拉角的通用矩阵形式进行比较：
$$
R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \ r_{21} & r_{22} & r_{23} \ r_{31} & r_{32} & r_{33} \end{bmatrix} =
\begin{bmatrix}
\dots & \dots & s\alpha s\beta \
\dots & \dots & -c\alpha s\beta \
s\beta s\gamma & s\beta c\gamma & c\beta
\end{bmatrix}
$$

• Case 1: 奇异点
  如果 \(\beta = 0\) 或 \(\beta = \pi\)，则 \(s\beta = 0\)，矩阵简化，此时只能求解 \(\alpha \pm \gamma\)。

• Case 2: 非奇异点
  如果 \(\beta \ne 0\) 且 \(\beta \ne \pi\)，则 \(\sin\beta > 0\) (因为 \(\beta \in (0, \pi)\))。

  1. 求解 \(\beta\):
     $$
     r_{33} = \cos\beta \implies \beta = \arccos(r_{33})
     $$
     $$
     \beta = \arccos(0.7071) = 45^\circ
     $$

  2. 求解 \(\alpha\):
     $$
     \alpha = \mathrm{atan2}(r_{13}, -r_{23})
     $$
     $$
     \alpha = \mathrm{atan2}(0.3536, -(-0.6124)) = \mathrm{atan2}(0.3536, 0.6124) = 30^\circ
     $$

  3. 求解 \(\gamma\):
     $$
     \gamma = \mathrm{atan2}(r_{31}, r_{32})
     $$
     $$
     \gamma = \mathrm{atan2}(0.6124, 0.3536) = 60^\circ
     $$

Comments