73 lines
5.9 KiB
Markdown
73 lines
5.9 KiB
Markdown
# RTK–IMU 独立标定链
|
||
|
||
本分支将 RTK–IMU 标定与原 LiDAR–IMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学，不复用 LiDAR 命名的结果对象或门限。
|
||
|
||
## 坐标与外参约定
|
||
|
||
唯一输出约定为 `p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU`，即 `T_RTK_IMU` 把 IMU 坐标变换到 RTK 传感器坐标。杆臂 `l_IMU_to_RTK_in_IMU` 是“IMU 原点指向 RTK 定位参考点”的向量，表达在 IMU 坐标中，因此 `t_RTK_IMU = -R_RTK_IMU @ l_IMU_to_RTK_in_IMU`。
|
||
|
||
世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用；因为设备输出的高程基准尚未单独核验，Z 方向使用更大的噪声并执行独立门控。
|
||
|
||
当前 GNHPR 解释候选把 heading 看作从真北顺时针，pitch 看作机头上仰为正，并按 Z-Y-X 生成 `R_ENU_RTK`。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别，因此 `frame_definition` 和 `reference_point` 为空时，最终状态必须是 diagnostic。
|
||
|
||
## 数据与时间规则
|
||
|
||
- IMU CSV：`t,gx,gy,gz,ax,ay,az`，陀螺为 rad/s，加速度为 m/s²。
|
||
- RTK CSV：位置使用 GGA，姿态使用 GNHPR；仅接受位置质量 4/5、姿态质量 4/5。
|
||
- RTK 的 `t` 必须是 NMEA 测量 UTC 经 IMU device→host affine clock 反变换得到的 IMU 设备时间。禁止回退到延迟约 3–4 s 的主机接收时间。
|
||
- 姿态使用 `hpr_measurement_utc_s` 恢复的 HPR 测量时间，不挂在最近邻 GGA 时刻。这一修正把 `motion_sms` 的旋转 RMS 从约 4.60° 降到约 1.37°。
|
||
- 残余时间偏移通过 RTK/IMU 角速度模长相关扫描。相关峰不唯一时保持 0 s，并把候选峰只作为诊断值。
|
||
|
||
## 求解方法
|
||
|
||
R0 数据和时间审计：检查固定解比例、共同时间范围、HPR 有效性、ENU 原点和残余时间偏移。`motion_sms` 自动只使用固定位置且有效姿态的连续段。
|
||
|
||
R1 旋转：对 0.75 s、1.5 s、3.0 s 相对运动构造 `Delta_R_RTK R_RTK_IMU = R_RTK_IMU Delta_R_IMU`。IMU 侧采用陀螺预积分。先用无偏置闭式手眼解筛选四种 GNHPR 符号候选，只对最优候选联合优化共享旋转和每会话常值陀螺偏置。输出 RMS/median/P95、边缘协方差、信息奇异值、逐会话 RMS 和固定偏置 conditional LOO。
|
||
|
||
T1 杆臂和平移：不对 RTK 位置做二次差分。固定 R1 的旋转和陀螺偏置，以可配置节点构造 IMU 位置/速度预积分因子，联合求解跨会话共享杆臂、每节点速度和每会话常值加速度偏置。输出边缘协方差、信息奇异值/精度秩、位置/速度残差和 LOO。上游旋转未通过时，平移无条件标为 diagnostic。
|
||
|
||
## 代码结构
|
||
|
||
- `imu_lidar/geodesy.py`：WGS84 → ECEF → 局部 ENU。
|
||
- `imu_lidar/rtk_io.py`：RTK CSV、质量掩码、GGA/HPR 独立测量时刻。
|
||
- `imu_lidar/rtk_attitude.py`：GNHPR 候选约定与 SO(3) 插值。
|
||
- `imu_lidar/rtk_imu_rotation.py`：时间审计、旋转手眼、偏置、协方差、LOO。
|
||
- `imu_lidar/rtk_imu_translation.py`：轻量全 IMU 预积分、杆臂线性因子系统、协方差、LOO。
|
||
- `imu_lidar/rtk_imu_replay.py`：清单加载、端到端运行、结构化结果发布。
|
||
- `tools/run_rtk_imu_calibration.py`：命令行入口，支持会话/批次筛选和 rotation-only。
|
||
- `tests/test_rtk_imu_calibration.py`：ENU、heading 方向、HPR 时刻、预积分等价性测试。
|
||
|
||
公共 IMU 预积分的端点插值已从每小段扫描完整数组，改为当前相邻样本内的等价局部插值；公共 IMU CSV 加载器也改为先校验表头、再按列加载。完整项目测试通过。
|
||
|
||
## 当前结果（8 会话，2026-08-21）
|
||
|
||
结果目录：`artifacts/rtk_imu_calibration_v1/all_sessions`。
|
||
|
||
```text
|
||
RPY(R_RTK_IMU) = [0.102824, -0.145072, -1.432698] deg
|
||
rotation RMS / median / P95 = 1.627683 / 0.569594 / 3.090395 deg
|
||
rotation std = [0.234013, 0.200486, 2.314692] deg
|
||
max conditional rotation LOO = 5.144191 deg（去掉 priority_175910_180530）
|
||
|
||
t_RTK_IMU = [0.771094, 0.569302, -21.073037] m
|
||
translation std = [0.367658, 0.367640, 2.960859] m
|
||
position RMS XYZ = [0.002606, 0.002617, 0.000871] m
|
||
velocity RMS XYZ = [1.034408, 1.001846, 0.081893] m/s
|
||
max translation LOO Z = 1.774476 m（去掉 loop_194223_195003）
|
||
```
|
||
|
||
预期 GNHPR 符号候选胜出，预筛 RMS 1.6313°；pitch 反号为 1.7376°，heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定，但固定 RTK 轴定义仍未判定。
|
||
|
||
平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收，不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。
|
||
|
||
逐日期旋转：0808 为 `[-0.232660, -0.039716, -1.563562]°`、RMS 2.121957°；0815 为 `[0.188773, -0.139741, -0.582331]°`、RMS 0.942835°。两批 yaw 相差约 0.98°，但 yaw 自身标准差约 3.60°/2.57°，不能据此声称安装发生变化。
|
||
|
||
## 当前阻断项与下一步
|
||
|
||
1. RTK `frame_definition` 和 `reference_point` 为空。必须从协议、天线安装方向和原 LiDAR–RTK 手眼结果确认 heading 物理轴、pitch/roll 旋转轴及 GGA 相位中心。
|
||
2. 残余时间候选约 +0.04 s，但峰宽且次峰接近，当前按门限固定为 0 s。需要独立高动态数据或同步边沿确认。
|
||
3. `priority_175910_180530` 的 conditional LOO 牵引达到 5.14°、自身 RMS 3.10°，应检查其 HPR 突跳、IMU 丢帧、动态挠曲和安装状态。
|
||
4. 坡道会话 RMS 2.39°，说明 pitch 数据或姿态轴/符号仍可能不完整。
|
||
5. 平移水平速度因子 RMS 约 1 m/s、Z 杆臂漂到约 21 m。应先冻结平移，完成 RTK 帧和旋转验证，再检查加速度轴、重力符号、高程基准与噪声模型。
|
||
6. 只有旋转、时间和 RTK 帧定义全部通过后，才重新运行平移并做严格的全 nuisance LOO/留出验证。
|