FA：Formulas and Algorithm
任意一个三维空间刚体姿态，都可以拆解成绕三根正交坐标轴，分三次有序旋转得到，用来描述这三次旋转大小的角度，就叫欧拉角（Euler Angles），同类型的还有轴角的定义。上述的“分三次有序旋转”可分为两大类：内旋（本体旋转）和外旋（固定旋转），航空最常用Z-Y-X内旋（航向、俯仰和横滚）、图形学常用X-Y-Z外旋（φ、θ和ψ）。但是，只要是欧拉角表示的姿态都存在万向锁和歧义性，以下将展开说明：

一、万向锁（Gimbal Lock）

这是一种自由度退化的现象，对应的四元数中的奇点、矩阵奇异、除零。具体来说是，中间轴转到 ±90° 时，第一根、第三根旋转轴空间重合，3 个自由度退化成 2 个：

• 以 Z→Y→X 为例：$pitch=\pm 90°$，yaw 和 roll 转动效果完全一样，无法独立控制；角速度矩阵奇异、数值震荡。

二、歧义

欧拉角中的歧义，既是多解问题，同一个刚体的姿态，可以从参考坐标系的初始位置按照多个变换方法（对应多组欧拉角）到达该刚体姿态，也就是说不只一组欧拉角能够表示该刚体姿态。同时细分如下：

1. 周期歧义（全局歧义，任意姿态都存在）

旋转角度±360°、±180° 周期重复。例如：roll=30° 与 roll=390°、roll=-330° 是同一个姿态；绕轴转 180° 存在对称等效解。

2. 互补等效歧义（最典型，非奇点也有）

对任意姿态，存在两组完全不同的角组合表达同一姿态，以Z→Y→X的顺序为例：
$(\psi ,\theta ,\varphi )$ 和 $(\psi \pm \pi ,\pi -\theta ,\varphi \pm \pi )$ 代表完全相同刚体姿态。

三、四元数表示姿态

1. 数学形式

四元数由 1 个实部 + 3 个虚部构成，记为 q=[w, x, y, z]，单位四元数用于描述旋转 / 姿态，满足归一化约束：
$w^2+x^2+y^2+z^2=1$
几何含义（轴角等价）：绕单位向量 $n=(n_x,n_y,n_z)$旋转 θ 角，对应四元数：
$\{\begin{array}{l}w=cos(\theta /2)\\ x=n_{x}sin(\theta /2)\\ y=n_{y}sin(\theta /2)\\ z=n_{z}sin(\theta /2)\end{array}$

• w：标量实部；
• x,y,z：虚部，对应旋转轴分量；
• 只有单位四元数才能合法（一一对应）表示三维姿态。

2. 姿态运算规则

• 姿态叠加（两次旋转合并）：四元数乘法 $q_{t}otal=q_2\otimes q_1$​；
• 矢量旋转变换：$v\prime =q\otimes [0,v]\otimes q^{\ast }$（$q^{\ast }$ 是共轭四元数 [w,−x,−y,−z]）；
• 姿态微分（IMU 角速度更新）：$q\dot{}=21q\otimes [0,\omega x,\omega y,\omega z]$。

3. 唯一小歧义

仅存在符号等价：q 和 −q 描述完全同一个姿态，仅此一组等效解，远少于欧拉角无穷多组歧义。

四、四元数表示姿态的优点和缺点

优点 1：不存在万向锁（最关键优势）

欧拉角三段有序轴旋转天生存在奇点，中间轴 ±90° 时自由度丢失、角速度矩阵奇异；
四元数是 SO (3) 旋转流形的全局光滑参数化，全定义域无奇点，俯仰 90°、机头朝上朝下都不会出现轴耦合、数值爆炸、控制抖动。
飞控、机械臂大俯仰 / 翻转场景刚需。

优点 2：姿态连续平滑，无欧拉角式角度跳变

欧拉角存在互补多解歧义，连续运动的姿态转欧拉角会突然跳变 ±180°；
单位四元数在姿态连续变化时，w,x,y,z 四个分量全程连续渐变，不会发生突变，插值、滤波、轨迹规划天然稳定。

优点 3：参数极少，计算开销远小于旋转矩阵

• 旋转矩阵：9 个浮点数，冗余度极高；
• 欧拉角：3 个参数，但带致命缺陷；
• 四元数：仅 4 个浮点数，存储、传输、迭代计算成本最低；
• IMU 高频解算（几百 Hz）、嵌入式 MCU、无人机、SLAM 大量使用。

优点 4：球面插值 Slerp 完美平滑过渡姿态

姿态插值如果用欧拉角线性插值，会出现运动速度不均、轨迹扭曲；
四元数球面线性插值 Slerp 沿旋转最短路径匀速插值，动画、机器人轨迹、相机位姿平滑过渡效果最优。

优点 5：旋转求导、角速度更新公式简洁稳定

欧拉角转角速度的雅可比矩阵在万向锁位置行列式趋近 0，数值奇异、微分计算崩溃；
四元数姿态微分方程全程无分母、无奇异性，IMU 积分、EKF/UKF 滤波数值稳定性极强。

优点 6：求逆旋转极简单

旋转反向（逆姿态）只需取共轭 $q^{\ast }=[w,-x,-y,-z]$；
旋转矩阵求逆需要转置；欧拉角逆旋转要重新拆解角度，计算更繁琐。

优点 7：归一化修正成本低，抗数值漂移

长时间积分后四元数会轻微失归一化，只需要简单缩放：
$q_{new}=w^2+x^2+y^2+z^2$
​​$q$运算量极小，长期姿态解算不易漂移；旋转矩阵正交化修正计算量大很多。

优点 8：统一描述任意旋转，不分内外旋

欧拉角必须严格区分内旋 / 外旋、12 种不同轴顺序，混用极易出错；
四元数只描述最终空间旋转结果，不关心旋转拆解顺序，无内外旋概念混淆问题。

唯一缺点：不直观，转换到欧拉角要防护

人无法直观读取航向 / 俯仰 / 横滚，不能直接给操作员、界面展示姿态；工程标准做法：内部滤波、控制、积分全程用四元数，仅可视化时临时转换成欧拉角展示。