第 2 章旋转的表示与互转¶

朝向（orientation）是位姿里最易出错的一半：同一个旋转可以用旋转矩阵、欧拉角、轴角、单位四元数四种表示，各自参数量、奇异性与约定都不同，工程里还得在它们之间来回转换。本章给每种表示的数学定义与失效边界，并把它们统一落到 scipy.spatial.transform.Rotation（SciPy 的旋转 API），示例以它为准。几何镜头服务于正运动学与雅可比（第 3–4 章）；控制侧的姿态误差与 IMU 映射不在这里，归 control-note。

1. 旋转矩阵与 SO(3)¶

旋转矩阵 \(R \in SO(3)\) 用 \(3\times3\) 个实数表示朝向，其列是动坐标系三轴在静坐标系下的坐标。它受 \(R^\top R = I\)（三个正交单位向量，4 个独立约束）与 \(\det R = +1\)（保手性，1 个约束）约束，共 5 个约束作用在 9 个元素上，自由度恰 \(9-6=3\)。

import numpy as np
from scipy.spatial.transform import Rotation as R

Rz = R.from_rotvec([0, 0, np.pi/2]).as_matrix()   # 绕 z 转 90°
print(Rz)                       # [[ 0. -1.  0.]
                                 #  [ 1.  0.  0.]
                                 #  [ 0.  0.  1.]]
print(Rz.T @ Rz, np.linalg.det(Rz))   # 单位阵、行列式 +1

旋转矩阵的优点是复合即矩阵乘、无奇异；缺点是冗余存储（9 数存 3 自由度），且数值漂移后需正交化才回到 \(SO(3)\)。它作为第 1 章齐次变换 \(T\) 的左上分块出现。

2. 欧拉角与万向锁¶

欧拉角（Euler angles）把旋转拆成绕三个坐标轴的顺序基本旋转。危险在于顺序与内/外旋约定：scipy 用大写表示外旋（固定轴）、小写表示内旋（活动轴），from_euler('ZYX', [a,b,c]) 与 from_euler('zyx', [a,b,c]) 给出不同矩阵。第 1 章「固定轴与活动轴旋转」已给出内外旋反序等价关系。

同一组角度换顺序即换旋转：

mXYZ = R.from_euler('XYZ', [10, 20, 30], degrees=True).as_matrix()
mZYX = R.from_euler('ZYX', [10, 20, 30], degrees=True).as_matrix()
print(np.linalg.norm(mXYZ - mZYX))   # 0.7290386058754026

万向锁（gimbal lock）发生在中间轴角为 \(\pm 90^\circ\) 时：首轴与末轴共线，只剩二者之差可辨，三个角退化到两个自由度。

m = R.from_euler('ZYX', [30, 90, 50], degrees=True)
print(m.as_euler('ZYX', degrees=True))   # [-20.  90.   0.]：仅剩 α−γ = 30−50 的信息

输入 \((30,90,50)^\circ\) 回读为 \((-20,90,0)^\circ\)——矩阵仍可逆还原，但角度的绝对分配丢了。欧拉角因此不适合做插值与状态积分，工程里多用四元数或指数坐标规避。

3. 轴角与指数映射¶

任意旋转等价于绕某单位轴 \(\hat{\omega} \in \mathbb{R}^3\) 转某角 \(\theta\)，把 \(\omega = \hat{\omega}\theta \in \mathbb{R}^3\) 称为旋转矢量（rotation vector），它是 \(SO(3)\) 在单位处的"指数坐标"（exponential coordinates），3 参数无冗余。

反对称矩阵（skew-symmetric，记 \([\omega] \in \mathfrak{so}(3)\)）把叉积写成矩阵乘法 \([\omega]v = \omega \times v\)：

\[ [\omega]=\begin{bmatrix}0&-\omega_3&\omega_2\\ \omega_3&0&-\omega_1\\ -\omega_2&\omega_1&0\end{bmatrix},\qquad R = e^{[\omega]\theta} = I + \sin\theta\,[\hat\omega] + (1-\cos\theta)\,[\hat\omega]^2, \]

末式即 Rodrigues 公式。

def hat(w):                          # 向量 -> 反对称矩阵 [w]
    return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])
w, th = np.array([0., 0., 1.]), np.pi/2
Rrod = np.eye(3) + np.sin(th)*hat(w) + (1-np.cos(th))*hat(w) @ hat(w)
print(np.linalg.norm(Rrod - R.from_rotvec(w*th).as_matrix()))   # ≈1.6e-16

手工 Rodrigues 与 scipy 的 from_rotvec 差约 \(1.6\times10^{-16}\)，指数映射得证。旋转矢量在 \(\theta = \pi\) 处 \(\omega\) 与 \(-\omega\)（对应 \(\theta\) 取到 \(-\pi\) 边界）表示同一旋转，故其参数空间有边界识别，非全局单值。

4. 单位四元数与双覆盖¶

单位四元数（unit quaternion）\(q = (q_x,q_y,q_z,q_w)\)，\(\|q\|=1\)，用 4 个数存 3 自由度（单位模长约束掉 1）。scipy 采用 Hamilton 约定、标量在末位（scalar-last，\((x,y,z,w)\)）：

q = R.from_rotvec([0, 0, np.pi/2]).as_quat()
print(q, np.linalg.norm(q))   # [0. 0. 0.707107 0.707107]  1.0

四元数乘 \(q \otimes p\) 对应旋转复合，作用于向量用 \(v \mapsto q\,v\,q^{-1}\)。关键性质是双覆盖（double cover）：\(q\) 与 \(-q\) 表示同一旋转，因为 \(SO(3)\) 是单位四元数球 \(S^3\) 的二重覆叠。

print(np.linalg.norm(R.from_quat(q).as_matrix() - R.from_quat(-q).as_matrix()))  # 0.0

\(q\) 与 \(-q\) 的矩阵差为 \(0.0\)。做球面插值（slerp）时若两支符号相反，会绕远路，须先把待插值四元数翻到与起点同半球。四元数无万向锁、归一化便宜，是姿态积分与插值的主流表示。

四种表示两两可转，各自带着参数个数与奇异性代价：

flowchart LR
    R["旋转矩阵 SO(3)<br/>9 参数、约束正交"]
    Q["单位四元数<br/>双覆盖 q ~ −q"]
    E["欧拉角<br/>万向锁奇点"]
    AX["轴角 / 指数映射<br/>角度 0 处不唯一"]
    R <--> Q
    R <--> E
    R <--> AX
    Q <--> E
    Q <--> AX
    api["scipy Rotation<br/>统一互转入口"] -.-> R

5. 四表示互转与 Rotation 落地¶

5.1 scipy 的 Rotation 作为基准¶

scipy.spatial.transform.Rotation 以旋转矩阵为内部规范形态，四种表示经它互转：

r = R.from_matrix(Rz)                          # 从矩阵构造（其它: from_quat/from_rotvec/from_euler）
r.as_matrix(); r.as_quat(); r.as_rotvec(); r.as_euler('ZYX')   # 读出四种形态
print(np.linalg.norm(R.from_quat(r.as_quat()).as_matrix() - Rz))   # 往返矩阵差 ≈0.0

as_* / from_* 两族方法覆盖矩阵、四元数、旋转矢量、欧拉角互转，任意两形态间往返的矩阵差在 \(10^{-16}\) 量级（浮点噪声）。落地建议：存储与积分用四元数、复合与作用于向量用矩阵、与人交互或对接 URDF/ROS 的 RPY 时才用欧拉角，并始终显式写明内外旋与轴向顺序（第 3 章 DH 与 URDF 的 rpy 约定即依赖此处）。

补一个轻量选项：pyquaternion 提供 Quaternion 类（四元数乘法、求逆、插值），API 小、依赖少，约定与 scipy 一致（scalar-last）；但缺旋转矩阵/欧拉角的完整互转，只在依赖树里已经有它时用，不替代 Rotation。

5.2 C++ 侧的对应物：Sophus 与 Eigen 几何¶

工程代码大多跑在 C++ 上，同一套表示在那里由两个库承担：Eigen 的 Geometry 模块（Matrix3d 旋转矩阵、Quaterniond 四元数、AngleAxisd 轴角，与上面四种表示一一对应）与 Sophus（SO3d/SE3d 李群封装，补上 Eigen 没有的指数/对数映射与伴随）。Sophus 是 header-only 库（CMake FetchContent 或 submodule 引入，依赖 Eigen），最小用法：

#include <sophus/se3.hpp>
#include <sophus/so3.hpp>

Eigen::Vector3d w(0, 0, 1.57);
Sophus::SO3d R = Sophus::SO3d::exp(w);          // 指数映射：旋转矢量 → 旋转
Eigen::Matrix3d Rm = R.matrix();                // 取回旋转矩阵
Sophus::SE3d T = Sophus::SE3d(R, Eigen::Vector3d(1, 0, 0));   // 旋转 + 平移
Sophus::SE3d::Tangent xi = T.log();             // 对数映射：位姿 → 6 维切线向量
Eigen::Matrix<double, 6, 6> AdT = T.Adj();      // 伴随矩阵（误差传播用）

与 scipy 的分工：学习和验证用 scipy（签名直观、可逐行对公式）；C++ 运行时用 Eigen（数值底盘，几乎所有框架都依赖它）+ Sophus（李代数运算，控制与估计里的误差定义、插值都需要它）。

一个跨库必踩的约定差异：Eigen 四元数构造是 Quaterniond(w, x, y, z)，而 .coeffs() 的存储顺序是 (x, y, z, w)——与 scipy 的 scalar-last (x,y,z,w) 恰好对应存储而不是构造。跨库对接时以“存储顺序”为基准对齐，不要拿构造函数参数序当存储序。

6. 符号约定与单位坑¶

四种表示各有约定分歧，跨库、跨教材时须逐一核对：

标量位置：Hamilton 约定常见两种排布，scipy 与 ROS/tf 用 scalar-last \((x,y,z,w)\)，而 Eigen 与部分控制文献用 scalar-first \((w,x,y,z)\)。混用会把 \((0,0,\sin\frac\theta2,\cos\frac\theta2)\) 读成另一个旋转。
Hamilton vs JPL：两组约定的四元数乘法中虚部符号相反（JPL 相当于对旋转部分取反），同一姿态在两组约定下分量符号不同。scipy 属 Hamilton。
度 vs 弧度：as_euler/from_euler 默认弧度，需 degrees=True 才按度；as_quat/as_rotvec 恒用弧度，不受该开关影响。
欧拉顺序：同一机器人手册可能写 roll-pitch-yaw 为固定轴 XYZ，另一些写为活动轴 zyx，二者反序才等价（见第 2 节）。

print(R.from_quat([0, 0, np.sin(np.pi/4), np.cos(np.pi/4)]).as_euler('ZYX', degrees=True))
## [0. 0. 90.]：绕 z 90°——scalar-last、ZYX 外旋、degrees=True 三者同时约定到位

约定错配不会报错，只会静默给出错误姿态；跨接口对接时以一段最小往返（构造 → 读出 → 比对）锁定约定，是本章唯一的防御手段。

7. 要点¶

旋转矩阵 \(SO(3)\)：9 元素 6 约束、3 自由度，复合无奇异但冗余存储。
欧拉角：顺序与内外旋约定不可省；中间轴 \(\pm90^\circ\) 触发万向锁，只剩首末角之差。
轴角/指数坐标：3 参数无冗余，Rodrigues 公式 \(R=e^{[\omega]\theta}\) 与 from_rotvec 一致到 \(10^{-16}\)。
单位四元数：scalar-last Hamilton，\(q\) 与 \(-q\) 同旋转（双覆盖），slerp 须同半球。
跨库对接先锁四件事：标量位置、Hamilton/JPL、度/弧度、欧拉顺序；用最小往返脚本验证，不靠记忆。

参考文献与延伸阅读¶

Shuster, M. D. A Survey of Attitude Representations. Journal of the Astronautical Sciences, 1993.
Solà, J. Quaternion Kinematics for the Error-State Kalman Filter. arXiv:1711.02508, 2017. arXiv
Markley, F. L., & Crassidis, J. L. Fundamentals of Spacecraft Attitude Determination and Control. Springer, 2014. DOI

第 2 章 旋转的表示与互转¶