做机器人、做具身智能几乎每一行代码都会碰到旋转。真机上姿态跳变五度抓取就掉件手眼标定误差大了抓杯子变成撞杯子优化器跑着跑着不收敛追到最后往往是位姿的更新量没加对地方。这些问题看着五花八门根子经常是同一件事李群、李代数、四元数这套描述旋转与刚体变换的语言没吃透。我见过太多人卡在这一步看公式能看懂一到写代码就懵不知道为什么要在旋转矩阵上套一层 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人眼可读参数少万向锁不连续插值会拧配置、日志、UI 展示旋转向量轴角3有明确物理意义就是转速方向乘角度模长接近 2π 时符号不稳误差表示、调试打印四元数4紧凑、无奇异、插值稳定、乘法快有双覆盖需要归一化状态估计、网络输出、消息传输旋转矩阵9直接乘向量复合方便无奇异冗余优化后会失去正交性内部计算、渲染、链式复合连续 6D 表示6神经网络友好无奇异无跳变需要重新正交化视觉姿态回归、策略网络输出这张表里最值得说一句的是最后一行。做具身智能的人迟早会碰到“让神经网络直接吐一个姿态”的需求。如果网络输出三个欧拉角训练会很难收敛因为同样的姿态在参数空间里可能对应完全不同的数值损失函数变成了锯齿状。如果输出四元数又得加一个归一化约束还得处理正负号的歧义。工程上比较管用的做法是让网络输出一个 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, dtypefloat) 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, dtypefloat) 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 等于 kj 乘 k 等于 ik 乘 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(n1000, seed0): rng np.random.default_rng(seed) err_log_exp, err_exp_log 0.0, 0.0 for _ in range(n): phi rng.normal(size3) 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, eps1e-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 问题速查表现象最可能的原因排查方向姿态在两个角度之间突然跳变欧拉角跨界或四元数符号翻转打印四元数四个分量检查内积符号是否连续优化器收敛很慢或来回震荡雅可比符号或系数错误用数值差分对一遍解析雅可比平移误差随旋转角增大而增大SE(3) 指数映射漏了 V 矩阵用一个 90 度旋转加单位平移做单点验证相机位姿估计在小角度准确、大角度跑偏对数映射没有处理接近 π 的分支构造 179 度的旋转矩阵单独测试滤波跑几分钟后姿态漂移明显协方差失去正定或矩阵未正交化检查最小特征值检查每次更新后是否重新正交化双臂协同的时候末端相对位姿不对链式复合的顺序或逆矩阵写错用单位变换和纯平移做端到端验证神经网络输出的姿态抖动大用了不连续的旋转表示换成连续 6D 表示并加正交化步骤插值轨迹中间姿态奇怪用线性插值代替 slerp检查插值函数确认内积为负时做了翻转这张表里的每一条我都在真实项目里碰到过有的不止一次。它有价值的点在于“现象到原因”的映射多数时候问题的表象和根因差得很远凭直觉找很容易走冤枉路。6.3 我个人的几条经验第一条先写测试再写优化。我现在的习惯是位姿工具库的每个函数写完立刻配一个往返测试或者数值验证跑通了再接到上层去。这个习惯一次都没让我后悔过。反过来先接上层再调试底层最后你会同时怀疑三个模块。第二条能在矩阵上算就别在四元数上算。复合变换、作用到点上、求逆这些操作矩阵形式最直观出错概率最低。四元数的优势在于存储紧凑、插值方便把它用在保存状态和网络输出上其余转换到矩阵去算。有人喜欢全程用四元数追求效率结果一维护起来全是符号错误。第三条坐标系命名要啰嗦。w 和 c 这种单字母的坐标系名一个月后自己都看不懂。我现在都写成 world、camera_base、end_effector 这种带下划线的全名虽然变量名变长了但接手的人包括未来的自己能看懂。第四条存数据和传数据用旋转矩阵或者四元数别用欧拉角。哪怕日志里为了人眼好看要打印欧拉角内部存储也一定是别的形式。欧拉角是给人看的不是给机器算的。最后分享一个我最近才养成的小习惯写位姿相关的函数时函数签名里一定带上坐标系的名字比如transform_point(point_in_a, T_ba)这种读函数名就知道输入是哪个系的点、变换矩阵的方向是什么。看起来是小事但自从这么写之后坐标系的低级错误少了大概八成。这套东西学起来确实有点门槛第一遍看不懂很正常我自己也是推导了三四遍、写了两个小库之后才真正建立起直觉。但一旦打通你会发现后面所有涉及姿态的东西都变得清晰——不管是看论文里的公式还是调别人的开源代码或者给自己的模型设计输出头。