1. 为什么旋转矩阵不是“背公式”就能搞定的数学工具

很多人第一次接触旋转矩阵，是在学机器人运动学、计算机图形学或者无人机姿态解算时。老师在黑板上写下那个3×3的矩阵，说“这是绕Z轴旋转θ角的旋转矩阵”，然后推导出cosθ和sinθ的位置——接着就是一连串习题：已知点P(1,2,3)，绕X轴转45°，求新坐标。做完题，交了作业，期末考完就忘。这不是你记性差，而是从一开始，你就被带偏了方向：旋转矩阵的本质，从来不是一组静态的代数表达式，而是一套描述“空间如何被刚性扭曲”的坐标变换协议。

我带过三届自动化专业的本科生课程设计，也给五家工业机器人公司做过运动控制模块的技术支持。最常听到的抱怨是：“欧拉角转旋转矩阵的公式表我抄了八遍，可一到实际调试机械臂末端位姿，还是对不准；用OpenCV做AR标定，明明按文档填了R，画面却总往左偏15度。”问题不出在计算错误，而出在——他们把旋转矩阵当成了“查表工具”，却没意识到：它背后绑定了三个不可妥协的前提：右手系约定、主动旋转定义、列向量默认表示法。任何一个前提在工程链路中被悄悄替换（比如Unity用左手系、ROS的tf2默认用四元数、MATLAB的rotm函数输入是角度但输出是double型矩阵），整个链条就会像齿轮错齿一样发出刺耳噪音。

更隐蔽的坑在于“13码旋转矩阵”这个热词。它根本不是标准术语，而是某次大厂校招笔试题里出现的编码陷阱：题目给出13个数字（如[0.866, -0.5, 0, 0.5, 0.866, 0, 0, 0, 1]），要求判断是否为合法旋转矩阵。很多学生直接套用“正交+行列式=1”的判据，却漏掉了数值精度导致的微小失范——实际工程中，一个0.9999999999999999被截断成1.0，可能让SVD分解后的重构矩阵产生0.002弧度的累积误差，这在精密装配场景下足以导致螺栓拧不进孔位。

所以这篇推导，不从“cosθ sinθ”开始，也不从“李群SO(3)”这种高维抽象切入。我们回到最原始的动作：用手转动一个纸盒，观察它的角点在房间坐标系里怎么移动。这个动作里藏着旋转矩阵全部的物理意义、数学约束和工程边界。接下来的每一步推导，都会对应一个真实可触摸的操作，每一个公式都会解释“为什么必须长成这样”，而不是“书上就这么写的”。

提示：本文所有推导均基于标准右手笛卡尔坐标系（X右、Y前、Z上），采用主动旋转（active rotation）定义——即向量本身在空间中转动，坐标系不动；所有向量默认为列向量，矩阵左乘作用于向量。这是绝大多数工程库（如NumPy、Eigen、SciPy）的默认约定。若你正在用Unity（左手系）或某些老版本MATLAB工具箱，请务必在最后一步手动检查坐标系转换。

2. 从纸盒转动开始：三维空间中一次最朴素的旋转操作

拿一个鞋盒，用胶带在六个面上分别贴上红、绿、蓝三色标签，对应X、Y、Z正方向。把它放在桌面上，定义桌面为世界坐标系{W}：原点O在盒底中心，X轴指向盒右侧面，Y轴指向盒正面，Z轴垂直向上穿过盒顶。现在，保持盒底紧贴桌面，用手指捏住盒体，让它绕Z轴（也就是垂直轴）顺时针转30度。注意观察：盒右侧面（红色标签）现在不再正对你的右手，而是偏转了一个角度；盒正面（绿色标签）也不再正对你，同样发生了偏移。

这个动作，就是绕固定轴的刚体旋转。它的数学本质，是把盒体自身携带的局部坐标系{B}（以盒中心为原点，三色标签为轴向）相对于世界坐标系{W}进行了一次定向重置。而我们要找的旋转矩阵R，就是这个重置关系的线性映射表达：它能把{B}中任意向量v_B，准确地变换成它在{W}中的坐标v_W。

关键来了：为什么这个映射一定是线性的？因为刚体旋转不改变向量长度、不改变向量间夹角、不改变平行关系——这些正是线性变换的核心特征。伸缩、剪切、投影会破坏其中至少一项，但纯旋转不会。所以R必然是一个3×3的方阵。

现在聚焦盒子的一个角点P。假设它在{B}中的坐标是(1,1,1)（单位：厘米）。转动后，它在{W}中的坐标是多少？我们可以不用计算器，只靠几何直觉来逼近：

• Z坐标不变：因为绕Z轴转，高度没变，仍是1；
• X和Y坐标变化：原来在右前方上方的点，顺时针转30°后，会向“右前之间”的方向滑动。具体来说，它在XY平面的投影（即点(1,1)）绕原点顺时针转30°。

这里就引出了二维旋转的基石。在XY平面内，一个点(x,y)绕原点顺时针转θ角，新坐标(x',y')满足： x' = x·cosθ + y·sinθ
y' = -x·sinθ + y·cosθ

等等——为什么是“+y·sinθ”而不是“-y·sinθ”？因为这是顺时针旋转。而教科书上经典的“绕Z轴逆时针旋转θ”的公式是： x' = x·cosθ - y·sinθ
y' = x·sinθ + y·cosθ

两者的区别，就在于旋转方向的定义。工程中绝大多数标准（如ROS、URDF、OpenGL）采用逆时针为正方向，这与数学上复平面旋转一致。所以，我们统一采用逆时针定义。那么刚才的鞋盒操作，实际是逆时针转-30°，或等价于逆时针转330°。但为简化，我们直接使用θ = 30°，并明确这是逆时针。

于是，点(1,1)逆时针转30°后： x' = 1·cos30° - 1·sin30° = (√3/2) - (1/2) ≈ 0.366
y' = 1·sin30° + 1·cos30° = (1/2) + (√3/2) ≈ 1.366
z' = 1

所以新坐标是(0.366, 1.366, 1)。这个结果，可以用一个矩阵乘法一次性完成：

[ x' ] [ cosθ -sinθ 0 ] [ x ] [ y' ] = [ sinθ cosθ 0 ] [ y ] [ z' ] [ 0 0 1 ] [ z ]

这就是绕Z轴旋转的旋转矩阵R_z(θ)。它不是凭空造出来的，而是由“XY平面内点的坐标变换规则”自然生长出来的。第一列[cosθ, sinθ, 0]^T，正是{B}的X轴单位向量(1,0,0)_B，在{W}中的坐标——因为{B}的X轴本身，就在绕Z轴转θ后，落到了(cosθ, sinθ, 0)这个方向上。同理，第二列[-sinθ, cosθ, 0]^T，是{B}的Y轴单位向量(0,1,0)_B在{W}中的坐标；第三列[0,0,1]^T，是{B}的Z轴单位向量(0,0,1)_B在{W}中的坐标——它没动，所以还是(0,0,1)。

注意：这个结论极其重要——旋转矩阵的每一列，都是局部坐标系{B}的对应坐标轴单位向量，在世界坐标系{W}中的表示。这是理解所有旋转矩阵构造的金钥匙。它意味着，只要你知道{B}的三个轴在{W}里指向哪，把这三个向量并排写成矩阵，你就得到了R。反之，如果你有一个R，它的三列直接告诉你：当前物体的“右”、“前”、“上”方向，在世界坐标系里分别是哪三个向量。

3. 三轴旋转矩阵的完整拼图：X、Y、Z轴各自独立推导

既然绕Z轴的矩阵是从二维投影自然导出的，那么绕X轴和绕Y轴的矩阵，就应该从另外两个正交平面的投影来推。我们继续用鞋盒演示。

3.1 绕X轴旋转：观察YZ平面的变形

保持鞋盒在桌面上，这次用手指捏住盒的左右两侧（X轴两端），让盒体绕X轴向前翻转（即Y轴向Z轴方向转动）。想象盒正面（绿色）原本朝前（+Y），翻转后开始抬头，指向斜上方。此时，X坐标不变（左右位置没动），而Y和Z坐标在YZ平面内发生二维旋转。

YZ平面内的二维旋转，与XY平面完全同构，只是变量名换了。一个点(y,z)绕Y轴（注意：这里是绕X轴，所以看的是YZ平面，旋转中心是X轴）逆时针转θ角，其新坐标(y',z')为： y' = y·cosθ - z·sinθ
z' = y·sinθ + z·cosθ

X坐标不变：x' = x。

因此，绕X轴旋转的矩阵R_x(θ)为：

[ x' ] [ 1 0 0 ] [ x ] [ y' ] = [ 0 cosθ -sinθ ] [ y ] [ z' ] [ 0 sinθ cosθ ] [ z ]

验证一下它的列向量含义：

• 第一列[1,0,0]^T：{B}的X轴单位向量，在{W}中仍是(1,0,0)，没动；
• 第二列[0,cosθ,sinθ]^T：{B}的Y轴单位向量(0,1,0)_B，绕X轴转θ后，落在了(0, cosθ, sinθ)方向——这正是它在YZ平面内逆时针转θ的结果；
• 第三列[0,-sinθ,cosθ]^T：{B}的Z轴单位向量(0,0,1)_B，绕X轴转θ后，落在了(0, -sinθ, cosθ)方向。

这个负号出现在第三列第二行，是绕X轴旋转的固有特征，源于右手螺旋定则：拇指指向+X，四指弯曲方向即为正旋转方向，它使得Z分量向-Y方向偏移。

3.2 绕Y轴旋转：观察XZ平面的变形

最后一次，捏住盒的上下两端（Z轴），让盒体绕Y轴向右倾斜（即X轴向Z轴方向转动）。盒右侧面（红色）原本朝右（+X），倾斜后开始向“右上之间”移动。此时，Y坐标不变，X和Z在XZ平面内旋转。

XZ平面内，点(x,z)绕Y轴（即看XZ平面）逆时针转θ角，新坐标(x',z')为： x' = x·cosθ + z·sinθ
z' = -x·sinθ + z·cosθ

注意这里的符号：因为Y轴是“朝向观察者”的轴（在标准右手系中，Y轴指向屏幕外），所以XZ平面的逆时针旋转，从我们的视角看，其实是顺时针的。但数学定义不依赖视角，只依赖坐标系。标准推导中，绕Y轴的旋转矩阵R_y(θ)为：

[ x' ] [ cosθ 0 sinθ ] [ x ] [ y' ] = [ 0 1 0 ] [ y ] [ z' ] [ -sinθ 0 cosθ ] [ z ]

验证列向量：

• 第一列[cosθ,0,-sinθ]^T：{B}的X轴(1,0,0)_B绕Y轴转θ后，落到(cosθ, 0, -sinθ)；
• 第二列[0,1,0]^T：{B}的Y轴没动；
• 第三列[sinθ,0,cosθ]^T：{B}的Z轴(0,0,1)_B绕Y轴转θ后，落到(sinθ, 0, cosθ)。

你会发现，R_y(θ)的结构和其他两个不同：非零元素不在主对角线上对称分布，而是呈现一种“十字形”。这是因为Y轴在标准右手系中是“奇数序号”轴（X=1,Y=2,Z=3），其旋转矩阵的符号模式遵循一个通用规则：对于绕第i轴旋转，矩阵中第i行第i列是cosθ，其余两个非i行非i列的对角线元素也是cosθ；而sinθ项出现在(i+1,i+2)和(i+2,i+1)位置，但其中一个带负号，具体取决于循环顺序（X→Y→Z→X）。

3.3 三轴矩阵的统一结构与记忆口诀

把三个矩阵并排写出来：

R_x(θ) = [1 0 0 ] R_y(θ) = [cosθ 0 sinθ ] R_z(θ) = [cosθ -sinθ 0 ] [0 cosθ -sinθ ] [ 0 1 0 ] [sinθ cosθ 0 ] [0 sinθ cosθ ] [-sinθ 0 cosθ ] [ 0 0 1 ]

它们共同构成旋转矩阵家族的“原子单元”。记住它们，不需要死记硬背，用一个生活化口诀：

“绕谁转，谁那行那列是cos；剩下两行两列，画个L形，横竖各一个sin，L的拐角处是负号。”

• 绕X转：X行X列是cosθ；剩下Y、Z行和Y、Z列，构成右下角2×2块，就是标准的二维旋转矩阵，左上为cos，右下为cos，右上为-sin，左下为+sin → 对应R_x的右下角。
• 绕Y转：Y行Y列是cosθ；剩下X、Z行和X、Z列，构成一个“横着的L”：X行Z列是+sinθ，Z行X列是-sinθ → 对应R_y中[0,2]和[2,0]位置。
• 绕Z转：Z行Z列是cosθ；剩下X、Y行和X、Y列，构成左上角2×2块，就是标准二维旋转矩阵 → 对应R_z的左上角。

这个口诀我在给产线工程师做培训时用了五年，反馈是“比背三角函数值还牢”。因为它把抽象矩阵，锚定在了你手指转动鞋盒的那个具身体验上。

4. 复合旋转的陷阱：顺序决定一切，乘法不可交换

现实中的物体，极少只绕单一轴旋转。无人机悬停时，机头可能先偏航（绕Z）、再俯仰（绕Y）、最后滚转（绕X）；机械臂末端执行器，需要先调整方位（绕Z），再伸展（绕Y），最后微调抓取角度（绕X）。这时，我们就需要把多个基本旋转矩阵按时间顺序串联起来。

假设一个物体，初始姿态与世界坐标系重合。它先绕Z轴转α角，得到中间姿态；再绕该中间姿态的Y轴（注意：是自身Y轴，不是世界Y轴）转β角；最后绕该新姿态的X轴转γ角。这一系列操作，称为ZYX欧拉角，是机器人学中最常用的约定之一。

它的总旋转矩阵R_total，并不是简单地把三个矩阵相加，也不是随意相乘。正确的计算顺序是：R_total = R_x(γ) × R_y(β) × R_z(α)

为什么是这个顺序？因为矩阵乘法是从右向左作用的。R_z(α)最先作用于向量v，将其转到第一个中间姿态；然后R_y(β)作用于这个已经转过的向量，再转一次；最后R_x(γ)作用于最终结果。所以，最“早”发生的旋转，要写在乘积的最右边。

关键警告：R_x(γ) × R_y(β) × R_z(α) ≠ R_z(α) × R_y(β) × R_x(γ)
矩阵乘法不满足交换律。这意味着，先偏航再俯仰，和先俯仰再偏航，得到的最终朝向是完全不同的。你可以用手机APP（如Sensor Kinetics）实时查看：把手机平放桌上（Z向上），先绕Z转90°（机头朝左），再绕Y转90°（机头朝上）；然后恢复初始，先绕Y转90°（机头朝上），再绕Z转90°（机头朝里）。两次结束姿态，天壤之别。

这个不可交换性，是复合旋转中最大的认知鸿沟。很多初学者以为“反正都是转，顺序无所谓”，直到在ROS的tf树里看到坐标系漂移，才明白问题所在。我曾帮一家AGV公司调试导航定位，他们的IMU数据融合脚本里，把欧拉角的旋转顺序写反了（用了XYZ而非ZYX），导致小车在直线行驶时，激光雷达点云持续向左偏移，偏差随距离线性累积。修复方法就是把矩阵乘法顺序从R_xR_yR_z改成R_zR_yR_x，一行代码，三天工时。

更复杂的情况是绕固定轴（世界坐标系）旋转vs绕动轴（自身坐标系）旋转。上面的ZYX是绕动轴：每一次旋转，都是绕物体当前自身的轴。而如果要求“先绕世界Z轴转α，再绕世界Y轴转β，最后绕世界X轴转γ”，那么顺序就反过来了：R_total = R_z(α) × R_y(β) × R_x(γ)。因为世界轴是固定的，第二次旋转时，物体虽然已经转过了，但你要绕的还是原来的世界Y轴，所以这个R_y(β)必须在R_z(α)之前应用，才能保证它作用在原始坐标系上。

如何区分？看需求描述里的关键词：

• “绕自身Y轴”、“绕机体Y轴”、“绕当前朝向的Y轴” → 动轴 → 顺序：后发生的旋转写在左边；
• “绕世界Y轴”、“绕全局Y轴”、“绕惯性系Y轴” → 固定轴 → 顺序：先发生的旋转写在左边。

这是一个必须刻在工程直觉里的条件反射。在写任何姿态控制代码前，我都会在纸上画出两次旋转的草图，标出每次旋转的轴是“动”还是“静”，再决定乘法顺序。这个习惯帮我避开了超过70%的姿态相关bug。

5. 旋转矩阵的四大铁律：正交性、行列式、逆与迹的物理意义

一个3×3矩阵R，要被称为“合法的旋转矩阵”，它必须同时满足四个数学约束。这些约束不是为了刁难人，而是对“刚体旋转”这一物理过程的精确数学刻画。违反其中任何一个，就意味着这个矩阵描述的不是一个真实的、无变形的旋转。

5.1 铁律一：正交性（Orthogonality）——R^T × R = I

这是最核心的约束。它意味着R的行向量和列向量，都必须是标准正交基：每个向量长度为1（单位向量），且两两互相垂直（点积为0）。

为什么？因为旋转不改变向量长度，也不改变向量间夹角。所以，{B}的三个轴（X_B, Y_B, Z_B），在{W}中表示出来，必须仍然是三个互相垂直、长度为1的向量。而一个矩阵的列向量构成标准正交基，其充要条件就是R^T × R = I。

验证R_z(θ)： R_z^T × R_z = [ cosθ sinθ 0 ] [ cosθ -sinθ 0 ] [ cos²θ+sin²θ -cosθsinθ+sinθcosθ 0 ] [1 0 0] [-sinθ cosθ 0 ] × [ sinθ cosθ 0 ] = [ -sinθcosθ+cosθsinθ sin²θ+cos²θ 0 ] = [0 1 0] [ 0 0 1 ] [ 0 0 1 ] [ 0 0 1 ] [0 0 1]

恒等于单位矩阵I。这就是正交性的威力：它自动保证了所有旋转操作的保距性和保角性。

实操心得：在代码中，当你从传感器（如IMU）获得一个旋转矩阵R，或者从优化算法（如ICP配准）输出一个R，第一步永远是检查正交性。用NumPy可以这样写：

import numpy as np def is_rotation_matrix(R): RtR = R.T @ R I = np.eye(3) return np.allclose(RtR, I, atol=1e-4) and np.isclose(np.linalg.det(R), 1.0, atol=1e-4)

atol=1e-4是关键——工程中没有绝对精确的1.0，浮点运算必然有微小误差。容忍10^-4量级的偏差，是稳健工程实践的标志。

5.2 铁律二：行列式为+1（det(R) = 1）——排除镜像翻转

正交矩阵的行列式只能是+1或-1。det(R) = +1 对应真旋转（proper rotation），即可以通过连续的、不撕裂物体的转动实现；det(R) = -1 对应瑕旋转（improper rotation），即包含一次镜像翻转（reflection），比如把左手手套变成右手手套。

旋转矩阵必须满足det(R) = +1，因为刚体旋转无法把左手系变成右手系。如果你的R满足正交性但det(R) = -1，那它很可能是一个“旋转+翻转”的混合体，这在机器人、图形学中是非法姿态。

计算R_z(θ)的行列式： det(R_z) = cosθ·cosθ - (-sinθ)·sinθ = cos²θ + sin²θ = 1。

5.3 铁律三：逆等于转置（R^{-1} = R^T）——旋转的可逆性

这是正交性的直接推论。它有着深刻的物理意义：对一个旋转执行其逆操作，等价于绕同一轴反向旋转相同角度。例如，R_z(30°)的逆，就是R_z(-30°)，而R_z(-30°) = R_z^T(30°)。

在工程中，这带来了巨大便利。当你需要把一个在{B}坐标系下的向量v_B，变换到{W}下，用R × v_B；而要把{W}下的向量v_W，变换回{B}下，你不需要重新计算R的逆矩阵（那需要O(n³)的计算量），直接用R^T × v_W即可。这在实时控制系统中，能节省大量CPU周期。

5.4 铁律四：迹（Trace）与旋转角的关系——tr(R) = 1 + 2cosθ

矩阵的迹（trace），即主对角线元素之和。对于任意旋转矩阵R，其迹与旋转角θ存在确定关系：tr(R) = R_{11} + R_{22} + R_{33} = 1 + 2cosθ

这个公式太有用了。它提供了一种从任意R中反解出旋转角θ的方法。例如，你从OpenCV的cv2.Rodrigues()函数得到一个R，想知道它代表多大的旋转，就计算它的迹，然后： θ = arccos((tr(R) - 1) / 2)

但要注意：arccos函数返回的是[0, π]范围内的值，而实际旋转角可能是-π到π。所以更稳健的做法是结合R的反对称部分（即R - R^T）来判断符号。

避坑经验：“13码旋转矩阵”题目的核心，就是考察你对这四大铁律的综合运用。给定13个数字，首先要检查它是不是3×3矩阵（13个数显然不是，所以“13码”大概率是干扰项，或是指13位浮点数精度下的存储格式）；其次，提取3×3子块，验证正交性和行列式；最后，计算迹，看是否在[-1, 3]范围内（因为cosθ ∈ [-1,1]，所以tr(R) ∈ [-1,3]）。一个合法的R，其迹绝不可能是4.2或-2.1。

6. 从旋转矩阵到欧拉角：解析解的唯一性危机与万向节死锁

旋转矩阵是描述姿态最无歧义的方式：一个3×3矩阵，9个数，但受4个铁律约束（3个正交性方程 + 1个行列式方程），自由度恰好是3，完美匹配三维空间的旋转自由度。而欧拉角（如ZYX）用三个角度（α, β, γ）来表示，看起来更简洁。但将R转换为欧拉角的过程，却暗藏玄机。

6.1 ZYX欧拉角的标准解析公式

给定一个旋转矩阵R，其元素记为r_ij（i行j列），则对应的ZYX欧拉角(ψ, θ, φ)（即偏航Yaw、俯仰Pitch、滚转Roll）可通过以下公式解出：

• θ = arcsin(-r_31)
• ψ = arctan2(r_21, r_11)
• φ = arctan2(r_32, r_33)

其中，arctan2(y,x)是四象限反正切函数，能根据x,y符号正确返回[-π, π]内的角度。

这个公式的推导，就是把R_z(ψ) × R_y(θ) × R_x(φ)的乘积展开，然后令其等于给定的R，逐个元素比对，解出ψ, θ, φ。过程繁琐但机械。

6.2 唯一性危机：一个R对应无穷多个欧拉角组合

问题来了：arcsin函数的值域是[-π/2, π/2]，所以θ被强制限制在这个区间。但物理上，俯仰角完全可以是-180°到+180°。例如，θ = 90°和θ = -90°，在arcsin中都映射到同一个值（因为sin(90°)=sin(-90°)=1），但它们对应的姿态完全不同：一个是机头垂直向上，一个是机头垂直向下。

更严重的是，当θ = ±90°时，会发生万向节死锁（Gimbal Lock）。此时，r_31 = ±1，导致r_11 = r_21 = r_32 = r_33 = 0。公式中的ψ和φ的计算会失效，因为arctan2(0,0)是未定义的。物理上，这意味着绕Z轴和绕X轴的旋转，此时产生了相同的效应——系统失去了一个自由度。一架飞机在垂直爬升（θ=90°）时，偏航和滚转无法独立控制，这就是死锁的直观体现。

所以，一个旋转矩阵R，理论上可以对应两组不同的ZYX欧拉角：(ψ, θ, φ) 和 (ψ±π, π-θ, φ±π)。选择哪一组，取决于你的应用需求和连续性要求。在飞行控制中，我们通常选择θ在(-90°, 90°)内，以避免死锁；而在动画系统中，可能会选择角度变化最平滑的那一组，以防止关节突兀跳变。

实操心得：在ROS的tf2库中，rotation_matrix_to_euler_angles()函数内部，就包含了对死锁情况的专门处理。它会检测|r_31|是否接近1（如>0.999），如果是，则将θ设为sign(r_31)*π/2，并将ψ设为0，φ则由其他元素计算。这个“就近取整”的策略，保证了在死锁边缘，角度值不会发散。我在移植一个无人机视觉定位模块时，就因为没处理死锁，导致飞机在接近垂直姿态时，ROS的/tf消息疯狂发布NaN值，最终触发了安全急停。教训是：任何涉及欧拉角的工程代码，死锁检测必须是第一道防线。

7. 工程落地：在Python、C++和嵌入式MCU上的实操要点

理论推导再完美，最终都要落到代码里跑通。不同平台对旋转矩阵的处理，侧重点截然不同。

7.1 Python生态：NumPy与SciPy的高效组合

Python是算法验证和快速原型的首选。核心是numpy的矩阵运算和scipy.spatial.transform.Rotation类。

import numpy as np from scipy.spatial.transform import Rotation as R # 创建绕Z轴30度的旋转矩阵 r_z = R.from_euler('z', 30, degrees=True).as_matrix() print(r_z) # 输出: [[ 0.866 -0.5 0. ] # [ 0.5 0.866 0. ] # [ 0. 0. 1. ]] # 将一个向量旋转 v = np.array([1, 0, 0]) v_rot = r_z @ v # @ 是矩阵乘法 print(v_rot) # [0.866 0.5 0.] # 从旋转矩阵反解欧拉角（自动处理死锁） euler = R.from_matrix(r_z).as_euler('zyx', degrees=True) print(euler) # [30. 0. 0.]

scipy的Rotation类封装了所有细节：它内部自动选择最优算法（如使用四元数避免死锁），并提供了.as_quat(),.as_rotvec()等多种表示法的无缝转换。强烈建议，不要自己手写rotation_matrix_to_euler函数，直接用scipy。它经过了数千次测试用例的锤炼，比99%的手写代码都可靠。

7.2 C++/Eigen：面向性能的零开销抽象

在机器人底层驱动或实时控制中，C++是主流。Eigen库是事实标准。

#include <Eigen/Dense> #include <iostream> int main() { // 创建绕Z轴30度的旋转矩阵 double theta = M_PI / 6.0; // 30 degrees in radians Eigen::Matrix3d R_z; R_z << std::cos(theta), -std::sin(theta), 0, std::sin(theta), std::cos(theta), 0, 0, 0, 1; // 旋转向量 Eigen::Vector3d v(1, 0, 0); Eigen::Vector3d v_rot = R_z * v; std::cout << "Rotated vector: " << v_rot.transpose() << std::endl; return 0; }

Eigen的优势在于编译期优化：Matrix3d是固定大小的栈分配对象，没有动态内存开销；*运算符被重载为高效的SIMD指令。在STM32H7这类高性能MCU上，Eigen也能跑得飞快。

7.3 嵌入式MCU（如STM32）：资源受限下的精打细算

在资源紧张的MCU上（如Cortex-M3，只有64KB Flash），引入大型库不现实。这时，你需要手写最简化的旋转函数，并做极致优化。

核心原则：

• 绝不使用sin/cos函数：它们是浮点运算大户，且需要链接math库。改用查表法（LUT）或CORDIC算法。
• 用定点数代替浮点数：例如，用int16_t表示-1.0到+1.0，分辨率为1/32768。
• 预计算常量：如果旋转角是固定的（如机械臂关节的零位偏移），就把cosθ和sinθ预先算好，存为const数组。

一个极简的绕Z轴定点旋转函数（Q15格式，15位小数）：

// Q15: value = integer * 2^(-15) typedef int16_t q15_t; void rotate_z_q15(q15_t* x, q15_t* y, q15_t cos_theta, q15_t sin_theta) { q31_t x_temp = (q31_t)(*x) * cos_theta - (q31_t)(*y) * sin_theta; q31_t y_temp = (q31_t)(*x) * sin_theta + (q31_t)(*y) * cos_theta; *x = (q15_t)(x_temp >> 15); // 右移15位，恢复Q15 *y = (q15_t)(y_temp >> 15); } // 调用示例：绕Z轴30度，cos30≈0.8660 -> 0x EDB8, sin30=0.5 -> 0x4000 q15_t x = 0x7FFF; // 1.0 in Q15 q15_t y