重构RTK-IMU标定链路并完成机械先验工程验证
This commit is contained in:
lichun.qu
+152
-56
@@ -1,72 +1,168 @@
|
||||
# RTK–IMU 独立标定链
|
||||
# RTK–IMU 标定链路审计与当前方案
|
||||
|
||||
本分支将 RTK–IMU 标定与原 LiDAR–IMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学，不复用 LiDAR 命名的结果对象或门限。
|
||||
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||||
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||||
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||||
|
||||
## 坐标与外参约定
|
||||
## 结论
|
||||
|
||||
唯一输出约定为 `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`。
|
||||
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||||
|
||||
世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用；因为设备输出的高程基准尚未单独核验，Z 方向使用更大的噪声并执行独立门控。
|
||||
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||||
|
||||
当前 GNHPR 解释候选把 heading 看作从真北顺时针，pitch 看作机头上仰为正，并按 Z-Y-X 生成 `R_ENU_RTK`。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别，因此 `frame_definition` 和 `reference_point` 为空时，最终状态必须是 diagnostic。
|
||||
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||||
|
||||
## 数据与时间规则
|
||||
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||||
|
||||
- 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）
|
||||
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||||
```
|
||||
|
||||
预期 GNHPR 符号候选胜出，预筛 RMS 1.6313°；pitch 反号为 1.7376°，heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定，但固定 RTK 轴定义仍未判定。
|
||||
RTK 车固坐标定义：
|
||||
|
||||
平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收，不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。
|
||||
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||||
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||||
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||||
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||||
|
||||
逐日期旋转：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°，不能据此声称安装发生变化。
|
||||
位置参考点：
|
||||
|
||||
## 当前阻断项与下一步
|
||||
- GGA 为 ANT1/主天线相位中心。
|
||||
- 相位中心离地高度为 1.916499878 m。
|
||||
|
||||
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/留出验证。
|
||||
## 求解链路
|
||||
|
||||
### R0：协议、数据质量和时间
|
||||
|
||||
1. 校验 NMEA checksum。
|
||||
2. GGA 位置只接收 Q=4 固定解。
|
||||
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
|
||||
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
|
||||
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
|
||||
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。
|
||||
|
||||
### T0：残余时间偏移
|
||||
|
||||
只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。
|
||||
|
||||
时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。
|
||||
|
||||
### R1：双天线基线一致性
|
||||
|
||||
对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：
|
||||
|
||||
```text
|
||||
angle(u_RTK(t0), u_RTK(t1))
|
||||
≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
|
||||
```
|
||||
|
||||
该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。
|
||||
|
||||
### R2：旧式完整 HPR 诊断
|
||||
|
||||
为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。
|
||||
|
||||
LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。
|
||||
|
||||
### T1：平移
|
||||
|
||||
只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：
|
||||
|
||||
- 不运行平移优化；
|
||||
- 不输出 `translation_result.json`；
|
||||
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
|
||||
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。
|
||||
|
||||
## 当前 8 会话结果
|
||||
|
||||
结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。
|
||||
|
||||
```text
|
||||
可观测基线一致性：
|
||||
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
|
||||
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
|
||||
slope RMS / P95 = 1.264514 / 2.475899 deg
|
||||
最坏区间 = priority_175910，约 5.084 deg
|
||||
baseline gate = passed
|
||||
|
||||
时间偏移：
|
||||
全局候选 = +0.005 s
|
||||
峰值相关 = 0.909019
|
||||
近峰宽度 = [-0.110, +0.170] s
|
||||
实际应用 = 0 s
|
||||
|
||||
旧式完整 HPR 诊断：
|
||||
RPY = [-0.132765, -0.297433, -1.558358] deg
|
||||
RMS / P95 = 1.172397 / 2.404977 deg
|
||||
std = [0.615416, 0.437419, 3.687145] deg
|
||||
priority_175910 re-optimized LOO = 1.698555 deg
|
||||
max re-optimized LOO = 1.699427 deg（删除 slope）
|
||||
legacy numeric gate = failed
|
||||
full attitude observable = false
|
||||
|
||||
平移：
|
||||
frozen / not run
|
||||
```
|
||||
|
||||
旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。
|
||||
|
||||
## 已排除或仍存在的漏洞
|
||||
|
||||
- 已修复：heading 误当车前方向。
|
||||
- 已修复：GNHPR pitch 误当纵向车体 pitch。
|
||||
- 已修复：不可观 roll 注入完整姿态。
|
||||
- 已修复：Q5 浮点 HPR 进入标定。
|
||||
- 已修复：HPR 质量字段在导出时丢失。
|
||||
- 已修复：相对运动跨越无效段或跳变。
|
||||
- 已修复：样本多的会话权重过大。
|
||||
- 已修复：LOO 固定全量会话零偏。
|
||||
- 已修复：时间相关使用三轴角速度模长。
|
||||
- 已修复：旋转未通过仍继续优化平移。
|
||||
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
|
||||
- 仍存在：基线绕轴自由度没有独立传感器观测。
|
||||
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。
|
||||
|
||||
## 如何得到可交付的完整旋转
|
||||
|
||||
按优先级建议：
|
||||
|
||||
1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
|
||||
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
|
||||
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
|
||||
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
|
||||
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。
|
||||
|
||||
当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。
|
||||
|
||||
## 与主流开源方法的对应
|
||||
|
||||
- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
|
||||
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
|
||||
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
|
||||
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。
|
||||
|
||||
本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。
|
||||
|
||||
参考：
|
||||
|
||||
- Unicore N4 Reference Commands Manual
|
||||
- Unicore UM982 User Manual
|
||||
- https://github.com/Unsigned-Long/iKalibr
|
||||
- https://github.com/ethz-asl/kalibr
|
||||
- https://github.com/rpng/MINS
|
||||
- https://github.com/chichengcn/gici-open
|
||||
|
||||
## 代码入口
|
||||
|
||||
- `imu_lidar/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
|
||||
- `imu_lidar/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
|
||||
- `imu_lidar/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
|
||||
- `imu_lidar/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
|
||||
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
|
||||
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
|
||||
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
|
||||
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。
|
||||
|
||||
Reference in New Issue
Block a user