做点云配准、机械臂运动学或者三维可视化的人几乎都会在某个时刻撞上同一个问题给一个三维向量再给一条空间里随手画的轴让它绕这根轴转一个指定角度转完之后的坐标到底是多少。教材上一般只讲绕 X、Y、Z 三根坐标轴旋转因为那三个矩阵好写、好记考试也爱考。可真实项目里的转轴往往歪着、斜着甚至根本不过原点这时候如果还硬拆成三次绕坐标轴旋转光是角度顺序就能绕晕自己。三维向量绕任意轴的旋转公式也就是常说的轴角旋转、罗德里格斯公式就是专门收拾这个场景的工具它把绕哪根轴、转多少度直接翻译成一个 3×3 矩阵一次乘法搞定不需要中间状态也不会出现万向锁那种尴尬。围绕任意轴旋转后坐标形式还有一种经典的拆解写法叫七矩阵连乘把任意轴先搬到坐标轴上、转完再搬回去思路特别直观。这篇内容我会从几何直觉出发把公式推一遍手算一组真实数据校验再把七矩阵连乘逐步拆开最后给能直接抄的代码和踩坑清单适合刚接触三维变换的同学也适合做了几年但每次都要翻笔记的老手。1. 绕任意轴旋转这个问题为什么值得单独拎出来讲1.1 从欧拉角到轴角中间那一层才是工程里最常用的很多人学三维旋转的路径是这样的先学绕单根坐标轴转的矩阵然后学欧拉角三个角一拼就完事。这条路径在课本里没问题但在工程里会迅速暴露短板。欧拉角本身是个描述方式它依赖参考系的选取顺序XYZ、ZYX、ZYZ 各有各的矩阵两个人对接接口时如果没把顺序讲清楚出来的姿态能差出几十度。更麻烦的是万向锁当中间那个角转到 ±90 度时第一个角和第三个角的作用会退化到同一根轴上姿态自由度实际少了一个做插值的时候会出现抖跳。轴角表示绕开了这一层。它不描述分三步转而是直接描述绕哪根轴、转多少度这在物理上是唯一的角度限定在 0 到 π 之间时不存在顺序歧义。机械臂的关节、点云配准求出的刚体变换、惯性测量单元解算出的姿态增量底层几乎都是轴角或者与之等价的四元数。所以绕任意轴旋转的公式不是一个补充知识点它是三维旋转这座楼里的承重墙欧拉角反倒更像是墙上贴的一层装饰。1.2 轴角表示真正解决的问题把问题描述得再具体一点。假设有一条空间直线由轴上一点 P0 和一个单位方向向量 u 确定另有一个点 P。我们要求 P 绕这条直线旋转 θ 角之后的新位置 P。注意这里有两个要素容易被忽略。第一轴不一定过原点所以不能简单地做矩阵乘法得先把坐标系平移到轴上那个点转完再平移回去。这一步是很多新手第一次写代码时结果绕着原点乱飞的根本原因。第二方向向量必须单位化。公式里所有推导都建立在 |u| 1 的前提上如果直接拿一个模长为 5 的方向向量代进去结果会整体被放大而且轴方向也会算错——因为叉乘项和 kk^T 项的量纲不一致了。这两点看着简单实际项目里十次调不通有六次栽在这里。1.3 先把记号约定统一为了避免后面读到一半卡住先把记号钉死。全文使用右手坐标系向量默认为列向量矩阵左乘向量也就是 v R·v。旋转轴用单位向量 k 表示k (kx, ky, kz)满足 kx² ky² kz² 1。旋转角记作 θ逆时针为正这里的逆时针是站在轴的负方向朝正方向看的样子也就是右手拇指指向 k 方向时四指弯曲的方向。叉乘矩阵也叫反对称矩阵记作 [k]×它的定义是让 [k]×·v 等于 k × v。这个记号能把叉乘写成矩阵乘法是推导罗德里格斯公式的关键一步。$$ [k]_\times \begin{bmatrix} 0 -k_z k_y \ k_z 0 -k_x \ -k_y k_x 0 \end{bmatrix} $$有了它后面那串推导才写得下去。建议你自己动手把这个矩阵乘上一个任意向量验证一遍确认三个分量和叉乘结果完全一致这一步花两分钟能省掉后面半小时的困惑。2. 罗德里格斯公式从几何直觉一步步推出来2.1 把待旋转向量拆成平行与垂直两部分推导的起点是一个非常朴素的想法绕某根轴旋转时向量在轴方向上的投影是不动的。你想象一根竖着的晾衣杆上挂着一件衣服绕着杆子转衣服在杆子高度方向的位置一点没变变的只是水平面上那个离杆子的方位。按这个思路把 v 拆开。平行于轴的分量记作 v∥它等于 (v·k)k。垂直分量记作 v⊥等于 v - (v·k)k。旋转之后v∥ 原封不动v⊥ 则在一个垂直于 k 的平面内转了 θ 角。而这个平面里有一组天然的基一个是 v⊥另一个是 k × v。后者和 v⊥ 垂直长度也恰好等于 |v⊥|因为 |k × v| |k||v|sinφ |v⊥|其中 φ 是 v 与 k 的夹角。这就好比在平面里有了 x 轴和 y 轴接下来只需要用余弦和正弦组合就行。2.2 垂直分量在圆盘里转一圈把 v⊥ 和 k × v 当作平面内的两个正交基长度都是 |v⊥|。旋转 θ 之后新的垂直分量应该是这两个方向按余弦正弦配比的结果。写成公式就是 v⊥ v⊥ cosθ (k × v) sinθ。这和二维平面里把点 (1,0) 转 θ 得到 (cosθ, sinθ) 是同一个道理只不过x 轴换成了 v⊥y 轴换成了 k × v。三角函数的符号也值得确认一下当 θ 是小的正角度时新分量应该朝 k × v 的方向偏一点点这正是 sinθ 项系数为正的含义。如果你把符号写反得到的旋转方向会镜像做动画的时候一眼能看出来但只看数值结果往往要查很久。2.3 合并结果并与矩阵形式对照把平行部分加回来就得到了罗德里格斯公式的向量形式$$ v v\cos\theta (k \times v)\sin\theta k(k \cdot v)(1 - \cos\theta) $$这三项的物理含义很清晰第一项是原地不动的投影第二项是垂直于轴的那部分在平面内转动第三项把平行分量补回来因为它在 v 里的原贡献是 v·? 不完全等于 (v·k)k需要乘一个 (1 - cosθ) 修正。你可以用 θ 0 代入检验三项变成 v 0 0 v符合转零度等于没转。用 θ π 代入得到 2k(k·v) - v这正是关于轴做镜面对称再取反的结果几何上也对得上。把叉乘换成矩阵形式向量形式就变成了矩阵形式$$ R I \sin\theta,[k]\times (1 - \cos\theta),[k]\times^2 $$这里用到了一个恒等式 [k]ײ kkᵀ - I它成立的前提正是 |k| 1。把它代进去R 就写成 I sinθ[k]× (1-cosθ)(kkᵀ - I)合并同类项后等价于 (1-cosθ)kkᵀ sinθ[k]× cosθ·I。这个形式更紧凑也更容易手工展开成 9 个元素。2.4 三种等价写法与各自的适用场合工程里常见的写法有三种分清它们的适用场合能省不少事。写法表达式适用场景说明向量形式v v cosθ (k×v) sinθ k(k·v)(1-cosθ)单点变换、需要保留中间量无需求矩阵计算量最小矩阵展开式R I sinθ[k]× (1-cosθ)[k]ײ批量点云、需要复用矩阵一次构造多次使用分量展开式9 个元素逐个写出手工计算、嵌入式定点实现可提前算好 cos、sin分量展开式长这样把它抄在笔记本上比每次重推省事$$ R \begin{bmatrix} c k_x^2(1-c) k_xk_y(1-c) - k_z s k_xk_z(1-c) k_y s \ k_yk_x(1-c) k_z s c k_y^2(1-c) k_yk_z(1-c) - k_x s \ k_zk_x(1-c) - k_y s k_zk_y(1-c) k_x s c k_z^2(1-c) \end{bmatrix} $$其中 c 代表 cosθs 代表 sinθ。这三行九列里藏着明显的规律对角线是同构的 c k_i²(1-c)非对角线可以看成对称部分 k_i k_j (1-c) 加上一个反对称修正项反对称修正项的系数就是 sinθ。看穿这个结构你就能在不翻资料的情况下把它默写出来实在记不住时按对称部分 反对称部分两块现场拼。3. 拿一组数手算到底轴 (1,1,1)、角度 90 度3.1 参数准备与归一化光看公式容易产生我懂了的错觉真正手算一遍才能把每个环节钉死。取一个足够经典又不会太丑的例子轴方向取 (1, 1, 1)待旋转向量取 v (1, 0, 0)旋转角 90 度。先做归一化。|(1,1,1)| √3 ≈ 1.7320508所以 k (0.5773503, 0.5773503, 0.5773503)。这一步千万别跳过我见过太多人直接把 (1,1,1) 代进公式结果 R 的行列式算出来是 7而不是 1一看就不是旋转矩阵。判断一个矩阵是不是合法旋转矩阵最快的方法就是看行列式是否等于 1等于 1 才说明它同时满足正交和保向两个条件。角度方面θ 90° 对应 cosθ 0sinθ 1。这两个值越简单中间步骤越不容易出错所以做验证用例时优先挑 30°、90°、180° 这种角度。3.2 代入罗德里格斯公式逐项计算用向量形式算分三项。第一项 v cosθ (1,0,0) × 0 (0, 0, 0)。第二项 (k × v) sinθ。先算叉乘k × v (ky·vz - kz·vy, kz·vx - kx·vz, kx·vy - ky·vx)代入 k (0.5774, 0.5774, 0.5774)、v (1, 0, 0)x 分量0.5774×0 - 0.5774×0 0y 分量0.5774×1 - 0.5774×0 0.5774z 分量0.5774×0 - 0.5774×1 -0.5774所以 k × v (0, 0.5774, -0.5774)乘以 sinθ 1 保持不变。第三项 k(k·v)(1 - cosθ)。点乘 k·v 0.5774×1 0.5774×0 0.5774×0 0.5774。系数 (1 - cosθ) 1。所以这一项等于 0.5774 × (0.5774, 0.5774, 0.5774) (0.3333, 0.3333, 0.3333)。三项相加v (0,0,0) (0, 0.5774, -0.5774) (0.3333, 0.3333, 0.3333) (0.3333, 0.9107, -0.2440)。3.3 三项校验模长、转轴不动点、正交性算出结果只是第一步必须校验。校验不通过的数不如不算。第一项校验模长不变。原向量模长是 1。新向量的模长平方 0.3333² 0.9107² 0.2440² 0.1111 0.8294 0.0595 1.0000。通过。旋转是刚体变换不改变长度这是最基本的门槛。第二项校验转轴上的点不动。取 k 本身作为输入代入公式叉乘 k × k 0点乘 k·k 1得到 v k·0 0 k·1·1 k原样返回。这符合轴上的点旋转后还在轴上。第三项用矩阵形式交叉验证。因为 θ 90°R [k]× kkᵀ。kkᵀ 是 1/3 的全 1 矩阵[k]× 的第一列是 (0, 0.5774, -0.5774)。两者第一列相加得 (0.3333, 0.9107, -0.2440)与前面手算结果完全一致。三项校验都过说明公式、符号、归一化三个环节都没问题。这套校验流程建议做成固定模板每次换轴换角度都跑一遍比事后调试省力得多。4. 七矩阵连乘是怎么冒出来的4.1 核心思路把任意轴先搬到坐标轴上罗德里格斯公式虽然优雅但推导过程用到了叉乘和二次型对纯做图形管线的人来说不够图像化。于是就有了另一种同样经典、思路更直白的做法既然绕 Z 轴旋转的矩阵最简单那就想办法把任意轴先转到 Z 轴上去转完再把轴转回来。这就像搬家。你要把一个歪着放的柜子转 90 度直接转很难描述清楚但如果先把柜子搬到正对着门口的位置转 90 度就变成一件显而易见的事转完再搬回原位。整个流程包含平移 两次对齐旋转 目标旋转 两次逆旋转 平移回位一共七个矩阵相乘这就是绕任意轴旋转后坐标形式的七矩阵连乘来源。4.2 七个矩阵逐个拆开看设旋转轴过点 P0(x0,y0,z0)方向单位向量 u (a, b, c)a² b² c² 1。记 d √(b² c²)它是 u 在 YZ 平面上的投影长度。七个矩阵按从右到左的作用顺序分别是序号矩阵作用关键参数1T(-P0)把轴上一点平移到原点x0, y0, z02Rx(α)绕 X 轴转让轴落入 XOZ 平面sinα b/d, cosα c/d3Ry(β)绕 Y 轴转让轴与 Z 轴重合sinβ -a, cosβ d4Rz(θ)绕 Z 轴完成目标旋转θ 为旋转角5Ry(-β)撤销第 3 步同上取负角6Rx(-α)撤销第 2 步同上取负角7T(P0)平移回原位x0, y0, z0合成的完整表达式写作 M T(P0) · Ry(-β) · Rx(-α) · Rz(θ) · Ry(β) · Rx(α) · T(-P0)。注意这里面用的是 Ry(β)·Rx(α) 这个正向顺序所以它的逆是 Rx(-α)·Ry(-β)逆矩阵的书写顺序必须反过来这一点是很多人第一次实现七矩阵连乘时最容易搞错的地方。4.3 为什么是七个而不是六个或八个有人会问既然平移只在首尾出现为什么不算作一次而算作两次。原因在于矩阵连乘的写法里T(-P0) 和 T(P0) 是两个独立矩阵虽然它们互为逆但在表达式里各占一个位置。中间的五次旋转里Rx(α) 和 Ry(β) 是把轴对齐到 Z 轴Rz(θ) 是真正干活的那一步Ry(-β) 和 Rx(-α) 是复位。加起来就是 2 3 2 7。如果旋转轴恰好过原点第 1 和第 7 个矩阵退化成单位矩阵可以省掉六个矩阵就够。如果轴本身就平行于某个坐标轴前面还有几步能退化。但工程上写通用函数时我建议一律按七个矩阵来写把退化情况交给数值计算去处理不要写一堆 if-else 分支那样只会让维护成本上升。4.4 与直接套罗德里格斯公式的取舍两种做法的结果完全一致但代价不同。七矩阵连乘要做六次 4×4或者三次 3×3 旋转加两次平移矩阵乘法浮点运算量更大误差也会随乘法次数累积。罗德里格斯公式只需要构造一次 3×3 矩阵数值上更干净。那为什么还要讲七矩阵连乘因为它有不可替代的教学价值。当你面对一个陌生的、绕过原点的斜轴旋转七矩阵连乘的每一步几何意义都能说出来先平移、再对齐、再转、再复位。这种知道每一步在干嘛的感觉是纯背公式给不了的。实际项目里我会用罗德里格斯公式做计算用七矩阵连乘做推导和验证两个都实现一遍互为参照出错概率会大幅下降。顺便说一句前面那个 (1,1,1) 轴的例子里d √(1/3 1/3) 0.8165sinα 0.5774/0.8165 0.7071cosα 0.7071所以 α 45°sinβ -0.5774cosβ 0.8165β ≈ -35.26°。也就是说绕 (1,1,1) 转 90 度可以被拆成绕 X 转 45 度 绕 Y 转 -35.26 度 绕 Z 转 90 度 反向复位这一整套动作几何上完全说得通。5. 坐标系绕任意轴旋转主动视角与被动视角5.1 转物体和转坐标系差在哪标题里的热词提到了坐标系绕任意轴旋转这是个很容易混淆的点必须单独讲。同一套公式在两种视角下含义相反。主动旋转也叫 alibi 变换指的是坐标系不动物体或者点自己在转。点从 P 转到 P用的是 P R·P。被动旋转也叫 alias 变换指的是物体不动坐标系转了我们问同一个点在新的坐标系里怎么表示。这时候变换关系是 P_new R⁻¹·P_old Rᵀ·P_old。很多渲染引擎里相机旋转用的是被动视角模型旋转用的是主动视角两者差一个转置。如果你发现画面里的东西转动方向反了或者相机绕着自己转但场景没动第一件事就检查这里。5.2 转置关系与左乘右乘因为旋转矩阵是正交矩阵Rᵀ R⁻¹ 这个性质让转置操作不费什么代价只需要交换行列就行比求逆省事太多。所以在写代码时如果需要一个反向旋转直接对矩阵取转置千万别调用通用求逆函数那既慢又容易引入数值误差。左乘和右乘的差别也在这里。R·P 是同一个坐标系内对点做变换P 左乘 R而 P·R 相当于对 P 做行变换通常出现在把多个点排成行向量的场景。如果你用的是行主序的数据比如某些图形 API 的顶点数据整个变换链的乘法顺序都要反过来写否则结果会看起来差不多但就是不对。5.3 顺序问题先平移还是先旋转在七矩阵连乘里T(-P0) 必须在最右边、最先作用。如果顺序写反先旋转再平移等于把轴当成过原点的轴来转然后整体再挪一个位置结果会偏离正确的轨迹。我个人的记忆口诀是近的先做从右往左读矩阵连乘距离待变换点最近的那个矩阵最先作用。连续旋转时的顺序同样重要。R1·R2 表示先做 R2 再做 R1和 R2·R1 完全不是一回事。三维旋转不可交换绕 X 转 90 度再绕 Y 转 90 度和反过来做最终的姿态差得很远。这跟二维里转加转、平移加平移都能交换的直觉完全不同需要重新建立肌肉记忆。6. 代码落地Python 实现与数值稳定性6.1 从公式直译的一份实现把公式翻译成代码最重要的是结构清晰、边界情况明确。下面这份实现直接对应罗德里格斯公式的矩阵形式输入轴向量不要求预先单位化函数内部处理。import numpy as np def rodrigues(k, theta): 绕单位轴 k 旋转 theta 弧度返回 3x3 旋转矩阵 k np.asarray(k, dtypenp.float64) n np.linalg.norm(k) if n 1e-12: raise ValueError(旋转轴向量模长过小无法确定旋转平面) k k / n K np.array([[0.0, -k[2], k[1]], [k[2], 0.0, -k[0]], [-k[1], k[0], 0.0]]) c, s np.cos(theta), np.sin(theta) return np.eye(3) s * K (1.0 - c) * (K K)这段代码里有三个地方值得说。第一用 K K 而不是显式写出 kkᵀ - I好处是不用担心单位化遗漏带来的偏差因为 K 本身已经用了单位向量。第二dtype 显式指定 float64避免整数输入导致的截断。第三零轴抛异常而不是返回单位矩阵因为静默返回单位矩阵会让上游很难发现参数传错。6.2 单位化、零轴、零角度这些边界边界情况处理得好不好决定了这个函数能不能上生产。零轴必须报错。旋转轴是零向量时旋转平面压根不存在这时候返回任何结果都是错的宁可让程序崩在源头。接近零但非零的轴也要小心模长很小的轴归一化之后方向对噪声极度敏感最好在外层做一次参数有效性检查。零角度应该返回单位矩阵这一点罗德里格斯公式天然满足sinθ 0(1-cosθ) 0结果就是 I。不需要特判公式自己搞定。同理θ 2π 时结果也是 I符合转一整圈回到原地。接近 π 的角度是个麻烦点。当 θ 趋近 π 时sinθ 趋近于 0矩阵里的反对称部分快没了只剩下对称部分做矩阵到轴角的反解时会变得不唯一——因为绕 k 转 π 和绕 -k 转 π 结果相同。如果你需要从矩阵反解轴角这一点必须单独处理。6.3 与四元数、与现成库的交叉验证写完之后一定要和别的实现对比。最方便的是和四元数互转因为四元数路径独立于罗德里格斯公式两者如果一致说明实现基本可靠。def quat_to_matrix(q): w, x, y, z q / np.linalg.norm(q) return np.array([ [1 - 2*(y*y z*z), 2*(x*y - z*w), 2*(x*z y*w)], [2*(x*y z*w), 1 - 2*(x*x z*z), 2*(y*z - x*w)], [2*(x*z - y*w), 2*(y*z x*w), 1 - 2*(x*x y*y)] ]) k np.array([1.0, 1.0, 1.0]) theta np.pi / 2 R1 rodrigues(k, theta) half theta / 2 R2 quat_to_matrix(np.array([np.cos(half), *(np.sin(half) * k / np.linalg.norm(k))])) print(np.max(np.abs(R1 - R2))) # 实测输出在 1e-16 量级除了四元数还可以用现成的科学计算库做第三方校验。构造一个绕给定轴的旋转用它算一遍再和你的实现对比误差在 1e-12 以内就可以放心用。我习惯在单元测试里固定几组参数轴取 (1,0,0)、(0,1,0)、(0,0,1) 三个基准方向加上 (1,1,1) 和 (1,-2,3) 两个斜轴角度取 0、π/6、π/2、π、1.9π 这几个点。零角度和满圈是必测项很多 bug 就藏在这两个看似平凡的位置。7. 实际踩坑记录与排查速查表7.1 结果不对时按这个顺序查调试三维旋转问题的效率很大程度取决于有没有固定排查顺序。我的习惯是四步走从易到难。第一步查归一化。把轴向量取模打印出来确认是 1。这一步能解决大约一半的问题。第二步查矩阵行列式。对构造出的 R 求 det应该等于 1。如果等于 -1说明掺进了反射如果不等于 ±1说明公式项写错或者单位化漏了。第三步查不动点。把轴向量本身代进去结果应该原样返回。如果不返回说明平行分量的系数写错了通常是把 (1 - cosθ) 写成了 1。第四步查手性。取 v (1,0,0)、k (0,0,1)、θ 90°正确结果是 (0,1,0)。如果得到 (0,-1,0)说明叉乘矩阵的符号写反了或者旋转方向定义成了顺时针。这四步走完九成问题都能定位。7.2 常见问题与处理对照表现象可能原因定位方法处理方式结果整体被放大轴未单位化打印k绕原点乱转漏了平移矩阵检查轴是否过原点补 T(-P0) 和 T(P0)行列式为 -1反射项被误加算 det检查三处非对角符号旋转方向反了叉乘项符号错误用 Z 轴 90 度基准测试交换反对称部分符号角度接近 π 时结果跳变轴角反解不唯一观察 sinθ 趋零区间单独处理或改用四元数组合旋转结果不对乘法顺序写反交换两个矩阵看是否变化统一从右往左书写大量点云结果累积漂移反复乘矩阵累积误差比较单次与多次结果定期对旋转矩阵做正交化提示旋转矩阵用久了会缓慢偏离正交性尤其是做上千次连乘之后。长期运行的系统里建议每隔一段时间对矩阵做一次正交化处理比如用极分解或者简单的 Gram-Schmidt 修正别让误差无限累积。7.3 几条不太好查的经验还有一种坑不太容易通过公式检查发现就是坐标系的约定不一致。有些建模软件用 Z 轴向上有些用 Y 轴向上仿真环境和渲染环境有时也对不上。这种情况下公式本身没错但整个场景的方向感是歪的。排查方法很笨但有效在场景里放一个明显不对称的物体绕世界 Z 轴转 90 度看它往哪边倒就能判断出当前用的是什么约定。另外打印中间变量时别只打印矩阵的迹或者行列式那种信息量太低。直接把整个矩阵按行打印出来对照分量展开式一项一项核对虽然土但定位准确。我遇到过一次非常隐蔽的 bug是有人把 kx 和 kz 在某一项里写反了迹和行列式都对因为那两个位置的对称性掩盖了错误只有把九个数全打出来才看出来。8. 最后写点自己的体会这套公式我前后重新推导过至少四遍每一遍都有新收获。第一次是照着课本抄第二次是自己从几何分解推第三次是为了给同事讲清楚所以画了图第四次是写代码时发现边界情况处理不完。每次重推最花时间的地方都不是公式本身而是符号约定——右手系还是左手系、列向量还是行向量、叉乘矩阵的第一行到底该放谁。这提醒我在三维旋转这个领域公式从来不是难点把约定写清楚、把校验做扎实才是。如果你正在做相关的项目我建议把罗德里格斯公式和七矩阵连乘两条路都实现一遍互为参照。再准备一组固定的测试用例包含三根坐标轴、一个斜轴、角度覆盖 0 到 π 的关键点每次改动代码后跑一遍。这套组合拳下来绝大多数错误在提交之前就会被拦下。至于四元数等这套矩阵路径彻底梳理清楚之后再去了解你会觉得它其实只是同一件事的另一种写法而不是什么全新的东西。 三维向量绕任意轴旋转：罗德里格斯公式与七矩阵连乘
做点云配准、机械臂运动学或者三维可视化的人几乎都会在某个时刻撞上同一个问题给一个三维向量再给一条空间里随手画的轴让它绕这根轴转一个指定角度转完之后的坐标到底是多少。教材上一般只讲绕 X、Y、Z 三根坐标轴旋转因为那三个矩阵好写、好记考试也爱考。可真实项目里的转轴往往歪着、斜着甚至根本不过原点这时候如果还硬拆成三次绕坐标轴旋转光是角度顺序就能绕晕自己。三维向量绕任意轴的旋转公式也就是常说的轴角旋转、罗德里格斯公式就是专门收拾这个场景的工具它把绕哪根轴、转多少度直接翻译成一个 3×3 矩阵一次乘法搞定不需要中间状态也不会出现万向锁那种尴尬。围绕任意轴旋转后坐标形式还有一种经典的拆解写法叫七矩阵连乘把任意轴先搬到坐标轴上、转完再搬回去思路特别直观。这篇内容我会从几何直觉出发把公式推一遍手算一组真实数据校验再把七矩阵连乘逐步拆开最后给能直接抄的代码和踩坑清单适合刚接触三维变换的同学也适合做了几年但每次都要翻笔记的老手。1. 绕任意轴旋转这个问题为什么值得单独拎出来讲1.1 从欧拉角到轴角中间那一层才是工程里最常用的很多人学三维旋转的路径是这样的先学绕单根坐标轴转的矩阵然后学欧拉角三个角一拼就完事。这条路径在课本里没问题但在工程里会迅速暴露短板。欧拉角本身是个描述方式它依赖参考系的选取顺序XYZ、ZYX、ZYZ 各有各的矩阵两个人对接接口时如果没把顺序讲清楚出来的姿态能差出几十度。更麻烦的是万向锁当中间那个角转到 ±90 度时第一个角和第三个角的作用会退化到同一根轴上姿态自由度实际少了一个做插值的时候会出现抖跳。轴角表示绕开了这一层。它不描述分三步转而是直接描述绕哪根轴、转多少度这在物理上是唯一的角度限定在 0 到 π 之间时不存在顺序歧义。机械臂的关节、点云配准求出的刚体变换、惯性测量单元解算出的姿态增量底层几乎都是轴角或者与之等价的四元数。所以绕任意轴旋转的公式不是一个补充知识点它是三维旋转这座楼里的承重墙欧拉角反倒更像是墙上贴的一层装饰。1.2 轴角表示真正解决的问题把问题描述得再具体一点。假设有一条空间直线由轴上一点 P0 和一个单位方向向量 u 确定另有一个点 P。我们要求 P 绕这条直线旋转 θ 角之后的新位置 P。注意这里有两个要素容易被忽略。第一轴不一定过原点所以不能简单地做矩阵乘法得先把坐标系平移到轴上那个点转完再平移回去。这一步是很多新手第一次写代码时结果绕着原点乱飞的根本原因。第二方向向量必须单位化。公式里所有推导都建立在 |u| 1 的前提上如果直接拿一个模长为 5 的方向向量代进去结果会整体被放大而且轴方向也会算错——因为叉乘项和 kk^T 项的量纲不一致了。这两点看着简单实际项目里十次调不通有六次栽在这里。1.3 先把记号约定统一为了避免后面读到一半卡住先把记号钉死。全文使用右手坐标系向量默认为列向量矩阵左乘向量也就是 v R·v。旋转轴用单位向量 k 表示k (kx, ky, kz)满足 kx² ky² kz² 1。旋转角记作 θ逆时针为正这里的逆时针是站在轴的负方向朝正方向看的样子也就是右手拇指指向 k 方向时四指弯曲的方向。叉乘矩阵也叫反对称矩阵记作 [k]×它的定义是让 [k]×·v 等于 k × v。这个记号能把叉乘写成矩阵乘法是推导罗德里格斯公式的关键一步。$$ [k]_\times \begin{bmatrix} 0 -k_z k_y \ k_z 0 -k_x \ -k_y k_x 0 \end{bmatrix} $$有了它后面那串推导才写得下去。建议你自己动手把这个矩阵乘上一个任意向量验证一遍确认三个分量和叉乘结果完全一致这一步花两分钟能省掉后面半小时的困惑。2. 罗德里格斯公式从几何直觉一步步推出来2.1 把待旋转向量拆成平行与垂直两部分推导的起点是一个非常朴素的想法绕某根轴旋转时向量在轴方向上的投影是不动的。你想象一根竖着的晾衣杆上挂着一件衣服绕着杆子转衣服在杆子高度方向的位置一点没变变的只是水平面上那个离杆子的方位。按这个思路把 v 拆开。平行于轴的分量记作 v∥它等于 (v·k)k。垂直分量记作 v⊥等于 v - (v·k)k。旋转之后v∥ 原封不动v⊥ 则在一个垂直于 k 的平面内转了 θ 角。而这个平面里有一组天然的基一个是 v⊥另一个是 k × v。后者和 v⊥ 垂直长度也恰好等于 |v⊥|因为 |k × v| |k||v|sinφ |v⊥|其中 φ 是 v 与 k 的夹角。这就好比在平面里有了 x 轴和 y 轴接下来只需要用余弦和正弦组合就行。2.2 垂直分量在圆盘里转一圈把 v⊥ 和 k × v 当作平面内的两个正交基长度都是 |v⊥|。旋转 θ 之后新的垂直分量应该是这两个方向按余弦正弦配比的结果。写成公式就是 v⊥ v⊥ cosθ (k × v) sinθ。这和二维平面里把点 (1,0) 转 θ 得到 (cosθ, sinθ) 是同一个道理只不过x 轴换成了 v⊥y 轴换成了 k × v。三角函数的符号也值得确认一下当 θ 是小的正角度时新分量应该朝 k × v 的方向偏一点点这正是 sinθ 项系数为正的含义。如果你把符号写反得到的旋转方向会镜像做动画的时候一眼能看出来但只看数值结果往往要查很久。2.3 合并结果并与矩阵形式对照把平行部分加回来就得到了罗德里格斯公式的向量形式$$ v v\cos\theta (k \times v)\sin\theta k(k \cdot v)(1 - \cos\theta) $$这三项的物理含义很清晰第一项是原地不动的投影第二项是垂直于轴的那部分在平面内转动第三项把平行分量补回来因为它在 v 里的原贡献是 v·? 不完全等于 (v·k)k需要乘一个 (1 - cosθ) 修正。你可以用 θ 0 代入检验三项变成 v 0 0 v符合转零度等于没转。用 θ π 代入得到 2k(k·v) - v这正是关于轴做镜面对称再取反的结果几何上也对得上。把叉乘换成矩阵形式向量形式就变成了矩阵形式$$ R I \sin\theta,[k]\times (1 - \cos\theta),[k]\times^2 $$这里用到了一个恒等式 [k]ײ kkᵀ - I它成立的前提正是 |k| 1。把它代进去R 就写成 I sinθ[k]× (1-cosθ)(kkᵀ - I)合并同类项后等价于 (1-cosθ)kkᵀ sinθ[k]× cosθ·I。这个形式更紧凑也更容易手工展开成 9 个元素。2.4 三种等价写法与各自的适用场合工程里常见的写法有三种分清它们的适用场合能省不少事。写法表达式适用场景说明向量形式v v cosθ (k×v) sinθ k(k·v)(1-cosθ)单点变换、需要保留中间量无需求矩阵计算量最小矩阵展开式R I sinθ[k]× (1-cosθ)[k]ײ批量点云、需要复用矩阵一次构造多次使用分量展开式9 个元素逐个写出手工计算、嵌入式定点实现可提前算好 cos、sin分量展开式长这样把它抄在笔记本上比每次重推省事$$ R \begin{bmatrix} c k_x^2(1-c) k_xk_y(1-c) - k_z s k_xk_z(1-c) k_y s \ k_yk_x(1-c) k_z s c k_y^2(1-c) k_yk_z(1-c) - k_x s \ k_zk_x(1-c) - k_y s k_zk_y(1-c) k_x s c k_z^2(1-c) \end{bmatrix} $$其中 c 代表 cosθs 代表 sinθ。这三行九列里藏着明显的规律对角线是同构的 c k_i²(1-c)非对角线可以看成对称部分 k_i k_j (1-c) 加上一个反对称修正项反对称修正项的系数就是 sinθ。看穿这个结构你就能在不翻资料的情况下把它默写出来实在记不住时按对称部分 反对称部分两块现场拼。3. 拿一组数手算到底轴 (1,1,1)、角度 90 度3.1 参数准备与归一化光看公式容易产生我懂了的错觉真正手算一遍才能把每个环节钉死。取一个足够经典又不会太丑的例子轴方向取 (1, 1, 1)待旋转向量取 v (1, 0, 0)旋转角 90 度。先做归一化。|(1,1,1)| √3 ≈ 1.7320508所以 k (0.5773503, 0.5773503, 0.5773503)。这一步千万别跳过我见过太多人直接把 (1,1,1) 代进公式结果 R 的行列式算出来是 7而不是 1一看就不是旋转矩阵。判断一个矩阵是不是合法旋转矩阵最快的方法就是看行列式是否等于 1等于 1 才说明它同时满足正交和保向两个条件。角度方面θ 90° 对应 cosθ 0sinθ 1。这两个值越简单中间步骤越不容易出错所以做验证用例时优先挑 30°、90°、180° 这种角度。3.2 代入罗德里格斯公式逐项计算用向量形式算分三项。第一项 v cosθ (1,0,0) × 0 (0, 0, 0)。第二项 (k × v) sinθ。先算叉乘k × v (ky·vz - kz·vy, kz·vx - kx·vz, kx·vy - ky·vx)代入 k (0.5774, 0.5774, 0.5774)、v (1, 0, 0)x 分量0.5774×0 - 0.5774×0 0y 分量0.5774×1 - 0.5774×0 0.5774z 分量0.5774×0 - 0.5774×1 -0.5774所以 k × v (0, 0.5774, -0.5774)乘以 sinθ 1 保持不变。第三项 k(k·v)(1 - cosθ)。点乘 k·v 0.5774×1 0.5774×0 0.5774×0 0.5774。系数 (1 - cosθ) 1。所以这一项等于 0.5774 × (0.5774, 0.5774, 0.5774) (0.3333, 0.3333, 0.3333)。三项相加v (0,0,0) (0, 0.5774, -0.5774) (0.3333, 0.3333, 0.3333) (0.3333, 0.9107, -0.2440)。3.3 三项校验模长、转轴不动点、正交性算出结果只是第一步必须校验。校验不通过的数不如不算。第一项校验模长不变。原向量模长是 1。新向量的模长平方 0.3333² 0.9107² 0.2440² 0.1111 0.8294 0.0595 1.0000。通过。旋转是刚体变换不改变长度这是最基本的门槛。第二项校验转轴上的点不动。取 k 本身作为输入代入公式叉乘 k × k 0点乘 k·k 1得到 v k·0 0 k·1·1 k原样返回。这符合轴上的点旋转后还在轴上。第三项用矩阵形式交叉验证。因为 θ 90°R [k]× kkᵀ。kkᵀ 是 1/3 的全 1 矩阵[k]× 的第一列是 (0, 0.5774, -0.5774)。两者第一列相加得 (0.3333, 0.9107, -0.2440)与前面手算结果完全一致。三项校验都过说明公式、符号、归一化三个环节都没问题。这套校验流程建议做成固定模板每次换轴换角度都跑一遍比事后调试省力得多。4. 七矩阵连乘是怎么冒出来的4.1 核心思路把任意轴先搬到坐标轴上罗德里格斯公式虽然优雅但推导过程用到了叉乘和二次型对纯做图形管线的人来说不够图像化。于是就有了另一种同样经典、思路更直白的做法既然绕 Z 轴旋转的矩阵最简单那就想办法把任意轴先转到 Z 轴上去转完再把轴转回来。这就像搬家。你要把一个歪着放的柜子转 90 度直接转很难描述清楚但如果先把柜子搬到正对着门口的位置转 90 度就变成一件显而易见的事转完再搬回原位。整个流程包含平移 两次对齐旋转 目标旋转 两次逆旋转 平移回位一共七个矩阵相乘这就是绕任意轴旋转后坐标形式的七矩阵连乘来源。4.2 七个矩阵逐个拆开看设旋转轴过点 P0(x0,y0,z0)方向单位向量 u (a, b, c)a² b² c² 1。记 d √(b² c²)它是 u 在 YZ 平面上的投影长度。七个矩阵按从右到左的作用顺序分别是序号矩阵作用关键参数1T(-P0)把轴上一点平移到原点x0, y0, z02Rx(α)绕 X 轴转让轴落入 XOZ 平面sinα b/d, cosα c/d3Ry(β)绕 Y 轴转让轴与 Z 轴重合sinβ -a, cosβ d4Rz(θ)绕 Z 轴完成目标旋转θ 为旋转角5Ry(-β)撤销第 3 步同上取负角6Rx(-α)撤销第 2 步同上取负角7T(P0)平移回原位x0, y0, z0合成的完整表达式写作 M T(P0) · Ry(-β) · Rx(-α) · Rz(θ) · Ry(β) · Rx(α) · T(-P0)。注意这里面用的是 Ry(β)·Rx(α) 这个正向顺序所以它的逆是 Rx(-α)·Ry(-β)逆矩阵的书写顺序必须反过来这一点是很多人第一次实现七矩阵连乘时最容易搞错的地方。4.3 为什么是七个而不是六个或八个有人会问既然平移只在首尾出现为什么不算作一次而算作两次。原因在于矩阵连乘的写法里T(-P0) 和 T(P0) 是两个独立矩阵虽然它们互为逆但在表达式里各占一个位置。中间的五次旋转里Rx(α) 和 Ry(β) 是把轴对齐到 Z 轴Rz(θ) 是真正干活的那一步Ry(-β) 和 Rx(-α) 是复位。加起来就是 2 3 2 7。如果旋转轴恰好过原点第 1 和第 7 个矩阵退化成单位矩阵可以省掉六个矩阵就够。如果轴本身就平行于某个坐标轴前面还有几步能退化。但工程上写通用函数时我建议一律按七个矩阵来写把退化情况交给数值计算去处理不要写一堆 if-else 分支那样只会让维护成本上升。4.4 与直接套罗德里格斯公式的取舍两种做法的结果完全一致但代价不同。七矩阵连乘要做六次 4×4或者三次 3×3 旋转加两次平移矩阵乘法浮点运算量更大误差也会随乘法次数累积。罗德里格斯公式只需要构造一次 3×3 矩阵数值上更干净。那为什么还要讲七矩阵连乘因为它有不可替代的教学价值。当你面对一个陌生的、绕过原点的斜轴旋转七矩阵连乘的每一步几何意义都能说出来先平移、再对齐、再转、再复位。这种知道每一步在干嘛的感觉是纯背公式给不了的。实际项目里我会用罗德里格斯公式做计算用七矩阵连乘做推导和验证两个都实现一遍互为参照出错概率会大幅下降。顺便说一句前面那个 (1,1,1) 轴的例子里d √(1/3 1/3) 0.8165sinα 0.5774/0.8165 0.7071cosα 0.7071所以 α 45°sinβ -0.5774cosβ 0.8165β ≈ -35.26°。也就是说绕 (1,1,1) 转 90 度可以被拆成绕 X 转 45 度 绕 Y 转 -35.26 度 绕 Z 转 90 度 反向复位这一整套动作几何上完全说得通。5. 坐标系绕任意轴旋转主动视角与被动视角5.1 转物体和转坐标系差在哪标题里的热词提到了坐标系绕任意轴旋转这是个很容易混淆的点必须单独讲。同一套公式在两种视角下含义相反。主动旋转也叫 alibi 变换指的是坐标系不动物体或者点自己在转。点从 P 转到 P用的是 P R·P。被动旋转也叫 alias 变换指的是物体不动坐标系转了我们问同一个点在新的坐标系里怎么表示。这时候变换关系是 P_new R⁻¹·P_old Rᵀ·P_old。很多渲染引擎里相机旋转用的是被动视角模型旋转用的是主动视角两者差一个转置。如果你发现画面里的东西转动方向反了或者相机绕着自己转但场景没动第一件事就检查这里。5.2 转置关系与左乘右乘因为旋转矩阵是正交矩阵Rᵀ R⁻¹ 这个性质让转置操作不费什么代价只需要交换行列就行比求逆省事太多。所以在写代码时如果需要一个反向旋转直接对矩阵取转置千万别调用通用求逆函数那既慢又容易引入数值误差。左乘和右乘的差别也在这里。R·P 是同一个坐标系内对点做变换P 左乘 R而 P·R 相当于对 P 做行变换通常出现在把多个点排成行向量的场景。如果你用的是行主序的数据比如某些图形 API 的顶点数据整个变换链的乘法顺序都要反过来写否则结果会看起来差不多但就是不对。5.3 顺序问题先平移还是先旋转在七矩阵连乘里T(-P0) 必须在最右边、最先作用。如果顺序写反先旋转再平移等于把轴当成过原点的轴来转然后整体再挪一个位置结果会偏离正确的轨迹。我个人的记忆口诀是近的先做从右往左读矩阵连乘距离待变换点最近的那个矩阵最先作用。连续旋转时的顺序同样重要。R1·R2 表示先做 R2 再做 R1和 R2·R1 完全不是一回事。三维旋转不可交换绕 X 转 90 度再绕 Y 转 90 度和反过来做最终的姿态差得很远。这跟二维里转加转、平移加平移都能交换的直觉完全不同需要重新建立肌肉记忆。6. 代码落地Python 实现与数值稳定性6.1 从公式直译的一份实现把公式翻译成代码最重要的是结构清晰、边界情况明确。下面这份实现直接对应罗德里格斯公式的矩阵形式输入轴向量不要求预先单位化函数内部处理。import numpy as np def rodrigues(k, theta): 绕单位轴 k 旋转 theta 弧度返回 3x3 旋转矩阵 k np.asarray(k, dtypenp.float64) n np.linalg.norm(k) if n 1e-12: raise ValueError(旋转轴向量模长过小无法确定旋转平面) k k / n K np.array([[0.0, -k[2], k[1]], [k[2], 0.0, -k[0]], [-k[1], k[0], 0.0]]) c, s np.cos(theta), np.sin(theta) return np.eye(3) s * K (1.0 - c) * (K K)这段代码里有三个地方值得说。第一用 K K 而不是显式写出 kkᵀ - I好处是不用担心单位化遗漏带来的偏差因为 K 本身已经用了单位向量。第二dtype 显式指定 float64避免整数输入导致的截断。第三零轴抛异常而不是返回单位矩阵因为静默返回单位矩阵会让上游很难发现参数传错。6.2 单位化、零轴、零角度这些边界边界情况处理得好不好决定了这个函数能不能上生产。零轴必须报错。旋转轴是零向量时旋转平面压根不存在这时候返回任何结果都是错的宁可让程序崩在源头。接近零但非零的轴也要小心模长很小的轴归一化之后方向对噪声极度敏感最好在外层做一次参数有效性检查。零角度应该返回单位矩阵这一点罗德里格斯公式天然满足sinθ 0(1-cosθ) 0结果就是 I。不需要特判公式自己搞定。同理θ 2π 时结果也是 I符合转一整圈回到原地。接近 π 的角度是个麻烦点。当 θ 趋近 π 时sinθ 趋近于 0矩阵里的反对称部分快没了只剩下对称部分做矩阵到轴角的反解时会变得不唯一——因为绕 k 转 π 和绕 -k 转 π 结果相同。如果你需要从矩阵反解轴角这一点必须单独处理。6.3 与四元数、与现成库的交叉验证写完之后一定要和别的实现对比。最方便的是和四元数互转因为四元数路径独立于罗德里格斯公式两者如果一致说明实现基本可靠。def quat_to_matrix(q): w, x, y, z q / np.linalg.norm(q) return np.array([ [1 - 2*(y*y z*z), 2*(x*y - z*w), 2*(x*z y*w)], [2*(x*y z*w), 1 - 2*(x*x z*z), 2*(y*z - x*w)], [2*(x*z - y*w), 2*(y*z x*w), 1 - 2*(x*x y*y)] ]) k np.array([1.0, 1.0, 1.0]) theta np.pi / 2 R1 rodrigues(k, theta) half theta / 2 R2 quat_to_matrix(np.array([np.cos(half), *(np.sin(half) * k / np.linalg.norm(k))])) print(np.max(np.abs(R1 - R2))) # 实测输出在 1e-16 量级除了四元数还可以用现成的科学计算库做第三方校验。构造一个绕给定轴的旋转用它算一遍再和你的实现对比误差在 1e-12 以内就可以放心用。我习惯在单元测试里固定几组参数轴取 (1,0,0)、(0,1,0)、(0,0,1) 三个基准方向加上 (1,1,1) 和 (1,-2,3) 两个斜轴角度取 0、π/6、π/2、π、1.9π 这几个点。零角度和满圈是必测项很多 bug 就藏在这两个看似平凡的位置。7. 实际踩坑记录与排查速查表7.1 结果不对时按这个顺序查调试三维旋转问题的效率很大程度取决于有没有固定排查顺序。我的习惯是四步走从易到难。第一步查归一化。把轴向量取模打印出来确认是 1。这一步能解决大约一半的问题。第二步查矩阵行列式。对构造出的 R 求 det应该等于 1。如果等于 -1说明掺进了反射如果不等于 ±1说明公式项写错或者单位化漏了。第三步查不动点。把轴向量本身代进去结果应该原样返回。如果不返回说明平行分量的系数写错了通常是把 (1 - cosθ) 写成了 1。第四步查手性。取 v (1,0,0)、k (0,0,1)、θ 90°正确结果是 (0,1,0)。如果得到 (0,-1,0)说明叉乘矩阵的符号写反了或者旋转方向定义成了顺时针。这四步走完九成问题都能定位。7.2 常见问题与处理对照表现象可能原因定位方法处理方式结果整体被放大轴未单位化打印k绕原点乱转漏了平移矩阵检查轴是否过原点补 T(-P0) 和 T(P0)行列式为 -1反射项被误加算 det检查三处非对角符号旋转方向反了叉乘项符号错误用 Z 轴 90 度基准测试交换反对称部分符号角度接近 π 时结果跳变轴角反解不唯一观察 sinθ 趋零区间单独处理或改用四元数组合旋转结果不对乘法顺序写反交换两个矩阵看是否变化统一从右往左书写大量点云结果累积漂移反复乘矩阵累积误差比较单次与多次结果定期对旋转矩阵做正交化提示旋转矩阵用久了会缓慢偏离正交性尤其是做上千次连乘之后。长期运行的系统里建议每隔一段时间对矩阵做一次正交化处理比如用极分解或者简单的 Gram-Schmidt 修正别让误差无限累积。7.3 几条不太好查的经验还有一种坑不太容易通过公式检查发现就是坐标系的约定不一致。有些建模软件用 Z 轴向上有些用 Y 轴向上仿真环境和渲染环境有时也对不上。这种情况下公式本身没错但整个场景的方向感是歪的。排查方法很笨但有效在场景里放一个明显不对称的物体绕世界 Z 轴转 90 度看它往哪边倒就能判断出当前用的是什么约定。另外打印中间变量时别只打印矩阵的迹或者行列式那种信息量太低。直接把整个矩阵按行打印出来对照分量展开式一项一项核对虽然土但定位准确。我遇到过一次非常隐蔽的 bug是有人把 kx 和 kz 在某一项里写反了迹和行列式都对因为那两个位置的对称性掩盖了错误只有把九个数全打出来才看出来。8. 最后写点自己的体会这套公式我前后重新推导过至少四遍每一遍都有新收获。第一次是照着课本抄第二次是自己从几何分解推第三次是为了给同事讲清楚所以画了图第四次是写代码时发现边界情况处理不完。每次重推最花时间的地方都不是公式本身而是符号约定——右手系还是左手系、列向量还是行向量、叉乘矩阵的第一行到底该放谁。这提醒我在三维旋转这个领域公式从来不是难点把约定写清楚、把校验做扎实才是。如果你正在做相关的项目我建议把罗德里格斯公式和七矩阵连乘两条路都实现一遍互为参照。再准备一组固定的测试用例包含三根坐标轴、一个斜轴、角度覆盖 0 到 π 的关键点每次改动代码后跑一遍。这套组合拳下来绝大多数错误在提交之前就会被拦下。至于四元数等这套矩阵路径彻底梳理清楚之后再去了解你会觉得它其实只是同一件事的另一种写法而不是什么全新的东西。