VINS-Fusion 坐标变换全梳理：从相机光学系到无人机任务系

LSIDLSID

128人浏览 · 2026-11-02 22:40:30

LSIDLSID · 2026-11-02 22:40:30 发布

摘要：在无人机上跑 VINS-Fusion 做轨迹跟踪时，最容易翻车的往往不是算法本身，而是坐标系对不上。本文把"相机光学系 → 机体系 → 水平系 → 任务系"这条变换链从头到尾讲清楚，覆盖相机斜装外参、去 roll/pitch 水平化、世界系到任务系的对齐矩阵，以及位置与速度两条不同变换链。文中给出每一重变换的矩阵与易错点，并附一份可复现的 MATLAB 可视化脚本，把六层坐标系链画在同一张图上直观对比。

关键词：VINS-Fusion、坐标变换、旋转矩阵、四元数、机体系、任务系、无人机、IMU

1. 现实问题：VINS-Fusion 的输出不能直接拿来控无人机

这篇文章来自我们把 D435i、VINS-Fusion 接到无人机轨迹跟踪时遇到的坐标问题。我的思路是先把每一步旋转单独说明白，再合成控制器需要的变换，最后用 MATLAB 把同一条轨迹和各坐标架画出来。先看三个需要分清的地方：

相机轴向与飞机不同：即使相机正向安装，光学系（右/下/前）与飞机机体系（前/左/上）也不同；若再有安装倾角，还要把这个固定角度算进去。
两种模式的 world 不同：纯双目的 world 默认与初始 VINS body 重合，不自动判断竖直方向；双目＋IMU才会利用重力对齐。body 是否就是飞机机体系，还要看配置。
起飞时飞机不一定水平：飞机停在地面有 roll/pitch 倾角，哪怕 VINS 的世界系"接近水平"，机体自身的倾斜也会让"机头方向"和"水平面"错开。

本文沿着“相机 → 机体 → 水平 → 北向 → ENU”逐步解释，再把 VINS world 接到固定任务系。为使这条链保持直观，后文统一使用相机在 world 中的位姿；若实际订阅的是 body 里程计，需要先按 §2.1 换成相机位姿。

2. 先分清相机、机体和世界系

记一个约定：下文所有旋转矩阵 R_XY 表示把 Y 系坐标转到 X 系，即

$x_X = R_{XY} \cdot x_Y$

R_XY 的列就是 Y 系各坐标轴在 X 系中的表达。这个约定一定要从头到尾统一，否则推导必乱。

2.1 相机光学系 `C`（cam0）

RealSense / OpenCV 标准：X 右、Y 下、Z 前（Z 沿光轴朝前）。

先区分两个名字：VINS 内部的 body 记作 S，飞机机体系记作 B。VINS body 由配置定义，不一定是相机，也不一定是飞机 FLU。 官方 odometry 发布 body 位姿，camera_pose 发布相机位姿；使用自己的分支时应核对实现。

若拿到的是 body 位姿，用配置外参 body_T_cam0 = T_SC 转为相机位姿：

$R_{WC}=R_{WS}R_{SC},\qquad p_{WC}=p_{WS}+R_{WS}t_{SC}$

后文的 R_WC、p_W 都指相机姿态和相机原点位置，不能直接把任意 body 四元数当成 R_WC。

2.2 机体系 `B`（FLU，ROS REP-103）

X 前、Y 左、Z 上。右手系：前 × 左 = 上。本文统一按这个系讨论姿态；飞控若使用 FRD/NED，应先转换到本文约定。

2.3 VINS 世界系 `W`

VINS 输出的里程计所在系，它的定义取决于有没有用 IMU：

模式	`W` 是什么	特点
纯双目 `imu:0`	默认与初始 VINS body 重合	不自动重力对齐，Z 不一定朝上
双目+IMU `imu:1`	重力对齐的局部世界系	Z 朝上，水平航向不代表绝对东、北

纯双目时，world 不就是机体系吗？ 如果 body 已按飞机 FLU 定义，是的——更准确地说，是初始化那一刻的机体系固定在原地。飞机后来转动，当前机体系跟着转，world 不跟着转。如果 body_T_cam0 为单位阵，body 与左目相机重合，此时 world 才是初始相机光学系。若沿用相机—IMU外参，则 world 对应初始配置 body，不能直接认成飞机 FLU。

后面的 MATLAB 默认演示“body 与相机重合”的纯双目配置，也提供开关切到“body 与飞机机体系重合”，两种情况的区别可以直接看图。双目＋IMU示意只要求 world 重力对齐，不把脚本选取的水平轴当作 VINS 固定规则。

D435i 实例：IMU 已按光学轴向输出，为什么还要变换？

用 D435i 时，我们容易把“IMU 输出轴向”和“VINS 世界系”当成一回事。按 librealsense 对 D435i 的说明，SDK 会在内部变换 IMU 样本，使其采用深度相机的坐标约定：X 右、Y 下、Z 前。因此，通过这套 SDK / ROS 驱动拿到的数据，不需要再凭 IMU 芯片的物理安装方向补一次 90° 旋转。这里说的是输出轴向处理，不代表设备已完成 IMU 零偏、比例因子等完整标定。

本文使用 ROS 1 驱动发布的 /camera/imu。实际接入时先检查 header.frame_id 和 TF；不同驱动版本可能使用不同帧名，但数据的轴向不能只靠改帧名来改变。设备正常水平、镜头朝前且静止时，光学系加速度应主要落在 −Y，约 −9.8 m/s²，可以据此检查方向。

双目＋IMU时，这个随设备转动的 IMU 系是 VINS 的 body 参考，不是 world。 VINS 利用重力建立 Z 朝上的固定 world，再估计 IMU/body 在 world 中的姿态。相机放歪、转弯时，IMU 系随设备转，world 不随之转动。

所以本例可以这样串起来：D435i IMU 按右/下/前输出 → VINS 估计其在重力对齐 world 中的运动 → 利用设备与飞机的安装关系，转到我们的任务系。 相机与 IMU 轴向接近，也不意味着两者原点重合或标定外参严格为单位阵；body_T_cam 仍使用与实际输入一致的标定结果。

2.4 水平系 `H` / 任务系 `M`

水平系 H：Z 朝上，X 沿初始机头的水平投影，Y 向左。后文用于位置对齐的 H 固定在初始时刻；实时去倾斜得到的水平系则会随 yaw 转动。
任务系 M：水平系的基础上，把原点定在起飞点、x 轴定在起飞瞬间机头水平投影。这是控制器实际用的系。

2.5 导航系 ENU

标准导航系 A：X 东、Y 北、Z 上。为了把航向转换分开看，我们在 H 与 A 之间引入北向系 N：X 北、Y 西、Z 上。下面会把 H → N、N → A 两次旋转都写出来；局部任务控制可以不经过它们。

坐标系关系总览（六层链）

相机 C ──R_BC──▶ 机体 B ──R_level──▶ 水平 H ──R_H2N──▶ 北向 N ──R_N2A──▶ ENU A
(右/下/前)      (前/左/上)      (去roll/pitch)     (去yaw)        (北→东)
   │
   └────────── R_WC ──────────▶ 世界 W（VINS 输出）

3. 欧拉角与旋转约定：先把符号钉死

坐标变换的 bug，十有八九是符号搞反。以下统一按本文矩阵使用的约定说明；接飞控时需用实际姿态输出核对：

角	绕轴	正方向	是否右手
roll	+x（前）	右翼下沉	✓
pitch	+y（左）	低头	✓
yaw	—	罗盘航向：0=北、90=东、顺时针为正	✗ 特殊

两个关键点：

pitch 正是"低头"（FLU 标准，绕左轴右手旋转 +x→−z 即低头），不是 FRD 的"抬头正"。
yaw 是罗盘航向（0=北、顺时针为正），不是标准的 Z 轴右手旋（逆时针为正）。这个"非标准"会在用到导航系时被 90°−yaw 一次性吸收，后面会再提。

4. 第一重变换：相机 → 机体（固定外参 `R_BC`）

相机与机身刚性连接时，光学系和机体系之间是一个固定旋转。先从没有额外俯仰、侧倾的正前视安装说起：

$R_{BC} = \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix}, \qquad R_{CB} = R_{BC}^\top = \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix}$

逐列校验（相机各轴在机体系中的表达）：

相机 X（右）→ (0, −1, 0) = 机体 −Y（向右）✓
相机 Y（下）→ (0, 0, −1) = 机体 −Z（向下）✓
相机 Z（前）→ (1, 0, 0) = 机体 +X（前）✓

相机若有实际安装倾角，还要把它包含进 R_BC，不能只套上面的轴向置换矩阵。它也不是设备内部的相机—IMU外参。

本文演示先忽略相机与飞机原点的平移。若杆臂不能忽略，定义 t_CB 为飞机原点在相机系中的位置，则 p_WB = p_WC + R_WC t_CB，后面应使用修正后的飞机位置。原地转动时，这个差别尤其容易看出来。

5. 第二重变换：机体 → 水平系（去 roll/pitch）

机体相对水平面的倾斜，由飞控的姿态角 roll/pitch 描述。把机体"放平"（去 roll/pitch、留 yaw），旋转为：

$R_{level} = R_y(\text{pitch}) \cdot R_x(\text{roll})$

为什么这样就能水平化？因为 roll/pitch 本身就是"机体相对竖直（重力）方向的倾角"——飞控静止时用加速度计测到重力反作用力，反推出机体的 roll/pitch。所以 R_level 恰好把机体转到"Z 轴沿重力（竖直）"的系。

这里不含 yaw：确定竖直方向只需要 roll/pitch；水平面的 X/Y 朝向则仍与航向有关。公式采用 ZYX 姿态分解，输入角度的轴向和正负号必须与 §3 一致。

再把两次航向变换展开：水平 → 北向 → ENU

设初始罗盘航向为 yaw0（北为 0°、东为 90°），H 的 X 轴沿初始机头。在北向系 N 中，这个方向对应右手角 −yaw0，因此：

$R_{NH}=R_z(-\mathrm{yaw}_0),\qquad p_N=R_{NH}p_H$

N 的 X 朝北、Y 朝西；放到 ENU 中分别是 +Y、−X，所以再绕 Z 转 +90°：

$R_{AN}=R_z(90^\circ),\qquad p_A=R_{AN}p_N$

合起来就是：

$R_{AH}=R_z(90^\circ-\mathrm{yaw}_0),\qquad p_A=R_{AH}p_H$

直观检查：机头朝北时，H 的前向应变为 ENU 的北向 +Y；机头朝东时，H 的前向就是 ENU 的 +X。这里把 N 作为中间系，是为了单独看清罗盘顺时针与右手旋转的关系。脚本中对应变量为 R_H2N、R_N2A。需要真实 ENU 时，航向本身也要有可靠的绝对参考。

6. 第三重变换：世界 → 任务系（对齐矩阵，一次锁存）

这是把 VINS 输出接到控制器的核心一步。推导用"同一个相机在不同世界系下的姿态"这个事实：

相机 → 机体的旋转是固定外参 R_BC；
机体 → 水平系是 R_level = R_y(\text{pitch})·R_x(\text{roll})；
相机 → 世界系是 VINS 四元数给的 R_WC。

所以"相机 → 水平系" = R_level · R_BC；而"世界 → 相机" = R_WCᵀ。合起来得到世界 → 水平系（即任务系）的对齐矩阵：

$R_{HW} = R_y(\text{pitch}_0) \cdot R_x(\text{roll}_0) \cdot R_{BC} \cdot R_{WC}(t_0)^\top$

在静止时刻 t0 一次性算出并锁存（下标 0 表示锁存值），之后不再变。逐帧应用：

$p_M = R_{HW} \cdot (p_W - p_0), \qquad p_0 = p_W(t_0)$

几个要点：

只依赖 pitch0 / roll0 / R_WC(t0) 三个锁存量，不含 yaw、不经过 ENU 中间层。yaw0 只作为任务系 x 轴（起飞时机头水平投影）的锚点，不参与矩阵数值。
为什么纯双目也能重力对齐：纯双目 VINS 的 W 不知道哪边朝天，但飞控知道（它的 roll/pitch 由重力反推）。R_HW 用飞控姿态把 VINS 的任意 world 拽到"Z 朝上"的水平系，重力对齐靠的是飞控姿态，不是 VINS。
锁存后保持参考系不变：若 VINS 重启或重置 world，需要重新建立对齐关系。
锁存必须选静止时刻：静止时飞机不转，姿态和位姿在锁存窗口内几乎不变，可以减小姿态和里程计时间不同步的影响，但不能代替时间戳对齐。

7. 实际使用：位置和速度是两条不同的链

这是最容易混淆的地方：位置来自 VINS，速度/加速度可能来自 IMU，两者变换链不一样。

7.1 位置（VINS → 任务系）

用对齐矩阵完整变换：

$p_M = R_{HW} \cdot (p_W - p_0)$

其中 R_HW 含全部三重变换（R_BC + R_level0 + R_WCᵀ）。

7.2 加速度 / 速度（IMU 比力 → 水平系）

如果速度由飞控加速度计（IMU）积分得到，比力 a_body 直接就是机体系（FLU）坐标，不需要经过相机外参和 VINS 世界系，只需要去倾斜：

$a_H = R_{level} \cdot a_{body} + g_H, \qquad g_H = (0, 0, -g)$

这里的 R_level = R_y(\text{pitch})·R_x(\text{roll}) 用当前帧实时姿态（不是锁存值），因为加速度计随飞机实时动。加 g_H 是"去重力"：IMU 测的是比力（specific force = 实际加速度 − 重力），补回重力才是真实加速度。

去倾斜不需要 yaw，但水平加速度的方向会随 yaw 改变。因此这里的 H(t) 是随航向转动的水平系。只有航向保持在初始值附近时，才可近似当作固定任务系。转弯时应补上相对航向旋转，而不是认为 IMU 不能使用：

$a_M=R_z\bigl(\mathrm{yaw}_0-\mathrm{yaw}(t)\bigr)R_{level}(t)a_{body}+g_M$

其中 yaw 沿用本文罗盘角约定，g_M=(0,0,-g)。输入应确实是已转为飞机 FLU 的比力；积分还涉及偏置与漂移，本文只说明坐标方向。若使用已经表达在 world 中的 VINS 速度，直接取 v_M = R_HW v_W，无需重新按 IMU 链计算。

两条链对照

量	数据源	变换	yaw 处理
位置	VINS 里程计	`R_HW`（含 `R_BC`+`R_level0`+`R_WCᵀ`）	锁存时相消，不显式出现
加速度/速度	飞控 IMU 比力	实时 `R_level`，固定任务系还需相对 yaw	航向变化时补相对旋转

8. 一个完整数值例子

设相机正前视安装，飞机起始倾斜：roll0 = 30°、pitch0 = 30°、yaw0 = 30°。采用纯双目且 body 与相机重合，初始化与锁存取同一静止姿态（此例 W 与初始相机系重合）。VINS 报出相机在世界系的位置和四元数，问机体在任务系里的位置。

一步步来：

相机 → 机体（固定外参）：
$R_{BC} = \begin{bmatrix}0&0&1\\-1&0&0\\0&-1&0\end{bmatrix}$
机体 → 水平系（锁存姿态）：
$R_{level} = R_y(30^\circ)\,R_x(30^\circ)$
相机 → 世界（VINS 四元数 q 转矩阵）：
$R_{WC} = \text{quat2rot}(q)$

这里 quat2rot 仅表示四元数转旋转矩阵。MATLAB 若使用 quat2rotm，输入顺序为 [w x y z]；ROS 消息字段为 x、y、z、w，读取后需按函数要求排列并归一化。
对齐矩阵（一次锁存）：
$R_{HW} = R_{level} \cdot R_{BC} \cdot R_{WC}(t_0)^\top$
逐帧位置：
$p_M = R_{HW} \cdot (p_W - p_W(t_0))$

在这个例子里 R_WC(t0)=I，所以 R_HW=R_level R_BC。世界系中沿相机前方 1 m，即 (0,0,1)，会变为任务系中的约 (0.866,0,−0.5) m：飞机机头低了 30°，沿机头前进自然包含下降分量。

如果纯双目 body 已是飞机 FLU，那么初始化时 R_WB=I、R_WC=R_BC，代入同一公式，得到 R_HW=R_level，不会重复做相机轴向置换。若是双目＋IMU，world 本身已重力对齐，相机安装不会把它重新带歪；剩下主要是任务航向与原点的对齐。

9. 易错点清单

yaw 是罗盘航向（0=北、顺时针），不是右手旋。凡用到导航系的地方要 90°−yaw 纠正，但直连任务系的实现里 yaw 不参与矩阵数值，只作 x 轴锚点，别画蛇添足。
pitch 正是"低头"（FLU），不是 FRD 的"抬头正"；一旦混进 FRD，pitch 符号整个反掉。
IMU 给的是比力，不是重力：飞机 FLU 系水平静止时，Z 分量约为 +g；D435i 光学系水平静止时，则主要为 −Y。旋到水平系后，加 g_H=(0,0,−g) 才得到实际加速度。
纯双目 vs 双目+IMU 的 world 定义不同：前者不保证 Z 朝上，后者正常初始化后重力对齐，下游判断不能混。
对齐矩阵要静止锁存：飞行中锁存会引入 ω·Δt_sync 的旋转误差。
相机外参影响相机与飞机的姿态关系；已经表达在飞机 B 系中的比力，不再经过 R_BC。
水平系不等于固定任务系：IMU 比力只去 roll/pitch 后，水平轴仍随 yaw 转动；转弯时补相对航向旋转。

10. 总结

把 VINS-Fusion 的输出接到无人机控制器，本质是把"相机 → 世界系"翻成"机体 → 任务系"，中间只有三件事：

固定外参 R_BC：相机斜装，先转成机体系；
去倾斜 R_level：去 roll/pitch，让 Z 朝上；
对齐矩阵 R_HW：静止锁存一次，把 VINS 世界系整体掰到起飞点水平任务系。

记住两条不同的链——位置走 VINS 的固定对齐，IMU 比力走实时姿态旋转（转到固定任务系还需处理 yaw）——再把欧拉角符号（低头正、yaw 顺时针）钉死，坐标变换这个坑就填平了。

附：可复现的 MATLAB 可视化脚本

上面的文章逐步展开了相机、机体、水平、北向和 ENU 之间的关系；接控制器时，则用 R_HW 合并世界到任务系的旋转，工程落地时为了少转一次、避开磁偏，确实不需要显式经过 ENU。但为了理解这串变换，把中间层补全、画出来会更直观——这就是下方可视化脚本的作用。

脚本做什么

脚本把六个坐标系完整画出来：主链为 相机 C → 机体 B → 水平 H → 北向 N → ENU A，world W 作为 VINS 的参考系并排展示，并把同一段轨迹在这六个坐标系里的样子并排画在六张子图上，再叠加六个坐标架、以及姿态变化时相机系沿轨迹变化的静态姿态快照。核心变换与正文完全一致，读者可改初始 roll0/pitch0/yaw0 和 imu_mode，直观对比"同一段轨迹在不同坐标系里长什么样"。

运行时主要调整以下参数：

roll0 / pitch0 / yaw0：起飞瞬间的姿态角（度）。
stereo_body：纯双目时选 'camera' 或 'flu'，分别观察 world 与初始相机或初始飞机系重合。
imu_mode：0 = 纯双目（由 stereo_body 选择初始 body）；1 = 双目+IMU（W 重力对齐，轨迹在 W 里已"斜"）。

核心矩阵对应关系：

R_BC    = [0 0 1; -1 0 0; 0 -1 0];      % 相机 → 机体（固定外参）
R_level = roty(pitch0) * rotx(roll0);   % 机体 → 水平（去 roll/pitch）
R_H2N   = rotz(-yaw0);                  % 水平 → 北向（去 yaw）
R_N2A   = rotz(90);                     % 北向 → ENU（北转东）

完整脚本（保存为 .m 文件运行，使用脚本内局部函数，需要 MATLAB R2016b+）：

轨迹由初始相机系 C0 中的航点生成，是几何示意；图 1、图 2 用固定初始坐标架比较，图 3 在轨迹采样点叠加相机姿态快照，不代表传感器实测数据。

%% =========================================================================
%  VINS 坐标系变换可视化 —— 机体自运动（相机系为根）
%
%  轨迹为几何示意，航点定义在固定的初始相机系 C0 中：
%      相机前=+Z，相机右=+X，相机下=+Y。
%      相机前进/右移/上升，在 相机/世界(imu:0)/机体 里是"自然"的（沿轴向），
%      到"水平 H"和 ENU 里才"斜"（因为整机相对重力倾斜）。
%
%  主链：相机 C → 机体 B → 水平 H → 北向 N → ENU A；W 并排展示
%
%  相机"垂直正前"安装，相机光学系 ≠ 机体系：
%      相机 Z(前)=机体 +X(前)，相机 Y(下)=机体 -Z(下)，相机 X(右)=机体 -Y(右)
%  固定外参 R_BC（相机→机体），与设备内部相机—IMU外参分开。
%  VINS body 是否与相机重合取决于配置。
%
%  世界系 W 取决于有没有重力（imu_mode）：
%      imu:0 纯双目：W 与初始 body 重合，由 stereo_body 选择。
%      imu:1 双目+IMU：有重力，W 重力对齐（Z 上；水平轴在脚本中选定，仅作示意）
%
%  水平 H ：机体 → 水平（去 roll/pitch 留 yaw）
%  北向 N ：水平 → 北向（机头转北，消航向 yaw）
%  ENU  A ：北向 → ENU（北转东，固定 90°）
%
%  本文欧拉角约定：roll 右翼下沉正；pitch 低头正；yaw 罗盘航向顺时针
% =========================================================================
clear; clc; close all;

%% ----------------------- 1. 初始参数（改这里） -----------------------------
roll0    = 30;     % 初始横滚 (度)
pitch0   = 30;     % 初始俯仰 (度)；-20 = 抬头 20°（想看"斜"设这个）
yaw0     = 30;    % 初始航向 (度)
imu_mode = 1;     % 0=纯双目；1=双目+IMU（W 重力对齐）
stereo_body = 'camera'; % 纯双目：camera=初始相机；flu=初始飞机机体系

%% ----------------------- 2. 六层链的旋转矩阵 -------------------------------
% 相机 → 机体（挂载，固定）
R_BC = [0 0 1; -1 0 0; 0 -1 0];
R_CB = R_BC';

% 机体 → 水平 H → 北向 N → ENU A
R_level = roty(pitch0) * rotx(roll0);   % 机体 → 水平 H（去 roll/pitch）
R_H2N   = rotz(-yaw0);                  % 水平 H → 北向 N（机头转北，消航向）
R_N2A   = rotz(90);                     % 北向 N → ENU（北转东，固定 90°）
R_yaw   = R_N2A * R_H2N;                % 水平 H → ENU（= rotz(90-yaw0)）
R_AB0   = R_yaw * R_level;              % 机体 → ENU
R_AC0   = R_AB0 * R_BC;                 % 相机 → ENU

% 构造示意世界 W；实际 R_WC 应由相机位姿取得
if imu_mode == 0
    if strcmp(stereo_body, 'camera')
        R_AW = R_AC0;
        modeStr = '纯双目 (W = 初始相机/body)';
    elseif strcmp(stereo_body, 'flu')
        R_AW = R_AB0;
        modeStr = '纯双目 (W = 初始飞机/body)';
    else
        error('stereo_body 必须为 camera 或 flu');
    end
else
    Wx = R_AC0(:,1); Wx(3) = 0; % 示例水平轴，不复刻 VINS 初始化
    if norm(Wx) < 1e-8, Wx = [1;0;0]; end
    Wx = Wx / norm(Wx);
    Wz = [0;0;1];
    Wy = cross(Wz, Wx);
    R_AW    = [Wx, Wy, Wz];             % W 重力对齐
    modeStr = '双目+IMU (W 重力对齐)';
end
R_WC = R_AW' * R_AC0;                   % 相机→世界，非任意 body 的 Rs
R_HW = R_level * R_BC * R_WC';           % 固定对齐矩阵

disp('============ 旋转矩阵 ============');
disp('R_BC  (相机→机体, 挂载)  :'); disp(R_BC);
disp('R_WC  (相机→世界姿态)    :'); disp(R_WC);
disp('R_level(机体→水平 H)     :'); disp(R_level);
disp('R_H2N  (水平 H→北向 N)    :'); disp(R_H2N);
disp('R_N2A  (北向 N→ENU, 90°)  :'); disp(R_N2A);
disp('R_yaw  (水平 H→ENU, 航向) :'); disp(R_yaw);
disp('R_AW   (世界→ENU)        :'); disp(R_AW);
disp(['模式：' modeStr]);

%% ----------------------- 3. 轨迹（相机系自运动，相机系为根） --------------
% 初始相机系 C0 中的示意航点：前(+Z)→左(-X)→后(-Z)→升(-Y)→右(+X)→再前(+Z)
p_C = interpPath([0 0 0; 0 0 1; -1 0 1; -1 0 0; -1 -0.5 0; 0 -0.5 0], 25);
p_C2 = interpPath([0 -0.5 0; 0 -0.5 1], 40);
p_C = [p_C; p_C2];
N = size(p_C,1);

% 各系坐标：从相机 C 出发（单个常量旋转）
p_B = (R_BC    * p_C')';   % 机体系 B
p_W = (R_WC    * p_C')';   % 世界系 W
p_H = (R_level * p_B')';   % 水平系 H（重力对齐）
p_N = (R_H2N   * p_H')';   % 北向系 N（机头朝北）
p_A = (R_N2A   * p_N')';   % ENU 系 A
p_M = (R_HW * (p_W - p_W(1,:))')';
align_error = max(abs(p_M(:) - p_H(:)));
assert(align_error < 1e-10, '任务系对齐与逐步旋转不一致');

% 统一坐标轴范围（对称，覆盖六系轨迹与坐标架）
lim = 1.8 * max([max(abs(p_C(:))), max(abs(p_W(:))), max(abs(p_B(:))), ...
                 max(abs(p_H(:))), max(abs(p_N(:))), max(abs(p_A(:))), 1.0]);

%% ----------------------- 4. 图1：六层链，轨迹在六个坐标系中 --------------
if imu_mode == 0
    wName = ['② 世界 W：', modeStr];
else
    wName = '② 世界 W（重力对齐）';
end
figure('Name','轨迹在六个坐标系中的样子','Color','w');
dat  = {p_C, p_W, p_B, p_H, p_N, p_A};
ttl  = {'① 相机 C(右/下/前)', wName, '③ 机体 B(前/左/上)', '④ 水平 H(重力对齐)', '⑤ 北向 N(机头朝北)', '⑥ ENU A(东/北/上)'};
refR = {R_AC0', R_AW', R_AB0', R_yaw', R_N2A', eye(3)};   % ENU 参考轴在各系中的表达（虚线）

for i = 1:6
    subplot(2,3,i); hold on; grid on; axis equal; view([-32 22]);
    plot3(dat{i}(:,1), dat{i}(:,2), dat{i}(:,3), 'k-','LineWidth',1.4);
    plot3(dat{i}(1,1), dat{i}(1,2), dat{i}(1,3), 'ko','MarkerFaceColor','k');  % 起点
    drawFrameRGB(eye(3),   0.8, '-');                 % 本系坐标轴（实线）
    drawFrameMono(refR{i}, 0.5, '--', [0.5 0.5 0.5], 'ENU');  % ENU 参考轴（虚线）
    title(ttl{i},'FontSize',9); xlabel('X'); ylabel('Y'); zlabel('Z');
    xlim([-lim lim]); ylim([-lim lim]); zlim([-lim lim]);
end
sgtitle(sprintf('六系对照：相机 / 世界 / 机体 / 水平 / 北向 / ENU   初始 roll=%g° pitch=%g° yaw=%g° （%s）', ...
    roll0, pitch0, yaw0, modeStr));

%% ----------------------- 5. 图2：六个坐标架叠加 --------------------------
figure('Name','六个坐标架的相对姿态','Color','w'); hold on; grid on; axis equal; view([-32 22]);
drawFrameMono(eye(3),   1.0, '-', [0.15 0.15 0.15], 'A (ENU)');    % ENU：黑
drawFrameMono(R_yaw,    0.9, '-', [0.9  0.5  0.0 ], 'H (水平)');   % 水平：橙
drawFrameMono(R_N2A,    0.85,'-', [0.6  0.3  0.8 ], 'N (北向)');   % 北向：紫
drawFrameMono(R_AB0,    0.9, '-', [0.0  0.6  0.0 ], 'B (机体)');   % 机体：绿
drawFrameMono(R_AC0,    0.9, '-', [1.0  0.0  1.0 ], 'C (相机)');   % 相机：品红
drawFrameMono(R_AW,     0.8, '-', [0.0  0.7  0.7 ], 'W (世界)');   % 世界：青
plot3(p_A(:,1), p_A(:,2), p_A(:,3), 'k-','LineWidth',1.4);         % 轨迹（ENU）
plot3(p_A(1,1), p_A(1,2), p_A(1,3), 'ko','MarkerFaceColor','k');
xlabel('East'); ylabel('North'); zlabel('Up');
title('六个坐标架 + ENU 轨迹（A黑 / H橙 / N紫 / B绿 / C品红 / W青）');
xlim([-lim lim]); ylim([-lim lim]); zlim([-lim lim]);

%% ----------------------- 6. 图3：姿态变化时，相机系沿轨迹旋转 -------------
figure('Name','姿态变化：相机系沿轨迹旋转','Color','w'); hold on; grid on; axis equal; view([-32 22]);
drawFrameMono(eye(3),  1.0, '-', [0.15 0.15 0.15], 'A (ENU)');
drawFrameMono(R_AW,    0.8, '-', [0.0  0.7  0.7 ], 'W (世界)');
plot3(p_A(:,1), p_A(:,2), p_A(:,3), 'k-','LineWidth',1.2);

% 相机系沿轨迹（姿态随转弯/俯仰变化）
n1 = 125;  % Part1 点数（与 interpPath 采样一致）
samp = round(linspace(1, N, 12));
for idx = samp
    if idx <= n1                      % Part1 恒定
        rr = roll0; pp = pitch0; yy = yaw0;
    else                              % Part2 变化
        s2 = (idx - n1) / (N - n1);
        rr = roll0 + 15 * sin(pi*s2); pp = pitch0 - 25 * sin(pi*s2); yy = yaw0 + 90 * s2;
    end
    R_AC_k = rotz(90 - yy) * roty(pp) * rotx(rr) * R_BC;
    drawFrameAt(R_AC_k, p_A(idx,:), 0.28, [1.0 0.0 1.0]);
end

xlabel('East'); ylabel('North'); zlabel('Up');
title('相机系 C(t) 随姿态（转弯/俯仰）旋转；W 与 ENU 固定不动');
xlim([-lim lim]); ylim([-lim lim]); zlim([-lim lim]);

%% =========================================================================
%  局部函数（R2016b+ 支持脚本内定义）
% =========================================================================
function R = rotx(a)   % 绕 X 轴，输入度
    c = cosd(a); s = sind(a);
    R = [1 0 0; 0 c -s; 0 s c];
end

function R = roty(a)   % 绕 Y 轴，输入度
    c = cosd(a); s = sind(a);
    R = [c 0 s; 0 1 0; -s 0 c];
end

function R = rotz(a)   % 绕 Z 轴，输入度
    c = cosd(a); s = sind(a);
    R = [c -s 0; s c 0; 0 0 1];
end

function drawFrameRGB(R, scale, ls)
% 画一个坐标架：三轴用 X红/Y绿/Z蓝 区分。R 的列 = 各轴方向（显示系中）。
    cols = {'r','g','b'}; labs = {'X','Y','Z'};
    for i = 1:3
        v = R(:,i) * scale;
        quiver3(0,0,0, v(1),v(2),v(3), 0, 'Color',cols{i}, ...
            'LineStyle',ls, 'LineWidth',1.5, 'MaxHeadSize',0.4);
        text(v(1)*1.15, v(2)*1.15, v(3)*1.15, labs{i}, ...
            'Color',cols{i}, 'FontWeight','bold');
    end
end

function drawFrameMono(R, scale, ls, col, name)
% 画一个坐标架：三轴同色（便于区分不同 frame），并在原点标名字。
    for i = 1:3
        v = R(:,i) * scale;
        quiver3(0,0,0, v(1),v(2),v(3), 0, 'Color',col, ...
            'LineStyle',ls, 'LineWidth',1.5, 'MaxHeadSize',0.4);
    end
    text(0.04, 0.04, 0.04, name, 'Color',col, 'FontWeight','bold', 'FontSize',11);
end

function drawFrameAt(R, origin, scale, col)
% 在指定位置 origin 画一个单色坐标架。R 的列 = 各轴方向（显示系中）。
    for i = 1:3
        v = R(:,i) * scale;
        quiver3(origin(1),origin(2),origin(3), v(1),v(2),v(3), 0, ...
            'Color',col, 'LineWidth',1.3, 'MaxHeadSize',0.4);
    end
end

function P = interpPath(wp, nseg)
% 对航点矩阵 wp（Nx3）做逐段线性插值采样，返回 Mx3 点列。
    P = [];
    for k = 1:size(wp,1)-1
        seg = [linspace(wp(k,1),wp(k+1,1),nseg)', ...
               linspace(wp(k,2),wp(k+1,2),nseg)', ...
               linspace(wp(k,3),wp(k+1,3),nseg)'];
        P = [P; seg]; %#ok<AGROW>
    end
end

三张图怎么看

图 1（六层链轨迹）：同一段轨迹在六个坐标系里各画一遍。注意 imu:0、stereo_body='camera' 时，W 与初始相机系重合；切到 'flu' 时，W 与初始飞机系重合。两者都可能倾斜。imu:1 时 W 已重力对齐，但水平轴仍可与 H 不同。这张图最能直观看出"坐标系对不上会错在哪"。
图 2（六坐标架叠加）：六个坐标架放在同一个 ENU 参照系里，直接看彼此的相对姿态——谁斜了、斜多少，一眼可见。
图 3（相机系沿轨迹旋转）：前段姿态恒定，后段加了转弯和俯仰，品红色相机坐标架静态排列在轨迹采样点上，展示各处的姿态变化，而世界 W 与 ENU 固定不动。它演示的是实时姿态变化与"锁存的静止时刻"的区别：正文里对齐矩阵只锁存起飞那一刻，飞行中姿态变了就不再更新。

改 roll0 / pitch0 / yaw0 / imu_mode 重跑，能快速验证自己的外参与符号约定是否写对——这正是本文 §9 易错点清单的"可执行"版本。

将上方完整代码保存为 C05_coordinate_demo.m，即可运行静态可视化。