169 lines
8.3 KiB
Markdown
169 lines
8.3 KiB
Markdown
# RTK–IMU 标定链路审计与当前方案
|
||
|
||
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||
|
||
## 结论
|
||
|
||
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||
|
||
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||
|
||
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||
|
||
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||
|
||
## 坐标系和参考点
|
||
|
||
统一输出约定：
|
||
|
||
```text
|
||
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||
```
|
||
|
||
RTK 车固坐标定义：
|
||
|
||
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||
|
||
位置参考点：
|
||
|
||
- GGA 为 ANT1/主天线相位中心。
|
||
- 相位中心离地高度为 1.916499878 m。
|
||
|
||
## 求解链路
|
||
|
||
### 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、时间和预积分回归测试。
|