02 姿态的四种表示¶

一句话定义：同一个"姿态"（载体相对导航系的三维朝向，数学上是 3 自由度旋转群 SO(3)）至少有四种主流参数化方式——欧拉角、旋转矩阵(DCM)、四元数、旋转向量——它们彼此等价、可互相转换，但在奇点、参数个数、计算开销和插值友好度上各有权衡。惯导工程的常识是：内部存四元数、对外给欧拉角、做增量更新用旋转向量。

一、先统一问题：姿态到底是什么（直觉）¶

上一讲（01 坐标系与变换）我们说惯导的本质是"不断在坐标系之间搬家"。但姿态本身是什么？

• 姿态 = 载体坐标系(body)相对于导航系(nav，本项目用 NED)的朝向。
• 数学上，朝向就是一个三维旋转：把 nav 系的向量旋转到 body 系（或反过来）。所有三维旋转构成一个群，叫 SO(3)（Special Orthogonal group，行列式为 +1 的正交矩阵群）。
• SO(3) 是 3 自由度的——你只需要 3 个数就能唯一确定任意姿态。

关键矛盾：既然是 3 自由度，为什么需要 4 种表示、最多的要用 9 个数？

类比：地球表面是 2 维流形，用经纬度(2 个数)描述。但经纬度在北极点会"奇点"——所有经线在那汇合，经度失去意义。也就是说，没有一种"全局无奇点、只用 2 个数"的方法能覆盖整个球面。姿态同理：SO(3) 没法用 3 个"全局无奇点"的参数完美覆盖，所以每种表示都牺牲点什么（有的有奇点，有的用冗余参数，有的有歧义）。

这就是为什么我们要认识全部四种——没有哪种是"万能"的，只能让它们各司其职。

二、四种表示总览¶

一句话记忆：欧拉角给人看、旋转矩阵当桥梁、四元数存内部、旋转向量做增量。下面逐个拆开。

三、欧拉角（Euler Angles）¶

1. 定义与顺序¶

欧拉角把任意姿态拆成三次基本旋转的复合，最常用的是 Z-Y-X 顺序（也叫 yaw-pitch-roll / 偏航-俯仰-横滚）：

• yaw（偏航 \(\psi\)）：绕 nav 的竖直轴(Z)转 —— "车头朝哪"
• pitch（俯仰 \(\theta\)）：绕机体横轴(Y)转 —— "抬头/低头"
• roll（横滚 \(\phi\)）：绕机体纵轴(X)转 —— "左翼上扬/右翼上扬"

⚠️ 顺序极其重要：Z-Y-X 和 Y-X-Z 给出的数字完全不同，同一个物理姿态用不同约定得到的 (ψ,θ,φ) 数值不一样。本项目统一 body→NED 的 Z-Y-X，与 01 的旋转矩阵、以及代码 quat_to_euler 完全一致。

2. 复合旋转矩阵¶

Z-Y-X 组合出来的旋转矩阵，本项目统一用被动旋转约定（与 坐标变换与旋转矩阵（推导入门） 第一章一致，也是课本主流写法）：

• nav→body（推导入门已推导）：\(C_n^b = R_x(\phi)\,R_y(\theta)\,R_z(\psi)\)，矩阵从右往左读，最先发生的旋转（Z）放在最右。
• body→nav（本篇 §四 DCM 段记为 \(R_{nb}\)，与这里的 \(C_b^n\) 是同一物体、两种记法）：旋转矩阵正交，直接取转置即可： \(C_b^n = (C_n^b)^T = R_z(-\psi)\,R_y(-\theta)\,R_x(-\phi)\)。即"原路倒着转、角度取负"——相对 \(C_n^b\) 顺序反过来（Z 到了最左），正是 推导入门 第五节的 \(C_b^n\)。两种写法数值完全相等（被动正角 \(R^{pass}(\alpha)\) 与主动形式 \(R^{act}(\alpha)\) 互为转置，也等于 \(R^{pass}(-\alpha)\)），只是约定不同，不要混用。

下面三个被动单轴矩阵（与推导入门第一章完全相同，直接复用，不再重列推导）：

把上面三个被动单轴矩阵按 \(R_z(-\psi)\,R_y(-\theta)\,R_x(-\phi)\) 直接乘开，得到完整姿态矩阵（body→nav，本篇最常用形式）：

此矩阵与 推导入门 第五节逐元素一致，可直接当参考表用。

从矩阵元素反解欧拉角（这 9 个格子不是摆设，直接就能读出角度）：把上面 \(C_b^n\) 的元素代进去即可反向解出三个欧拉角——其中 \(C_b^n(i,j)\) 指第 \(i\) 行、第 \(j\) 列（行 = nav 的 N/E/D，列 = body 的 x/y/z）。已数值验证与矩阵在任意姿态下逐元素自洽，前提 \(|\theta|<90^\circ\)：

💡 这正是下方折叠块里“四元数->欧拉角”公式的矩阵版：两者是同一件事——折叠块用四元数分量 \(q\) 写，这里用矩阵格子写。当 \(|\theta|\to90^\circ\)（万向锁）时 \(C_b^n(1,1),C_b^n(2,1)\to0\)，yaw 的数值解会退化，此时四元数无奇点、更稳。

3. 万向锁（Gimbal Lock）—— 欧拉角的死穴¶

当 pitch = ±90°（机头正上/正下）时，俯仰环把"横滚轴(X)"转到了和"偏航轴(Z)"共线的位置——两个本该独立的旋转变成了一个，系统丢失一个自由度。此时你无论怎么调 yaw 还是 roll，效果都一样，姿态输出会跳变/失控。

这不是数学笔误，是欧拉角参数化本身的拓扑缺陷——球面坐标在极点必然奇点，无法避免。所以飞控/惯导绝不会在内部用欧拉角做积分，只在最后一步"翻译"给人看。

addpath(genpath('docs/惯性导航/assets/miniins'));
% 靠近奇异点 pitch≈90°：pitch +2° 和 yaw +2° 几乎同一姿态
C1 = euler2cnb([(89.9+2)*pi/180; 0; 0]);   % miniins: att=[pitch;roll;yaw]
C2 = euler2cnb([89.9*pi/180; 0; 2*pi/180]);
fprintf('靠近90° 两姿态差 = %.3e\n', norm(C1-C2));   % → 很小(约 1e-3)
% 远离奇异点 pitch≈10°：同样 +2° 差异明显
C3 = euler2cnb([(10+2)*pi/180; 0; 0]);
C4 = euler2cnb([10*pi/180; 0; 2*pi/180]);
fprintf('远离90° 两姿态差 = %.3f\n', norm(C3-C4));   % → 明显大(约 0.05)

​

四、旋转矩阵 / 方向余弦矩阵（DCM）¶

1. 定义¶

一个 \(3\times3\) 正交矩阵 \(R\in\mathrm{SO}(3)\)，满足：

\(R^TR = I,\qquad \det R = +1\)

它把 body 系向量旋到 nav 系（或反之，看下标约定）。9 个数里只有 3 个是独立的——其余 6 个被正交约束吃掉了。

\(\mathrm{SO}(3)\) 这两个条件分别在说什么？（理解了这个，DCM 就不用背）

• \(R^TR=I\) ⇒ 三个列向量单位长且互相垂直 ⇒ 这样的矩阵只能做两件事：旋转或镜像。
• \(\det R=+1\) ⇒ 把镜像排除掉，所以 \(\mathrm{SO}(3)\) 里的元素只能是旋转。 （\(\det=-1\) 的正交矩阵是镜像 / 反射，手性翻转了，它不是姿态。别只看正交性就下结论。）

那"旋转"又是怎么被编码进 9 个数的？看列就够了：

记 \(R\) 的三个列向量为 \(c_1,\ c_2,\ c_3\)，则

把 \(e_1=[1,0,0]^T\) 喂进去（此时 \(a=1,\ b=c=0\)），套上面那句定义，结果正好是 \(c_1\) ⇒ 第 1 列 = 旧 X 轴在新系下的坐标，第 2、3 列同理。所以"9 个数里只有 3 个独立"的几何含义就是：姿态只有 3 个自由度，而它们其实就是三个旧轴各自在新系里怎么读。

👉 完整三步推导见 坐标变换与旋转矩阵 · 第零节；可拖动的演示见 旋转矩阵三步推导 · 交互演示

2. 性质（务必记住）¶

• 串联 = 矩阵乘法：\(R_{C\leftarrow A} = R_{C\leftarrow B}\,R_{B\leftarrow A}\)。多个姿态复合不用"转回欧拉角再算"，直接乘。
• 逆 = 转置：\(R^{-1}=R^T\)（因为正交）。所以 body→nav 是 \(R_{nb}\)，nav→body 是 \(R_{bn}=R_{nb}^T\)。
• 直接作用向量：\(\boldsymbol v_{\text{nav}} = R_{nb}\,\boldsymbol v_{\text{body}}\)，不用任何三角函数。

3. 优点与代价¶

• ✅ 无奇点，且左乘即可旋转向量，是"底层真理"表达。
• ❌ 9 个数只装 3 自由度：数值积分后 \(R\) 会慢慢偏离正交（浮点漂移），必须周期性正交化/重投影，否则越积越歪。
• ❌ 插值不自然（两矩阵之间线性插值得不到合法旋转）。

工程里 DCM 主要当换算枢纽：四元数↔欧拉角、雅可比矩阵求导，往往都要先经 \(R\) 中转。本项目 quat_to_rotmat / q2r 就是干这个的。

五、四元数（Quaternion）¶

1. 定义与几何意义¶

单位四元数 \(\boldsymbol q = (q_0, q_1, q_2, q_3) = (w, x, y, z)\) 表示"绕单位轴 \(\boldsymbol u\) 转角度 \(\phi\)"：

\(\boldsymbol q = \big(\cos\tfrac{\phi}{2},\; \boldsymbol u\sin\tfrac{\phi}{2}\big)\)

也就是说四元数把"旋转轴"和"半角"编码进 4 个数里。本项目约定 Hamilton 乘积、\([w,x,y,z]\) 顺序（注意：ROS/TensorFlow 等部分库用 \([x,y,z,w]\) 的 JPL 序，混用是最常见的移植 bug，见第九节）。

2. 为什么四元数是工程首选¶

结论：四元数比旋转矩阵省一半存储和计算，又无欧拉角的奇点，所以几乎所有实时惯导都在内部用四元数存姿态。

3. 双覆盖（Double Cover）—— 四元数的唯一"坑"¶

四元数对旋转的映射是 2 对 1：\(\boldsymbol q\) 和 \(-\boldsymbol q\) 表示同一个三维旋转（转 \(\phi\) 绕 \(\boldsymbol u\)，与转 \(2\pi-\phi\) 绕 \(-\boldsymbol u\) 等价）。

float w = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
float x = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
float y = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
float z = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];

🔬 PSINS 演示 ①：旋转向量 → 四元数（rv2q）¶

本系列「PSINS 演示」统一三件套：① PSINS 参考代码（带署名）→ ② C 语言实现对照 → ③ 结论。 参考来源：严龚敏《PSINS》（西北工业大学），保留版权、要求引用。 本页共 4 个面板：① rv2q（§五/§六 姿态增量）② qmul（§五 四元数乘法）③ q2mat（§四 DCM）④ q2att（§三 欧拉角，⚠️ 差异警示）。

① PSINS 参考实现（MATLAB）

% PSINS: base0/rv2q.m — Gongmin Yan, NWPU (psins260705)
function q = rv2q(rv)
    n2 = rv(1)^2 + rv(2)^2 + rv(3)^2;
    if n2 < 1.0e-8                      % 小角泰勒展开
        q(1) = 1 - n2*(1/8 - n2/384);  s = 1/2 - n2*(1/48 - n2/3840);
    else
        n = sqrt(n2); q(1) = cos(n/2); s = sin(n/2)/n;
    end
    q(2:4) = s * rv;

② C 语言实现（本项目 ins_eskf_15d.c :: rv2q）

static void rv2q(const float dt[3], float dq[4])
{
    float a = sqrtf(dt[0]*dt[0] + dt[1]*dt[1] + dt[2]*dt[2]), h = 0.5f * a;
    if (a < 1e-8f) { dq[0]=1; dq[1]=dq[2]=dq[3]=0; return; }
    float s = sinf(h) / a;            /* = sin(n/2)/n */
    dq[0] = cosf(h);
    dq[1] = s*dt[0]; dq[2] = s*dt[1]; dq[3] = s*dt[2];
    qnorm(dq);
}

③ 结论

✅ 逐字等价：令 n = a = ‖dt‖，则 h = n/2，s = sin(n/2)/n——与 PSINS 主分支完全一致。 小角处理上 PSINS 用泰勒展开提高精度，本项目在 a<1e-8 时直接退化为单位四元数（此时 dt≈0，dq≈(1,0)，等价且更简单）。 这说明本项目底层姿态数学与经过大量工程验证的 PSINS 完全对齐——我们不是在"自己拍脑袋"，而是站在经典参考的肩膀上。

🔬 PSINS 演示 ②：四元数乘法（qmul）¶

对应正文 §五-2：四元数乘法是姿态串联（\(q \leftarrow q \otimes dq\)）的引擎，属于纯代数——它只跟 Hamilton 约定有关，与坐标系（ENU/NED）无关，所以可以直接逐项对照。

① PSINS 参考实现（MATLAB）

% PSINS: base0/qmul.m — Gongmin Yan, NWPU (psins260705)
function q = qmul(q1, q2)      % q = q1 ⊗ q2（Hamilton 乘积，标量在首）
    q = [ q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
          q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
          q1(1)*q2(3) + q1(3)*q2(1) + q1(4)*q2(2) - q1(2)*q2(4);
          q1(1)*q2(4) + q1(4)*q2(1) + q1(2)*q2(3) - q1(3)*q2(2) ];

② C 语言实现（本项目 ins_eskf_15d.c :: qmul）

static void qmul(float o[4], const float q1[4], const float q2[4])
{
    float w = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
    float x = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
    float y = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
    float z = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
    o[0] = w; o[1] = x; o[2] = y; o[3] = z;
}

③ 结论

✅ 逐项一致：四项 Hamilton 乘积公式完全对应（qmul.m 与 C 的 qmul 逐字等价）。这也再次确认存储序兼容——PSINS 标量在首 [q0,q1,q2,q3] 与本项目 [w,x,y,z] 同构，直接互通。

🔬 PSINS 演示 ③：四元数 → 旋转矩阵（q2mat / q2r）¶

对应正文 §四-3：DCM 当"换算枢纽"，雅可比矩阵求导常经它中转。⚠️ 注意方向语义：PSINS 的 q2mat 返回 Cnb = nav→body（从导航系到机体系），本项目的 q2r 也是 world→body（= NED→body）——两边方向一致，公式才能逐项对照。

① PSINS 参考实现（MATLAB）

% PSINS: base0/q2mat.m — Gongmin Yan, NWPU (psins260705)
function Cnb = q2mat(qnb)     % Cnb = nav→body 的 DCM（行向量写法）
    q11=qnb(1)*qnb(1); q12=qnb(1)*qnb(2); q13=qnb(1)*qnb(3); q14=qnb(1)*qnb(4);
    q22=qnb(2)*qnb(2); q23=qnb(2)*qnb(3); q24=qnb(2)*qnb(4);
    q33=qnb(3)*qnb(3); q34=qnb(3)*qnb(4); q44=qnb(4)*qnb(4);
    Cnb = [ q11+q22-q33-q44,  2*(q23-q14),     2*(q24+q13);
            2*(q23+q14),      q11-q22+q33-q44, 2*(q34-q12);
            2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];

② C 语言实现（本项目 ins_eskf_15d.c :: q2r，行主序）

/* world->body (= NED->body = 参考 R.') */
static void q2r(const float q[4], float R[9])
{
    float xx=q[1]*q[1], yy=q[2]*q[2], zz=q[3]*q[3];
    float xy=q[1]*q[2], xz=q[1]*q[3], yz=q[2]*q[3];
    float wx=q[0]*q[1], wy=q[0]*q[2], wz=q[0]*q[3];
    R[0]=1-2*(yy+zz); R[1]=2*(xy+wz); R[2]=2*(xz-wy);
    R[3]=2*(xy-wz);   R[4]=1-2*(xx+zz); R[5]=2*(yz+wx);
    R[6]=2*(xz+wy);   R[7]=2*(yz-wx);  R[8]=1-2*(xx+yy);
}

③ 结论

✅ 逐项一致：9 个元素一一对应（MATLAB 行序 vs C 行主序正好对齐）。而且两边的语义都是 nav→body（Cnb / world→body）——公式本身不依赖 ENU/NED 的轴向选择，只要"导航系→机体系"的方向约定一致就能直接对照。

🔬 补齐：旋转矩阵 → 四元数（m2q / Shepperd trace 法）¶

上面 demo ③ 讲了"四元数→矩阵"(q2r)，但反向（DCM→四元数）在固件里同样常用——align_triad 用 TRIAD 求出旋转矩阵后，就要用 m2q 把它落成四元数。本项目的 m2q(q2r(q)) = ±q，二者严格互逆。下面补齐这一方向。

① 标准 Shepperd / trace 法（四分支，数值稳定）

由 \(R\)（行主序 \(R[i\cdot3+j]\)）反解，先算迹 \(\operatorname{tr}=R_{00}+R_{11}+R_{22}\)，再按"迹与最大对角元"四选一开方——永远挑最大量开方、避免小分母放大误差：

⚠️ 只写 \(\operatorname{tr}>0\) 一支会在接近 180° 旋转时算飞（根号里变负 / 分母塌缩）。四分支就是为数值稳定，别省。

② 本固件的约定扭曲（关键）：m2q 必须"先解标准公式、再取共轭"

本项目 q2r 输出的不是教科书标准 \(R\)，而是 \(R\) 的转置（q2r(q)=R_{\text{std}}(q)^T$，已在 demo ③ 注明 world→body）。为让m2q成为它的严格逆，m2q的做法是：先按"标准公式"对输入 $R$ 解出v（实则是真 $q$ 的**共轭** $(w,-x,-y,-z)$），再对v取一次共轭q=(v_0,-v_1,-v_2,-v_3)翻回固件约定。于是m2q(q2r(q))=±q` 恒成立。

③ C 语言实现（本项目 ins_align.c :: m2q）

static void m2q(const float R[9], float q[4])
{
    float v[4];
    float tr = R[0] + R[4] + R[8];
    if (tr > 0.0f) {
        float s = 2.0f * sqrtf(tr + 1.0f);
        v[0] = 0.25f * s; v[1] = (R[7]-R[5])/s; v[2] = (R[2]-R[6])/s; v[3] = (R[3]-R[1])/s;
    } else if (R[0] > R[4] && R[0] > R[8]) {
        float s = 2.0f * sqrtf(1.0f + R[0] - R[4] - R[8]);
        v[0] = (R[7]-R[5])/s; v[1] = 0.25f*s; v[2] = (R[1]+R[3])/s; v[3] = (R[2]+R[6])/s;
    } else if (R[4] > R[8]) {
        float s = 2.0f * sqrtf(1.0f + R[4] - R[0] - R[8]);
        v[0] = (R[2]-R[6])/s; v[1] = (R[1]+R[3])/s; v[2] = 0.25f*s; v[3] = (R[5]+R[7])/s;
    } else {
        float s = 2.0f * sqrtf(1.0f + R[8] - R[0] - R[4]);
        v[0] = (R[3]-R[1])/s; v[1] = (R[2]+R[6])/s; v[2] = (R[5]+R[7])/s; v[3] = 0.25f*s;
    }
    q[0] =  v[0]; q[1] = -v[1]; q[2] = -v[2]; q[3] = -v[3];   /* 取共轭还原固件约定 */
    float n = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    if (n > 1e-8f) { n = 1.0f/n; q[0]*=n; q[1]*=n; q[2]*=n; q[3]*=n; }
}

④ 结论 / 验证

✅ 本项目 verify/ins_align/test_m2q.c 已跑通双向验证：Demo1 m2q(q2r(q0))==±q0（YES）；Demo2 喂"标准 90° 绕 Z"矩阵得共轭（逆旋转）、q2r 还原（YES）。所以 DCM↔四元数在本固件里是严格互逆的一对，只是因 q2r 的转置约定，m2q 内部多取了一次共轭。

💡 呼应 §五-3 / §九：双覆盖意味着"互逆"只保证 ±——比较 m2q 输出时永远比 ±，不能直接 ==；slerp/求差前先判点积符号。

🔬 PSINS 演示 ④：四元数 → 欧拉角（q2att / quat_to_euler）⚠️ 差异警示¶

对应正文 §三-2 的折叠。这一对不能直接对照——PSINS 用 ENU（x 东、y 北、z 天），本项目用 NED（x 北、y 东、z 地），姿态角顺序与符号约定都不同。差异正是坐标系约定的具象化。

① PSINS 参考实现（MATLAB，ENU 约定）

% PSINS: base0/q2att.m — Gongmin Yan, NWPU (psins260705)
% 输出 att = [pitch; roll; yaw]（注意顺序！）—— ENU: x东 y北 z天
function att = q2att(qnb)
    q11=qnb(1)*qnb(1); q12=qnb(1)*qnb(2); q13=qnb(1)*qnb(3); q14=qnb(1)*qnb(4);
    q22=qnb(2)*qnb(2); q23=qnb(2)*qnb(3); q24=qnb(2)*qnb(4);
    q33=qnb(3)*qnb(3); q34=qnb(3)*qnb(4); q44=qnb(4)*qnb(4);
    C12=2*(q23-q14); C22=q11-q22+q33-q44;
    C31=2*(q24-q13); C32=2*(q34+q12); C33=q11-q22-q33+q44;
    if C32>0.999999                     % 万向锁: pitch=±90°
        C11=q11+q22-q33-q44; C13=2*(q24+q13);
        att=[pi/2; atan2(C13,C11); 0];
    elseif C32<-0.999999
        C11=q11+q22-q33-q44; C13=2*(q24+q13);
        att=[-pi/2; atan2(C13,C11); 0];
    else
        att = [ asin(C32);
                atan2(-C31, C33);
                atan2(-C12, C22) ];
    end

② C 语言实现（本项目 ins_math.h :: quat_to_euler，NED 约定）

/* 四元数 -> 欧拉角 (Yaw, Pitch, Roll)。约定 FRD 机体系 -> NED 导航系。 */
static inline void quat_to_euler(const float q[4], float *yaw, float *pitch, float *roll)
{
    float q0=q[0], q1=q[1], q2=q[2], q3=q[3];
    if (yaw)   *yaw   = atan2f(2.0f*(q1*q2 + q0*q3), 1.0f - 2.0f*(q2*q2 + q3*q3));
    if (pitch) *pitch = -asinf(2.0f*(q1*q3 - q0*q2));          /* NED: pitch 带负号 */
    if (roll)  *roll  = atan2f(2.0f*(q2*q3 + q0*q1), 1.0f - 2.0f*(q1*q1 + q2*q2));
}

③ 结论

⚠️ 结构同构、约定不同——公式不能互换： - 输出顺序：PSINS 给 [pitch; roll; yaw]，本项目给 (yaw, pitch, roll)。 - 轴向语义：ENU 与 NED 的 z 轴一上一下，pitch 的符号、部分 atan2 的分子差个负号；我们的 pitch 显式带 - 号就是 NED 约定的结果（代码注释也写了）。 - 正确姿势：各用各的取角函数，不要拿 PSINS 的 q2att 公式替换固件的 quat_to_euler——同一物理姿态下两者给出的数字不同（除非恰好对称）。这也解释了 01 坐标系与变换 里为什么坐标系必须先定死。

4. 工程常识：内部永远存四元数，只在输出时转欧拉角¶

把姿态在滤波器里永远存成四元数（nom.q），需要给人/给其他模块看时才用 quat_to_euler 转。原因已经清楚了：四元数无奇点、好积分；欧拉角只适合"最后一步翻译"。本项目 eskf15_get_euler 内部就一行 quat_to_euler(e->nom.q, y, p, r)。

六、旋转向量 / 角轴（Axis-Angle）¶

1. 定义¶

用一个三维向量表示旋转：方向 = 旋转轴 \(\boldsymbol u\)，长度 = 旋转角 \(\phi\)：

\(\boldsymbol\theta = \phi\,\boldsymbol u \quad (\|\boldsymbol u\|=1)\)

只有 3 个参数（最小参数化），而且物理直觉极强——"绕某个轴转了多少度"。

2. 为什么它适合"增量更新"¶

IMU 每帧给出的就是角增量 \(\Delta\boldsymbol\theta \approx \boldsymbol\omega\,\Delta t\)（陀螺输出直接就是旋转向量！）。把它升级成四元数增量 dq = rv2q(Δθ) 再右乘到姿态上，就是姿态积分最自然的做法：

\(\boldsymbol q_{k+1} = \boldsymbol q_k \otimes \boldsymbol{dq}(\Delta\boldsymbol\theta)\)

本项目 ins_eskf_15d.c 的 eskf15_predict 正是这么干的（见第八节代码走读）。

3. 缺点：大角度歧义¶

• 转 \(\phi\) 和转 \(\phi+2\pi\) 是同一个姿态，但向量长度差 \(2\pi\)——所以旋转向量在 \(\pm\pi\)（或 \(\pm2\pi\)）处不连续，需要 wrap。
• 不适合做"绝对姿态"存储（超过半圈就乱），但做小增量完美无瑕，所以它是"更新步"的最佳载体，而非"状态存储"的载体。

七、Bortz 方程与"不可交换误差"（coning）¶

这一段是进阶。想深入姿态更新算法（本系列 07 篇）前，先在这里埋个种子。

一个朴素的想法：既然 \(\Delta\boldsymbol\theta=\boldsymbol\omega\Delta t\)，那一直累加 \(\boldsymbol q \leftarrow \boldsymbol q\otimes\text{rv2q}(\boldsymbol\omega_k\Delta t)\) 不就行了？在角速度恒定或变化很慢时是对的。但当载体做圆锥运动（coning，高频小幅振动+转动耦合）时，旋转在三维里不可交换——先转 A 再转 B，和先转 B 再转 A，结果不同。忽略这一项会累积出"不可交换误差"。

Bortz 一阶方程给出旋转向量随时间演化的精确形式：

\(\dot{\boldsymbol\phi} = \boldsymbol\omega + \frac12\boldsymbol\phi\times\boldsymbol\omega + \frac{1}{12}\boldsymbol\phi\times(\boldsymbol\phi\times\boldsymbol\omega) + \cdots\)

• 第一项 \(\boldsymbol\omega\)：就是朴素累加（零阶）。
• 第二项 \(\tfrac12\boldsymbol\phi\times\boldsymbol\omega\)：叉乘修正项，正是 coning 误差的来源——它在高频振动下不可忽略。

八、各司其职：本项目实战里的姿态表示¶

把上面四种放回我们这块板（AHRS-Board，1000 Hz ESKF）：

代码走读（ins_eskf_15d.c::eskf15_predict）：

/* 1) 用积分前的姿态算 R（线性化点，量测更新复用） */
q2r(e->nom.q, Rm);          /* world->body = R.' */
m3t(Rm, Rt);                 /* body->NED = R     */

/* 2) 陀螺去零偏 -> 角增量(旋转向量) -> 四元数增量 -> 右乘更新 */
float phi[3] = { ou[0]*dt, ou[1]*dt, ou[2]*dt };  /* Δθ = ω·dt (旋转向量) */
rv2q(phi, dq);                                  /* 旋转向量 -> 四元数增量 */
qmul(e->nom.q, e->nom.q, dq);                   /* q = q ⊗ dq (右乘) */
qnorm(e->nom.q);                                /* 保持单位四元数 */

注意 qmul(e->nom.q, e->nom.q, dq) 是右乘——因为角增量是在 body 系测得的，要右乘到当前姿态上。这正是第五、六节说的"旋转向量→四元数增量→右乘"。

另一个真实踩过的坑（eskf15_get_euler 注释）：取欧拉角必须用 quat_to_euler 由 \(q\) 直接解，绝不能"先用 q2r 得到 \(R\) 再反推欧拉角"——因为本项目里 q2r 输出的是 \(R^T\)（world→body），用它会算成逆旋转的姿态，yaw/pitch/roll 全错。这条坑已经写进代码注释，看到 q2r 和 quat_to_rotmat 的区别要警觉。

九、常见坑（把你未来会栽的提前标出来）¶

- 万向锁：pitch 接近 ±90° 时欧拉角输出跳变。对策——内部绝不用欧拉角积分，只在输出时转换；靠近极点时用四元数/slerp。 - 四元数双覆盖符号：q 与 -q 等价。slerp / 比较 / 求差前先算点积，若 \(\boldsymbol q\cdot\boldsymbol q'<0\) 取 \(-\boldsymbol q'\)，否则插值会"走远路"、姿态瞬跳。 - 存储序 \([w,x,y,z]\) vs \([x,y,z,w]\) 混用：本项目 Hamilton 用前者；ROS(geometry_msgs)、部分神经网络用后者。移植/对接外部库时第一件事就是确认顺序，差一位全错。 - 欧拉角顺序：Z-Y-X 与 Y-X-Z 数值不同。对接 COTS 模块（如 MTi）必须核对其 datasheet 的顺序与基准系。 - 用 q2r 反推欧拉角：本项目 q2r 给的是 \(R^T\)，硬推会得到逆旋转——务必走 quat_to_euler。 - 旋转矩阵忘记正交化：长时间积分后 \(R\) 漂移失正，必须周期重投影（如通过四元数重建 \(R\)）。 - 旋转向量 \(\pm\pi\) 跳变：当作"绝对姿态"存储时会在半圈处不连续，需要 wrap；它只适合做增量。

十、自测题（合上眼睛能答，才算懂）¶

1. 为什么没有一种"只用 3 个数、全局无奇点"的姿态表示？（用经纬度类比回答）
2. 欧拉角 Z-Y-X 的 pitch=90° 时发生了什么？为什么叫"丢失一个自由度"？
3. 旋转矩阵串联为什么是乘法 \(R=R_2R_1\) 而不是加法？它和四元数乘法比，贵在哪？
4. 四元数 \(\boldsymbol q\) 和 \(-\boldsymbol q\) 是什么关系？工程中为什么要"先判符号再 slerp"？
5. 本项目里姿态在滤波器内部存成什么？对外输出成什么？每帧增量是什么？三者如何衔接（写出那行 qmul 右乘的含义）？
6. 为什么说"陀螺角增量天然是旋转向量"？旋转向量的主要缺点是什么，为什么它仍适合做增量更新？

答案都藏在上面。第 1、3、5 题答不清，建议回看 01 坐标系与变换 与 数学地基：矩阵、概率与协方差 的旋转矩阵小节。

关联与延伸¶

• 系列首页：惯性导航与惯导解算 · 自学科普系列
• 上一篇：01 坐标系与变换（姿态张开前先对齐坐标系）
• 下一篇：03 传感器原理 —— 加速度计测"比力"、陀螺测角速度；MEMS vs FOG
• 代码落地：AHRS 板固件仿真与验证（ins_eskf_15d.c / ins_math.h 真实实现）
• 数学地基：矩阵、概率与协方差（旋转矩阵、四元数乘法都在这里）

外链 Wiki（随时回补）¶

• 旋转矩阵三步推导 · 交互演示 —— 「凭什么一堆数字能代表旋转」的可拖动演示（本文第四节 DCM 配套）
• 主动 vs 被动旋转 · 交互演示 —— 向量转 vs 坐标系转，双滑块对比
• 欧拉角 — 维基百科
• 万向锁 — 维基百科
• 四元数与空间旋转 — 维基百科
• 旋转矩阵 — 维基百科
• 旋转群 SO(3) — 维基百科
• 轴角表示 — 维基百科
• Slerp 球面线性插值 — Wikipedia