三维空间刚体运动 | 旋转矩阵、旋转向量、欧拉角转换

注：本文为 “三维空间刚体运动” 相关合辑。
图片清晰度受引文原图所限。
略作重排，如有内容异常，请看原文。

---

三维空间刚体运动 1：旋转矩阵与变换矩阵（详解加代码示例）

龙焰智能 于 2024-07-09 16:53:19 修改

序

本系列文章参照高翔老师《视觉 SLAM 十四讲：从理论到实践》，系统讲解三维空间刚体运动的相关理论，为读者构建扎实的数学基础。

在正式讲解内容前，分享学习体会如下：此前学习 SLAM 相关知识时，因前期基础概念理解不透彻，推进至第六讲时被迫中断；在完成强化学习相关内容的学习后，重新沉下心来研读 SLAM，方才取得进展。因此建议学习 SLAM 的读者：高翔博士在书中已涵盖知识点，仅表述较为精简。笔者反复研读第三、四讲共计四次，方能形成系统理解。由此可见，学习 SLAM 的关键在于温故知新，持续总结梳理，将前后关联的知识点融会贯通，方可理解后续进阶内容。

本文首先介绍向量及其坐标表示，以及向量间的基本运算；其次，采用欧式变换描述坐标系间的运动关系，该变换由旋转与平移两部分构成，其中旋转通过旋转矩阵 $SO(3)$ 描述，平移则直接由 ${\mathbb{R}}^3$ 空间中的向量描述；最后，将旋转与平移整合至同一矩阵中，形成变换矩阵 $SE(3)$（文中将对陌生符号逐一解释）。此外，在欧式变换的基础上，进一步讲解相似变换、仿射变换与射影变换的相关内容。

1. 点、向量和坐标系

本节阐述刚体、点、向量、坐标、坐标系、内积与外积的基本概念，为引入符号 $a^{\wedge }$ 奠定基础。

1.1 基本定义

-刚体：形状与尺寸不发生改变的物体。三维空间中，空间点的位置可由 3 个坐标参数确定；刚体除位置外，还具有姿态属性，姿态用于描述物体的朝向。
-点：空间中的基本几何元素，无长度、无体积。两个点的连线可构成向量。
-向量：可直观理解为从某一点指向另一点的有向线段，是空间中的几何实体。向量在坐标系中以坐标形式表示，同一向量在不同坐标系下的坐标取值不同。
-坐标：设线性空间中存在一组基（张成该空间的线性无关向量组，亦称为基底），记为 $(e_1,e_2,e_3)$，则任意向量 $a$ 在该组基下的坐标满足：
$$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3.\text{\hspace{1em}(1.1)}$$
其中 $(a_1,a_2,a_3{)}^T$ 称为向量 $a$ 在此基下的坐标。坐标的具体取值由两方面因素决定：一是向量本身的属性，二是坐标系（基）的选取方式。注：本文中所有向量均采用列向量形式表示，与通用数学教材规范一致。
-坐标系：通常由 3 个正交的坐标轴构成。给定 $x$ 轴与 $y$ 轴后，$z$ 轴可通过右手（或左手）法则由 $x\times y$ 确定。根据坐标轴定义方式的不同，坐标系分为左手系与右手系：右手系中，大拇指指向 $x$ 轴正方向，食指指向 $y$ 轴正方向，中指所指方向即为 $z$ 轴正方向。多数 3D 程序库采用右手系（如 OpenGL、3D Max 等），部分库采用左手系（如 Unity、Direct3D 等）。

1.2 向量运算

-内积：向量的数乘、加减法运算不再赘述。常规定义下，内积的表达式为：
$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|\cos ⟨a,b⟩.\text{\hspace{1em}(1.2)}$$
其中 $⟨a,b⟩$ 表示向量 $a$ 与 $b$ 的夹角。内积可用于描述向量间的投影关系。
-外积：外积的数学表达式为：
$$a\times b=\left|\begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right|=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\stackrel{\text{def}}{=}{a}^{\wedge }b.\text{\hspace{1em}(1.3)}$$
外积的运算结果为向量，其方向垂直于参与运算的两个向量，模长为 $|a||b|\sin ⟨a,b⟩$，等于两个向量张成的四边形的有向面积。引入符号 ${}^{\wedge }$ 可将向量 $a$ 转换为反对称矩阵（满足 $A^T=-A$），该符号读作 “hat”，由此可将外积运算 $a\times b$ 转化为矩阵与向量的乘法运算 $a^{\wedge }b$，使其成为线性运算。该符号具有一一映射特性：任意向量对应唯一的反对称矩阵，反之亦然，其具体形式为：
$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right].\text{\hspace{1em}(1.4)}$$

2. 坐标系间的欧式变换

本节内容是本文乃至相关知识体系的重要组成部分，需重点理解与掌握。首先从刚体运动出发，引入欧式变换的概念。

实际应用场景中常需定义多个坐标系，以运动的机器人（或相机）为例，通常设定一个固定不动的惯性坐标系（亦称为世界坐标系）。此时需解决的问题为：相机视野中的某一向量 $p$，其在相机坐标系下的坐标为 $p_c$，在世界坐标系下的坐标为 $p_w$，如何描述这两个坐标之间的转换关系？解决思路为：先确定该点在机器人坐标系下的坐标，再依据机器人的位姿将其变换至世界坐标系，这一过程可通过变换矩阵 $T$ 实现数学描述。

2.1 刚体运动与欧式变换

刚体运动：两个坐标系之间的运动变换由旋转与平移共同构成，此类运动称为刚体运动，相机的运动即为典型的刚体运动。刚体运动过程中，同一向量在不同坐标系下的长度与夹角保持不变。此时，称相机坐标系与世界坐标系之间存在一个欧式变换（Euclidean Transform），该变换由旋转和平移两部分组成。

2.2 旋转

首先分析旋转过程，由此引出旋转矩阵与特殊正交群 $SO(n)$ 的概念。

-旋转矩阵：设某一单位正交基 $e=(e_1,e_2,e_3)$ 经过旋转后变为 $e^{\prime }=(e_1^{\prime },e_2^{\prime },e_3^{\prime })$。对于同一向量 $a$，其在两个坐标系下的坐标分别为 $[a_1,a_2,a_3]$ 与 $[a_1^{\prime },a_2^{\prime },a_3^{\prime }]$。由于向量本身未发生变化，根据坐标的定义可得：
$$[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{\prime },e_2^{\prime },e_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right].\text{\hspace{1em}(2.1)}$$
为描述两个坐标之间的关系，对上式两端左乘 $e^T$，左侧系数矩阵将变为单位矩阵，因此：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\text{def}}{=}\mathbf{R}{a}^{\prime }.\text{\hspace{1em}(2.2)}$$
矩阵 $\mathbf{R}$ 由两组基的内积构成，刻画了旋转前后同一向量的坐标变换关系，因此将其称为旋转矩阵（Rotation Matrix）。由于该矩阵的各元素为两个坐标系基向量的内积，本质上是基向量夹角的余弦值，故也称为方向余弦矩阵（Direction Cosine Matrix）。

旋转矩阵 $\mathbf{R}$ 为正交矩阵，其逆矩阵（即转置矩阵）描述反向的旋转过程。根据上述定义可得：
$$a^{\prime }={\mathbf{R}}^{-1}a={\mathbf{R}}^{T}a.\text{\hspace{1em}(2.3)}$$
显然，${\mathbf{R}}^{-1}$ 与 ${\mathbf{R}}^T$ 对应反向的旋转变换。

-特殊正交群 $SO(n)$：旋转矩阵 $\mathbf{R}$ 是行列式为 1 的正交矩阵（满足 $A^{-1}=A^T$）；反之，行列式为 1 的正交矩阵也可表示旋转矩阵。据此，$n$ 维旋转矩阵的集合定义为：
$$SO(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}|\mathbf{R}{\mathbf{R}}^T=\mathbf{I},\det (\mathbf{R})=1\right\}.\text{\hspace{1em}(2.4)}$$
$SO(n)$ 为特殊正交群（Special Orthogonal Group）的缩写，该集合包含 $n$ 维空间中的所有旋转矩阵，其中 $SO(3)$ 特指三维空间的旋转矩阵。通过旋转矩阵，可直接描述两个坐标系之间的旋转变换，无需再从基向量的角度分析。

2.3 平移

欧式变换中除旋转外，还包含平移分量。
考虑世界坐标系中的向量 $a$，经过旋转矩阵 $\mathbf{R}$ 旋转与平移向量 $\mathbf{t}$ 平移后得到向量 $a^{\prime }$，将旋转与平移整合可得：
$${\mathbf{a}}^{\prime }=\mathbf{Ra}+\mathbf{t}.\text{\hspace{1em}(2.5)}$$
通过上式，可利用旋转矩阵 $\mathbf{R}$ 与平移向量 $\mathbf{t}$ 完整描述欧式空间中的坐标变换关系。

关于下标标注的说明：实际应用中常定义坐标系 1 与坐标系 2，向量 $a$ 在两个坐标系下的坐标分别为 $a_1$ 与 $a_2$，二者满足如下关系：
$$a_1={\mathbf{R}}_{12}{a}_2+{\mathbf{t}}_{12}.\text{\hspace{1em}(2.6)}$$
其中 ${\mathbf{R}}_{12}$ 表示 “将坐标系 2 中的向量变换至坐标系 1”，即 “从坐标系 2 到坐标系 1 的旋转矩阵”。由于向量置于矩阵右侧进行乘法运算，因此其下标需从右向左解读。对于平移向量 ${\mathbf{t}}_{12}$，其物理意义为坐标系 1 原点指向坐标系 2 原点的向量在坐标系 1 下的坐标，建议将其记为 “从坐标系 1 到坐标系 2 的向量”，下标需从左向右解读，但需注意 ${\mathbf{t}}_{12}\ne -{\mathbf{t}}_{21}$。

3. 齐次坐标和变换矩阵

式（2.5）所描述的欧式空间旋转与平移变换存在一定局限性：该变换关系为线性关系，若进行多次变换（如依次进行 ${\mathbf{R}}_1,{\mathbf{t}}_1$ 与 ${\mathbf{R}}_2,{\mathbf{t}}_2$ 变换）：
$$\mathbf{b}={\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1,\mathbf{c}={\mathbf{R}}_2\mathbf{b}+{\mathbf{t}}_2.\text{\hspace{1em}(3.1)}$$
则从 $\mathbf{a}$ 到 $\mathbf{c}$ 的变换关系为：
$$\mathbf{c}={\mathbf{R}}_2({\mathbf{R}}_1\mathbf{a}+{\mathbf{t}}_1)+{\mathbf{t}}_2.\text{\hspace{1em}(3.2)}$$
多次变换后，表达式会变得繁琐。为此，引入齐次坐标与变换矩阵的概念。

3.1 齐次坐标

采用如下数学技巧：在三维向量末尾添加 1，将其转换为四维向量 $\tilde{a}=\left[\begin{array}{c}a\\ 1\end{array}\right]$，该向量称为齐次坐标。齐次坐标表示法是利用 $n+1$ 维向量描述 $n$ 维向量。

$n$ 维空间中点的位置向量，非齐次坐标表示为 $(P_1,P_2,\dots ,P_n)$，该表示形式具有 $n$ 个分量且唯一；齐次坐标表示为 $(h{P}_1,h{P}_2,\dots ,h{P}_n,h)$，该表示形式具有 $n+1$ 个分量且不唯一。

对于参数 $h$，通常取 $h=1$；若 $h\ne 1$ 且 $h\ne 0$，可将齐次坐标各分量除以 $h$，该操作称为齐次坐标的规范化；若 $h=0$，则该点表示无穷远点。三元组 $(0,0,0)$ 不表示任何空间点，原点的齐次坐标为 $(0,0,0,1)$。

3.2 变换矩阵

对于齐次坐标，可将旋转与平移整合至同一矩阵中，使变换关系转化为线性关系：
$${\tilde{a}}^{\prime }=\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\stackrel{\text{def}}{=}\mathbf{T}\left[\begin{array}{c}a\\ 1\end{array}\right]=\left[\begin{array}{c}\mathbf{R}a+\mathbf{t}\\ 1\end{array}\right].\text{\hspace{1em}(3.3)}$$
式中矩阵 $\mathbf{T}$ 称为变换矩阵（Transform Matrix）。

借助齐次坐标与变换矩阵，多次变换的叠加可简化为简洁的形式：
$$\tilde{b}={\mathbf{T}}_1\tilde{a},\tilde{c}={\mathbf{T}}_2\tilde{b}\Rightarrow \tilde{c}={\mathbf{T}}_2{\mathbf{T}}_1\tilde{a}.\text{\hspace{1em}(3.4)}$$

为简化表述，在无歧义的情况下，后续直接将其写作 $\mathbf{b}=\mathbf{Ta}$ 的形式，默认已完成齐次坐标的转换。

3.3 特殊欧式群 $SE(3)$

变换矩阵 $\mathbf{T}$ 具有特定的结构：左上角为旋转矩阵，右上角为平移向量，左下角为零向量，右下角为 1。此类矩阵构成的集合称为特殊欧式群（Special Euclidean Group），定义为：
$$SE(3)=\left\{{\mathbf{T}}_E=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}.\text{\hspace{1em}(3.5)}$$

与 $SO(3)$ 类似，变换矩阵的逆矩阵 ${\mathbf{T}}^{-1}$ 对应反向的变换，其表达式为：
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right].\text{\hspace{1em}(3.6)}$$

同样，采用 ${\mathbf{T}}_{12}$ 表示从坐标系 2 到坐标系 1 的变换矩阵。在无歧义时，后续不再区分齐次坐标与普通坐标的符号，默认使用符合运算法则的坐标形式，因齐次坐标与非齐次坐标之间的转换操作较为简便。

4. 相似、仿射和射影变换

除欧式变换外，三维空间还存在其他变换形式，欧式变换是其中最为简单的一种。部分变换与测量几何相关，后续讲解中可能涉及，故在此先行罗列。欧式变换保持向量的长度与夹角不变，相当于将刚体进行平移或旋转操作，不改变其几何形态；但实际场景中，由于视角等因素的影响，物体往往会产生畸变，因此需引入相似变换、仿射变换与射影变换，这些变换会改变物体的外形，且均具有类似的矩阵表示形式。

4.1 相似变换

相似变换相比欧式变换增加了一个自由度，允许物体进行均匀缩放，其矩阵表示形式为：
$${\mathbf{T}}_S=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(4.1)}$$
需注意，旋转部分新增缩放因子 $s$，表示向量经旋转后，可在 $x、y、z$ 三个坐标轴方向进行均匀缩放。由于包含缩放操作，相似变换不再保持图形的面积不变（例如边长为 1 的立方体经相似变换后可变为边长为 10 的立方体）。

三维相似变换的集合称为相似变换群，记作 $Sim(3)$。

4.2 仿射变换

仿射变换的矩阵形式为：
$${\mathbf{T}}_A=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(4.2)}$$
与欧式变换不同，仿射变换仅要求矩阵 $\mathbf{A}$ 为可逆矩阵，无需满足正交性。仿射变换也称为正交投影，立方体经仿射变换后不再为立方体，但各面仍保持为平行四边形。

4.3 射影变换

射影变换是形式最一般的变换，亦称为投影变换，其矩阵形式为：
$${\mathbf{T}}_P=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right]\text{\hspace{1em}(4.3)}$$
该矩阵左上角为可逆矩阵 $\mathbf{A}$，右上角为平移向量 $\mathbf{t}$，左下角为缩放向量 ${\mathbf{a}}^T$，右下角为整体变换比例 $v$。由于采用齐次坐标表示，当 $v\ne 0$ 时，可将整个矩阵除以 $v$，使右下角元素变为 1；当 $v=0$ 时，右下角元素为 0。因此，二维射影变换具有 8 个自由度，三维射影变换具有 15 个自由度。

射影变换是前文所述变换中形式最通用的一种，从真实世界到相机照片的变换可视为射影变换。例如，方形的地板砖在相机照片中不再是方形，受近大远小的透视效应影响，甚至不再是平行四边形，而是呈现为不规则四边形，这是位姿估计中常见的场景。

4.4 变换性质总结

下表对比总结上述四种变换的性质，需注意 “不变性质” 列中，从上至下存在包含关系（例如欧式变换除保持体积不变外，同时具有保平行、保相交等性质）。

表 4-1 常见变换的性质比较

变换名称	矩阵形式	自由度	不变性质	变换形态
欧氏变换	${\mathbf{T}}_E=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	6	长度、夹角、体积	位置、方向改变
相似变换	${\mathbf{T}}_S=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	7	体积比	按比例缩放
仿射变换	${\mathbf{T}}_A=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ 0^T& 1\end{array}\right]$	12	平行性、体积比	正交投影，平行性不变
射影变换	${\mathbf{T}}_P=\left[\begin{array}{cc}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^T& v\end{array}\right]$	15	接触平面的相交和相切	大小、平行性均发生改变

从真实世界到相机照片的变换为射影变换；若相机焦距为无穷远，则该变换退化为仿射变换。在深入学习相机模型前，只需对这些变换建立基本认知即可。

5. 实践：Eigen

本节讲解如何使用 Eigen 库表示矩阵与向量，并进一步实现旋转矩阵与变换矩阵的相关运算。KDevelop 工程形式的代码文件见附件。

5.1 Eigen 库简介

Eigen 是一款开源的 C++ 线性代数库，提供高效的矩阵线性代数运算功能，同时支持解方程等高级操作。众多上层软件库（如 g2o、Sophus）均基于 Eigen 实现矩阵运算。Eigen 的显著特点是纯头文件库，仅包含头文件，无.so 或.a 等二进制库文件，使用时只需引入头文件，无需链接库文件。本节示例仅涵盖基础矩阵运算，更多功能可参考 Eigen 官网教程 。

5.2 Eigen 库安装

若未安装 Eigen 库，可执行以下命令完成安装：

sudo apt install libeigen3-dev

5.3 代码示例

以下代码演示 Eigen 库的基本使用方法（含详细注释）：

#include<iostream>
using namespace std;

#include<ctime>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>  // 稠密矩阵的代数运算，如逆、特征值等
using namespace Eigen;

#define MATRIX_SIZE 50

int main (int argc, char**argv){
    // Eigen 中所有向量和矩阵均基于 Eigen::Matrix 模板类实现，前三个模板参数依次为数据类型、行数、列数
    // 声明一个 2×3 的 float 类型矩阵
    Matrix<float, 2, 3> matrix_23f;
    // Eigen 通过 typedef 提供了多种内置类型，底层仍为 Eigen::Matrix
    // 例如 Vector3d 等价于 Eigen::Matrix<double, 3, 1>（三维向量）
    Vector3d v_3d;
    Matrix<float, 3, 1> matrix_31f;
    
    // 初始化 3×3 的 double 类型矩阵，所有元素置 0
    Matrix3d matrix_33d = Matrix3d::Zero ();
    // 动态大小矩阵：Matrix<double, Dynamic, Dynamic> 与 MatrixXd 等价
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    MatrixXd matrix_x;
    
    // 矩阵初始化：逐行输入数据
    matrix_23f << 1, 2, 3, 4, 5, 6;
    cout << "matrix 2*3 from 1 to 6:\n" << matrix_23f << endl;
    
    // 通过 () 运算符访问矩阵元素
    cout << "print matrix 2*3:" << endl;
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 3; j++) {
            cout << matrix_23f (i, j) << "\t";
        }
        cout << endl;
    }
    
    // 向量初始化
    v_3d << 3, 2, 1;
    matrix_31f << 4, 5, 6;
    
    // Eigen 不支持不同数据类型矩阵的混合运算，需显式类型转换；同时需保证矩阵维度匹配
    Matrix<double, 2, 1> result = matrix_23f.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose () << endl;
    
    Matrix<float, 2, 1> result2 = matrix_23f * matrix_31f;
    cout << "[1,2,3;4,5,6]*[4,5,6]=" << result2.transpose () << endl;

    // 维度不匹配会导致编译错误，以下为错误示例（注释掉）
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23f.cast<double>() * v_31d;
    
    // 矩阵基本运算
    matrix_33d = Matrix3d::Random ();    // 生成随机数矩阵
    cout << "random matrix: \n" << matrix_33d << endl;
    cout << "transpose: \n" << matrix_33d.transpose () << endl;    // 转置
    cout << "sum:" << matrix_33d.sum () << endl;    // 所有元素求和
    cout << "trace:" << matrix_33d.trace () << endl;    // 迹（主对角线元素和）
    cout << "times 10: \n" << 10 * matrix_33d << endl;    // 数乘
    cout << "inverse: \n" << matrix_33d.inverse () << endl;    // 逆矩阵
    cout << "det:" << matrix_33d.determinant () << endl;    // 行列式
    
    // 特征值与特征向量求解（实对称矩阵可保证对角化成功）
    SelfAdjointEigenSolver<Matrix3d> eigen_solver (matrix_33d.transpose () * matrix_33d);
    cout << "Eigen values=\n" << eigen_solver.eigenvalues () << endl;
    cout << "Eigen vectors=\n" << eigen_solver.eigenvectors () << endl;
    
    // 求解线性方程组：matrix_NN * x = v_N1d
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random (MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose ();  // 构造正定矩阵
    Matrix<double, MATRIX_SIZE, 1> v_N1d = MatrixXd::Random (MATRIX_SIZE, 1);
    
    // 计时：直接求逆法
    clock_t time_stt = clock ();
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse () * v_N1d;
    cout << "time of normal inverse is" << 1000 * (clock () - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x =" << x.transpose () << endl;
    
    // 计时：QR 分解法（速度更快）
    time_stt = clock ();
    x = matrix_NN.colPivHouseholderQr ().solve (v_N1d);
    cout << "time of Qr decomposition is" << 1000 * (clock () - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x =" << x.transpose () << endl;
    
    // 计时：Cholesky 分解法（适用于正定矩阵）
    time_stt = clock ();
    x = matrix_NN.ldlt ().solve (v_N1d);
    cout << "time of ldlt decomposition is" << 1000 * (clock () - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x =" << x.transpose () << endl;
    
    // 计时：LU 分解法
    time_stt = clock ();
    x = matrix_NN.lu ().solve (v_N1d);
    cout << "time of lu decomposition is" << 1000 * (clock () - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x =" << x.transpose () << endl;
    
    return 0;
}

5.4 CMakeLists.txt 配置

cmake_minimum_required (VERSION 3.0)
project (rigidMotion)
add_executable (useEigen useEigen.cpp)
set (CMAKE_BUILD_TYPE "Debug")

5.5 运行结果

编译并运行程序后，输出结果示例如下：

matrix 2*3 from 1 to 6:
1 2 3
4 5 6
print matrix 2*3:
1	2	3	
4	5	6	
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]=32 77
random matrix: 
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
transpose: 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values=
0.0242899
 0.992154
  1.80558
Eigen vectors=
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 1.967ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 2.409ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 0.667ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of lu decomposition is 0.787ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

附件包含本讲所有相关代码。后续将讲解刚体运动第二部分（旋转向量与欧拉角）及第三部分（四元数表示旋转），欢迎留言讨论，您的关注是内容更新的动力。

本文基于《视觉 SLAM 十四讲：从理论到实践》与《Quaternions, Interpolation and Animation》编写，在原文基础上进行适当精简，并结合网络优质资源与作者理解补充注解及扩展知识点。

---

三维空间刚体运动 2：旋转向量与罗德里格斯公式（最详细推导）

龙焰智能 于 2024-11-08 20:34:10 修改

序

本系列文章参照高翔老师《视觉 SLAM 十四讲：从理论到实践》，系统讲解三维空间刚体运动的相关理论，为读者构建扎实的数学基础。

1. 旋转向量定义

旋转矩阵的表示方式存在以下两方面局限：

1.$SO(3)$ 中的旋转矩阵包含 9 个元素，但一次三维旋转仅需 3 个自由度即可描述，因此这种表示存在冗余；同理，$SE(4)$ 也存在类似冗余问题。是否存在更紧凑的表示方式？
2. 旋转矩阵与变换矩阵具有固有约束：旋转矩阵需满足正交性且行列式为 1，变换矩阵需维持特定结构。这些约束会增加旋转矩阵或变换矩阵的估计与优化难度。

基于上述问题，需要一种更紧凑的方式描述旋转与平移。

旋转向量：任意旋转均可由旋转轴与旋转角共同刻画。定义单位向量 $u$（方向与旋转轴一致，也可采用 $n$、$k$ 等符号表示），旋转角为 $\theta$，则向量 $\theta u$ 可完整描述该旋转，此类向量称为旋转向量（或轴角 / 角轴，Axis-Angle），仅需三维向量即可表征旋转过程。

三维空间中，一个方向可由两个参数确定（如地球经纬度），第三个参数可用于表示旋转角度。对于变换过程，采用一个旋转向量与一个平移向量即可完整描述，变量维度恰好为六维，无冗余信息。

2. 罗德里格斯公式 - 向量转换为矩阵

2.1 定义

罗德里格斯公式：旋转向量与旋转矩阵之间的转换可通过罗德里格斯公式（Rodrigues’s Formula）实现。由于任意旋转由旋转轴 $u$ 与旋转角 $\theta$ 刻画，罗德里格斯公式的具体形式如下：
$$R=\cos \theta \cdot I+(1-\cos \theta )\cdot u{u}^T+\sin \theta \cdot u^{\wedge }\text{\hspace{1em}(2.1)}$$
其中符号 ${}^{\wedge }$ 为向量到反对称矩阵的转换运算符，具体定义见上一篇博客《三维空间刚体运动 1：旋转矩阵与变换矩阵》的公式（1.4）。该公式也可表示为：
$$R=I+(1-\cos \theta )U^2+\sin \theta \cdot U\text{\hspace{1em}(2.2)}$$
式中 $U$ 为向量 $u$ 对应的反对称矩阵 $u^{\wedge }$。假设单位旋转向量 $u$ 的坐标为 $u=\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]$，旋转角为 $\theta$，则旋转矩阵 $R$ 的展开式为：
$$R=\left[\begin{array}{ccc}\cos \theta +u_x^2(1-\cos \theta )& u_x{u}_y(1-\cos \theta )-u_z\sin \theta & u_y\sin \theta +u_x{u}_z(1-\cos \theta )\\ u_z\sin \theta +u_x{u}_y(1-\cos \theta )& \cos \theta +u_y^2(1-\cos \theta )& -u_x\sin \theta +u_y{u}_z(1-\cos \theta )\\ -u_y\sin \theta +u_x{u}_z(1-\cos \theta )& u_x\sin \theta +u_y{u}_z(1-\cos \theta )& \cos \theta +u_z^2(1-\cos \theta )\end{array}\right]\text{\hspace{1em}(2.3)}$$

2.2 推导

首先明确几何模型：定义 $u$ 为旋转轴的单位向量，$v$ 为待旋转向量，$w$ 为 $u\times v$ 方向的单位向量。向量 $v$ 绕 $u$ 旋转角度 $\theta$ 后得到 $v_{rot}$。将 $v$ 分解为平行于旋转轴 $u$ 的分量 $v_{\parallel }$ 与正交于 $u$ 的分量 $v_{\perp }$；同理，$v_{rot}$ 分解为 $v_{rot\parallel }$ 与 $v_{rot\perp }$，其中 $v_{rot\parallel }=v_{\parallel }$（旋转不改变平行于旋转轴的分量）。向量 $a$ 与 $b$ 分别为 $v_{rot\perp }$ 在 $w$ 与 $v_{\perp }$ 方向上的分量。

图 2.1：旋转向量 3D 图（数学绘图软件推荐 geogebra）

罗德里格斯公式建立 $v_{rot}$ 与 $v$ 的线性关系 $v_{rot}=Rv$，进而推导旋转矩阵 $R$。该公式存在两种表达形式，对应两种推导方法，差异在于 $v_{\perp }$ 的表示方式，具体推导过程如下。

2.2.1 推导一

1.向量分解：
待旋转向量 $v$ 可分解为平行于旋转轴的分量与正交分量之和：
$$v=v_{\perp }+v_{\parallel }\text{\hspace{1em}(2.4)}$$
移项可得正交分量：
$$v_{\perp }=v-v_{\parallel }$$
旋转后向量 $v_{rot}$ 的分解形式为：
$$v_{rot}=v_{rot\parallel }+v_{rot\perp }\text{\hspace{1em}(2.5)}$$
后续需分别推导 $v_{rot\parallel }$ 与 $v_{rot\perp }$。

2.平行分量 $v_{rot\parallel }$：
由于旋转过程中平行于旋转轴的向量保持不变，故 $v_{rot\parallel }=v_{\parallel }$。$v_{\parallel }$ 为 $v$ 在 $u$ 上的正交投影，根据正交投影公式：
$$v_{\parallel }={\text{proj}}_u(v)=\frac{u\cdot v}{u\cdot u}u=(u\cdot v)u\text{\hspace{1em}(2.6)}$$
其中 $u$ 为单位向量（$||u||=1$），故 $u\cdot u=||u|{|}^2=1$，点积 $u\cdot v$ 为标量，与 $u$ 相乘后得到向量。

3.正交分量 $v_{rot\perp }$：
正交分量的旋转可视为二维平面内的旋转（旋转轴 $u$ 垂直于该平面）。为描述该旋转，需构造平面内的正交基向量 $w$，满足同时正交于 $u$ 与 $v_{\perp }$，通过叉乘可得：
$$w=u\times v_{\perp }\text{\hspace{1em}(2.7)}$$
基于右手坐标系，$w$ 指向 $v_{\perp }$ 逆时针旋转 $\pi /2$ 后的方向，且与 $v_{\perp }$ 同处于垂直于 $u$ 的平面内。由于 $||u||=1$ 且 $u$ 与 $v_{\perp }$ 正交，根据叉乘模长公式：
$$||w||=||u\times v_{\perp }||=||u||\cdot ||v_{\perp }||\cdot \sin (\frac{\pi }{2})=||v_{\perp }||\text{\hspace{1em}(2.8)}$$
即 $w$ 与 $v_{\perp }$ 模长相等，构成平面内的一组正交单位基。

利用三角关系，$v_{rot\perp }$ 可分解为 $w$ 与 $v_{\perp }$ 方向的分量之和：
$$v_{rot\perp }=a+b=\sin \theta \cdot w+\cos \theta \cdot v_{\perp }=\sin \theta (u\times v_{\perp })+\cos \theta \cdot v_{\perp }\text{\hspace{1em}(2.9)}$$
由叉乘分配律及 $u$ 与 $v_{\parallel }$ 平行（叉乘结果为零向量），可得：
$$u\times v_{\perp }=u\times (v-v_{\parallel })=u\times v-u\times v_{\parallel }=u\times v\text{\hspace{1em}(2.10)}$$

4.合并推导：
将 $v_{rot\parallel }$ 与 $v_{rot\perp }$ 代入 $v_{rot}$ 的分解式：
$$\begin{array}{rl}{v}_{rot}& =v_{rot\perp }+v_{rot\parallel }\\ & =\sin \theta (u\times v)+\cos \theta \cdot v_{\perp }+(u\cdot v)u\\ & =\sin \theta (u\times v)+\cos \theta (v-v_{\parallel })+(u\cdot v)u\\ & =\sin \theta (u\times v)+\cos \theta (v-(u\cdot v)u)+(u\cdot v)u\\ & =\cos \theta \cdot v+(1-\cos \theta )(u\cdot v)u+\sin \theta (u\times v)\end{array}\text{\hspace{1em}(2.11)}$$

5.矩阵形式转换：
为将上式转化为矩阵乘法形式，需对两项进行矩阵表示：

• 对于 $(u\cdot v)u$：由于向量为列向量，点积 $u\cdot v=u^{T}v$，因此：
  $$(u\cdot v)u=u(u^{T}v)=(u{u}^T)v\text{\hspace{1em}(2.12)}$$
• 对于 $u\times v$：利用反对称矩阵 $U=u^{\wedge }$，叉乘可表示为矩阵与向量的乘法：
  $$u\times v=Uv\text{\hspace{1em}(2.13)}$$
  其中 KaTeX parse error: \tag works only in display equations

将上述矩阵表示代入式（2.11），整理得：
$$\begin{array}{rl}{v}_{rot}& =\cos \theta \cdot Iv+(1-\cos \theta )\cdot u{u}^{T}v+\sin \theta \cdot Uv\\ & =\left(\cos \theta \cdot I+(1-\cos \theta )u{u}^T+\sin \theta \cdot U\right)v\end{array}\text{\hspace{1em}(2.15)}$$
因此，旋转矩阵为：
$$R=\cos \theta \cdot I+(1-\cos \theta )u{u}^T+\sin \theta \cdot U\text{\hspace{1em}(2.16)}$$
其中 $I$ 为三维单位矩阵。

2.2.2 推导二

该推导方法是利用叉乘表示正交分量 $v_{\perp }$，具体过程如下：
由向量叉乘的性质，正交分量 $v_{\perp }$ 可表示为：
$$v_{\perp }=-u\times (u\times v)\text{\hspace{1em}(2.17)}$$
将其代入推导一中 $v_{rot}$ 的表达式，结合 $U=u^{\wedge }$ 及反对称矩阵的性质 $U^2=u{u}^T-I$（可通过矩阵乘法验证），推导如下：
$$\begin{array}{rl}{v}_{rot}& =v_{rot\perp }+v_{rot\parallel }\\ & =\sin \theta (u\times v)+\cos \theta \cdot v_{\perp }+v-v_{\perp }\\ & =v+\sin \theta (u\times v)+(cos\theta -1)v_{\perp }\\ & =v+\sin \theta \cdot Uv-(1-\cos \theta )\cdot u\times (u\times v)\\ & =v+\sin \theta \cdot Uv+(1-\cos \theta )\cdot U^{2}v\\ & =\left(I+(1-\cos \theta )U^2+\sin \theta \cdot U\right)v\end{array}\text{\hspace{1em}(2.18)}$$
由此得到旋转矩阵的另一种表达式：
$$R=I+(1-\cos \theta )U^2+\sin \theta \cdot U\text{\hspace{1em}(2.19)}$$
该形式在计算中涉及的参数更少，是实际旋转操作中常用的表达式。

2.2.3 推导向量 $a$ 和 $b$

向量 $a$ 与 $b$ 是 $v_{rot\perp }$ 在 $w$ 与 $v_{\perp }$ 方向的正交分量，需分别求解其大小与方向：

-向量 $a$：

• 大小：设 ${\theta }_1=\pi -\theta$，${\theta }_2$ 为 $v$ 与 $u$ 的夹角，由三角关系及叉乘模长公式 $|u\times v|=|u||v|\sin {\theta }_2=|v|\sin {\theta }_2$，可得：
  $$|a|=\sin {\theta }_1|v_{rot\perp }|=\sin (\pi -\theta )|v_{\perp }|=\sin \theta \cdot |v|\sin {\theta }_2=\sin \theta \cdot |u\times v|\text{\hspace{1em}(2.20)}$$
• 方向：与 $u\times v$ 一致，单位方向向量为 $\frac{u\times v}{|u\times v|}$。
• 综上：KaTeX parse error: \tag works only in display equations

-向量 $b$：

• 大小：由图中几何关系，$|b|=\cos {\theta }_1|v_{rot\perp }|=\cos (\pi -\theta )|v_{\perp }|=-\cos \theta |v_{\perp }|$（负号表示方向与 $v_{\perp }$ 相反）。
• 方向：与 $v_{\perp }$ 相反，单位方向向量为 $-\frac{v_{\perp }}{|v_{\perp }|}$。
• 综上：KaTeX parse error: \tag works only in display equations

至此，罗德里格斯公式的两种形式均推导完成。若需了解 Oxyz 坐标系下的旋转变换关系，可参考博客《图形变换之旋转变换公式推导》。

罗德里格斯公式描述了旋转向量到旋转矩阵的正向转换，下文将推导旋转矩阵到旋转向量的反向转换方法。

3. 旋转矩阵到旋转向量

3.1 旋转角 $\theta$ 的求解

对旋转矩阵 $R$ 取迹（主对角线元素之和），结合罗德里格斯公式：
$$\begin{array}{rl}\text{tr}(R)& =\cos \theta \cdot \text{tr}(I)+(1-\cos \theta )\cdot \text{tr}(u{u}^T)+\sin \theta \cdot \text{tr}(u^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )+0\\ & =1+2\cos \theta \end{array}\text{\hspace{1em}(3.1)}$$
其中：

-$\text{tr}(I)=3$（单位矩阵主对角线元素和为 3）；
-$\text{tr}(u{u}^T)=u^{T}u=1$（$u{u}^T$ 为秩 1 矩阵，迹等于主对角线元素和 $u_x^2+u_y^2+u_z^2=1$）；
-$\text{tr}(u^{\wedge })=0$（反对称矩阵主对角线元素均为 0）。

整理可得旋转角：
$$\theta =\arccos \left(\frac{\text{tr}(R)-1}{2}\right)\text{\hspace{1em}(3.2)}$$

3.2 旋转轴 $u$ 的求解

旋转轴上的向量在旋转后保持不变，即满足：
$$Ru=u\text{\hspace{1em}(3.3)}$$
该式表明 $u$ 是旋转矩阵 $R$ 特征值为 1 对应的特征向量。通过求解特征方程 $(R-I)u=0$，得到特征向量后进行归一化处理，即可得到单位旋转轴 $u$。

结合旋转角 $\theta$ 与旋转轴 $u$，旋转向量为 $v=\theta u$。

实践部分的代码将在第三部分（四元数表示旋转）中统一演示。

本文基于《视觉 SLAM 十四讲：从理论到实践》与《Quaternions, Interpolation and Animation》编写，在原文基础上进行适当精简，并结合网络优质资源与作者理解补充注解及扩展知识点。

---

三维空间刚体运动 3：欧拉角表示旋转（全面理解万向锁、RPY 角和欧拉角）

龙焰智能 于 2024-07-09 16:52:33 修改

序

本系列文章参照高翔老师《视觉 SLAM 十四讲：从理论到实践》，系统讲解三维空间刚体运动的相关理论，为读者构建扎实的数学基础。

1. 欧拉角

1.1 定义

旋转矩阵与旋转向量虽能准确描述旋转，但缺乏直观性，而欧拉角通过三个分离的转角，将一次旋转分解为三次绕不同轴的旋转操作，提供了更符合人类认知习惯的描述方式。

欧拉角：在三维空间中，欧拉角用于描述从固定参考系的初始方向，经一系列基本旋转后，得到另一参考系方向的过程。该过程仅涉及姿态变换，原点位置保持不变。

在讨论欧拉角具体形式前，需明确以下概念：

1.坐标系类型：旋转描述依赖坐标系定义，本文统一采用右手坐标系进行讨论，不同坐标系类型（左手 / 右手）会导致数据呈现结果存在差异。

2.旋转形式：旋转可分为绕定点、绕坐标轴、绕任意轴等多种类型，本文聚焦三维空间中绕坐标轴的旋转操作，不同旋转形式对应不同数学表达方法。

3.欧拉角顺规：欧拉角的旋转次序称为顺规（Rotation Sequence），即三次旋转的轴顺序规定。相同转角 $(\alpha ,\beta ,\gamma )$ 在不同顺规下会产生不同旋转结果，本文将根据具体旋转方式采用对应顺规。

4.静态与动态欧拉角：按参考坐标系分类，静态欧拉角以固定不变的绝对坐标系为参考（通常用小写 $x$-$y$-$z$ 表示）；动态欧拉角以随刚体运动的物体坐标系为参考（通常用大写 $X$-$Y$-$Z$ 表示）。

1.2 RPY 角与 Z-Y-X 欧拉角

描述坐标系 $B$ 相对于参考坐标系 $A$ 的姿态，常用两种欧拉角形式：RPY 角与 Z-Y-X 欧拉角。

RPY 角（Roll-Pitch-Yaw）：绕固定参考坐标系 $A$ 的旋转过程，又称 $X$-$Y$-$Z$ 固定角。初始状态下两坐标系重合，依次绕 $A$ 的 $X$ 轴旋转 $\alpha$（滚转角 Roll）、$Y$ 轴旋转 $\beta$（俯仰角 Pitch）、$Z$ 轴旋转 $\gamma$（偏航角 Yaw），最终得到目标姿态。其旋转矩阵满足：
$$R_{XYZ}(\alpha ,\beta ,\gamma )=R_Z(\gamma )R_Y(\beta )R_X(\alpha )\text{\hspace{1em}(1.1)}$$
旋转矩阵的乘法顺序与旋转操作顺序相反（从右至左），即先执行 $R_X(\alpha )$，再执行 $R_Y(\beta )$，最后执行 $R_Z(\gamma )$。

Z-Y-X 欧拉角：绕刚体自身坐标系 $B$ 的旋转过程，又称动态欧拉角。初始状态下两坐标系重合，依次绕 $B$ 的 $Z$ 轴旋转 $\gamma$、$Y$ 轴旋转 $\beta$、$X$ 轴旋转 $\alpha$，最终得到目标姿态。其旋转矩阵满足：
$$R_{X^{\prime }{Y}^{\prime }{Z}^{\prime }}(\alpha ,\beta ,\gamma )=R_{Z^{\prime }}(\gamma )R_{Y^{\prime }}(\beta )R_{X^{\prime }}(\alpha )\text{\hspace{1em}(1.2)}$$
旋转矩阵的乘法顺序与旋转操作顺序一致（从左至右），即先执行 $R_{Z^{\prime }}(\gamma )$，再执行 $R_{Y^{\prime }}(\beta )$，最后执行 $R_{X^{\prime }}(\alpha )$。

需要注意的是，上述两种旋转方式的展开式完全相等，即绕固定坐标系 $X$-$Y$-$Z$ 的 RPY 角旋转，与绕动态坐标系 $Z$-$Y$-$X$ 的欧拉角旋转等价。

结合航空领域常用定义进一步阐释：RPY 角（偏航 - 俯仰 - 滚转，Yaw-Pitch-Roll）的旋转顺序为绕固定坐标系 $Z$ 轴（偏航 Yaw）→$Y$ 轴（俯仰 Pitch）→$X$ 轴（滚转 Roll），对应顺规为 $Z$-$Y$-$X$，与上述 Z-Y-X 欧拉角的旋转效果一致。此时可通过三维向量 $[r,p,y{]}^T$（滚转角、俯仰角、偏航角）描述任意旋转。为减少万向锁发生概率，实际应用中常采用 $Z$-$X$-$Y$ 顺规，但无法完全消除该问题，具体原因将在后续章节说明。

图 1.1 Oxyz 坐标轴旋转

2. 欧拉角到旋转矩阵

旋转矩阵具有三个自由度，可与欧拉角相互转换。欧拉角对应的旋转矩阵可通过三次基本旋转矩阵（绕 $X$、$Y$、$Z$ 轴的旋转矩阵）的乘积得到，以下为具体推导过程：

2.1 二维基本旋转矩阵推导

以绕 $Z$ 轴旋转为例，推导二维旋转矩阵，再扩展至三维。设平面内点 $P(x,y)$ 绕原点旋转 $\theta$ 后得到 $P^{\prime }(x^{\prime },y^{\prime })$，$OP=O{P}^{\prime }=d=\sqrt{x^2+y^2}$，初始角度为 $\alpha$（$P$ 与 $X$ 轴夹角）。

根据三角函数定义：
$$\{\begin{array}{l}\sin \alpha =\frac{y}{d}\\ \cos \alpha =\frac{x}{d}\end{array}\text{\hspace{1em}(2.1)}$$
$$\{\begin{array}{l}\sin (\alpha +\theta )=\frac{y^{\prime }}{d}\\ \cos (\alpha +\theta )=\frac{x^{\prime }}{d}\end{array}\text{\hspace{1em}(2.2)}$$
利用两角和差公式展开：
$$\{\begin{array}{l}\sin (\alpha +\theta )=\sin \alpha \cos \theta +\cos \alpha \sin \theta \\ \cos (\alpha +\theta )=\cos \alpha \cos \theta -\sin \alpha \sin \theta \end{array}\text{\hspace{1em}(2.3)}$$
将式（2.1）代入式（2.3），消去 $d$ 与 $\alpha$，得到：
$$\{\begin{array}{l}{x}^{\prime }=x\cos \theta -y\sin \theta \\ y^{\prime }=x\sin \theta +y\cos \theta \end{array}\text{\hspace{1em}(2.4)}$$
以齐次坐标表示，旋转过程可写为矩阵乘法形式：
$$\widetilde{P^{\prime }}=\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=R_Z(\theta )\tilde{P}\text{\hspace{1em}(2.5)}$$
因此，绕 $Z$ 轴的三维旋转矩阵为：
$$R_Z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2.6)}$$

2.2 绕 $X$、$Y$ 轴的旋转矩阵

同理，可推导出绕 $X$ 轴与 $Y$ 轴的三维旋转矩阵：

• 绕 $X$ 轴旋转矩阵：
  $$R_X(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\text{\hspace{1em}(2.7)}$$
• 绕 $Y$ 轴旋转矩阵：
  $$R_Y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\text{\hspace{1em}(2.8)}$$

2.3 组合旋转矩阵

欧拉角对应的组合旋转矩阵为三次基本旋转矩阵的乘积，乘法顺序与旋转操作顺序相反（从右至左）。对于 $Z$-$Y$-$X$ 顺规（先绕 $Z$ 轴，再绕 $Y$ 轴，最后绕 $X$ 轴），组合旋转矩阵为：
$$R_{ZYX}(\alpha ,\beta ,\gamma )=R_X(\alpha )R_Y(\beta )R_Z(\gamma )\text{\hspace{1em}(2.9)}$$
其中 $\alpha$ 为绕 $X$ 轴的滚转角，$\beta$ 为绕 $Y$ 轴的俯仰角，$\gamma$ 为绕 $Z$ 轴的偏航角。

3. 旋转矩阵到欧拉角

已知旋转矩阵 $R$，可通过矩阵元素与三角函数的对应关系求解欧拉角。将式（2.9）展开，得到 $Z$-$Y$-$X$ 顺规下旋转矩阵的具体形式：
$$\begin{array}{rl}R& =\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos \beta \cos \gamma & -\cos \beta \sin \gamma & \sin \beta \\ \sin \alpha \sin \beta \cos \gamma +\cos \alpha \sin \gamma & -\sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma & -\sin \alpha \cos \beta \\ -\cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma & \cos \alpha \sin \beta \sin \gamma +\sin \alpha \cos \gamma & \cos \alpha \cos \beta \end{array}\right]\end{array}\text{\hspace{1em}(3.1)}$$
通过矩阵元素求解欧拉角：

1. 由 $R_{13}=\sin \beta$，得俯仰角：
   $$\beta =\arcsin (R_{13})\text{\hspace{1em}(3.2)}$$
2. 由 $-\frac{R_{23}}{R_{33}}=\tan \alpha$，得滚转角：
   $$\alpha =\arctan \left(-\frac{R_{23}}{R_{33}}\right)\text{\hspace{1em}(3.3)}$$
3. 由 $-\frac{R_{12}}{R_{11}}=\tan \gamma$，得偏航角：
   $$\gamma =\arctan \left(-\frac{R_{12}}{R_{11}}\right)\text{\hspace{1em}(3.4)}$$
   上述推导过程中，当 $\beta =\pm 90^{\circ }$ 时会出现奇异值，即万向锁问题，具体分析如下。

4. 万向锁

4.1 定义

万向锁问题（Gimbal Lock）：欧拉角的本质是通过三次绕轴旋转描述三维姿态，但当中间轴的旋转角度为 $\pm 90^{\circ }$ 时，第一次旋转与第三次旋转的轴会重合，导致系统丢失一个自由度（三次旋转退化为两次旋转），这种奇异现象称为万向锁。

理论上，任何通过三个实数描述三维旋转的方式都无法避免奇异性问题。因此，欧拉角不适用于插值、迭代等需要连续姿态变化的场景，仅常用于人机交互等对直观性要求较高的场景，在 SLAM 程序、滤波与优化算法中较少使用。

4.2 Z-Y-X 顺规下的万向锁分析

以 $Z$-$Y$-$X$ 顺规为例，结合几何原理与数学推导说明万向锁的产生机制：

4.2.1 几何解释

$Z$-$Y$-$X$ 顺规的旋转轴存在层级关系：$Z$ 轴为 $Y$ 轴的父级，$Y$ 轴为 $X$ 轴的父级。绕 $Z$ 轴旋转时，$Y$ 轴与 $X$ 轴随刚体同步旋转；绕 $Y$ 轴旋转时，仅 $X$ 轴随刚体旋转；绕 $X$ 轴旋转时，$Z$ 轴与 $Y$ 轴保持固定。

当绕 $Y$ 轴旋转 $\beta =90^{\circ }$ 时，$X$ 轴会旋转至与初始 $Z$ 轴重合的位置，此时绕 $X$ 轴的旋转与绕 $Z$ 轴的旋转效果等价，原本独立的三个自由度退化为两个，导致无法实现绕初始 $X$ 轴的独立旋转，即丢失一个自由度。

图 4.1 ZYX 欧拉旋转 - 1

图 4.2 ZYX 欧拉旋转 - 2

图 4.3 ZYX 欧拉旋转 - 3

4.2.2 数学验证

将 $\beta =\frac{\pi }{2}$ 代入式（2.9），展开旋转矩阵：

$$\begin{array}{rl}{R}_{ZYX}\left(\alpha ,\frac{\pi }{2},\gamma \right)& =R_X(\gamma )R_Y\left(\frac{\pi }{2}\right)R_Z(\alpha )\\ & =\left[\begin{array}{ccc}0& 0& 1\\ \sin \alpha \cos \gamma +\cos \alpha \sin \gamma & -\sin \alpha \sin \gamma +\cos \alpha \cos \gamma & 0\\ -\cos \alpha \cos \gamma +\sin \alpha \sin \gamma & \cos \alpha \sin \gamma +\sin \alpha \cos \gamma & 0\end{array}\right]\\ & =\left[\begin{array}{ccc}0& 0& 1\\ \sin (\alpha +\gamma )& \cos (\alpha +\gamma )& 0\\ -\cos (\alpha +\gamma )& \sin (\alpha +\gamma )& 0\end{array}\right]\\ & =R_Y\left(\frac{\pi }{2}\right)R_Z(\alpha +\gamma )\end{array}\text{\hspace{1em}(4.1)}$$

推导结果表明，原本由三个旋转矩阵组成的组合变换，退化为两个旋转矩阵的乘积，$\alpha$ 与 $\gamma$ 合并为一个角度 $\alpha +\gamma$，证明系统丢失一个自由度。

对于其他顺规（如 $X$-$Y$-$Z$、$Z$-$X$-$Y$ 等），当中间轴旋转角度为 $\pm 90^{\circ }$ 时，均会出现类似的轴重合现象，导致万向锁。

4.3 解决方法

针对万向锁问题，常用两种应对策略：

1.优化顺规规避风险：选择刚体运动中较少出现中间轴旋转至 $\pm 90^{\circ }$ 的顺规，可最大程度减少万向锁发生概率，但无法完全避免。例如，相机姿态描述中常用 $Z$-$X$-$Y$ 顺规，利用相机较少朝天 / 朝地拍摄的使用场景，降低俯仰角达到 $\pm 90^{\circ }$ 的可能性。

2.采用四元数描述旋转：四元数通过四个实数描述三维旋转，不存在奇异性问题，可彻底解决万向锁。具体做法为：将欧拉角转换为四元数，通过球面线性插值（SLERP）获取连续姿态，再将四元数转回欧拉角应用于刚体。该方法的缺点是需额外存储四元数参数，消耗一定内存，四元数的详细讲解见后续文章。

5. 实践：Eigen 几何模块

本节通过 Eigen 库演示旋转向量、欧拉角与旋转矩阵之间的转换操作，代码包含详细注释，可直接编译运行。

5.1 代码实现

#include <iostream>
#include <cmath>
using namespace std;

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace Eigen;

// 本程序演示 Eigen 几何模块的旋转表示与转换
int main (int argc, char**argv) {
    // 1. 初始化旋转矩阵（单位矩阵）
    Matrix3d rotation_matrix = Matrix3d::Identity ();
    
    // 2. 定义旋转向量（绕 Z 轴旋转 45°，即 M_PI/4 弧度）
    AngleAxisd rotation_vector (M_PI / 4, Vector3d (0, 0, 1));
    cout.precision (3);  // 设置输出精度为 3 位小数
    cout << "旋转向量对应的矩阵:\n" << rotation_vector.matrix () << endl;
    
    // 3. 旋转向量转换为旋转矩阵
    rotation_matrix = rotation_vector.toRotationMatrix ();
    cout << "旋转向量转旋转矩阵:\n" << rotation_matrix << endl;
    
    // 4. 坐标变换（使用旋转向量与旋转矩阵）
    Vector3d v (1, 0, 0);  // 初始向量
    Vector3d v_rotated_by_vector = rotation_vector * v;
    Vector3d v_rotated_by_matrix = rotation_matrix * v;
    cout << "初始向量 (1,0,0) 经旋转向量变换后:" << v_rotated_by_vector.transpose () << endl;
    cout << "初始向量 (1,0,0) 经旋转矩阵变换后:" << v_rotated_by_matrix.transpose () << endl;
    
    // 5. 旋转矩阵转换为欧拉角（顺规：Z-Y-X，对应 yaw-pitch-roll）
    Vector3d euler_angles = rotation_matrix.eulerAngles (2, 1, 0);
    cout << "欧拉角 (yaw, pitch, roll):" << euler_angles.transpose () << endl;
    
    // 6. 定义欧式变换矩阵（包含旋转与平移）
    Isometry3d T = Isometry3d::Identity ();  // 初始为单位矩阵
    T.rotate (rotation_vector);              // 加入旋转
    T.pretranslate (Vector3d (1, 3, 4));      // 加入平移（x=1, y=3, z=4）
    cout << "欧式变换矩阵:\n" << T.matrix () << endl;
    
    // 7. 欧式变换矩阵进行坐标变换
    Vector3d v_transformed = T * v;  // 注意：Eigen 中乘法已重载，无需显式转换为齐次坐标
    cout << "初始向量 (1,0,0) 经欧式变换后:" << v_transformed.transpose () << endl;
    
    return 0;
}

5.2 运行结果

rotation matrix =
1 0 0
0 1 0
0 0 1
rotation vector =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
rotation vector to Matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Isometry3d T = 
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
T rotate rotation_vector = 
 0.707 -0.707      0      0
 0.707  0.707      0      0
     0      0      1      0
     0      0      0      1
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v = 1 0 0
v transofrmed = 1.71 3.71    4

5.3 结果分析

1. 旋转向量与旋转矩阵对同一向量的变换结果一致，验证了两者的等价性；
2. 欧拉角输出中，yaw = 0.785 rad（即 45°），pitch 与 roll 为 0，符合绕 Z 轴旋转 45° 的设定；
3. 欧式变换矩阵同时包含旋转（上左 3×3 子矩阵）与平移（上右 3×1 子矩阵），变换后向量的坐标为旋转与平移的叠加结果。

参考文献

1. 《视觉 SLAM 十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. 罗德里格斯公式推导
3. 四元数与三维旋转
4. 四元数与欧拉角（RPY 角）的相互转换
5. 图形变换之旋转变换公式推导
6. Bonus: Gimbal Lock By Krasjet

---

via：

• 三维空间刚体运动 1：旋转矩阵与变换矩阵（详解加代码示例）_二维平面刚体变换矩阵 - 优快云 博客
  https://blog.youkuaiyun.com/shao918516/article/details/105062787
• 三维空间刚体运动 2：旋转向量与罗德里格斯公式（最详细推导）_空间刚体位移计算公式 - 优快云 博客
  https://blog.youkuaiyun.com/shao918516/article/details/105109278
• 三维空间刚体运动 3：欧拉角表示旋转（全面理解万向锁、RPY 角和欧拉角）-优快云 博客
  https://blog.youkuaiyun.com/shao918516/article/details/105172829