做机器人、做具身智能，几乎每一行代码都会碰到旋转。真机上姿态跳变五度，抓取就掉件；手眼标定误差大了，抓杯子变成撞杯子；优化器跑着跑着不收敛，追到最后往往是位姿的更新量没加对地方。这些问题看着五花八门，根子经常是同一件事：李群、李代数、四元数这套描述旋转与刚体变换的语言没吃透。

我见过太多人卡在这一步：看公式能看懂，一到写代码就懵，不知道为什么要在旋转矩阵上套一层 exp，不知道为什么优化里要做“扰动”，更不知道为什么两个四元数插值要走 slerp 而不做线性平均。具身智能这个方向，位姿表示不是选修课，是每天都要用的底层工具——从机械臂的正逆运动学、移动机器人的位姿跟踪、多传感器外参标定，到点云配准、模仿学习里的动作空间设计，全在这上面。

这篇东西不打算做成教科书，而是把我在实际工程里反复踩过的点摊开讲：哪些概念必须建立直觉，指数映射怎么一步步推出来，代码怎么写才不会在边界情况翻车，优化里左扰动右扰动到底怎么选。适合刚入行想把这套东西彻底打通的朋友，也适合写过一些但总在细节上栽跟头的人。数学底子薄一点没关系，我会尽量用能想象出来的画面讲。

1. 为什么具身智能绕不开旋转这件事

1.1 从一次抓取失败说起：位姿表示的三类典型故障

先说个具体场景。机械臂末端执行器要去抓桌面上的一个圆柱形零件，视觉给的位姿看起来没问题，可是规划出来的轨迹走一半，末端绕自身轴拧了将近九十度，爪子合拢的时候是侧着夹的，零件直接弹飞。查了半天，视觉识别没错，逆运动学也没错，问题出在两次姿态之间做插值的方式上：用欧拉角三个分量各自做线性插值，中间经过一个特殊角度，姿态被“拧”了一下。

这类故障在我经历过的项目里可以归成三类。第一类是表示不唯一：同一个姿态，欧拉角有无数组解，四元数有正负两个，旋转向量有模长 2π 的周期性；模块之间传递时谁也没错，拼在一起就错了。第二类是奇异与不连续：欧拉角在特定角度附近会丢失一个自由度，角度值还会从 179 度突然跳到 -179 度，滤波器一看到这个跳变就会认为机器人瞬间转了 358 度。第三类是约束被破坏：旋转矩阵本来是正交的，可优化迭代几百轮之后，九个元素各自被独立更新，正交性早就漂掉了，用它算出来的点云配准结果自然越来越歪。

把这三类故障记在心里，后面所有的公式推导都能对上号。李群解决的其实是“怎么把旋转当成一个整体对象来运算”，李代数解决的是“怎么做局部的小幅更新还不破坏约束”，四元数解决的是“怎么用四个数紧凑地存一个旋转，并且插值插得漂亮”。这四句话就是整篇文章的主干。

1.2 旋转不是三个数：SO(3) 的约束到底卡在哪

一个 3×3 矩阵有九个元素，描述旋转只用了三个自由度，剩下的六个自由度全被约束掉了。约束写出来很朴素：矩阵的列向量必须是单位向量，且两两正交，也就是 R 转置乘 R 等于单位阵；再加一条行列式等于 1，把镜像反射这种情况排除掉。满足这些条件的矩阵集合，数学上记作 SO(3)，读作“三维特殊正交群”。

问题在于，这六个约束是非线性的。你没办法用三个独立的数去参数化这个集合，还能保持全局连续、无奇异——这是拓扑性质决定的，不是工程上偷懒。既然躲不掉，就只能换个思路：不去想“用几个数表示旋转”，而是把“所有旋转组成的集合”本身当成一个弯曲的空间，在这个空间上做微积分。这就是流形的视角，也是后面所有内容的出发点。

我常跟人举一个例子：地球表面是二维曲面，你站在上面，脚下那一小块看起来就是平的，可以用一张地图（经纬度或者平面坐标）描述。但没有任何一张平面地图能把整个地球无失真地铺满。旋转集合 SO(3) 也一样，它是个三维的弯曲空间，局部看起来像三维欧氏空间，全局却没法用三个数一张图铺完。欧拉角的万向锁，本质上就是某张地图在极点附近失效了。

1.3 五种位姿表示到底怎么选：一张对照表说清楚

工程里常见的选择就那么几种，我用一张表把它们摆在一起，顺便说清楚我自己的取舍习惯。

这张表里最值得说一句的是最后一行。做具身智能的人迟早会碰到“让神经网络直接吐一个姿态”的需求。如果网络输出三个欧拉角，训练会很难收敛，因为同样的姿态在参数空间里可能对应完全不同的数值，损失函数变成了锯齿状。如果输出四元数，又得加一个归一化约束，还得处理正负号的歧义。工程上比较管用的做法是让网络输出一个 3×2 的矩阵，取旋转矩阵的前两列，然后在应用时做一次正交化把第三列叉乘出来。这个表示在整个 SO(3) 上是连续且唯一的，训练会顺利得多。代价是多了两个参数，以及每次使用前必须重新正交化，忘了这一步误差会慢慢累积。

选型的核心逻辑其实只有一条：这个环节需不需要做插值、求导、或者做最小二乘更新。需要，就别用欧拉角；不需要，只是给人看或者写进配置文件，那欧拉角最省事。我自己的习惯是，系统内部全部用旋转矩阵和四元数，只在边界处（配置文件、人机界面、调试日志）转成欧拉角。

2. 李群：先把“一群旋转”当成一个整体

2.1 群的四条规矩与 SO(3)、SE(3) 的成员资格

“群”这个词听着吓人，规矩其实只有四条：任意两个成员运算后的结果还在群里（封闭性），连着做三次运算顺序不影响结果（结合律），群里有一个“什么都不做”的成员（单位元），每个成员都有一个能抵消自己的成员（逆元）。就这么四条，没有别的。

SO(3) 完全符合。两个旋转矩阵相乘还是旋转矩阵，乘法天然满足结合律，单位阵就是“不旋转”，每个旋转矩阵的逆就是它的转置。所以 SO(3) 是个群。但它不能满足交换律，先绕 X 转再绕 Y 转，和先绕 Y 再绕 X，结果不一样。这一点非常关键，后面所有关于“顺序”的坑都源于此。

刚体变换组成的集合叫 SE(3)，成员是 4×4 的齐次变换矩阵，左上角是旋转矩阵，右上角是平移向量，最后一行是 0 0 0 1。它同样满足那四条规矩。工程上所有“位姿”这个概念，严格来说指的都是 SE(3) 里的一个元素，包含姿态和平移两部分。这个区分很重要：很多人说“位姿”，脑子里只想着姿态，结果平移和旋转的耦合关系没处理好，标定出来的外参总是差一点。

为什么叫“李群”？因为这群成员不仅满足群的四条规矩，还额外长在一张光滑的曲面上，成员之间可以连续地走过去，可以做微积分。这个“光滑”二字，就是李群区别于普通群的全部。有了光滑性，我们才能谈“在某个位姿附近做一点小改动”，才能谈导数，才能做优化。

2.2 流形视角：站在球面上的人只能看到一张平面地图

理解李群最关键的一步，是把“集合”换成“空间”。SO(3) 里的所有元素堆在一起，形成一个三维的光滑曲面——注意，是三维的曲面，不是三维空间，它弯曲地嵌在九维空间里，但我们只关心它自身的三维结构。

想象你站在一个巨大的球面上。你的活动范围很小，脚下那一块几乎就是平的，你可以用一张方格纸（两个坐标）描述附近的位置。这张方格纸就是切空间，切空间里的坐标就是李代数。但你走远了，方格纸就不管用了，必须换一张。整颗球没法用一张方格纸铺满，这就是为什么参数化一定有奇异点。

这个类比能解释很多工程现象。比如为什么旋转的“小增量”可以直接用三个数表示，因为增量足够小的时候，切空间的近似成立，误差是二阶的；而为什么大幅度旋转不能就用三个数加一加，因为你在球面上跨了大半步，切平面的近似就崩了。我见过有人在卡尔曼滤波里直接对欧拉角做加减，短时间看着还行，机器人转两圈之后姿态就彻底跑飞了。

还有一个常见的误解：切空间在原点的切平面和任意一点的切平面，作为向量空间是一回事，但作为“贴在哪个位置”是两回事。这个差别在优化里就体现为左扰动和右扰动的区别，后面会专门讲。

2.3 复合与逆：坐标系顺序错一位，整条链子全废

位姿复合是我见过出错率最高的地方，比公式推导错还常见。根本原因是记号不统一，有人写 T_AB 表示“从 A 到 B 的变换”，有人写表示“B 在 A 坐标系下的位姿”，这两种约定描述的可能是同一个矩阵，也可能是互为逆矩阵，完全取决于作者的流派。

我自己的习惯是固定用一种：T_AB 表示“把 A 坐标系下的点变换到 B 坐标系下”，也就是 p_B 等于 T_AB 乘以 p_A。这样写有两个好处。第一，链式法则看图就能写出来，从 A 到 C 经过 B，就是 T_AC 等于 T_BC 乘以 T_AB，下标首尾相接，中间的被消掉，跟单位换算一个道理。第二，逆矩阵的形式很直观，T_AB 的逆就是 T_BA，解析式是旋转部分取转置，平移部分变成负的转置乘以平移。

注意：T_AB 的逆不是把矩阵里每个元素取倒数，也不是把平移取负就完事，必须写成 R 转置和 -R 转置乘 t。这个错误在初学者代码里出现的频率高得惊人，而且症状很隐蔽——小角度旋转时误差很小，跑长了才暴露。

链式复合还有一个常被忽略的点：平移的顺序不能交换。先绕自身轴转再平移，和先平移再绕原点转，结果是完全不同的两个位姿。很多“示教轨迹复现偏了”的问题，追根到底就是把这两步的顺序写反了。我现在的做法是，凡是超过三段的位姿链，一律拆成中间变量，每个变量都用注释标明它的物理含义，宁可变多几行，也不要一行写完。

3. 李代数：把弯的旋转拉直成三个数

3.1 hat 与 vee：反对称矩阵就是旋转的“瞬时角速度”

从旋转矩阵到李代数，中间要过一道桥，叫 hat 操作，写作一个方括号加下标叉乘。它把一个三维向量 [x, y, z] 变成一个 3×3 的反对称矩阵，特点是反对称且对角线上全是零。反过来把反对称矩阵变回向量叫 vee 操作。

这个操作的物理意义特别清晰：给你一个旋转，考虑它在极短时间内的变化率，变化率本身就是一个反对称矩阵，把它 vee 一下得到的向量，就是角速度。所以 so(3)（李代数）里的元素，你可以直观地理解为“角速度向量”，它的方向是转轴，模长是角速度大小。旋转矩阵描述的是“转到了哪”，李代数描述的是“正在怎么转”。

代码非常短，但这是后面一切的基础：

import numpy as np def hat(w): w = np.asarray(w, dtype=float) return np.array([[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]]) def vee(M): return np.array([M[2, 1], M[0, 2], M[1, 0]])

提示：vee 只对严格反对称的矩阵有意义。如果矩阵有微小的对称部分（数值误差导致），直接用上面这三个元素取值会把误差放大。工程上稳妥一点的做法是先做一次 (M - M.T) / 2 再取，或者干脆在做 vee 之前把矩阵投影回反对称空间。

hat 操作还有个特别好用的性质：对任意旋转矩阵 R 和任意向量 a，有 R 乘以 hat(a) 等于 hat(R a) 乘以 R。这个性质在推导雅可比的时候会反复用到，值得背下来。它说的其实是“先转再叉乘”和“先叉乘再转”是等价的，这符合直觉，因为叉乘本质上是个几何操作，跟着坐标系一起转就行。

3.2 指数映射：Rodrigues 公式是怎么从微分方程长出来的

现在到最关键的一步：给定一个角速度向量，怎么算出转完之后那个旋转矩阵？答案是指数映射，也就是把矩阵放到 e 的指数上。推导过程值得走一遍，因为走完你就再也不会觉得这个公式是天上掉下来的。

假设一个旋转矩阵随时间演化，角速度恒定，那么它的变化率满足一个微分方程：R 对时间的导数等于 hat(角速度) 乘以 R。这个方程的意思是，每一瞬间的增量都是在当前姿态的基础上左乘一个微小的旋转。解这个方程，形式上是 R(t) 等于 e 的 hat(角速度) t 次方。把指数按泰勒级数展开，得到单位阵、hat 项、hat 平方项除以二的阶乘、hat 三次方项除以三的阶乘，一直加下去。

关键技巧来了。hat 的三次方等于负的 hat 乘以模长平方，hat 的四次方等于负的 hat 平方乘以模长平方，以此类推。这是个循环规律，跟虚数单位的平方等于负一是同一回事。利用这个循环，把级数按“奇数次项”和“偶数次项”分开重排，偶数项凑成余弦，奇数项凑成正弦除以模长，最后就得到那个著名的公式：单位阵加上正弦除以角度乘以 hat，再加上一减余弦除以角度平方乘以 hat 平方。这就是 Rodrigues 公式。

写成代码时有一个必须处理的坑：角度趋近于零的时候，正弦除以角度和一减余弦除以角度平方都是 0 除以 0，直接用会得到 nan。数值上有两种做法，一是加一个阈值判断，小角度时用泰勒展开的前几项近似；二是用 sinc 函数或者直接判断分母。我习惯用阈值判断，把小角度时把公式退化成单位阵加 hat 加半个 hat 平方，在这个量级上误差可以忽略。

def so3_exp(phi): phi = np.asarray(phi, dtype=float) theta = np.linalg.norm(phi) K = hat(phi) if theta < 1e-8: return np.eye(3) + K + 0.5 * (K @ K) return (np.eye(3) + (np.sin(theta) / theta) * K + ((1.0 - np.cos(theta)) / theta**2) * (K @ K))

这段代码值得多看两眼。注意那个阈值不是在“防止除以零”，而是在“选择用哪个近似公式”。阈值取多大其实有讲究，取太大则小角度时精度损失，取太小则大角度时泰勒级数不够项。实测下来 1e-8 到 1e-6 之间都够用，双精度下 1e-8 比较稳妥。单精度浮点（比如在嵌入式或者 GPU 上跑）就得放宽到 1e-4 左右，否则三角函数本身的有效位数就不够了。

3.3 对数映射：反解的时候那两个边界情况能要命

指数映射是“从李代数到李群”，对数映射是反方向，从旋转矩阵反解出旋转向量。中心公式很简洁：先由矩阵的迹算旋转角，再用矩阵减去转置、除以两倍正弦、最后乘上角度。但这里藏着两个必须单独处理的边界。

第一个边界是旋转角趋近于零。此时正弦也趋近于零，分母没了，公式直接爆炸。这时候的正确做法是用一阶近似，旋转向量约等于反对称部分 vee 出来的一半。因为在原点附近，指数映射可以近似为“单位阵加 hat”，所以反过来就是“反对称部分的一半”。

第二个边界是旋转角趋近于 π。此时正弦同样趋近于零，但这次不是因为角度小，而是因为角度接近 180 度，正弦在 π 处本来就过零。这个情况更难缠，因为接近 π 的时候，旋转轴的正负号几乎无法从矩阵的反对称部分判断出来——转 179.9 度和转 -179.9 度，矩阵几乎一样。我的处理办法是换一条路：从矩阵加单位阵除以二里面提取旋转轴，因为这部分等于轴向量乘自己的转置，对角线元素开方就得到轴的各个分量，符号可以通过非对角线元素相对最大的那个分量来确定。虽然麻烦，但这是硬件和算法里都通用的做法。

def so3_log(R): cos_t = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0) theta = np.arccos(cos_t) if theta < 1e-6: return 0.5 * vee(R - R.T) if np.pi - theta < 1e-4: A = 0.5 * (R + np.eye(3)) i = int(np.argmax(np.diag(A))) axis = np.zeros(3) axis[i] = np.sqrt(max(A[i, i], 0.0)) for j in range(3): if j != i and axis[i] > 1e-8: axis[j] = A[i, j] / axis[i] axis /= np.linalg.norm(axis) return theta * axis return (theta / (2.0 * np.sin(theta))) * vee(R - R.T)

注意：接近 π 的那一支里，解出来的轴方向可能和你期望的差一个负号。别慌，这不影响正确性——绕轴转 π 度和绕反方向转 π 度是同一个旋转。但如果你在做一个需要连续轨迹的系统（比如插值），符号翻转会造成轨迹突变。解决办法是在对 log 的输出做序列化时，跟前一帧的结果比一下点积，为负就翻转。

对数映射还有一个容易忽略的性质：它天然是多值的。绕某轴转 θ 和绕反轴转 2π 减 θ 是同一个旋转，所以旋转向量的模长永远可以取在 0 到 π 之间。这给了我们一个天然的“规范形式”：把对数映射的输出模长限制在 0 到 π，方向取对应的那个。做误差计算的时候，如果两个姿态之间差得比 π 还大，算出来的误差方向可能是反的，会让优化器朝着错的方向走。这一点在训练模仿学习策略时特别要命——动作空间里表示旋转的那几个分量如果是用 log 算出来的，一定要先规范化，否则网络学出来的东西会自相矛盾。

3.4 SE(3) 的指数映射与那个总被漏掉的 V 矩阵

刚体变换的指数映射，形式上是 4×4 矩阵的指数，但实际计算不用真的去算矩阵指数，可以拆成两部分：旋转部分直接复用 SO(3) 的公式，平移部分则是输入里那个平移分量乘上一个叫 V 的矩阵。

这个 V 矩阵长得和 Rodrigues 公式很像，只是系数不同：单位阵、加一个一减余弦除以角度平方乘以 hat、再加一个角度减正弦除以角度立方乘以 hat 平方。它其实是个积分项，含义是“在旋转的过程中，把每一瞬间的平移速度累积起来”。

为什么必须要它？想象一个机器人一边旋转一边前进。你在体坐标系下给出一个恒定的线速度，机器人实际在世界系里走出来的轨迹是一条螺旋线，不是直线。终点位置不等于“速度乘以时间”，因为速度的方向一直在跟着机体转。V 矩阵就是在补偿这个旋转带来的方向变化。

我见过太多人写 SE(3) 指数映射时忘了 V，直接把平移分量照抄过去。这种代码在小旋转角度下测试完全通过，因为 V 退化成了单位阵，一旦旋转超过几十度，位置误差就肉眼可见了。这个 bug 的特点是“实验室里永远测不出来，客户现场第一周就炸”，因为它对旋转角度敏感，而测试用例往往只测平移。

def se3_exp(xi): rho, phi = np.asarray(xi[:3], float), np.asarray(xi[3:], float) theta = np.linalg.norm(phi) K = hat(phi) R = so3_exp(phi) if theta < 1e-8: V = np.eye(3) + 0.5 * K + (1.0 / 6.0) * (K @ K) else: V = (np.eye(3) + ((1.0 - np.cos(theta)) / theta**2) * K + ((theta - np.sin(theta)) / theta**3) * (K @ K)) return R, V @ rho

顺带说一句，V 矩阵同时也是 SO(3) 的右雅可比。这个巧合不是偶然，它俩本来就是同一个积分在两种语境下的不同外貌。下一节会用到这个事实。

3.5 雅可比：加了扰动之后，指数映射该怎么展开

做优化必须求导。旋转空间上的求导和普通向量不一样，因为你不能直接说“旋转加一个小量”，得先定义清楚这个小量是怎么加上去的。有两种加法：左乘一个小旋转，或者右乘一个小旋转。对应的导数就是左雅可比和右雅可比。

具体展开式是这样的：如果你在旋转向量的输入上加上一个小扰动，那么在输出端，这个扰动会先被一个叫右雅可比的矩阵变换一下，然后以右乘的方式作用在原来的旋转上。反过来说，如果是左乘，那就是左雅可比在起作用。左雅可比等于右雅可比的转置，也等于把输入取负之后的右雅可比。

右雅可比的显式形式是：单位阵减去一减余弦除以角度平方乘以 hat，加上角度减正弦除以角度立方乘以 hat 平方。注意和 V 矩阵比一下，符号不一样，阶数一样，所以脑子里记一个就行。

为什么要区分左右？因为这决定了你在优化里怎么定义误差。如果误差定义成“预测位姿右乘一个校正量等于观测位姿”，那用的就是右扰动模型；如果定义成“校正量左乘预测位姿”，那就是左扰动。两种都能用，但是雅可比的形式不同，而且当误差比较大时，两种模型的线性化精度也不一样。

我自己的习惯是：世界坐标系下的目标和约束用左扰动，体坐标系下的传感器读数和控制量用右扰动。理由是右扰动相当于“在物体自己的参考系里微调”，跟传感器和电机的工作方式对得上；左扰动相当于“从外面推它一把”，跟全局目标函数的写法对得上。这个习惯不是硬规定，但保持一致性比选哪个更重要——同一个项目里左右混用，是产生“雅可比符号对了但量级不对”这类灵异 bug 的经典来源。

4. 四元数：不是玄学，是旋转的另一种坐标

4.1 从复数到四元数：三个虚部的乘法规则

四元数的故事起点是复数。复数用一个实部一个虚部表示平面上的旋转，乘法规则是虚部平方等于负一。有人想把这件事推广到三维，发现两个虚部不够用，必须要三个，而且三个虚部互相乘的时候还有额外的规则：i 乘 j 等于 k，j 乘 k 等于 i，k 乘 i 等于 j，反过来乘就是取负。这套乘法规则不是随便定的，它对应着三维空间里三个坐标轴的循环关系。

所以四元数就是一个实数加三个虚数部分，记作 w 加 xi 加 yj 加 zk。写成向量形式，就是 [w, x, y, z]。工程上一般把实部放在第一个，这叫 Hamilton 约定；也有的库把实部放最后，那是另一种约定。这两种约定写出来的乘法公式、转换公式长得几乎一样，但顺序全反，混用会让旋转方向完全反过来而不报任何错。

注意：这是四元数使用中杀伤力最大的一类问题。同一个四元数，按错的约定解释，得到的旋转可能是正确的逆旋转，也可能是完全不相干的另一个旋转。判断依据只有一条：查清楚你手上这个库或者这个数据格式到底用哪种约定，然后全项目统一。我在一个项目里见过视觉模块输出的四元数是实部在前，控制模块按实部在后解析，机器人自己的姿态在自转，而两个模块的单元测试全部通过。

一个单位四元数描述旋转的方式是：实部等于旋转角一半的余弦，三个虚部等于旋转角一半的正弦乘上旋转轴。这个“一半”是四元数最反直觉的地方，也是它双覆盖的来源。转 360 度对应四元数变成负的自己，转 720 度才回到原样。物理上这确实对应着一些真实存在的现象，不过在工程里我们只关心它的副作用：q 和 -q 表示同一个旋转。

4.2 双覆盖带来的实际麻烦：符号一致性怎么保证

q 和 -q 等价这件事，听起来只是个数学注脚，实际用起来非常烦人。最典型的场景是姿态跟踪的日志：你把每一帧的姿态存成四元数，回放的时候发现某些帧之间会突然出现一个巨大的“翻转”，看起来像是姿态瞬间跳变，其实只是符号换了。

解决办法很朴素：维护符号的连续性。每次算出一个新的四元数，跟上一个比较一下内积，如果内积为负，就把新的四元数整体取反。这样得到的序列在数值上是连续的，画图、插值、滤波都不会出问题。同样的事情在四元数插值之前也得做一次，否则 slerp 会绕远路——本来转 10 度就够，它给你转 350 度。

另一个麻烦是归一化。四元数只有单位长度时才表示纯旋转，一旦长度偏离 1，用它旋转向量就会产生缩放。做过大量四元数乘法之后，浮点误差会让长度缓慢偏离，所以每次乘法之后、每次插值之后、每次从网络输出取出四元数之后，都应该归一化一次。这一步成本极低，但不做的后果是误差慢慢累积，几万帧之后姿态明显偏斜。同理，从旋转矩阵转四元数时，如果输入的矩阵本身不正交，得到的四元数也可能不是单位长度，转换完必须再归一化。

4.3 转换关系与代码：矩阵、四元数、轴角之间的来回

这几种表示之间的转换公式在手册上都能查到，我不打算抄一遍，只说几个实际写代码时的关键点。

先看四元数转旋转矩阵。给定单位四元数，矩阵的对角线元素是 1 减去两倍的另两个虚部平方，非对角线元素是两倍的两个虚部乘积加减两倍的实部乘第三个虚部。这里那个“两倍”很容易漏，漏了之后矩阵不再正交，但因为对角线看起来还挺正常，测试时可能发现不了。

反过来，旋转矩阵转四元数，要分四种情况讨论，取决于四个分量里哪个绝对值最大。为什么？因为如果直接用迹反解实部，当旋转角接近 180 度时迹接近负一，开方前的数接近零，精度全丢。所以标准做法是找出四个分量中最大的那个，用它作为分母反解其余三个，这样分母永远远离零。这个分支写法看起来很啰嗦，但它解决了接近 180 度时的数值稳定性问题，值得照着写。

def quat_to_R(q): q = np.asarray(q, float) q = q / np.linalg.norm(q) w, x, y, z = q return np.array([ [1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y)], [2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x)], [2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y)], ]) def R_to_quat(R): t = np.trace(R) if t > 0: s = np.sqrt(t + 1.0) * 2 q = np.array([0.25 * s, (R[2, 1] - R[1, 2]) / s, (R[0, 2] - R[2, 0]) / s, (R[1, 0] - R[0, 1]) / s]) elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 q = np.array([(R[2, 1] - R[1, 2]) / s, 0.25 * s, (R[0, 1] + R[1, 0]) / s, (R[0, 2] + R[2, 0]) / s]) elif R[1, 1] > R[2, 2]: s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 q = np.array([(R[0, 2] - R[2, 0]) / s, (R[0, 1] + R[1, 0]) / s, 0.25 * s, (R[1, 2] + R[2, 1]) / s]) else: s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 q = np.array([(R[1, 0] - R[0, 1]) / s, (R[0, 2] + R[2, 0]) / s, (R[1, 2] + R[2, 1]) / s, 0.25 * s]) return q / np.linalg.norm(q)

写这段代码的时候有个小验证技巧：随便生成一个旋转矩阵，转成四元数、再转回矩阵，两次结果应该一模一样（误差在 1e-12 量级）。如果不一样，多半是某个符号写反了。更进一步，把四元数整体取反再转一次矩阵，结果也应该一样，这能验证双覆盖的处理是否到位。

4.4 slerp 插值：为什么航点姿态不能用线性平均

现在说四元数最实用的一个性质：插值。给定两个姿态，要在它们之间平滑过渡，比如机械臂从抓取位姿移动到放置位姿，中间几十个控制周期该怎么走。

直接用四个分量各自线性插值，看起来是“最短路径”，实际上完全不是。原因还是那个：四元数住在一个三维的单位球面上，直线插值会穿进球体内部，插值出来的结果长度小于 1，归一化之后对应的旋转角速度不均匀，快慢不一。更糟的是当两个姿态相差较大时，线性插值的中间结果可能完全偏离了测地线。

正确做法是 slerp，球面线性插值。它的思路和球面上两点之间走最短弧一样：先算两个四元数的夹角（用内积的反余弦），然后按正弦比例对两个四元数加权求和。角度很小的时候，正弦反余弦的比值又是 0 除 0，需要退化成线性插值。这个退化分支必须写，否则在姿态几乎不变的时候（比如机器人静止时的连续帧）会出 nan。

还有一个细节：slerp 的“最短弧”要求两个四元数在同一半球，也就是内积为正。如果内积为负，先翻转其中一个再插。这一步和前面说的符号连续性是同一件事的两种表现。

提示：做轨迹规划的时候，位置用三次或者五次多项式插值可以做到速度和加速度连续，但姿态如果只用 slerp，角速度是分段常值，角加速度在航点处是冲激。真机上这会激励结构振动。想要更平顺，得用样条或者最小加加速度规划，把姿态当作 SO(3) 上的曲线来处理。这是个进阶话题，但知道有这回事很重要，否则调参时会以为是自己 PID 没调好。

5. 动手：写一个三百行的位姿小工具并逐项验证

5.1 基础往返测试：exp 和 log 必须互为逆运算

理论讲完，最重要的还是写完代码之后怎么证明它是对的。我的习惯是先做三组往返测试，任何一组不过就不往下走。

第一组：随机生成一个旋转向量，做 exp 得到矩阵，再 log 回向量，比较两者。注意 log 的返回值模长一定在 0 到 π 之间，所以输入的旋转向量也得先规范化到同样的范围再比较。旋转角度接近 π 的那批样本要专门挑出来单独测，因为这时候符号可能翻转，得比较矩阵而不是直接比较向量。

第二组：随机生成矩阵，先 log 再 exp，这个方向应该无条件通过，因为矩阵到向量再到矩阵的信息没有损失。误差应该在 1e-12 以内。如果这个测试挂了，问题一定在 exp 或者 log 的公式本身。

第三组：正交性漂移测试。拿一个旋转矩阵，反复乘自己几百次（或者做几百次小的扰动更新），然后检查转置乘自己离单位阵有多远。这个测试能暴露出“更新方式不对”的问题，比如你是不是每次都用矩阵加法在做更新。

def test_roundtrip(n=1000, seed=0): rng = np.random.default_rng(seed) err_log_exp, err_exp_log = 0.0, 0.0 for _ in range(n): phi = rng.normal(size=3) phi = phi / (np.linalg.norm(phi) + 1e-12) * rng.uniform(0, np.pi) R = so3_exp(phi) err_log_exp = max(err_log_exp, np.linalg.norm(so3_log(R) - phi)) R2 = so3_exp(so3_log(R)) err_exp_log = max(err_exp_log, np.linalg.norm(R2 - R)) return err_log_exp, err_exp_log

实测下来，第一组的误差在 1e-15 到 1e-8 之间波动，具体取决于角度落在哪个区间，接近 π 的样本误差最大；第二组稳定在 1e-15。如果第一组出现 1e-2 量级的误差，八成是 log 的边界分支写错了或者根本没有写。

5.2 数值差分对解析雅可比：别相信自己手推的公式

优化里用到雅可比，手推的公式一定要用数值差分对一遍。做法很简单：在某个旋转向量 phi 上，对第 i 个分量加上一个极小的扰动 eps，算出对应的旋转，然后用“扰动后的旋转乘以扰动前的转置”的 log 除以 eps，作为数值导数。把三个方向都算一遍，组成 3×3 矩阵，和解析的右雅可比比较。

理论上的关系是：exp(phi 加 eps e_i) 约等于 exp(phi) 乘以 exp(Jr(phi) eps e_i)。所以数值算出来的那个 3×3 矩阵应该等于 Jr(phi)。我把这个测试写成了单元测试，每次改代码都跑一遍。

def numerical_right_jacobian(phi, eps=1e-7): phi = np.asarray(phi, float) R0 = so3_exp(phi) J = np.zeros((3, 3)) for i in range(3): d = np.zeros(3); d[i] = eps R1 = so3_exp(phi + d) J[:, i] = so3_log(R1 @ R0.T) / eps return J

这个测试特别值得做，因为雅可比的手推公式里符号、系数、阶数三个地方都可能出错，而且出错了优化照样“能跑”——只是收敛慢，或者收敛到错的地方。我遇到过一次收敛慢到以为是自己学习率调大了，查了整整两天，最后发现是右雅可比里那个正弦项的系数写成了两次方而不是三次方。

5.3 落地案例：把位姿更新塞进优化和滤波里

举一个实际用得最多的场景：点云配准。给定两组点，已知大致对应关系，要解出它们之间的刚体变换。这本质上是一个最小二乘问题，每一步要解一个六元的线性方程。

问题的形式是把当前的位姿估计乘以一个小的增量，让变换后的点云到目标点云的距离最小。把误差对增量的导数写出来，就是一个 6×6 的线性系统。这里的关键是：增量到底加在哪一侧，决定了雅可比的形式。如果我们把变换写成“当前旋转矩阵乘 exp(增量)”，那就是右扰动，每个点对应的雅可比可以写得很干净，3×6 的矩阵，前三维是旋转部分，后三维是平移部分。

还有一种情况是位姿图优化，节点是各时刻的位姿，边是相对位姿的观测。误差定义为“观测的相对位姿的逆，乘以两个估计位姿的相对位姿，再取 log”。这个误差对两个节点的雅可比也有解析形式，用的是右雅可比和左雅可比的组合。

滤波这边的用法稍微不同。误差状态卡尔曼滤波里，状态里的姿态不是一个矩阵，而是一个小旋转向量。预测的时候用角速度积分更新矩阵，更新的时候算出卡尔曼增益，把校正量以右乘的方式作用到矩阵上，作用完再把这个校正量清零。这套流程的关键是不能让矩阵“累加”误差，每次都要通过 exp 把增量变成旋转再乘上去，这样矩阵始终严格正交。

注意：如果你在滤波里真的把协方差里的姿态误差当欧拉角处理，短时间可能看不出问题，但协方差矩阵会慢慢失去正定性，最后滤波器发散。判断方法很简单：打印协方差的最小特征值，如果出现负数，就是这个原因。

6. 踩坑记录与速查表

6.1 五个我反复见到的错误

第一个错误，把旋转矩阵当普通矩阵做加法。优化里都是 R 加上一个增量，这会导致正交性破坏。哪怕只是加上一个极小的矩阵，累积几百次之后误差也会显现。正确做法是本期矩阵乘以 exp(增量)。

第二个错误，欧拉角的顺序搞混。绕 XYZ 转和绕 ZYX 转是完全不同的两个旋转。RGB 和 ZYX 这种简写尤其容易出错，因为“内旋”和“外旋”的差别藏在里面。我的建议是永远不要用简写，代码里把三个轴的顺序写全，配置里也写全。

第三个错误，四元数的约定前后不一致。前面说过了，这里不再重复，只想强调一点：接手别人的代码，第一件事就是找四元数的定义和乘法实现，对照一下再说别的。

第四个错误，忘记处理接近 180 度的旋转。这类样本在任何随机分布里只占很小一部分，但真实系统里经常出现——机器人调头、相机倒扣、物体翻转，都是这个角度。测试集里必须专门构造这类样本。

第五个错误，跨模块坐标系约定没写清楚。视觉给的是相机系的位姿，规划要的是世界系的位姿，中间要经过外参，外参的方向搞反了，机械臂就会朝着镜像方向去抓。这个错误的可怕之处在于结果看起来“差不多对”，只是偏了，很容易被当成标定精度问题。我现在的规矩是，所有接口的注释里必须写清楚变换的方向，写成“把 A 系下的点变换到 B 系”这种句子，不许只写一个矩阵名。

6.2 问题速查表

这张表里的每一条我都在真实项目里碰到过，有的不止一次。它有价值的点在于“现象到原因”的映射，多数时候问题的表象和根因差得很远，凭直觉找很容易走冤枉路。

6.3 我个人的几条经验

第一条，先写测试再写优化。我现在的习惯是，位姿工具库的每个函数写完立刻配一个往返测试或者数值验证，跑通了再接到上层去。这个习惯一次都没让我后悔过。反过来，先接上层再调试底层，最后你会同时怀疑三个模块。

第二条，能在矩阵上算就别在四元数上算。复合变换、作用到点上、求逆，这些操作矩阵形式最直观，出错概率最低。四元数的优势在于存储紧凑、插值方便，把它用在保存状态和网络输出上，其余转换到矩阵去算。有人喜欢全程用四元数追求效率，结果一维护起来全是符号错误。

第三条，坐标系命名要啰嗦。w 和 c 这种单字母的坐标系名，一个月后自己都看不懂。我现在都写成 world、camera_base、end_effector 这种带下划线的全名，虽然变量名变长了，但接手的人（包括未来的自己）能看懂。

第四条，存数据和传数据用旋转矩阵或者四元数，别用欧拉角。哪怕日志里为了人眼好看要打印欧拉角，内部存储也一定是别的形式。欧拉角是给人看的，不是给机器算的。

最后分享一个我最近才养成的小习惯：写位姿相关的函数时，函数签名里一定带上坐标系的名字，比如transform_point(point_in_a, T_ba)这种，读函数名就知道输入是哪个系的点、变换矩阵的方向是什么。看起来是小事，但自从这么写之后，坐标系的低级错误少了大概八成。

这套东西学起来确实有点门槛，第一遍看不懂很正常，我自己也是推导了三四遍、写了两个小库之后才真正建立起直觉。但一旦打通，你会发现后面所有涉及姿态的东西都变得清晰——不管是看论文里的公式，还是调别人的开源代码，或者给自己的模型设计输出头。