机器人学前置基础回顾

三维刚体运动的完整表示体系是移动机器人定位、建图与控制的数学基石，无论是SLAM中的位姿估计、路径规划中的坐标映射，还是运动控制中的姿态解算，都离不开对不同运动表示法的灵活运用。很多中级开发者在工程实践中踩过的“姿态突变”“轨迹偏移”“建图扭曲”等坑，本质上都是对运动表示体系的约束条件、转换规则、适用场景理解不到位导致的。本节将从物理意义、数学推导、工程适用性三个维度，系统梳理旋转矩阵、欧拉角、四元数、李代数四种核心表示方法，明确它们之间的转换关系与避坑要点。

1.1 旋转矩阵：最直观的线性表示

旋转矩阵是三维刚体旋转最直观的线性表示，它是一个3×3的正交矩阵，满足$R^T R = I$且$\det(R) = 1$，其中$I$是3×3单位矩阵。从物理意义上看，旋转矩阵的每一列（或每一行）是旋转后坐标系基向量在原始坐标系下的投影：若$R = [e_x', e_y', e_z']$，则$e_x', e_y', e_z'$分别是旋转后x、y、z轴在原始坐标系下的单位方向向量。

旋转矩阵的核心优势是计算简单，坐标变换只需要做矩阵乘法：若点$p$在坐标系A下的坐标为$p_A$，坐标系A到坐标系B的旋转矩阵为$R_{AB}$，则$p$在坐标系B下的坐标为$p_B = R_{AB} p_A$。但在工程中，旋转矩阵有两个明显的局限性：一是冗余度高，9个元素仅包含3个自由度，存储和优化时的计算开销更大；二是不满足交换律，即$R_1 R_2 \neq R_2 R_1$，多轴旋转的乘序错误会直接导致姿态解算错误。

> 💡 工程提示：在ROS等机器人开发框架中，tf2库的`matrix3x3`类默认存储的是旋转矩阵，若需要手动拼接多段旋转，一定要严格遵循“从右到左、从子坐标系到父坐标系”的乘序规则。例如机器人先绕自身x轴转30°再绕世界y轴转30°，对应的旋转矩阵应为$R_{world} = R_{y,30°} R_{x,30°} R_{base}$，而非反向相乘，否则会出现姿态完全错误的故障。

旋转矩阵的逆变换等价于转置，即$R_{AB}^{-1} = R_{AB}^T = R_{BA}$，这一性质在坐标反变换中非常常用。例如已知世界坐标系到机器人基坐标系的旋转矩阵$R_{wb}$，则基坐标系到世界坐标系的旋转矩阵直接取转置即可，无需重新计算。

1.2 欧拉角：最符合人类认知的表示

欧拉角是用三个绕固定轴或旋转轴依次旋转的角度表示三维旋转的方法，按照旋转轴的不同可分为“外旋”和“内旋”两类：外旋是每次旋转绕固定世界轴旋转，内旋是每次旋转绕当前旋转后的坐标系轴旋转。工程中最常用的是内旋的ZYX顺序（也叫偏航-俯仰-滚转，Yaw-Pitch-Roll，即RPY）：先绕当前z轴转偏航角$\psi$，再绕旋转后的y轴转俯仰角$\theta$，最后绕再次旋转后的x轴转转滚转角$\phi$，对应的旋转矩阵可表示为： $$ R(\phi, \theta, \psi) = R_z(\psi) R_y(\theta) R_x(\phi) $$ 其中单轴旋转矩阵为： $$ R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}, \quad R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, \quad R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} $$

欧拉角的优势是直观易懂，三个角度直接对应人类对姿态的描述（比如无人机的航向角、俯仰角），非常适合人机交互场景。但欧拉角存在致命的万向节死锁（Gimbal Lock）问题：当俯仰角$\theta = \pm 90^\circ$时，旋转后的z轴与初始x轴重合，此时绕z轴的偏航旋转和绕x轴的滚转旋转会作用在同一个轴上，导致旋转自由度从3降到2，无法唯一描述当前的旋转状态。

> ⚠️ 常见误区：很多新手认为只要避免俯仰角到±90°就不会出现死锁，实际上当俯仰角接近±90°时，欧拉角的数值对旋转的敏感度会急剧升高，微小的角度变化会导致旋转矩阵出现大幅跳变，在滤波、插值等场景下会产生严重的数值误差。例如无人机垂直爬升时，俯仰角接近90°，此时欧拉角的滚转和偏航会出现耦合，导致姿态解算抖动。

欧拉角的另一个局限是不适合做微分运算和插值：在奇点附近，欧拉角对旋转的导数趋于无穷大，直接对欧拉角做插值会出现非匀速旋转的问题。因此欧拉角仅适合做姿态的输入/输出展示，不适合作为内部的运动表示和优化变量。

1.3 四元数：无奇点的紧凑表示

四元数是爱尔兰数学家哈密顿在1843年提出的超复数，由一个实部和三个虚部组成，形式为$q = w + xi + yj + zk$，其中$i,j,k$满足$i^2=j^2=k^2=ijk=-1$，$ij=k, jk=i, ki=j$，$ji=-k, kj=-i, ik=-j$。在机器人领域，我们通常使用单位四元数表示三维旋转，它满足模长约束$|q| = \sqrt{w^2+x^2+y^2+z^2} = 1$。

从物理意义上看，单位四元数和三维旋转一一对应（除符号歧义外）：对于一个绕单位轴$\boldsymbol{u} = [u_x, u_y, u_z]^T$旋转角度$\theta$的旋转，对应的单位四元数为： $$ q = \cos\frac{\theta}{2} + \sin\frac{\theta}{2} (u_x i + u_y j + u_z k) $$ 可以看到，四元数的实部是旋转角度一半的余弦值，虚部是旋转轴与旋转角度一半正弦值的乘积，这种表示方式天然避免了欧拉角的奇点问题：无论旋转角度多大，四元数的模长始终为1，不存在自由度丢失的情况。

四元数的核心运算是乘法，两个四元数$q_1 = w_1 + x_1i + y_1j + z_1k$和$q_2 = w_2 + x_2i + y_2j + z_2k$的乘积为： $$ q_1 q_2 = (w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2) + (w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2)i + (w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2)j + (w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2)k $$ 从几何意义上看，四元数乘法对应旋转的复合：$q_1 q_2$表示先做$q_2$对应的旋转，再做$q_1$对应的旋转（注意乘序和旋转矩阵的乘序一致，不满足交换律）。这一性质在姿态融合、位姿复合场景中非常高效，例如机器人同时有IMU预测的旋转和激光观测的旋转，直接用四元数相乘即可得到融合后的旋转，计算开销远低于旋转矩阵的乘法。

四元数存在一个独特的符号歧义：$q$和$-q$对应完全相同的旋转。这是因为实部$\cos(\theta/2)$是偶函数，虚部$\sin(\theta/2)\boldsymbol{u}$在$\theta$增加$2\pi$时，$\sin((\theta+2\pi)/2) = \sin(\theta/2 + \pi) = -\sin(\theta/2)$，因此旋转角度增加$360^\circ$时，四元数整体取反，但旋转效果完全相同。这一特性在插值、求导等场景中会引发严重问题：若两个本应相近的四元数符号相反，它们的点积会是负值，此时做Slerp（球面线性插值）会走旋转角度最大的路径，而非最短路径。

> 💡 工程提示：所有涉及四元数运算的场景（插值、乘法、求平均）之前，必须先做符号对齐：计算两个四元数的点积，若点积小于0，则将其中一个四元数整体取反，保证点积为正。这一操作在视觉SLAM的位姿图优化、IMU与视觉的融合中几乎是必做步骤，否则会出现姿态跳变、轨迹抖动等难以排查的故障。

四元数与其他旋转表示法的转换关系是工程中最常用的操作，其中四元数转旋转矩阵的公式为： $$ R(q) = \begin{bmatrix} 1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx) \\ 2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) \end{bmatrix} $$ 该公式可通过四元数乘法的矩阵表示推导得到，无需死记硬背：将四元数乘向量$p$的运算表示为$p' = q p q^{-1}$，展开后即可得到上述旋转矩阵。反过来，旋转矩阵转四元数时，可通过计算旋转矩阵的迹（trace）得到： $$ w = \frac{\sqrt{1 + R_{11} + R_{22} + R_{33}}}{2}, \quad x = \frac{R_{32} - R_{23}}{4w}, \quad y = \frac{R_{13} - R_{31}}{4w}, \quad z = \frac{R_{21} - R_{12}}{4w} $$ 当迹接近-1时（对应旋转角度接近180°），上述公式会出现数值不稳定，此时可选择最大对角线元素对应的轴计算，例如若$R_{11}$最大，则$x = \frac{\sqrt{1 + 2R_{11} - \text{trace}(R)}}{2}$，其余分量通过对应元素计算得到。

1.4 李代数so(3)/se(3)：优化与求导的线性空间

旋转矩阵是三维旋转群$SO(3)$上的元素，$SO(3)$是一个带约束的非线性流形（所有满足正交且行列式为1的3×3矩阵的集合），无法直接进行加法、求导等线性运算。而李代数是李群对应的线性切空间，可以将流形上的非线性运算展开到线性空间中，方便做微分、优化等操作，是SLAM中位姿优化、 bundle adjustment 等模块的核心数学工具。

1.4.1 so(3)：旋转矩阵的李代数

$so(3)$是三维旋转群$SO(3)$的李代数，每个元素是一个3×3的反对称矩阵，形式为： $$ \boldsymbol{\Phi} = [\boldsymbol{\phi}]^\times = \begin{bmatrix} 0 & -\phi_z & \phi_y \\ \phi_z & 0 & -\phi_x \\ -\phi_y & \phi_x & 0 \end{bmatrix} $$ 其中$\boldsymbol{\phi} = [\phi_x, \phi_y, \phi_z]^T$是3维向量，和反对称矩阵一一对应。从物理意义上看，$\boldsymbol{\phi}$是旋转的轴角表示：$\boldsymbol{\phi}$的方向是旋转轴的单位向量，模长是旋转角度$\theta$，即$\boldsymbol{\phi} = \theta \boldsymbol{u}$。

$so(3)$和$SO(3)$之间通过指数映射和对数映射相互转换：指数映射将$so(3)$的反对称矩阵映射为$SO(3)$的旋转矩阵，对数映射将$SO(3)$的旋转矩阵映射为$so(3)$的反对称矩阵（或对应的3维向量）。指数映射的闭式解就是罗德里格斯公式： $$ \exp([\boldsymbol{\phi}]^\times) = I + \frac{\sin\theta}{\theta}[\boldsymbol{\phi}]^\times + \frac{1-\cos\theta}{\theta^2}[\boldsymbol{\phi}]^{\times 2} $$ 其中$\theta = \|\boldsymbol{\phi}\|$是旋转角度，$[\boldsymbol{\phi}]^{\times 2} = [\boldsymbol{\phi}]^\times [\boldsymbol{\phi}]^\times$。当$\theta$接近0时，上述公式会出现数值不稳定，此时可使用泰勒展开的近似式：$\exp([\boldsymbol{\phi}]^\times) \approx I + [\boldsymbol{\phi}]^\times$。

对数映射的闭式解可通过旋转