旋转几何 (Rotation Geometry)
约 8223 个字 13 张图片 预计阅读时间 27 分钟
旋转几何 (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)
-
\(R\) 是一个正交矩阵 (orthogonal matrix)，因为：
$$
{^0R_1}^T = {^0R_1}^{-1}
$$ -
旋转变换矩阵也称为方向余弦矩阵 (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}
$$ -
矩阵 \(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}
$$ -
矩阵中只有 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 中的坐标及其速度。
解:
-
旋转变换矩阵: 角度 \(\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}
$$ -
坐标关系: 点 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}
$$ -
速度: 对坐标关系式关于时间 \(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 角。
- 计算公式（前乘）：
$$
\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欧拉角
这是一个特殊的例子，它执行：
- Step1: 绕 Z 轴旋转 \(\alpha\) (Precession/进动)
- Step2: 绕新的 x 轴旋转 \(\beta\) (Nutation/章动)
- 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)\)
4. 三角度表示法的问题
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
解决方案：
为了获得唯一的解，通常会对中间的俯仰角 \(\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 角。
如果 \(\boxed{\beta \ne \pm \pi/2}\)（即 \(\cos\beta > 0\)），解是唯一的：
其中 \(\mathrm{atan2}(y, x)\) 函数定义如下：
为什么要用 \(\mathrm{arctan2}\)？
-
标准 \(\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°)，它无法告诉你点到底是在第一象限还是第三象限。
- 例如，点
-
\(\arctan 2\) 的优势:
\(\arctan 2(y,x)\)是一个双变量反正切函数。它同时考虑y和x的符号，从而能够唯一确定一个角度 \(\theta\)，其范围是 \((-\pi, \pi]\)，即覆盖了完整的360°。- 正如批注中所说：“给定y,x，唯一确定\(\theta\)”。例如，如果x是负的，y是正的（第二象限），\(\arctan 2\) 的返回值会自动落在正确的区间里（
π/2到π之间）。
- 正如批注中所说：“给定y,x，唯一确定\(\theta\)”。例如，如果x是负的，y是正的（第二象限），\(\arctan 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\) 时，旋转矩阵变为：
我们只能求解 \(\alpha\) 和 \(\gamma\) 的和或差，而无法唯一确定它们各自的值。
这种奇异性经常出现在机器人的手腕部分。
示例 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)\))。-
求解 \(\beta\):
$$
r_{33} = \cos\beta \implies \beta = \arccos(r_{33})
$$
$$
\beta = \arccos(0.7071) = 45^\circ
$$ -
求解 \(\alpha\):
$$
\alpha = \mathrm{atan2}(r_{13}, -r_{23})
$$
$$
\alpha = \mathrm{atan2}(0.3536, -(-0.6124)) = \mathrm{atan2}(0.3536, 0.6124) = 30^\circ
$$ -
求解 \(\gamma\):
$$
\gamma = \mathrm{atan2}(r_{31}, r_{32})
$$
$$
\gamma = \mathrm{atan2}(0.6124, 0.3536) = 60^\circ
$$
-
2.5. 轴角表示法 (Axis-angle Representation)
- 一个姿态的描述，不一定非得是绕坐标系主轴进行三次旋转的结果。
- 欧拉旋转定理指出：在三维空间中，任何刚体的位移，只要刚体上有一个点保持固定，就等效于绕某一个固定的轴进行的单次旋转。
这种表示方法由两个部分定义：
* 一个单位向量，表示旋转轴的方向。
* 一个标量，表示绕该轴旋转的角度。
1. 欧拉-罗德里格斯公式 (Euler-Lexell-Rodrigues formula)
旋转轴的单位向量 \(\mathbf{u}\) 由其方向余弦 \((u_1, u_2, u_3)\) 确定：
$$
\mathbf{u} = \begin{bmatrix} u_1 \ u_2 \ u_3 \end{bmatrix}, \quad |\mathbf{u}| = \sqrt{u_1^2 + u_2^2 + u_3^2} = 1
$$
对应的旋转矩阵 \(R\) 可以通过 罗德里格斯公式 计算得出：
$$
R_{\mathbf{u}}(\phi) = (\cos\phi)\mathbf{I} + (1-\cos\phi)\mathbf{u}\mathbf{u}^T + (\sin\phi)S(\mathbf{u})
$$
其中，\(\mathbf{I}\) 是单位矩阵，而 \(S(\mathbf{u})\) 是由向量 \(\mathbf{u}\) 生成的 斜对称矩阵 (Skew-symmetric matrix)：
$$
S(\mathbf{u}) \triangleq \begin{bmatrix} 0 & -u_3 & u_2 \ u_3 & 0 & -u_1 \ -u_2 & u_1 & 0 \end{bmatrix} = \frac{1}{2\sin\phi}(R - R^T)
$$
斜对称矩阵可以将向量叉乘 (cross-product) 运算转换为矩阵乘法：
$$
\mathbf{u} \times \mathbf{r} = S(\mathbf{u})\mathbf{r} = -\mathbf{r} \times \mathbf{u} = -S(\mathbf{r})\mathbf{u}
$$
重要性质
- \(S(\mathbf{u})\mathbf{u} = \mathbf{0}\)
- \(\mathbf{r}^T S(\mathbf{u}) \mathbf{r} = 0\)
- \(S^2(\mathbf{u}) = \mathbf{I} - \mathbf{u}\mathbf{u}^T\)
如果物体有一个初始姿态 \(R_0\)，再进行一次轴角旋转，则新的姿态为：
$$
R_u(\phi) R_0
$$
罗德里格斯公式的证明思路
如何证明绕任意轴的旋转等效于一系列绕主轴的旋转？
思路: 我们可以通过一系列绕本体坐标系主轴的旋转，来实现绕任意向量 \(\mathbf{u}\) 的旋转。这是一种相似变换。
- 绕 Z 轴旋转 \(\alpha\)角，使得 \(z\) 轴、向量 \(\mathbf{u}\) 和 \(x\) 轴位于同一个平面内。
- 绕新的 y 轴旋转 \(\beta\)角，使得 z 轴与向量 \(\mathbf{u}\) 重合。
- 绕当前的 z 轴 (也就是 \(\mathbf{u}\) 的方向) 旋转目标角度 \(\phi\)。
- （相当于多余步骤） 执行上述操作的逆过程：先绕 y 轴旋转 \(-\beta\)角，再绕 z 轴旋转 \(-\alpha\)角，将坐标系恢复原状。
由于所有旋转都是绕移动的本体坐标系进行的，我们使用后乘规则：
$$
R = R_z(\alpha) R_y(\beta) R_z(\phi) R_y(-\beta) R_z(-\alpha)
$$
证明展开
向量 \(\mathbf{u}=(u_1, u_2, u_3)\) 与角度 \(\alpha, \beta\) 的三角关系如下：
$$
\[ 将这些关系代入上述旋转矩阵的乘积中，并令 $c\phi = \cos\phi, s\phi = \sin\phi, v\phi = 1-\cos\phi$，最终可以得到罗德里格斯公式的矩阵形式： $$ R = \begin{bmatrix} c\phi + u_1^2 v\phi & -u_3 s\phi + u_1 u_2 v\phi & u_2 s\phi + u_1 u_3 v\phi \\ u_3 s\phi + u_1 u_2 v\phi & c\phi + u_2^2 v\phi & -u_1 s\phi + u_2 u_3 v\phi \\ -u_2 s\phi + u_1 u_3 v\phi & u_1 s\phi + u_2 u_3 v\phi & c\phi + u_3^2 v\phi \end{bmatrix} $$ 这可以被分解为： $$ = (c\phi)\mathbf{I} + (s\phi)\begin{bmatrix} 0 & -u_3 & u_2 \\ u_3 & 0 & -u_1 \\ -u_2 & u_1 & 0 \end{bmatrix} + (v\phi)\begin{bmatrix} u_1 \\ u_2 \\ u_3 \end{bmatrix} \begin{bmatrix} u_1 & u_2 & u_3 \end{bmatrix} $$ $$ = (\cos\phi)\mathbf{I} + (\sin\phi)S(\mathbf{u}) + (1-\cos\phi)\mathbf{u}\mathbf{u}^T \]
另一种证明方式 (几何法)
从\(\vec{OP}\)到\(\vec{OP'}\)：
旋转后的向量 \(\mathbf{r}'\) 可以分解为三个向量的和：\(\mathbf{r}' = \vec{ON} + \vec{NQ} + \vec{QP}'\)
-
\(\mathbf{r}\) 在旋转轴 \(\mathbf{u}\) 上的投影 (旋转过程中不变):
$$ \vec{ON} = (\mathbf{r} \cdot \mathbf{u})\mathbf{u} $$ -
\(\mathbf{r}\) 在垂直于 \(\mathbf{u}\) 的平面上的投影，旋转 \(\phi\) 角后的新位置在原投影方向上的分量:
$$ \vec{NQ} = [\mathbf{r} - (\mathbf{r} \cdot \mathbf{u})\mathbf{u}] \cos\phi $$ -
旋转 \(\phi\) 角后的新位置在与原投影垂直方向上的分量:
$$ \vec{QP}' = (\mathbf{u} \times \mathbf{r}) \sin\phi $$
将三者相加并整理，即可得到罗德里格斯旋转公式的向量形式：
$$
\begin{align}
\mathbf{r}' &= (\mathbf{r} \cdot \mathbf{u})\mathbf{u} + \left[\mathbf{r} - (\mathbf{r} \cdot \mathbf{u})\mathbf{u}\right]\cos\phi + (\mathbf{u} \times \mathbf{r})\sin\phi \
&= \mathbf{r}\cos\phi + (1 - \cos\phi)(\mathbf{r} \cdot \mathbf{u})\mathbf{u} + (\mathbf{u} \times \mathbf{r})\sin\phi \
&= \left[\cos\phi\, \mathbf{I} + (1 - \cos\phi)\mathbf{u}\mathbf{u}^T + \sin\phi\, S(\mathbf{u})\right]\mathbf{r}
\end{align}
$$
2. 从旋转矩阵 R 反解轴角
给定一个旋转矩阵 \(R\)，可以反向求解其对应的轴角表示 \((\mathbf{u}, \phi)\)。
Case 1: \(\phi \in (0, \pi)\) (非特殊情况)
这种情况下有 两个解：\((\mathbf{u}, \phi)\) 和 \((-\mathbf{u}, -\phi)\)。
* 求角度 \(\phi\):
$$ \cos\phi = \frac{1}{2}(\mathrm{tr}(R) - 1) = \frac{1}{2}(r_{11}+r_{22}+r_{33}-1) $$
* 求旋转轴 \(\mathbf{u}\):
$$ \mathbf{u} = \begin{bmatrix} u_1 \ u_2 \ u_3 \end{bmatrix} = \frac{1}{2\sin\phi} \begin{bmatrix} r_{32} - r_{23} \ r_{13} - r_{31} \ r_{21} - r_{12} \end{bmatrix} $$
Case 2: \(\phi = 0\)
\(R = \mathbf{I}\) (单位矩阵)。旋转角度为0，此时旋转轴 \(\mathbf{u}\) 未定义，可以是任意非零向量。
Case 3: \(\phi = \pi\) (\(180^\circ\) 旋转)
这种情况下也有 两个解 \((\mathbf{u}, \pi)\) 和 \((-\mathbf{u}, \pi)\)。
$$
R(\pi) = \begin{bmatrix} 2u_1^2-1 & 2u_1u_2 & 2u_1u_3 \ 2u_1u_2 & 2u_2^2-1 & 2u_2u_3 \ 2u_1u_3 & 2u_2u_3 & 2u_3^2-1 \end{bmatrix}
$$
旋转轴 \(\mathbf{u}\) 可以通过以下方式求解：
$$
\mathbf{u} = \begin{bmatrix} u_1 \ u_2 \ u_3 \end{bmatrix} = \pm \begin{bmatrix} \sqrt{(r_{11}+1)/2} \ r_{12}/\sqrt{2(r_{11}+1)} \ r_{13}/\sqrt{2(r_{11}+1)} \end{bmatrix}
$$
示例 EX 2-3-1
Question
一个坐标系 \(\Sigma_1\) 相对于 \(\Sigma_0\) 经历了 (30, 45, 60) 度的 z-x-z 欧拉旋转。求该旋转唯一的轴角表示 (限制 \(\phi \in [0, \pi]\))。
解:
- 首先计算 z-x-z 欧拉角对应的旋转矩阵 \(R\)：
$$
^0R_1 = \begin{bmatrix} 0.1268 & -0.9268 & 0.3536 \ 0.7803 & -0.1268 & -0.6124 \ 0.6124 & 0.3536 & 0.7071 \end{bmatrix}
$$
-
用反解公式计算角度 \(\phi\)：
$$\begin{align}
\cos\phi &= \frac{1}{2}[tr(R)-1] = \frac{1}{2}(0.1268 - 0.1268 + 0.7071 - 1) = -0.14645 \
\phi &= \arccos(-0.14645) = 98.42^\circ
\end{align}
$$ -
计算旋转轴 \(\mathbf{u}\)：
$$
S(\mathbf{u}) = \frac{R-R^T}{2\sin\phi} = \begin{bmatrix} 0 & -0.8629 & -0.1308 \ 0.8629 & 0 & -0.4882 \ 0.1308 & 0.4882 & 0 \end{bmatrix}
$$
从中提取出 \(\mathbf{u}\) 的分量：
$$
\mathbf{u} = \begin{bmatrix} 0.4882 \ -0.1308 \ 0.8629 \end{bmatrix}
$$
检验
求解旋转矩阵：
\(\(\begin{align} R &= (\cos\phi)\mathbf{I} + (1 - \cos\phi)\mathbf{u}\mathbf{u}^T + (\sin\phi)S(\mathbf{u})\\ &=
\begin{bmatrix}
0.1268 & -0.9268 & 0.3536 \\
0.7803 & -0.1268 & -0.6124 \\
0.6124 & 0.3536 & 0.7071
\end{bmatrix}\end{align}\)\)
3. 通过特征值与特征向量求解
- 对于一个绕轴 \(\mathbf{u}\) 的旋转，旋转轴 \(\mathbf{u}\) 本身在旋转中保持不变，即 \(R\mathbf{u} = \mathbf{u}\)。
- 这表明 \(\lambda=1\) 永远是旋转矩阵 \(R\) 的一个特征值 (eigen value)。
- 与特征值 \(\lambda=1\) 相关联的特征向量 (eigen vector) 就是旋转轴 \(\mathbf{u}\)。
旋转矩阵 \(R\) 的特征值方程为 \(R\mathbf{u} = \lambda\mathbf{u}\)。其特征方程为：
$$
(\lambda-1)[\lambda^2 - \lambda(\mathrm{tr}R-1)+1]=0
$$
三个特征值为：
$$
\lambda_1 = 1, \quad \lambda_2 = e^{i\phi}, \quad \lambda_3 = e^{-i\phi}
$$
- \(\mathbf{u} = \text{eigen vector (if } \lambda=1)\)
* \(\phi = \mathrm{atan2}(\mathrm{Im}(\lambda_2), \mathrm{Re}(\lambda_2))\)
示例 EX 2-3-2
Question
同样是 (30, 45, 60) 度的 z-x-z 欧拉旋转，求其旋转矩阵的特征值和特征向量。
解:
- 旋转矩阵 \(R\) 同上。
-
求解其特征值 \(\lambda\) 和特征向量矩阵 \(V\)：
$$
\lambda = \begin{bmatrix} -0.1464 + 0.9892i \ -0.1464 - 0.9892i \ 1.0000 \end{bmatrix}
$$ -
从复数特征值中求解角度 \(\phi\)：
$$
\phi = \mathrm{atan2}(0.9892, -0.1464) = 98.4186^\circ
$$ -
与特征值 \(\lambda=1\) 对应的特征向量即为旋转轴 \(\mathbf{u}\) (通常需要归一化)：
$$
\mathbf{u} = \begin{bmatrix} 0.4882 \ -0.1308 \ 0.8629 \end{bmatrix}
$$
2.6 欧拉参数 (Euler Parameters) 【本课程不使用】
欧拉引入了一组四个参数来确定轴角旋转，这种表示法没有奇异性问题。
* \(e_0\) 是一个标量
* \(e_1, e_2, e_3\) 是向量 \(\mathbf{e}\) 的三个分量
它们与轴角 \((\mathbf{u}, \phi)\) 的关系是：
$$
e_0 = \cos(\frac{\phi}{2})
$$
$$
\mathbf{e} = e_1\mathbf{i} + e_2\mathbf{j} + e_3\mathbf{k} = \sin(\frac{\phi}{2})\mathbf{u}
$$
这四个参数满足单位约束：
$$
e_0^2 + e_1^2 + e_2^2 + e_3^2 = e_0^2 + \mathbf{e}^T\mathbf{e} = 1
$$
旋转矩阵 \(R\) 可以用欧拉参数表示为：
$$
R_{\mathbf{u}}(\phi) = (e_0^2 - \mathbf{e}^T\mathbf{e})\mathbf{I} + 2\mathbf{e}\mathbf{e}^T + 2e_0 S(\mathbf{e})
$$
欧拉参数与罗德里格斯公式的联系
通过三角恒等式，我们可以将罗德里格斯公式中的 \(\cos\phi, \sin\phi, (1-\cos\phi)\) 替换为欧拉参数的表达式，从而证明两种形式是等价的。
$$
\cos\phi = 2\cos^2(\frac{\phi}{2})-1 = 2e_0^2 - 1 = e_0^2 - (e_1^2+e_2^2+e_3^2) = e_0^2 - \mathbf{e}^T\mathbf{e}
$$
$$
(1-\cos\phi)\mathbf{u}\mathbf{u}^T = 2\sin^2(\frac{\phi}{2})\mathbf{u}\mathbf{u}^T = 2\mathbf{e}\mathbf{e}^T
$$
$$
(\sin\phi)S(\mathbf{u}) = 2\sin(\frac{\phi}{2})\cos(\frac{\phi}{2})S(\mathbf{u}) = 2e_0 S(\mathbf{e})
$$
最终得到用欧拉参数表示的旋转矩阵 \(R\) 的展开形式：
$$
R = \begin{bmatrix}
2(e_0^2+e_1^2)-1 & 2(e_1e_2 - e_0e_3) & 2(e_1e_3 + e_0e_2) \
2(e_1e_2 + e_0e_3) & 2(e_0^2+e_2^2)-1 & 2(e_2e_3 - e_0e_1) \
2(e_1e_3 - e_0e_2) & 2(e_2e_3 + e_0e_1) & 2(e_0^2+e_3^2)-1
\end{bmatrix}
$$
如果给定了一组欧拉参数（或者更直接地说，由它们构成的旋转矩阵 \(R\)），那么就可以反向求解出对应的旋转轴 \(\mathbf{u}\) 和旋转角 \(\phi\)。
- 计算旋转角 \(\phi\):
$$
\cos\phi = \frac{1}{2}[\mathrm{tr}(R) - 1]
$$
- 计算旋转轴 \(\mathbf{u}\):
$$
\mathbf{u} = \begin{bmatrix} u_1 \ u_2 \ u_3 \end{bmatrix} = \frac{1}{2\sin\phi} \begin{bmatrix} r_{32} - r_{23} \ r_{13} - r_{31} \ r_{21} - r_{12} \end{bmatrix}
$$
从旋转矩阵 R 反解欧拉参数
通过对比旋转矩阵 \(R\) 的各项，可以反解出欧拉参数。这会得到四组解，但它们实际上只代表两个独特的姿态（因为 \((e_0, \mathbf{e})\) 和 \((-e_0, -\mathbf{e})\) 代表同一个旋转）。
-
求解 \(e_0\):
$$ e_0 = \pm \frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} $$ -
求解 \(\mathbf{e}\):
$$ \mathbf{e} = \frac{1}{4e_0} \begin{bmatrix} r_{32}-r_{23} \ r_{13}-r_{31} \ r_{21}-r_{12} \end{bmatrix} $$
为了避免当 \(e_0\) 接近于0时除法带来的数值不稳定问题，提供了另外三套求解公式。在实际计算中，应选择分母（\(e_0, e_1, e_2, e_3\)）绝对值最大的那组公式进行计算。
示例 EX 2-4-1
Question
同样是 (30, 45, 60) 度的 z-x-z 欧拉旋转，求其对应的欧拉参数和轴角表示。
解:
- 旋转矩阵 \(R\) 同上。
-
计算欧拉参数：
$$
e_0 = \pm \frac{1}{2}\sqrt{1 + 0.1268 - 0.1268 + 0.7071} = \pm 0.6533
$$
$$
\mathbf{e} = \pm \frac{1}{4(0.6533)} \begin{bmatrix} 0.3536 - (-0.6124) \ 0.6124 - 0.3536 \ 0.7803 - (-0.9268) \end{bmatrix} \rightarrow \pm \begin{bmatrix} 0.3696 \ -0.0990 \ 0.6533 \end{bmatrix}
$$ -
从欧拉参数反解轴角：
$$
\phi = 2 \arccos(e_0) = \pm 98.4225^\circ
$$
$$
\mathbf{u} = \mathbf{e} / \sin(\phi/2) = \pm \begin{bmatrix} 0.4882 \ -0.1308 \ 0.8629 \end{bmatrix}
$$
2.7 四元数 (Quaternions) 【本课程不使用】
5. 四元数 (Quaternions)
- 轴角表示法需要4个量（3个给轴，1个给角），但轴的3个量受单位长度约束，所以自由度仍为3。
- 轴角表示法不便于直接进行代数运算（如旋转的复合）。
- 四元数 (Quaternions) 可以解决这个问题。它是一种超复数，由一个标量部分 (scalar) 和一个向量部分 (vector) 组成。
一个四元数 \(q\) 定义为：
$$
q = q_0 + \mathbf{q} = q_0 + q_1\mathbf{i} + q_2\mathbf{j} + q_3\mathbf{k} = \langle q_0, \mathbf{q} \rangle = \langle s, \mathbf{v} \rangle
$$
其中 \(q_0 \in \mathbb{R}, \mathbf{q} \in \mathbb{R}^3\)。\(\mathbf{i, j, k}\) 是正交的虚数单位，满足：
$$
\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{i}\mathbf{j}\mathbf{k} = -1
$$
四元数的性质
-
加法:
$$ q+p = \langle q_0, \mathbf{q} \rangle + \langle p_0, \mathbf{p} \rangle = \langle q_0+p_0, \mathbf{q}+\mathbf{p} \rangle $$ -
乘法 (哈密顿积 Hamiltonian product):
$$ q \circ p = (q_0+\mathbf{q})(p_0+\mathbf{p}) = q_0p_0 + q_0\mathbf{p} + p_0\mathbf{q} + \mathbf{q}\mathbf{p} $$
其中 \(\mathbf{q}\mathbf{p} = \mathbf{q} \times \mathbf{p} - \mathbf{q} \cdot \mathbf{p}\)。展开后为：
$$ q \circ p = \langle q_0p_0 - \mathbf{q}\cdot\mathbf{p}, \ q_0\mathbf{p} + p_0\mathbf{q} + \mathbf{q}\times\mathbf{p} \rangle $$ -
乘法不满足交换律: \(q \circ p \ne p \circ q\)
- 加法满足交换律和结合律。
-
共轭 (Conjugate):
$$ q^* = \langle q_0, -\mathbf{q} \rangle = q_0 - \mathbf{q} $$ -
单位四元数 (Unit quaternion) 与欧拉参数一一对应。对于一个代表旋转的单位四元数 \(e\):
$$
e(\phi, \mathbf{u}) = \langle e_0, \mathbf{e} \rangle = e_0 + \mathbf{e} = \cos(\frac{\phi}{2}) + \sin(\frac{\phi}{2})\mathbf{u} = \langle s, \mathbf{v} \rangle
$$
它满足 \(|e|=1\)，且其逆等于其共轭 \(e^{-1} = e^*\)。
使用单位四元数进行旋转
定理: 由 \(^0\mathbf{r} = R_{\mathbf{u}}(\phi)\mathbf{r}\) 定义的旋转，等效于以下四元数乘法：
$$
^0\mathbf{r} = e(\phi, \mathbf{u}) \ \mathbf{r} \ e^*(\phi, \mathbf{u})
$$
(这里 \(\mathbf{r}\) 被当作一个标量部分为0的纯四元数 \(\langle 0, \mathbf{r} \rangle\))
连续两次旋转 \(R = R_2 R_1\) 等效于两个单位四元数的乘积：
$$
e(\phi, \mathbf{u}) = e(\phi_2, \mathbf{u}_2) e(\phi_1, \mathbf{u}_1)
$$
2.8 各种表示法的优缺点总结
Pros & Cons of Representations
1. 旋转矩阵 (Rotation Transformation Matrix)
- 优点:
- 明确，无奇异点。
- 便于进行坐标变换和旋转复合。
- 缺点:
- 需要9个参数来表示3个自由度的旋转，数据冗余。
- 这9个参数必须满足正交约束，在数值计算中容易因误差累积而“退化”。
- 不直观，无法直接看出旋转轴和角度。
2. 欧拉角/固定角 (Euler Angles)
- 优点:
- 参数最少（仅3个），无冗余。
- 非常直观，容易理解和可视化。
- 缺点:
- 存在万向节死锁 (Gimbal Lock) 的奇异性问题。
- 旋转的复合运算非常复杂，需要大量三角函数计算。
3. 轴角 (Axis-angle)
- 优点:
- 几何意义非常直观。
- 缺点:
- 存在表示不唯一的问题（\((\mathbf{u}, \phi)\) vs \((-\mathbf{u}, -\phi)\)）。
- 在 \(\phi=0\) 时轴未定义。
- 不便于直接进行旋转的复合运算。
4. 欧拉参数 / 单位四元数
- 优点:
- 计算效率高：旋转复合只需一次四元数乘法，远快于矩阵乘法。
- 无奇异点：完美避开万向节死锁问题。
- 存储紧凑（4个浮点数）。
- 在数值上稳定，容易通过归一化来修正误差。
- 缺点:
- 不如欧拉角直观。
- 存在双倍覆盖问题（\(q\) 和 \(-q\) 代表同一个旋转），但在大多数应用中不成问题。