做姿态解算的人大概都遇到过这种时刻把一段在 MATLAB 里跑得好好的姿态控制代码搬到 C 上仿真里静止时一切正常一给角速度指令姿态就往反方向跑或者把四元数塞进 ROS 消息里rviz 里的模型朝向和实际机身差了 180 度日志里的 (w,x,y,z) 看着也没错。这类问题的根子往往不在 PID 参数、不在传感器标定、也不在滤波器增益而在一个非常底层的地方——你手上这组四个数到底是用 Hamilton notation 还是 JPL notation 解释的。四元数旋转表达Hamilton notation JPL notation这个话题听起来像是数学系的事实际上它是每个做姿态解算、机器人状态估计、飞行控制、SLAM 后端、甚至游戏引擎的人迟早要撞上的一堵墙。原因很简单四元数的标准从来就不唯一。航空航天圈用 JPL 记法写了几十年机器人和图形学圈用 Hamilton 记法写了几十年两边的论文都自称四元数公式却长得不一样符号还常常相反。你只要在两边之间搬一次代码就有机会体验四个数都对旋转就是反的这种抓狂时刻。这篇东西想解决的问题很具体把两套记法的差别彻底拆开讲清楚讲明白它们到底差在哪儿、为什么差、差多少再给出一套能直接抄的验证流程和一份主流库的实测对照表。不管你是刚开始写四元数乘法的新手还是已经能背下旋转矩阵公式但每次移植都要重新推一遍符号的老手下面这些内容应该都能帮你省掉几个通宵。1. 被混为一谈的四件事标注约定其实是一个四维参数空间入门时看过的绝大多数资料都会用一句话概括差别Hamilton 是 [w,x,y,z]JPL 是 [x,y,z,w]。这句话是错的或者说最多算对了一半。它把一件很复杂的事情压缩成了一个数组下标问题导致后面无论怎么推导都对不上。真实情况是描述这组四元数怎么解释需要四个互相独立的维度而 Hamilton 和 JPL 只是这四个维度上的两个常见组合点。先把这四个维度摆出来后面所有内容都围绕它们展开。维度常见取值影响什么代数基元ijk-1 / ijk1旋转方向、叉乘项符号存储顺序scalar-first / scalar-last数组下标、序列化、内存布局旋转语义转向量 / 转坐标系旋转矩阵是 R 还是 R 的转置乘法顺序左乘优先 / 右乘优先复合旋转的结果、运动学更新式真正危险的地方在于这四个维度只要有一个对不上最终算出来的旋转就会错而且错的形态各不相同。有时候是整体反号有时候是中间某个轴反号有时候是复合顺序颠倒——仿真里可能完全看不出来只有上了真机、给了动态指令才会暴露。下面逐个说。1.1 代数基元ijk 到底等于 -1 还是 1Hamilton 在 1843 年定义四元数时用的规则是 i² j² k² ijk -1。由 ijk -1 出发配合 k² -1可以推出来 ij k、ji -k、jk i、kj -i、ki j、ik -j。这套规则是反交换的也就是交换两个虚部单位会变号是标准的 Hamilton 记法。JPL 记法这个名字来自加州理工的喷气推进实验室航天器姿态估计的经典文献基本都用它把 ijk 定义成 1其余平方关系 i² j² k² -1 不变。这一个符号的变化会让所有交叉项反号ij -k、ji k以此类推。这不是一个无所谓的癖好。因为四元数乘法里所有的叉乘项都来自虚部之间的乘积交叉项一旦反号乘法公式、旋转公式、运动学方程里的叉乘项会集体翻符号。你在论文里看到 q ⊗ ω 和 ω ⊗ q 换了个位置或者旋转矩阵里那个 2q₄[ρ]× 变成了 -2q₄[ρ]×八成就是这里来的。1.2 存储顺序scalar-first 与 scalar-last 是正交维度存储顺序讲的是同一组四个实数在数组里怎么排。scalar-first 是 [w, x, y, z]标量在前scalar-last 是 [x, y, z, w]标量在后。这里必须强调一遍存储顺序和代数约定是两件完全独立的事。你完全可以用 Hamilton 的代数规则配上 scalar-last 的存储ROS 就是这么干的。你也可以用 JPL 的代数配上 scalar-first。把JPL 就是 [x,y,z,w]当成定义会在遇到Hamilton scalar-last的场合彻底懵掉——因为那时候你按前面的说法判定它是 JPL但实际上它和 JPL 的旋转方向是相反的。实际工程里存储顺序带来的 bug 反而比代数约定更多因为它太容易被忽略。你从 MATLAB 导出一个 N×4 的矩阵列顺序是 [w x y z]直接喂给 scipy 的Rotation.from_quat它默认按 [x y z w] 解析于是 w 被当成 x。结果就是静止时姿态看起来还行因为小角度下 w≈1x 很小一旦开始旋转就彻底乱套。这种 bug 最难查因为它在零位附近是正确的。1.3 旋转语义转向量还是转坐标系第三个维度是这个四元数描述的旋转到底是在把向量转过去还是在把坐标系转过来。用数学话说前者对应主动旋转active rotation你有一个向量 v经过旋转 R 得到 v R v后者对应被动旋转或者坐标变换同一个物理向量在不同坐标系下有不同的坐标v_body C v_ref。这两者互为逆R 和 C 之间差一个转置。航天领域的习惯是描述坐标变换矩阵 C四元数的方向语义就是从参考系转到本体系机器人和图形学领域的习惯是描述点或者向量的旋转。这一点在公式上表现为旋转矩阵的叉乘项符号不同和 1.1 节说的基元符号是叠加在一起的所以光看一个公式很难判断到底是哪个原因导致的差异。一个实用的判别办法是问自己这组四元数乘上 (1,0,0) 之后得到的向量是原来的 x 轴转到哪儿去了还是原来的 x 轴在新坐标系里的坐标。前者是主动后者是被动。写代码验证的时候这两种测试的期望结果正好相反。1.4 乘法顺序q1 ⊗ q2 先作用谁第四个维度是复合旋转的顺序。Hamilton 记法下如果 q1 和 q2 都表示主动旋转那么 R(q1 ⊗ q2) R(q1) R(q2)也就是先用 q2 作用再用 q1 作用——和矩阵乘法的习惯一致离向量近的先作用。但如果你换成先转参考系再转本体系的语义或者换一套基元符号这个顺序就会反过来写成 R(q1 ⊗ q2) R(q2) R(q1)。很多控制律的推导里会写q 的期望值等于 q_desired 乘以 q_current 的逆这个乘法的左右顺序在不同文献里是不一样的不能照着抄。判断方式很简单拿一个明确的例子跑一遍。绕 x 轴转 90 度再绕 y 轴转 90 度和反过来做结果肯定不同三维旋转不可交换。把两种顺序都算出来和你的库对一下立刻知道它站在哪边。2. 从虚部乘法表推出 JPL 与 Hamilton 的共轭关系理解了上面四个维度接下来的问题是JPL 和 Hamilton 之间到底是怎么换算的市面上流传的规则有好几种说法有的说向量部分取负有的说取共轭有的说差一个转置这些说法其实是一回事只是描述的层面不同。这一章从最基本的乘法表推一遍把关系彻底焊死。推导过程不长但它能让后面所有的转换代码都不再靠猜。2.1 Hamilton 的乘法表与 (w, v) 形式Hamilton 四元数 q w xi yj zk习惯上拆成标量部分 w 和向量部分 v (x, y, z)。两个四元数相乘q1 ⊗ q2 (w1·w2 - v1·v2, w1·v2 w2·v1 v1×v2)这个公式值得背下来它是所有四元数代码的核心。手工验证一下它的自洽性如果 q1 q2 iw0, v(1,0,0)那么结果是 (0 - 1, 0 0 0) (-1, 0)对应 -1符合 i² -1。如果 q1 i、q2 j结果是 (0, 0 0 (1,0,0)×(0,1,0)) (0, (0,0,1)) k符合 ij k。旋转向量的公式是 v q ⊗ v̄ ⊗ q*其中 v̄ 是把三维向量补成标量为 0 的纯四元数q* 是共轭标量不变、向量取负。展开之后得到的就是那个大家熟悉的旋转矩阵R_H (w² - |v|²) I₃ 2 v vᵀ 2 w [v]×其中 [v]× 是反对称矩阵。注意这里的 2w[v]×符号很关键。2.2 把 i、j、k 全部取负JPL 的基元现在做一件看起来很奇怪的事保留 Hamilton 的代数规则不动但假设 JPL 用的虚部单位是 i -i、j -j、k -k。验证一下这个新基是不是满足 ijk 1ij (-i)(-j) ij k -k也就是 ij -k和 Hamilton 的 ij k 反号。再看 ijkijk (-i)(-j)(-k) -ijk -(-1) 1平方关系呢i² (-i)² i² -1保持不变。所以JPL 的 ijk 1和JPL 的基元是 Hamilton 基元取负是等价的两种说法。这就是符号差异的源头JPL 的每一个虚部单位都比 Hamilton 多带一个负号。把一个用 JPL 基元写出来的四元数 q q₄ q₁i q₂j q₃k 换回 Hamilton 基q q₄ - q₁i - q₂j - q₃k对比 Hamilton 的共轭定义 q* w - xi - yj - zk可以清楚地看到同一组四个数 (q₁, q₂, q₃, q₄)按 JPL 约定读出的四元数等于按 Hamilton 约定读出之后取共轭。写成公式就是q_H* q_JPL这里要特别注意这个等式的读法。它不是同一个旋转在两边表示互为共轭而是同一组数字在两边解释出来的旋转互为逆。这两个说法的区别在工程上很重要因为很多人会在错误的层面上做转换。2.3 q_JPL q_H* 及其几何含义共轭在几何上意味着什么四元数的逆是 q⁻¹ q*/|q|²单位四元数下 q⁻¹ q*。而旋转的逆就是反向旋转。所以 q_JPL q_H* 等价于说JPL 约定下这组数描述的旋转是 Hamilton 约定下同一组数描述的旋转的逆。把这个结论翻译成可操作的代码规则你有一段 Hamilton 记法的四元数(w, x, y, z)想要得到同一个物理旋转在 JPL 记法下的四个数对向量部分取负# Hamilton [w, x, y, z] - JPL [q1, q2, q3, q4] w, x, y, z q_h q_jpl (-x, -y, -z, w) # 即 [q1, q2, q3, q4] [-x, -y, -z, w]反过来一样因为取两次负就回来了# JPL [q1, q2, q3, q4] - Hamilton [w, x, y, z] q1, q2, q3, q4 q_jpl q_h (q4, -q1, -q2, -q3)注意看 Hamilton 的 scalar-last 写法 [x, y, z, w] 和 JPL 的 [q1, q2, q3, q4] [-x, -y, -z, w] 放在一起只差一个符号。这就是为什么只讲存储顺序不讲代数约定是有害的它恰好漏掉了那唯一重要的一个负号。2.4 一个数值算例绕 z 轴转 90 度在两套约定下的差别抽象说了半天来个能动手验算的例子。物理旋转绕 z 轴转 90 度右手定则从 z 轴正向往原点看是逆时针。Hamilton 记法下w cos(45°) 0.70710678 v sin(45°) · (0, 0, 1) (0, 0, 0.70710678)这组数作用到向量 (1, 0, 0) 上结果应该是 (0, 1, 0)——x 轴转到了 y 轴。手工验算一遍旋转矩阵w² - |v|² 0.5 - 0.5 0 2vvᵀ [[0,0,0],[0,0,0],[0,0,1]] 2w[v]× 2(0.7071)·[[0,-0.7071,0],[0.7071,0,0],[0,0,0]] [[0,-1,0],[1,0,0],[0,0,0]] R [[0,-1,0], [1, 0,0], [0, 0,1]]R·(1,0,0)ᵀ (0,1,0)ᵀ符合预期。现在把这组四个数按 JPL 约定读(q₁,q₂,q₃,q₄) (0, 0, 0.7071, 0.7071)。根据 2.3 节的结论它描述的旋转是 Hamilton 那个的逆也就是绕 z 轴转 -90 度作用到 (1,0,0) 上应该得到 (0,-1,0)。同一个数组一边给 (0,1,0)一边给 (0,-1,0)。这就是四个数都对、旋转就是反的的完整解释。把这个测试写进你的单元测试任何库站哪边立刻见分晓。3. 旋转矩阵与姿态运动学符号差异如何传导到微分方程静态的转换规则搞清楚了接下来要面对真正麻烦的部分姿态是会随时间演化的。四元数的微分方程、角速度的坐标系、误差状态的雅可比这些东西在 Hamilton 和 JPL 下的写法不一样而且不一样的形态比静态情况更隐蔽。滤波器和控制器里绝大多数的移植后发散问题都出在这一层。3.1 两套旋转矩阵公式的叉乘项符号把两套约定下的旋转矩阵并排写出来差异一目了然。Hamilton标量 w向量 vR_H (w² - |v|²) I₃ 2 v vᵀ 2 w [v]×JPL标量 q₄向量 ρC_JPL (q₄² - |ρ|²) I₃ 2 ρ ρᵀ - 2 q₄ [ρ]×除了符号两式完全一样。前面对角项和对称项相同只有反对称项含叉乘的那个反号。反对称项恰好是负责旋转方向的部分所以这个符号决定了结果。需要提醒的是我见过不少文献把 JPL 的矩阵写成 2q₄[ρ]×原因是那篇文献定义的 C 是本体系到参考系而不是参考系到本体系。这不是谁写错了而是 1.3 节说的旋转语义这一个维度的差异。所以看论文的时候一定要先找到它对 C(q) 或者 R(q) 的完整定义包括从哪个系到哪个系这句话再决定用哪个符号。只看公式的符号去猜十有八九要返工。3.2 角速度在体系与参考系下的两种更新式姿态运动学方程是移植时最容易出错的地方。Hamilton 约定下单位四元数的微分方程有两种常见形式区别只在于角速度在哪表达角速度在本体系 (body frame): q̇ 0.5 · q ⊗ ω̄_b 角速度在参考系 (world frame): q̇ 0.5 · ω̄_n ⊗ q其中 ω̄ 是把三维角速度补成标量为 0 的纯四元数q 是本体系到参考系的旋转。为什么会是这样用一个小角度做直觉验证。如果当前姿态接近单位q ≈ 1两种形式都退化成 q̇ ≈ 0.5 · ω̄也就是旋转向量的变化率等于角速度这符合物理直觉——小角度下两个坐标系几乎重合角速度在哪个系里表达都差不多。差别出现在大角度、快速旋转的场合那时两种形式的积分结果会分道扬镳。工程上判断该用哪个的原则是看你的角速度传感器输出在哪个坐标系。MEMS 陀螺仪IMU输出的是本体系角速度所以用右乘形式 q̇ 0.5 q ⊗ ω̄_b。如果你先通过姿态把角速度转到世界系那就用左乘形式。两者在很多开源代码里被随意混用短时间看不出来跑长了就会出现姿态漂移方向不一致。JPL 约定下这两个式子会怎么变对 q_H 的方程两边取共轭利用共轭的反自同构性质 (a ⊗ b)* b* ⊗ a*得到 q̇_JPL 0.5 · (-ω̄_b) ⊗ q_JPL。也就是说在 JPL 记法下本体系角速度的更新式变成左乘而且 ω̄ 前面多一个负号。写成矩阵形式就是教科书里那个 4×4 的 Ω(ω) 矩阵——你会发现它和 Hamilton 版本的 Ω 矩阵差了一个块符号。同一篇文献里 Ω 矩阵写成什么样取决于它怎么定义 C(q) 的方向、ω 在哪个坐标系以及 ijk 的符号。这三件事叠在一起光看一个矩阵根本推不出全部信息。所以我的做法是移植运动学方程时绝不背公式只在目标平台写一段固定输入的积分测试用数值结果反查符号。3.3 姿态误差与雅可比VIO/SLAM 里最容易翻车的地方如果你在做视觉惯性里程计、SLAM 后端或者任何基于优化的姿态估计还有一个更隐蔽的坑误差状态的参数化。优化里不能直接对四元数求导因为它有单位模长约束通用做法是引入一个局部的三维扰动 δθq_true q_est ⊗ δqδq ≈ [1, 0.5 δθ]Hamilton右乘扰动。残差对 δθ 的雅可比里会出现 [R v]× 和 [v]× 这类项它们之间差一个旋转矩阵。往左乘还是往右乘扰动会得到互为转置或者差一个 R 的两套雅可比。如果你的雅可比是从 JPL 记法的论文里抄来的而状态量是 ROS 里的 Hamilton 四元数两件事同时错有时候会负负得正看起来能收敛但收敛精度和协方差完全不对更多时候是优化直接发散或者收敛到一个镜像解。判断办法在优化前先做一个一致性测试用同一组观测跑一次然后人为把初始四元数取共轭重新跑。如果两次结果收敛到的旋转互为逆说明雅可比符号有问题如果收敛到同一个姿态哪怕四元数符号不同说明方向语义是一致的。3.4 双覆盖与符号跳变q 与 -q 的工程处理还有一个和约定无关但经常被混淆的问题单位四元数的双覆盖。q 和 -q 描述完全相同的旋转这不是 bug是四元数的固有性质单位四元数到旋转群是二对一的映射。麻烦出在两个地方。第一是插值。slerp 的两个端点如果点积为负走的是长弧姿态会绕一大圈。解决办法是在插值前检查点积为负就把其中一个取负。Eigen 和 tf2 的 slerp 内部会处理这件事但自己手写的 slerp 经常忘。第二是滤波和优化中的符号跳变。两个相邻时刻的四元数如果符号突然翻转数值上从 q 跳到 -q协方差递推或者数值雅可比会出现巨大的跳变表现为滤波器输出抖动或者优化步长异常。处理办法是在每一帧迭代前做半球对齐如果和上一帧的点积小于零就整体取负。这里要区分清楚符号跳变和 Hamilton/JPL 的差异是两码事。前者是四元数本身的数学性质后者是记法约定。我见过有人把符号跳变误判成约定搞反了然后去翻所有公式白折腾一天。4. 主流库和框架站在哪一边一份实测对照表理论讲完落到实际写代码。这一章把日常会用到的库和框架的约定列清楚重点标出那些文档里没写、但一定会踩的细节。表格里的结论都是我实际跑测试验证过的不是从文档里抄的一句话总结。库 / 框架代数约定存储顺序构造 / 接口顺序备注EigenHamiltonscalar-lastQuaterniond(w,x,y,z)coeffs()返回 (x,y,z,w)ROSgeometry_msgsHamiltonscalar-last字段 x, y, z, w消息定义里就是这个顺序tf2Hamiltonscalar-lastQuaternion(x,y,z,w)内部是 btQuaternionscipyHamiltonscalar-lastfrom_quat([x,y,z,w])旧版本无 scalar_first 参数MATLAB 四元数类Hamiltonscalar-firstquaternion(a,b,c,d)a 是实部UnityHamiltonscalar-lastQuaternion(x,y,z,w)左手坐标系UnrealHamiltonscalar-lastFQuat(X,Y,Z,W)左手坐标系MuJoCoHamiltonscalar-firstqpos 里 (w,x,y,z)这条最容易忽略航天 GNC 教材多为 JPLscalar-last[q1,q2,q3,q4]方向定义为 ref 到 body4.1 Eigen构造函数参数顺序与内存布局不一致Eigen 是 C 姿态计算的主力它的坑也最经典。写Eigen::Quaterniond q(w, x, y, z)的时候参数顺序是标量在前但q.coeffs()返回的是一个四维向量顺序是 (x, y, z, w)。同一个对象两个接口的顺序不同这在代码里非常容易看漏。更隐蔽的是从向量构造。Eigen 有Quaternion(const Vector4)这个重载它按 coeffs 顺序解析输入也就是 (x, y, z, w)。所以下面这段代码是错的// 错误写法以为 Vector4d 是 (w,x,y,z) Eigen::Vector4d v(w, x, y, z); Eigen::Quaterniond q(v); // 实际被解析成 (xv[0], yv[1], zv[2], wv[3])正确做法要么用四参数构造要么明确按 coeffs 顺序排列Eigen::Quaterniond q1(w, x, y, z); // 推荐 Eigen::Vector4d v(x, y, z, w); Eigen::Quaterniond q2(v); // 顺序必须对另一个细节是内存布局。如果你要把 Eigen 四元数直接 memcpy 到一个double[4]或者网络包里顺序是 (x, y, z, w)。我一般在这种场合显式写q.coeffs().data()并在旁边加注释说明顺序避免半年后被自己坑。还有一点Eigen 没有提供四元数乘三维向量的运算符重载q * v编译不过。常见做法是q.toRotationMatrix() * v比较慢或者q._transformVector(v)更快但属于内部接口。我个人偏好后者速度差在小规模计算里不明显但在紧循环里能省不少。4.2 ROS / TF2 / URDFHamilton 代数配 scalar-last 存储ROS 生态是Hamilton 加 scalar-last的典型geometry_msgs::Quaternion的字段定义顺序是 x、y、z、w但旋转语义遵循 Hamilton。这个组合本身就说明存储顺序不等于代数约定。从 Eigen 转到 ROS 消息的标准写法geometry_msgs::Quaternion msg; msg.x q.x(); msg.y q.y(); msg.z q.z(); msg.w q.w();不要偷懒写memcpy(msg, q.coeffs().data(), 4*sizeof(double))因为 coeffs 是 double消息是 float64在大多数平台上刚好相同但这依赖实现细节而且这种写法在换平台或者换消息类型时会静默出错。tf2 的构造是tf2::Quaternion(x, y, z, w)内部转发给 btQuaternion。tf2 还有setRPY(roll, pitch, yaw)用的是固定轴 XYZ 约定也就是 R Rz(yaw) · Ry(pitch) · Rx(roll)。URDF 里origin rpy...用的是同一套顺序这是 ROS 内部统一好的不用担心。但有一个语义问题必须记住tf 的变换表达的是从子坐标系到父坐标系的转换lookupTransform(map, base_link, ...)返回的是 base_link 在 map 下的位姿。这个语义和旋转向量的语义正好对偶写控制器的时候要看清方向。很多人在这里翻车把位姿的旋转部分当成base_link 到 map 的旋转矩阵然后发现控制律的符号反了。4.3 MATLAB、Python 生态与物理引擎的差异MATLAB 的四元数类是 scalar-firstquaternion(a, b, c, d)里 a 是实部。quat2rotm这类函数接受的也是 [w x y z]单个四元数或者 N×4 的矩阵。这一点和 ROS 完全相反所以从 MATLAB 往 ROS 搬数据时列顺序十有八九要换。scipy 的Rotation类默认是 scalar-lastRotation.from_quat([x, y, z, w])as_quat()也返回 (x, y, z, w)。新版本加了scalar_first关键字参数但在老代码里不会有人写所以默认行为还是 scalar-last。用 scipy 做离线验证再喂给 ROS 是个好习惯两边顺序一致用 MATLAB 做离线验证就要多一次转置。MuJoCo 是我见过最容易被忽略的一个。它的广义坐标 qpos 对自由关节用的是 7 个自由度3 个位置加 4 个四元数而这 4 个数的顺序是 (w, x, y, z)scalar-first。这和 ROS 完全相反。如果你在做强化学习或者仿真到实机的迁移从 MuJoCo 里读姿态直接发给 ROS姿态一定会歪。我在这个坑上浪费过整整一个下午最后靠打印 (1,0,0) 轴在两边的作用结果才定位到。Unity 和 Unreal 都是 scalar-last 的 Hamilton但两者都是左手坐标系。左手系本身不改变四元数的定义但会让旋转方向的直观判断反过来。从右手系的物理仿真往引擎里搬数据时除了四元数顺序还要额外处理坐标系手性这时候光调四元数是不够的。4.4 一张对照表解决不了的问题上面这张表能覆盖 90% 的场景但剩下 10% 才是真正消耗时间的。原因是同一个库里不同函数的约定也可能不一致。比如某些数值优化库的四元数参数块用 scalar-last但它内部用于计算残差的函数可能接受 scalar-first某些老版本的 ROS 驱动包把 IMU 的四元数按 [w,x,y,z] 发出来虽然sensor_msgs::Imu的标准字段是 x,y,z,w——它们直接往结构体里 memcpy坑了一大批下游用户。所以我的建议是不要相信任何表格包括这一张。表格用来快速缩小怀疑范围但最终判定必须靠数值测试。下一章给出三个能覆盖绝大多数情况的测试用例。5. 三个测试用例锁死约定可复现的验证流程前面说的都是应该这么想这一章给的是具体怎么做。判断一组四元数或者一个库用的是哪套约定最快的方法不是读文档而是跑测试。下面三个用例代码量都很小覆盖了静态旋转、复合顺序、往返一致性三个层面跑一遍基本能锁定所有约定。5.1 单轴 90 度旋转的符号测试第一个用例最简单也最有区分度绕 z 轴转 90 度作用到 x 轴上。import numpy as np def quat_to_R(w, x, y, z): Hamilton 约定scalar-first 输入返回主动旋转矩阵 n w*w x*x y*y z*z if n 1e-12: return np.eye(3) s 2.0 / n wx, wy, wz s*w*x, s*w*y, s*w*z xx, xy, xz s*x*x, s*x*y, s*x*z yy, yz, zz s*y*y, s*y*z, s*z*z return np.array([ [1-(yyzz), xy-wz, xzwy], [xywz, 1-(xxzz), yz-wx], [xz-wy, yzwx, 1-(xxyy)] ]) # 绕 z 轴 90 度 w np.cos(np.pi/4); z np.sin(np.pi/4) R quat_to_R(w, 0, 0, z) print(R np.array([1.0, 0, 0])) # 期望 (0, 1, 0)如果某个库给出的结果是 (0, -1, 0)那它要么是 JPL 约定要么它的旋转语义是转坐标系。这两者的区别要靠第二个用例进一步区分。这个测试有个小技巧用 90 度而不是 180 度。180 度的时候正负号会互相抵消旋转矩阵里很多项变成 ±1 或者 0区分度反而低。90 度的时候正负号同时出现在非对角项上方向对不对一眼就能看出来。5.2 复合旋转的顺序测试第二个用例解决乘法顺序这个维度分别做绕 x 转 90 度再绕 y 转 90 度和反过来看哪个结果和库的输出匹配。import numpy as np def qmul(a, b): Hamilton 乘法输入输出均为 (w,x,y,z) w1,x1,y1,z1 a w2,x2,y2,z2 b return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2 ]) h np.sqrt(0.5) qx np.array([h, h, 0, 0]) # 绕 x 轴 90 度 qy np.array([h, 0, h, 0]) # 绕 y 轴 90 度 q_xy qmul(qx, qy) # 先 qy 后 qxHamilton 约定下右 operand 先作用 q_yx qmul(qy, qx) # 先 qx 后 qy print(qx*qy , q_xy) print(qy*qx , q_yx)把这两个结果分别喂给待测库的旋转向量接口看哪个和先转 x 再转 y的物理预期一致。Hamilton 约定下R(qx ⊗ qy) R(qx) R(qy)意味着右操作数先作用。有些库尤其是从航天代码演化来的会反过来那时候你的 qx、qy 就要调换位置。这个测试还能顺手验证旋转语义如果单轴测试给的是逆旋转复合测试又能对上顺序那基本可以确认是 JPL 约定或者是转坐标系语义。5.3 往返转换与位姿闭合测试第三个用例是前两个的组合用来验证转换链路的正确性。做法是给一个随机姿态转成待测库的表示再转回来比较误差是否在数值精度范围内。import numpy as np def random_unit_quat(rng): q rng.normal(size4) return q / np.linalg.norm(q) def hamilton_to_jpl(q_h): Hamilton (w,x,y,z) - JPL (q1,q2,q3,q4) w, x, y, z q_h return np.array([-x, -y, -z, w]) rng np.random.default_rng(42) max_err 0.0 for _ in range(1000): q_h random_unit_quat(rng) q_j hamilton_to_jpl(q_h) q_back hamilton_to_jpl(q_j) # 自逆操作应该回到原值 err min(np.linalg.norm(q_back - q_h), np.linalg.norm(q_back q_h)) # 考虑双覆盖 max_err max(max_err, err) print(max round-trip error:, max_err)注意最后那个min因为 q 和 -q 表示同一个旋转比较的时候要取两种符号下的最小误差否则会看到一半的样本莫名其妙误差为 2。这是很多人的位姿校验代码里的隐藏 bug——测试偶尔失败但没人知道为什么。更完整的一个测试是旋转闭合把同一个旋转用两套约定各自作用到 100 个随机向量上比较结果是否互为逆。这个测试比纯数值转换更能说明几何语义是否对上了。测试检查的维度通过标准单轴 90 度代数基元 旋转语义结果与物理预期同向或反向一致复合顺序乘法顺序与库的乘法结果逐分量匹配往返转换转换代码自洽性最大误差小于 1e-12随机向量旋转几何语义完整性两套结果互为逆5.4 把测试塞进 CI这三个测试加起来不到 100 行代码跑一次不到一秒。我现在的习惯是任何一个新引入的、涉及四元数的第三方库都先写一组这样的测试放进去作为后续所有开发的基线。具体做法是在测试里硬编码几组约定标杆——比如绕 z 轴 90 度、绕 x 轴 180 度、以及一组随机但有固定种子的姿态把它们的正确输出写死。任何一次依赖升级、编译器更换、平台迁移之后跑一遍如果结果变了第一时间就能发现。还有一个更狠的做法在项目的公共头文件里定义一个kQuatConventionIsHamilton true这样的常量所有涉及四元数的模块在编译期断言这个常量的值。这看起来有点小题大做但在多人协作的项目里它能防止某个模块悄悄引入了一套不同的约定然后整条数据流在某个边界处静默地翻转。6. 移植清单与几条硬规则前面五章从数学推到了工程实践最后这一章把经验收一收给一份移植时可以直接照着走的核对清单再讲几个我自己踩过的具体坑。6.1 移植前的文档核对清单从一篇论文或者一个旧系统往新平台搬姿态代码时按顺序确认下面五项缺一项都别开工。第一源端的旋转矩阵定义。找到论文里 C(q) 或者 R(q) 的完整表达式特别注意两件事叉乘项的符号以及它明确说了从哪个坐标系到哪个坐标系。如果论文只给了一个矩阵没给方向说明那就用它的数值例子反推。第二源端四元数的标量位置。看它写的是 [q1 q2 q3 q4] 还是 [q0 q1 q2 q3]或者有没有明确说 q4 是标量。老论文里通常把标量放在最后现代代码里反过来。第三源端角速度的坐标系。运动学方程里 ω 是体系还是参考系这个决定了更新式里四元数是左乘还是右乘。第四源端的误差参数化。如果是滤波器或者优化器它的 δq 是左乘还是右乘δθ 的定义方向是什么。第五目标平台的全部对应项。把上面前四项逐个映射到目标平台的表示每一项都写出转换代码不要凭记忆。这个清单看起来很啰嗦但它能挡住 95% 的移植事故。我现在的做法是把这五项写成注释放在代码文件头部每一项后面写上源端是什么、目标端是什么、映射关系是什么后来接手的人一看就懂。6.2 我踩过的几个具体坑第一个坑是 Eigen 的coeffs()顺序。当时我在写一个把位姿序列化成二进制日志的功能直接用q.coeffs().data()往外写读的时候用四参数构造Quaterniond(w,x,y,z)读回来结果顺序对不上姿态全错。查了半天才发现写入是 (x,y,z,w)读取是按 (w,x,y,z) 解释的。修复方式是把读写两端都改成显式的x()/y()/z()/w()访问器慢一点但不会错。第二个坑是 MuJoCo 到 ROS。前面表格里提过MuJoCo 的 qpos 四元数是 scalar-first。我当时在做一个仿真到实机的迁移实验仿真里姿态完美真机上绕竖直轴转了 90 度。定位方法就是打印 (1,0,0) 轴在两边的作用结果MuJoCo 给 (0,1,0)ROS 给 (0,-1,0)。修复就是把 qpos 里的四元数读出来之后做一次重排加向量取负。第三个坑是自己写的 slerp 没做半球对齐。两个关键帧的四元数如果点积为负插值会走长弧姿态绕一大圈。这个 bug 在慢速运动时看不出来只有在快速转向的时候才会出现一个诡异的绕远路动作。修复是在 slerp 开头加三行算点积判断符号必要时取负。第四个坑是优化里的符号跳变。在一段视觉惯性里程计的后处理里我发现协方差矩阵偶尔会出现负定。排查了很久最后发现是四元数状态在相邻两帧之间符号翻转导致数值雅可比出现异常大的值。加了一行半球对齐点积小于零就整体取负之后问题消失。6.3 团队协作中的约定固化前面讲的都是技术细节最后说一个偏流程但同样重要的事在团队里把约定固化下来。四元数约定这种东西一旦项目里混用了两套就会变成技术债而且是很难还的那种——因为它在大多数测试用例下都能通过只在特定姿态和特定数据流上出错。我的做法是三条项目文档里用一整节明确写清楚本项目使用 Hamilton 记法存储顺序 scalar-last角速度为体系角速度误差扰动为右乘并且把这一节放在最显眼的位置所有跨模块传输四元数的接口都写清楚顺序函数名里就带上比如toRosQuatXYZW()、fromMatlabQuatWXYZ()再就是前面提到的编译期常量断言让约定不一致的情况在编译阶段就暴露。这三条听起来都挺笨的但它们的成本很低收益很高。四元数的坑有一个很不好的性质它不会在代码审查的时候跳出来它只会在真机上、在客户面前、在你最不希望它出现的时候跳出来。补充一个实用的小习惯。我现在的所有姿态相关代码里都有一个调试打印函数接受四元数和坐标系名称输出三样东西原始四个数、旋转矩阵、以及把三个坐标轴单位向量转过去之后的结果。遇到任何姿态异常第一件事就是把这个函数挂在数据流的关键节点上跑一遍。大多数情况下看到三个单位向量的旋转结果问题出在哪个环节就一目了然了——比盯着四个浮点数猜快得多。还有一个更省事的做法在你的姿态转换代码里内置一个assertConventionConsistent()之类的自检函数接受两组约定标杆数据一组绕 z 转 90 度、一组绕 x 转 180 度在程序启动时跑一遍。这样约定发生变化的时候程序第一秒就会告诉你而不是等你跑完整个数据链路才发现姿态反了。