1. 先把问题说清楚绕轴旋转到底在转什么三维向量绕任意轴旋转这句话里其实藏了三个容易混淆的概念很多人在项目里翻车不是因为公式记不住而是从一开始就没分清自己到底要转东西还是转视角。第一个是主动旋转把一个空间中的向量比如机械臂末端的速度矢量、粒子系统里的粒子位置绕某条轴转一个角度得到它的新坐标。第二个是被动旋转物体不动但坐标系转了同一个向量在新坐标系下的坐标变了。第三个是刚体的姿态变换不是转一个点而是把一整套机架、模型、局部坐标系整体转过去。这三种在数学上都能写成矩阵乘向量的形式但矩阵该取转置还是原样、该左乘还是右乘差别巨大。我做三维相关的东西有些年头了从早期的桌面端 CAD 插件、Unity 里的自定义物理到后来做机器人正逆运动学和点云配准绕任意轴旋转这个需求几乎每隔几个月就会冒出来一次。每次重新推一遍公式其实花不了十分钟真正耗时间的是排查方向和顺序——明明公式抄得一字不差结果模型转反了、转了 180 度、或者绕的轴不是我想的那条。所以这篇我不打算只丢一个公式给你而是把这个公式为什么长这样矩阵里九个元素分别代表什么七矩阵连乘是怎么来的怎么用四个不变量快速验证自己算对没算对全部铺开讲一遍。适合谁看如果你正在做 3D 可视化、游戏开发、机器人运动学、点云处理、计算机图形学作业或者只是想让一个向量绕着某条斜着的轴线精确转个角度这篇都能直接用。数学基础方面只要你记得矩阵乘法和向量点乘叉乘剩下的推导我会尽量用几何直觉讲不玩抽象代数那套。注意全文的绕轴旋转默认指右手坐标系下的逆时针为正从轴的正方向朝原点看。如果你的项目是左手系或者顺时针为正所有公式的 sin 项符号都要翻这一点后面会专门说。1.1 主动旋转和被动旋转同一个矩阵的两种解释先说一个我在代码评审里见过无数次的 bug题目要求把坐标系绕 z 轴转 30 度求点在新坐标系下的坐标有人直接把旋转矩阵乘到点上去了。结果当然不对因为转坐标系和转向量在数值上正好互为逆变换。设旋转矩阵为 $R$那么把向量绕轴主动转 $\theta$新向量是 $v Rv$而把坐标系绕同一条轴转 $\theta$同一个物理向量在新坐标系下的坐标是 $v R^T v$。因为正交矩阵的逆等于转置所以这两件事的矩阵恰好互为转置。从几何上理解这件事其实很朴素。你把头往左偏 30 度看到的世界整体往右转了 30 度但世界本身没动。主动旋转是世界动我的坐标系不动被动旋转是世界不动我的坐标系动。两者描述的是同一组相对关系只是站在谁的角度说话的区别。代码里如果发现结果角度对但方向反了八成就是把这两者搞混了先去检查是不是该转置。我习惯在写任何旋转相关代码之前先在手边写在注释里的一句话这次是转物还是转系。这句话能省掉后面半小时的调试。尤其是做坐标变换链的时候比如激光雷达坐标系到车体坐标系、车体到世界坐标系每一步到底是主动还是被动如果不标清楚最后误差会叠加得非常离谱。1.2 绕任意轴为什么不能直接查表绕 x、y、z 三个坐标轴旋转的矩阵是教科书标配闭着眼都能写出来。但一旦轴变成 $(1, 1, 1)$ 这种斜着的方向问题就来了绕坐标轴的旋转矩阵之间不能简单相乘得到绕任意轴的旋转。有人会想那我把绕 x 转、绕 y 转、绕 z 转按某种顺序乘起来不就行了可以但那是欧拉角它有三个致命问题顺序依赖xyz 和 zyx 结果不同、万向节死锁某些姿态下丢失一个自由度、以及插值不平滑。最关键的是欧拉角描述的是分三步转而绕任意轴旋转是一步转到位它们的中间状态完全不同。另外还有个更隐蔽的坑如果轴本身也是旋转后变的比如局部坐标系下的轴那么每次旋转后轴的方向都会变累乘就会漂移。所以绕任意轴的旋转标准的做法不是拆成三次坐标轴旋转而是走罗德里格斯公式或者四元数这条路。前者是纯向量形式几何意义清楚适合手推和理解后者是工程上最常用的因为只有四个数、插值方便、不累乘漂移。这篇主要讲罗德里格斯公式和它对应的矩阵形式四元数只在关系对照时提一下。2. 罗德里格斯公式的几何推导罗德里格斯公式我见过很多种写法但万变不离其宗。与其背它不如花五分钟推一遍推完你会发现这公式简直是不得不长这样。假设轴是单位向量 $\hat{k}$旋转角是 $\theta$待旋转向量是 $v$。核心思路就一句话把 $v$ 拆成平行于轴和垂直于轴的两部分平行部分转不动垂直部分在垂直于轴的平面里老老实实转圈。2.1 把向量拆成沿轴和垂直轴两块先做垂直分解。平行于轴的分量是 $v_\parallel (\hat{k} \cdot v)\hat{k}$这部分无论怎么绕轴转都不变因为它就躺在轴上。剩下的垂直分量是 $v_\perp v - (\hat{k} \cdot v)\hat{k}$它垂直于轴会随着旋转在垂直于轴的平面内画圆。数学上这叫正交投影分解你把 $v$ 看成一根斜插在旋转轴旁边的筷子旋转时只有它绕轴的甩出去的那部分在动贴着轴的那部分是稳定的。这一步看着简单但它解释了为什么公式里会出现 $(\hat{k} \cdot v)\hat{k}(1-\cos\theta)$ 这一项。垂直分量旋转之后原来的平行分量不变而垂直分量的损失和补偿通过 $(1-\cos\theta)$ 这个系数体现出来。很多人记公式时对 $(1-\cos\theta)$ 百思不得其解其实它就是垂直分量在旋转前后投影到轴方向上的差异累积。2.2 垂直分量在轴垂直平面内转圈现在处理垂直分量 $v_\perp$。在这个垂直于轴的平面里我们需要两个互相垂直的单位基一个是 $v_\perp$ 自己归一化后的方向另一个是 $\hat{k} \times v_\perp$ 的方向注意叉乘得到的是垂直于两者、也就垂直于轴的新向量。这两个向量构成了平面内的正交基而且长度相同都是 $|v_\perp|$。旋转就是在这两个基张成的平面里做一个二维旋转$$v_\perp v_\perp \cos\theta (\hat{k} \times v_\perp)\sin\theta$$这里 $\hat{k} \times v_\perp$ 就是那个提前 90 度的方向用二维旋转矩阵的 $(x\cos\theta - y\sin\theta, x\sin\theta y\cos\theta)$ 对照一下就能看出来它扮演的是 $-y$ 对应的那个角色。注意叉乘方向与右手定则一致这也是为什么正角度逆时针——因为 $\hat{k} \times v_\perp$ 的方向就是逆时针方向。2.3 合起来就是罗德里格斯公式把平行分量和旋转后的垂直分量相加得到完整公式$$v v_\parallel v_\perp (\hat{k} \cdot v)\hat{k} [v - (\hat{k} \cdot v)\hat{k}]\cos\theta (\hat{k} \times v)\sin\theta$$整理一下$$v v\cos\theta (\hat{k} \times v)\sin\theta \hat{k}(\hat{k} \cdot v)(1-\cos\theta)$$这就是罗德里格斯公式最常见的向量形式。三项分别对应原有分量按 $\cos\theta$ 缩放、叉乘项提供旋转方向、轴方向补偿项保证平行分量不丢。实际写代码时用这个向量形式最直观一次点乘、一次叉乘就能算出来比构造矩阵再乘还快一点。实操心得$\hat{k}$ 必须是单位向量。我见过有人拿未归一化的轴去套公式结果旋转角度算出来永远偏小或者偏大还以为是角度单位搞错了。归一化这一步一定不要省哪怕你觉得我这个轴看起来就是单位长度。3. 从向量公式到 3×3 旋转矩阵向量形式适合算单个向量但工程里我们经常需要把旋转和缩放、平移组合成变换矩阵或者把一整批点一次性转过去。这时候就得把罗德里格斯公式写成矩阵形式。好消息是它有一个非常漂亮的闭式表达$$R I\cos\theta [\hat{k}]_\times \sin\theta \hat{k}\hat{k}^T(1-\cos\theta)$$其中 $I$ 是 3×3 单位阵$[\hat{k}]_\times$ 是轴的反对称矩阵$\hat{k}\hat{k}^T$ 是轴的外积矩阵。这个式子我在很多场合推荐给同事因为它把旋转矩阵 轴方向 角度这个关系表达得极其清晰。3.1 反对称矩阵叉乘的矩阵化身叉乘 $\hat{k} \times v$ 可以写成矩阵乘向量的形式 $[\hat{k}]_\times v$这个矩阵就是反对称矩阵$$[\hat{k}]_\times \begin{bmatrix} 0 -k_z k_y \ k_z 0 -k_x \ -k_y k_x 0 \end{bmatrix}$$它满足 $[\hat{k}]\times^T -[\hat{k}]\times$对角线全零这也是反对称名字的由来。记住它的规律很简单每一行都是轴的分量错位并带符号第一行是 $(-k_z, k_y)$第二行是 $(k_z, -k_x)$第三行是 $(-k_y, k_x)$。这个矩阵在旋转、角速度、刚体动力学里到处都是值得记牢。外积矩阵 $\hat{k}\hat{k}^T$ 则是$$\hat{k}\hat{k}^T \begin{bmatrix} k_x^2 k_xk_y k_xk_z \ k_xk_y k_y^2 k_yk_z \ k_xk_z k_yk_z k_z^2 \end{bmatrix}$$它和反对称矩阵配合正好把罗德里格斯公式的三项拼成完整的旋转矩阵。这个分解思路非常通用后面讲七矩阵连乘时还会再见到它。3.2 矩阵九个元素逐项展开把上面三个矩阵代入并整理得到大家最常抄的那张表$$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\theta$$s \sin\theta$。这张表我建议你至少亲手推一次因为元素符号写错一个整个旋转就会变成镜面反射或者方向反了而且还不报错特别阴险。对角线上的规律是$c$ 加上对应轴分量的平方乘 $(1-c)$非对角线是两项乘积乘 $(1-c)$ 再加减一个带 $s$ 的项注意 $R_{12}$ 和 $R_{21}$ 里 $s$ 的符号相反这正是反对称部分的贡献。3.3 用 Python 把它写出来纸上推完代码直接抄下面这份就行。我特意没用四元数库纯 NumPy 手写方便你对着公式逐行核对import numpy as np def rotation_matrix_axis_angle(axis, angle_rad): 绕任意轴旋转的 3x3 旋转矩阵右手系逆时针为正 axis: 长度 3 的序列可以是未归一化的轴 angle_rad: 旋转角弧度制 k np.asarray(axis, dtypefloat) norm np.linalg.norm(k) if norm 1e-12: raise ValueError(旋转轴不能是零向量) k k / norm # 归一化这一步绝不能省 c np.cos(angle_rad) s np.sin(angle_rad) K np.array([ [0.0, -k[2], k[1]], [k[2], 0.0, -k[0]], [-k[1], k[0], 0.0] ]) I np.eye(3) R I * c K * s np.outer(k, k) * (1 - c) return R # 例子绕 (1,1,1) 轴转 45 度 R rotation_matrix_axis_angle([1, 1, 1], np.deg2rad(45)) print(np.round(R, 4))注意np.outer(k, k)就是外积矩阵 $\hat{k}\hat{k}^T$比手写九宫格更不容易错。另外矩阵形式的公式和向量形式在数学上完全等价但在批量处理点云时矩阵形式能把 N 个点拼成矩阵一次性乘完效率高得多。4. 七矩阵连乘绕空间中任意一条直线旋转到目前为止我们讲的旋转轴都经过原点。但现实中更常见的情况是轴穿过空间中某个点 $p$方向是 $\hat{k}$——比如绕着门轴转门板门轴不经过坐标原点或者绕机械臂的某个关节轴线旋转关节位置在空间里是任意的。七矩阵连乘这个说法正是处理这种情况时自然长出来的结构。4.1 为什么偏偏是七个矩阵思路很直接先把这条任意直线搬到过原点的 z 轴上转完再搬回去。具体拆解成五步平移到轴经过原点$T(-p)$一个矩阵。把轴方向 $\hat{k}$ 对齐到 z 轴需要两步旋转$R_z(-\alpha) R_y(-\beta)$两个矩阵。绕 z 轴旋转目标角度$R_z(\theta)$一个矩阵。把轴方向转回原来的方向$R_y(\beta) R_z(\alpha)$两个矩阵。平移回原位置$T(p)$一个矩阵。数一数1 2 1 2 1 7。这就是七矩阵连乘的来历。写成表达式$$M T(p), R_z(\alpha), R_y(\beta), R_z(\theta), R_y(-\beta), R_z(-\alpha), T(-p)$$作用在点上时从右往左读先平移、再对齐、再旋转、再还原、再平移回来。我第一次见到这个结构的时候觉得它啰嗦后来发现它极其稳健因为每一步都是绕坐标轴的标准旋转和平移不涉及任何奇异情况调试时每一步都能单独打印出来看。4.2 轴对齐的两步旋转角怎么求关键在 $\alpha$ 和 $\beta$ 怎么算。设归一化轴 $\hat{k} (k_x, k_y, k_z)$把它写成球坐标形式$$k_x \sin\beta\cos\alpha,\quad k_y \sin\beta\sin\alpha,\quad k_z \cos\beta$$反解得到$$\alpha \operatorname{atan2}(k_y, k_x),\qquad \beta \operatorname{atan2}!\left(\sqrt{k_x^2 k_y^2},; k_z\right)$$这里 $\beta \in [0, \pi]$对应极角。强烈建议用atan2而不是acos或asin因为 $\sqrt{k_x^2k_y^2}$ 在轴接近 z 轴时会趋近于零用acos(k_z)在数值上容易在 $k_z \to \pm 1$ 时出问题而atan2自带符号处理稳健得多。$\alpha$ 的范围是 $(-\pi, \pi]$两步配合就能把任意单位轴精确对齐到 z。实操心得如果 $\hat{k}$ 恰好平行于 z 轴$k_x k_y 0$那么 $\alpha$ 和 $\beta$ 都退化为 0对齐矩阵变成单位阵七矩阵自然简化成三个平移、绕 z 转、平移回来公式不会崩。很多人担心退化其实这套分解在退化点反而是最稳的因为它不依赖任何除法。4.3 完整七矩阵展开与顺序检查把七项乘起来其实数学上和前面的罗德里格斯矩阵完全等价区别只是表达路径。我平时分工是这样推导和理解用罗德里格斯实现和调试用七矩阵分解。因为一旦方向不对我可以把七步拆开来看看是平移写反了、对齐角度差了 90 度、还是具备旋转方向搞反了比盯着一张九宫格矩阵快得多。需要注意的坑是平移矩阵 $T$ 和旋转矩阵 $R$ 的乘法顺序。在列向量约定下$p Mp$变换链从右往左生效$T(-p)$ 必须放在最右边最先作用。如果你用的是行向量约定$p pM$整个链条要转置并反转顺序写成 $M T(-p)^T R_z(-\alpha)^T \cdots$ 那种形式。千万不要在同一个项目里混用两种约定这是三维编程里最容易埋雷的地方。5. 坐标系绕任意轴旋转换个视角的等价写法前面讲的都是点/向量绕着固定轴转对应热搜里的另一个词坐标系绕任意轴旋转说的是另一件事轴和原点都不动把整个坐标系转过去然后问某个固定向量在新坐标系里的坐标。这在做传感器标定、多坐标系对齐时天天遇到。5.1 主动和被动的关系$R MRM^T$设原坐标系下某个旋转用矩阵 $R$ 描述现在整个坐标系基被旋转矩阵 $M$ 变换到新姿态那么同一个物理旋转在新坐标系下的描述是$$R M, R, M^T$$这叫相似变换。正交矩阵的逆等于转置所以 $M^T$ 出现的位置就是 $M^{-1}$。几何意义先在当前坐标系里用 $R$ 转然后把结果翻译到新坐标系去看。很多人在做机器人手眼标定时卡在这一步本质上就是漏了共轭那一下。5.2 用基变换的视角理解换个更接地气的类比。你用尺子量桌子长度尺子的刻度是每格 1 厘米。如果换一把刻度是每格 2 厘米的尺子同一个桌子读数就减半。这里的尺子就是坐标系减半就是 $M$ 的作用。坐标系绕轴旋转等价于把整个测量基旋转了所以同一个向量的坐标描述要相应地逆变换回去。理解了这个类比$R MRM^T$ 和向量被动旋转 $v R^T v$ 就是同一个道理的两面。5.3 三种视角的对照场景变换对象公式适用场合向量主动旋转向量本身$v Rv$粒子运动、速度方向、动画向量被动旋转坐标系$v R^T v$传感器读数换基、姿态解算坐标系旋转后描述同一变换变换矩阵$R MRM^T$标定、多坐标系链接、SLAM绕过点直线的旋转点/刚体$T(p)RT(-p)$门轴、关节轴、绕梁翻转这张表我建议贴在显示器边上。每次写旋转代码之前对一眼能避免 80% 的方向性错误。6. 实操验证怎么确定自己算对了旋转矩阵最讨厌的地方在于它是连续量错了不一定崩可能只是差一点点。所以验证必须有套路不能靠肉眼。我总结了四个不变量检查不管用什么方法算旋转跑完这四个基本能筛掉绝大多数错误。6.1 四个金标准不变量第一正交性$R^TR$ 必须等于单位阵误差在 1e-10 量级。旋转不改变长度也不改变夹角所以正交是必要条件。第二行列式为 1$\det(R) 1$。如果算出来是 -1说明你不小心构造了个镜面反射通常是符号错了或者轴没归一化。第三轴向量是不动点$R\hat{k} \hat{k}$旋转轴自己绕自己转永远是它自己这个检查能立刻抓出绕错轴的 bug。第四旋转角迹 $tr(R) 1 2\cos\theta$反推出来的角度应该等于你设定的角度。import numpy as np def check_rotation(R, axis, angle, tol1e-8): R np.asarray(R, dtypefloat) k np.asarray(axis, dtypefloat) k k / np.linalg.norm(k) # 1. 正交性 assert np.allclose(R.T R, np.eye(3), atol1e-10), 不正交 # 2. 行列式 assert np.isclose(np.linalg.det(R), 1.0, atol1e-10), 行列式不为 1 # 3. 轴是不动点 assert np.allclose(R k, k, atoltol), 轴不是不动点 # 4. 迹反推角度 theta np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1)) assert np.isclose(theta, angle, atol1e-8), 角度不符 print(全部检查通过)6.2 手算一个例子练手光看公式容易飘拿一个具体的例子算一遍。取轴 $\hat{k} (0, 0, 1)$旋转角 $\theta 90°$。代入九宫格$c 0$$s 1$$k_x k_y 0$$k_z 1$。逐项算$R_{11} 0 0 0$$R_{12} 0 - 1\cdot 1 -1$$R_{13} 0$ $R_{21} 0 1 1$$R_{22} 0 0 0$$R_{23} 0$ $R_{31} 0$$R_{32} 0$$R_{33} 0 1 1$。得到 $\begin{bmatrix}0-10\100\001\end{bmatrix}$这正好是标准的绕 z 轴 90 度矩阵说明公式退化正确。再取 $v (1,0,0)$ 代入$Rv (0,1,0)$即 x 轴转到了 y 轴逆时针 90 度符合右手定则。这个手算步骤我建议每个第一次用这个公式的人至少做一遍做完对符号的敏感度会提高一个档次。6.3 用 NumPy 做交叉验证工程上最省事的验证方式是用两套独立方法算一遍对拍。比如手写的罗德里格斯矩阵和 SciPy 的Rotation.from_rotvec对比或者和四元数转矩阵的结果对比。两套方法都是独立推导的如果结果一致到 1e-12基本就能放心用了。对拍时注意 SciPy 的from_rotvec用的旋转向量是轴乘角度方向约定也是右手系直接把 $\hat{k}\theta$ 传进去即可。7. 常见坑与速查表这一节是我这些年踩过的坑里最值得说的部分基本都是那种原理都对但结果就是不对的疑难杂症。7.1 方向反了左乘右乘与转置旋转结果反向九成出在三个地方。第一角度单位C/C 的三角函数吃弧度Python 的math.sin也吃弧度但有些图形库的接口吃角度混用就差了 57 倍。第二转置问题主动旋转写成被动旋转的矩阵结果正好反向。第三左乘右乘$AB$ 和 $BA$ 在三维旋转里一般不相等多个变换叠加时顺序错了会让整体姿态歪掉。排查技巧是把角度设成一个很小的值比如 0.01 弧度小角度下旋转矩阵近似为 $I \theta[\hat{k}]_\times$方向对不对一眼就能看出来比大角度直观得多。7.2 归一化与数值漂移轴的长度只要不是 1整个旋转的尺度就变了。更隐蔽的是累乘漂移如果你把旋转矩阵反复相乘比如在一个循环里每帧乘一次增量旋转浮点误差会慢慢累积几百次之后矩阵就不再严格正交了表现是模型被缓慢拉升或压扁。解决办法有两个一是每几十次做一次正交化对矩阵做 SVD 或极分解把奇异值全设为 1二是干脆改用四元数每次乘完归一化一下只有四个数漂移慢得多维护成本低。7.3 问题速查表现象最可能原因快速定位方法旋转方向整体反了主动/被动搞混矩阵该转置没转置检查是否用了 $R^T$角度明显偏小/偏大轴未归一化打印norm(axis)看是不是 1只转了一点点就停角度单位用了角度制检查 deg2rad 有没有加轴接近 z 轴时结果跳变用acos算极角遇到数值不稳定改用atan2长时间运行模型被拉长矩阵累乘漂移周期性正交化或改四元数绕任意直线的旋转位置不对平移矩阵顺序放反核对 $T(p)RT(-p)$ 的左右位置实操心得真到了调试现场别急着改公式先打印三个东西——轴的模长、角度、旋转前后的向量模长。模长前后必须相等旋转是保长的如果不等问题一定出在归一化或者尺度上跟方向无关。这个检查我几乎每次都用能瞬间把问题范围砍一半。8. 我个人的一点收尾经验这些年用下来我对这个公式最大的体会是别把它当成一个要背的公式把它当成一条流水线。轴归一化、分解、旋转、验证四步里任何一步偷懒后面都要加倍还回来。我现在的习惯是写一个通用函数把罗德里格斯矩阵、七矩阵分解、四元数三条路都实现一遍遇到问题换个方法对拍比死磕一个实现快得多。另外就是前面反复强调的那句话——动手前先想清楚转物还是转系这一句话能省掉我大概三成的调试时间。至于四元数等你把罗德里格斯的几何意义吃透了再上会发现它其实就是同一个旋转的另一种坐标表示$q (\cos\frac{\theta}{2}, \hat{k}\sin\frac{\theta}{2})$一点都不神秘。
三维绕任意轴旋转：罗德里格斯公式与七矩阵连乘实战
1. 先把问题说清楚绕轴旋转到底在转什么三维向量绕任意轴旋转这句话里其实藏了三个容易混淆的概念很多人在项目里翻车不是因为公式记不住而是从一开始就没分清自己到底要转东西还是转视角。第一个是主动旋转把一个空间中的向量比如机械臂末端的速度矢量、粒子系统里的粒子位置绕某条轴转一个角度得到它的新坐标。第二个是被动旋转物体不动但坐标系转了同一个向量在新坐标系下的坐标变了。第三个是刚体的姿态变换不是转一个点而是把一整套机架、模型、局部坐标系整体转过去。这三种在数学上都能写成矩阵乘向量的形式但矩阵该取转置还是原样、该左乘还是右乘差别巨大。我做三维相关的东西有些年头了从早期的桌面端 CAD 插件、Unity 里的自定义物理到后来做机器人正逆运动学和点云配准绕任意轴旋转这个需求几乎每隔几个月就会冒出来一次。每次重新推一遍公式其实花不了十分钟真正耗时间的是排查方向和顺序——明明公式抄得一字不差结果模型转反了、转了 180 度、或者绕的轴不是我想的那条。所以这篇我不打算只丢一个公式给你而是把这个公式为什么长这样矩阵里九个元素分别代表什么七矩阵连乘是怎么来的怎么用四个不变量快速验证自己算对没算对全部铺开讲一遍。适合谁看如果你正在做 3D 可视化、游戏开发、机器人运动学、点云处理、计算机图形学作业或者只是想让一个向量绕着某条斜着的轴线精确转个角度这篇都能直接用。数学基础方面只要你记得矩阵乘法和向量点乘叉乘剩下的推导我会尽量用几何直觉讲不玩抽象代数那套。注意全文的绕轴旋转默认指右手坐标系下的逆时针为正从轴的正方向朝原点看。如果你的项目是左手系或者顺时针为正所有公式的 sin 项符号都要翻这一点后面会专门说。1.1 主动旋转和被动旋转同一个矩阵的两种解释先说一个我在代码评审里见过无数次的 bug题目要求把坐标系绕 z 轴转 30 度求点在新坐标系下的坐标有人直接把旋转矩阵乘到点上去了。结果当然不对因为转坐标系和转向量在数值上正好互为逆变换。设旋转矩阵为 $R$那么把向量绕轴主动转 $\theta$新向量是 $v Rv$而把坐标系绕同一条轴转 $\theta$同一个物理向量在新坐标系下的坐标是 $v R^T v$。因为正交矩阵的逆等于转置所以这两件事的矩阵恰好互为转置。从几何上理解这件事其实很朴素。你把头往左偏 30 度看到的世界整体往右转了 30 度但世界本身没动。主动旋转是世界动我的坐标系不动被动旋转是世界不动我的坐标系动。两者描述的是同一组相对关系只是站在谁的角度说话的区别。代码里如果发现结果角度对但方向反了八成就是把这两者搞混了先去检查是不是该转置。我习惯在写任何旋转相关代码之前先在手边写在注释里的一句话这次是转物还是转系。这句话能省掉后面半小时的调试。尤其是做坐标变换链的时候比如激光雷达坐标系到车体坐标系、车体到世界坐标系每一步到底是主动还是被动如果不标清楚最后误差会叠加得非常离谱。1.2 绕任意轴为什么不能直接查表绕 x、y、z 三个坐标轴旋转的矩阵是教科书标配闭着眼都能写出来。但一旦轴变成 $(1, 1, 1)$ 这种斜着的方向问题就来了绕坐标轴的旋转矩阵之间不能简单相乘得到绕任意轴的旋转。有人会想那我把绕 x 转、绕 y 转、绕 z 转按某种顺序乘起来不就行了可以但那是欧拉角它有三个致命问题顺序依赖xyz 和 zyx 结果不同、万向节死锁某些姿态下丢失一个自由度、以及插值不平滑。最关键的是欧拉角描述的是分三步转而绕任意轴旋转是一步转到位它们的中间状态完全不同。另外还有个更隐蔽的坑如果轴本身也是旋转后变的比如局部坐标系下的轴那么每次旋转后轴的方向都会变累乘就会漂移。所以绕任意轴的旋转标准的做法不是拆成三次坐标轴旋转而是走罗德里格斯公式或者四元数这条路。前者是纯向量形式几何意义清楚适合手推和理解后者是工程上最常用的因为只有四个数、插值方便、不累乘漂移。这篇主要讲罗德里格斯公式和它对应的矩阵形式四元数只在关系对照时提一下。2. 罗德里格斯公式的几何推导罗德里格斯公式我见过很多种写法但万变不离其宗。与其背它不如花五分钟推一遍推完你会发现这公式简直是不得不长这样。假设轴是单位向量 $\hat{k}$旋转角是 $\theta$待旋转向量是 $v$。核心思路就一句话把 $v$ 拆成平行于轴和垂直于轴的两部分平行部分转不动垂直部分在垂直于轴的平面里老老实实转圈。2.1 把向量拆成沿轴和垂直轴两块先做垂直分解。平行于轴的分量是 $v_\parallel (\hat{k} \cdot v)\hat{k}$这部分无论怎么绕轴转都不变因为它就躺在轴上。剩下的垂直分量是 $v_\perp v - (\hat{k} \cdot v)\hat{k}$它垂直于轴会随着旋转在垂直于轴的平面内画圆。数学上这叫正交投影分解你把 $v$ 看成一根斜插在旋转轴旁边的筷子旋转时只有它绕轴的甩出去的那部分在动贴着轴的那部分是稳定的。这一步看着简单但它解释了为什么公式里会出现 $(\hat{k} \cdot v)\hat{k}(1-\cos\theta)$ 这一项。垂直分量旋转之后原来的平行分量不变而垂直分量的损失和补偿通过 $(1-\cos\theta)$ 这个系数体现出来。很多人记公式时对 $(1-\cos\theta)$ 百思不得其解其实它就是垂直分量在旋转前后投影到轴方向上的差异累积。2.2 垂直分量在轴垂直平面内转圈现在处理垂直分量 $v_\perp$。在这个垂直于轴的平面里我们需要两个互相垂直的单位基一个是 $v_\perp$ 自己归一化后的方向另一个是 $\hat{k} \times v_\perp$ 的方向注意叉乘得到的是垂直于两者、也就垂直于轴的新向量。这两个向量构成了平面内的正交基而且长度相同都是 $|v_\perp|$。旋转就是在这两个基张成的平面里做一个二维旋转$$v_\perp v_\perp \cos\theta (\hat{k} \times v_\perp)\sin\theta$$这里 $\hat{k} \times v_\perp$ 就是那个提前 90 度的方向用二维旋转矩阵的 $(x\cos\theta - y\sin\theta, x\sin\theta y\cos\theta)$ 对照一下就能看出来它扮演的是 $-y$ 对应的那个角色。注意叉乘方向与右手定则一致这也是为什么正角度逆时针——因为 $\hat{k} \times v_\perp$ 的方向就是逆时针方向。2.3 合起来就是罗德里格斯公式把平行分量和旋转后的垂直分量相加得到完整公式$$v v_\parallel v_\perp (\hat{k} \cdot v)\hat{k} [v - (\hat{k} \cdot v)\hat{k}]\cos\theta (\hat{k} \times v)\sin\theta$$整理一下$$v v\cos\theta (\hat{k} \times v)\sin\theta \hat{k}(\hat{k} \cdot v)(1-\cos\theta)$$这就是罗德里格斯公式最常见的向量形式。三项分别对应原有分量按 $\cos\theta$ 缩放、叉乘项提供旋转方向、轴方向补偿项保证平行分量不丢。实际写代码时用这个向量形式最直观一次点乘、一次叉乘就能算出来比构造矩阵再乘还快一点。实操心得$\hat{k}$ 必须是单位向量。我见过有人拿未归一化的轴去套公式结果旋转角度算出来永远偏小或者偏大还以为是角度单位搞错了。归一化这一步一定不要省哪怕你觉得我这个轴看起来就是单位长度。3. 从向量公式到 3×3 旋转矩阵向量形式适合算单个向量但工程里我们经常需要把旋转和缩放、平移组合成变换矩阵或者把一整批点一次性转过去。这时候就得把罗德里格斯公式写成矩阵形式。好消息是它有一个非常漂亮的闭式表达$$R I\cos\theta [\hat{k}]_\times \sin\theta \hat{k}\hat{k}^T(1-\cos\theta)$$其中 $I$ 是 3×3 单位阵$[\hat{k}]_\times$ 是轴的反对称矩阵$\hat{k}\hat{k}^T$ 是轴的外积矩阵。这个式子我在很多场合推荐给同事因为它把旋转矩阵 轴方向 角度这个关系表达得极其清晰。3.1 反对称矩阵叉乘的矩阵化身叉乘 $\hat{k} \times v$ 可以写成矩阵乘向量的形式 $[\hat{k}]_\times v$这个矩阵就是反对称矩阵$$[\hat{k}]_\times \begin{bmatrix} 0 -k_z k_y \ k_z 0 -k_x \ -k_y k_x 0 \end{bmatrix}$$它满足 $[\hat{k}]\times^T -[\hat{k}]\times$对角线全零这也是反对称名字的由来。记住它的规律很简单每一行都是轴的分量错位并带符号第一行是 $(-k_z, k_y)$第二行是 $(k_z, -k_x)$第三行是 $(-k_y, k_x)$。这个矩阵在旋转、角速度、刚体动力学里到处都是值得记牢。外积矩阵 $\hat{k}\hat{k}^T$ 则是$$\hat{k}\hat{k}^T \begin{bmatrix} k_x^2 k_xk_y k_xk_z \ k_xk_y k_y^2 k_yk_z \ k_xk_z k_yk_z k_z^2 \end{bmatrix}$$它和反对称矩阵配合正好把罗德里格斯公式的三项拼成完整的旋转矩阵。这个分解思路非常通用后面讲七矩阵连乘时还会再见到它。3.2 矩阵九个元素逐项展开把上面三个矩阵代入并整理得到大家最常抄的那张表$$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\theta$$s \sin\theta$。这张表我建议你至少亲手推一次因为元素符号写错一个整个旋转就会变成镜面反射或者方向反了而且还不报错特别阴险。对角线上的规律是$c$ 加上对应轴分量的平方乘 $(1-c)$非对角线是两项乘积乘 $(1-c)$ 再加减一个带 $s$ 的项注意 $R_{12}$ 和 $R_{21}$ 里 $s$ 的符号相反这正是反对称部分的贡献。3.3 用 Python 把它写出来纸上推完代码直接抄下面这份就行。我特意没用四元数库纯 NumPy 手写方便你对着公式逐行核对import numpy as np def rotation_matrix_axis_angle(axis, angle_rad): 绕任意轴旋转的 3x3 旋转矩阵右手系逆时针为正 axis: 长度 3 的序列可以是未归一化的轴 angle_rad: 旋转角弧度制 k np.asarray(axis, dtypefloat) norm np.linalg.norm(k) if norm 1e-12: raise ValueError(旋转轴不能是零向量) k k / norm # 归一化这一步绝不能省 c np.cos(angle_rad) s np.sin(angle_rad) K np.array([ [0.0, -k[2], k[1]], [k[2], 0.0, -k[0]], [-k[1], k[0], 0.0] ]) I np.eye(3) R I * c K * s np.outer(k, k) * (1 - c) return R # 例子绕 (1,1,1) 轴转 45 度 R rotation_matrix_axis_angle([1, 1, 1], np.deg2rad(45)) print(np.round(R, 4))注意np.outer(k, k)就是外积矩阵 $\hat{k}\hat{k}^T$比手写九宫格更不容易错。另外矩阵形式的公式和向量形式在数学上完全等价但在批量处理点云时矩阵形式能把 N 个点拼成矩阵一次性乘完效率高得多。4. 七矩阵连乘绕空间中任意一条直线旋转到目前为止我们讲的旋转轴都经过原点。但现实中更常见的情况是轴穿过空间中某个点 $p$方向是 $\hat{k}$——比如绕着门轴转门板门轴不经过坐标原点或者绕机械臂的某个关节轴线旋转关节位置在空间里是任意的。七矩阵连乘这个说法正是处理这种情况时自然长出来的结构。4.1 为什么偏偏是七个矩阵思路很直接先把这条任意直线搬到过原点的 z 轴上转完再搬回去。具体拆解成五步平移到轴经过原点$T(-p)$一个矩阵。把轴方向 $\hat{k}$ 对齐到 z 轴需要两步旋转$R_z(-\alpha) R_y(-\beta)$两个矩阵。绕 z 轴旋转目标角度$R_z(\theta)$一个矩阵。把轴方向转回原来的方向$R_y(\beta) R_z(\alpha)$两个矩阵。平移回原位置$T(p)$一个矩阵。数一数1 2 1 2 1 7。这就是七矩阵连乘的来历。写成表达式$$M T(p), R_z(\alpha), R_y(\beta), R_z(\theta), R_y(-\beta), R_z(-\alpha), T(-p)$$作用在点上时从右往左读先平移、再对齐、再旋转、再还原、再平移回来。我第一次见到这个结构的时候觉得它啰嗦后来发现它极其稳健因为每一步都是绕坐标轴的标准旋转和平移不涉及任何奇异情况调试时每一步都能单独打印出来看。4.2 轴对齐的两步旋转角怎么求关键在 $\alpha$ 和 $\beta$ 怎么算。设归一化轴 $\hat{k} (k_x, k_y, k_z)$把它写成球坐标形式$$k_x \sin\beta\cos\alpha,\quad k_y \sin\beta\sin\alpha,\quad k_z \cos\beta$$反解得到$$\alpha \operatorname{atan2}(k_y, k_x),\qquad \beta \operatorname{atan2}!\left(\sqrt{k_x^2 k_y^2},; k_z\right)$$这里 $\beta \in [0, \pi]$对应极角。强烈建议用atan2而不是acos或asin因为 $\sqrt{k_x^2k_y^2}$ 在轴接近 z 轴时会趋近于零用acos(k_z)在数值上容易在 $k_z \to \pm 1$ 时出问题而atan2自带符号处理稳健得多。$\alpha$ 的范围是 $(-\pi, \pi]$两步配合就能把任意单位轴精确对齐到 z。实操心得如果 $\hat{k}$ 恰好平行于 z 轴$k_x k_y 0$那么 $\alpha$ 和 $\beta$ 都退化为 0对齐矩阵变成单位阵七矩阵自然简化成三个平移、绕 z 转、平移回来公式不会崩。很多人担心退化其实这套分解在退化点反而是最稳的因为它不依赖任何除法。4.3 完整七矩阵展开与顺序检查把七项乘起来其实数学上和前面的罗德里格斯矩阵完全等价区别只是表达路径。我平时分工是这样推导和理解用罗德里格斯实现和调试用七矩阵分解。因为一旦方向不对我可以把七步拆开来看看是平移写反了、对齐角度差了 90 度、还是具备旋转方向搞反了比盯着一张九宫格矩阵快得多。需要注意的坑是平移矩阵 $T$ 和旋转矩阵 $R$ 的乘法顺序。在列向量约定下$p Mp$变换链从右往左生效$T(-p)$ 必须放在最右边最先作用。如果你用的是行向量约定$p pM$整个链条要转置并反转顺序写成 $M T(-p)^T R_z(-\alpha)^T \cdots$ 那种形式。千万不要在同一个项目里混用两种约定这是三维编程里最容易埋雷的地方。5. 坐标系绕任意轴旋转换个视角的等价写法前面讲的都是点/向量绕着固定轴转对应热搜里的另一个词坐标系绕任意轴旋转说的是另一件事轴和原点都不动把整个坐标系转过去然后问某个固定向量在新坐标系里的坐标。这在做传感器标定、多坐标系对齐时天天遇到。5.1 主动和被动的关系$R MRM^T$设原坐标系下某个旋转用矩阵 $R$ 描述现在整个坐标系基被旋转矩阵 $M$ 变换到新姿态那么同一个物理旋转在新坐标系下的描述是$$R M, R, M^T$$这叫相似变换。正交矩阵的逆等于转置所以 $M^T$ 出现的位置就是 $M^{-1}$。几何意义先在当前坐标系里用 $R$ 转然后把结果翻译到新坐标系去看。很多人在做机器人手眼标定时卡在这一步本质上就是漏了共轭那一下。5.2 用基变换的视角理解换个更接地气的类比。你用尺子量桌子长度尺子的刻度是每格 1 厘米。如果换一把刻度是每格 2 厘米的尺子同一个桌子读数就减半。这里的尺子就是坐标系减半就是 $M$ 的作用。坐标系绕轴旋转等价于把整个测量基旋转了所以同一个向量的坐标描述要相应地逆变换回去。理解了这个类比$R MRM^T$ 和向量被动旋转 $v R^T v$ 就是同一个道理的两面。5.3 三种视角的对照场景变换对象公式适用场合向量主动旋转向量本身$v Rv$粒子运动、速度方向、动画向量被动旋转坐标系$v R^T v$传感器读数换基、姿态解算坐标系旋转后描述同一变换变换矩阵$R MRM^T$标定、多坐标系链接、SLAM绕过点直线的旋转点/刚体$T(p)RT(-p)$门轴、关节轴、绕梁翻转这张表我建议贴在显示器边上。每次写旋转代码之前对一眼能避免 80% 的方向性错误。6. 实操验证怎么确定自己算对了旋转矩阵最讨厌的地方在于它是连续量错了不一定崩可能只是差一点点。所以验证必须有套路不能靠肉眼。我总结了四个不变量检查不管用什么方法算旋转跑完这四个基本能筛掉绝大多数错误。6.1 四个金标准不变量第一正交性$R^TR$ 必须等于单位阵误差在 1e-10 量级。旋转不改变长度也不改变夹角所以正交是必要条件。第二行列式为 1$\det(R) 1$。如果算出来是 -1说明你不小心构造了个镜面反射通常是符号错了或者轴没归一化。第三轴向量是不动点$R\hat{k} \hat{k}$旋转轴自己绕自己转永远是它自己这个检查能立刻抓出绕错轴的 bug。第四旋转角迹 $tr(R) 1 2\cos\theta$反推出来的角度应该等于你设定的角度。import numpy as np def check_rotation(R, axis, angle, tol1e-8): R np.asarray(R, dtypefloat) k np.asarray(axis, dtypefloat) k k / np.linalg.norm(k) # 1. 正交性 assert np.allclose(R.T R, np.eye(3), atol1e-10), 不正交 # 2. 行列式 assert np.isclose(np.linalg.det(R), 1.0, atol1e-10), 行列式不为 1 # 3. 轴是不动点 assert np.allclose(R k, k, atoltol), 轴不是不动点 # 4. 迹反推角度 theta np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1)) assert np.isclose(theta, angle, atol1e-8), 角度不符 print(全部检查通过)6.2 手算一个例子练手光看公式容易飘拿一个具体的例子算一遍。取轴 $\hat{k} (0, 0, 1)$旋转角 $\theta 90°$。代入九宫格$c 0$$s 1$$k_x k_y 0$$k_z 1$。逐项算$R_{11} 0 0 0$$R_{12} 0 - 1\cdot 1 -1$$R_{13} 0$ $R_{21} 0 1 1$$R_{22} 0 0 0$$R_{23} 0$ $R_{31} 0$$R_{32} 0$$R_{33} 0 1 1$。得到 $\begin{bmatrix}0-10\100\001\end{bmatrix}$这正好是标准的绕 z 轴 90 度矩阵说明公式退化正确。再取 $v (1,0,0)$ 代入$Rv (0,1,0)$即 x 轴转到了 y 轴逆时针 90 度符合右手定则。这个手算步骤我建议每个第一次用这个公式的人至少做一遍做完对符号的敏感度会提高一个档次。6.3 用 NumPy 做交叉验证工程上最省事的验证方式是用两套独立方法算一遍对拍。比如手写的罗德里格斯矩阵和 SciPy 的Rotation.from_rotvec对比或者和四元数转矩阵的结果对比。两套方法都是独立推导的如果结果一致到 1e-12基本就能放心用了。对拍时注意 SciPy 的from_rotvec用的旋转向量是轴乘角度方向约定也是右手系直接把 $\hat{k}\theta$ 传进去即可。7. 常见坑与速查表这一节是我这些年踩过的坑里最值得说的部分基本都是那种原理都对但结果就是不对的疑难杂症。7.1 方向反了左乘右乘与转置旋转结果反向九成出在三个地方。第一角度单位C/C 的三角函数吃弧度Python 的math.sin也吃弧度但有些图形库的接口吃角度混用就差了 57 倍。第二转置问题主动旋转写成被动旋转的矩阵结果正好反向。第三左乘右乘$AB$ 和 $BA$ 在三维旋转里一般不相等多个变换叠加时顺序错了会让整体姿态歪掉。排查技巧是把角度设成一个很小的值比如 0.01 弧度小角度下旋转矩阵近似为 $I \theta[\hat{k}]_\times$方向对不对一眼就能看出来比大角度直观得多。7.2 归一化与数值漂移轴的长度只要不是 1整个旋转的尺度就变了。更隐蔽的是累乘漂移如果你把旋转矩阵反复相乘比如在一个循环里每帧乘一次增量旋转浮点误差会慢慢累积几百次之后矩阵就不再严格正交了表现是模型被缓慢拉升或压扁。解决办法有两个一是每几十次做一次正交化对矩阵做 SVD 或极分解把奇异值全设为 1二是干脆改用四元数每次乘完归一化一下只有四个数漂移慢得多维护成本低。7.3 问题速查表现象最可能原因快速定位方法旋转方向整体反了主动/被动搞混矩阵该转置没转置检查是否用了 $R^T$角度明显偏小/偏大轴未归一化打印norm(axis)看是不是 1只转了一点点就停角度单位用了角度制检查 deg2rad 有没有加轴接近 z 轴时结果跳变用acos算极角遇到数值不稳定改用atan2长时间运行模型被拉长矩阵累乘漂移周期性正交化或改四元数绕任意直线的旋转位置不对平移矩阵顺序放反核对 $T(p)RT(-p)$ 的左右位置实操心得真到了调试现场别急着改公式先打印三个东西——轴的模长、角度、旋转前后的向量模长。模长前后必须相等旋转是保长的如果不等问题一定出在归一化或者尺度上跟方向无关。这个检查我几乎每次都用能瞬间把问题范围砍一半。8. 我个人的一点收尾经验这些年用下来我对这个公式最大的体会是别把它当成一个要背的公式把它当成一条流水线。轴归一化、分解、旋转、验证四步里任何一步偷懒后面都要加倍还回来。我现在的习惯是写一个通用函数把罗德里格斯矩阵、七矩阵分解、四元数三条路都实现一遍遇到问题换个方法对拍比死磕一个实现快得多。另外就是前面反复强调的那句话——动手前先想清楚转物还是转系这一句话能省掉我大概三成的调试时间。至于四元数等你把罗德里格斯的几何意义吃透了再上会发现它其实就是同一个旋转的另一种坐标表示$q (\cos\frac{\theta}{2}, \hat{k}\sin\frac{\theta}{2})$一点都不神秘。
