重构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、时间和预积分回归测试。
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
# RTK–IMU Engineering 6DoF 分支
|
||||
|
||||
该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：
|
||||
|
||||
```text
|
||||
data_only_6dof_accepted = false
|
||||
```
|
||||
|
||||
工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
|
||||
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：
|
||||
|
||||
```text
|
||||
rotation_source = R2G_gravity_level_prior
|
||||
translation_conditional_on_rotation = true
|
||||
```
|
||||
|
||||
## 条件模型
|
||||
|
||||
估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。
|
||||
|
||||
静止相关因子分为两类：
|
||||
|
||||
- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
|
||||
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。
|
||||
|
||||
连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。
|
||||
|
||||
## 高程与残差口径
|
||||
|
||||
- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
|
||||
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
|
||||
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。
|
||||
|
||||
## 可观性与验收
|
||||
|
||||
杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：
|
||||
|
||||
```text
|
||||
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
|
||||
```
|
||||
|
||||
只对该 3×3 marginal lever information 做 SVD，并输出：
|
||||
|
||||
- `l_I_marginal_covariance_m2`、`l_I_std_m`
|
||||
- `lever_information_singular_values`
|
||||
- `lever_information_condition_number`
|
||||
- `lever_precision_rank`
|
||||
- `weakest_lever_direction_I`
|
||||
|
||||
门禁分为两层：
|
||||
|
||||
- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
|
||||
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。
|
||||
|
||||
任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。
|
||||
|
||||
Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。
|
||||
|
||||
## 机械杆臂软先验与双解输出
|
||||
|
||||
`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
|
||||
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
|
||||
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
|
||||
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
|
||||
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。
|
||||
|
||||
示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：
|
||||
|
||||
```powershell
|
||||
--manual-l-i-m -0.45072 -0.25682 0.73208 `
|
||||
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
|
||||
```
|
||||
## 坐标转换
|
||||
|
||||
```text
|
||||
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
|
||||
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
|
||||
```
|
||||
|
||||
RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。
|
||||
|
||||
## 分阶段运行
|
||||
|
||||
第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c>
|
||||
```
|
||||
|
||||
只有 base/LOO 合理后，才显式启动全量验证：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c> `
|
||||
--run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
|
||||
--run-rotation-sensitivity
|
||||
```
|
||||
|
||||
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
|
||||
@@ -0,0 +1,175 @@
|
||||
# RTK–IMU 多源原始数据与旋转标定 V3
|
||||
|
||||
## 可行性结论
|
||||
|
||||
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
|
||||
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
|
||||
重力约束，但本批数据尚不能通过完整旋转门禁：
|
||||
|
||||
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
|
||||
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
|
||||
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
|
||||
内部导航融合。
|
||||
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
|
||||
|
||||
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
|
||||
不足包装成“标定成功”。
|
||||
|
||||
## 三批原始数据
|
||||
|
||||
扫描根目录：
|
||||
|
||||
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
|
||||
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
|
||||
- 0819：D:\data\0819\raw_serial_capture_v2。
|
||||
|
||||
按文件名捕获开始时间在 1.5 s 内一一配对：
|
||||
|
||||
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|
||||
|---|---:|---:|---:|---:|---:|---:|
|
||||
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
|
||||
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
|
||||
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
|
||||
|
||||
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
|
||||
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
|
||||
没有回退到 host receive time。
|
||||
|
||||
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
|
||||
三个批次分别只剩 44、8、0 条候选。
|
||||
|
||||
## 统一导出
|
||||
|
||||
输出根目录：D:\data\rtk_imu_unified_v3。
|
||||
|
||||
每组捕获：
|
||||
|
||||
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
|
||||
PPS stamp、温度、气压和 host receive UTC。
|
||||
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
|
||||
HPR/BEST/PVT 最近邻挂到 GGA。
|
||||
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
|
||||
的仿射时钟诊断。
|
||||
- manifest.json：全部会话、批次、目录和 unmatched 记录。
|
||||
|
||||
时间规则：
|
||||
|
||||
1. HI13 system_time 是 IMU 主时间轴。
|
||||
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
|
||||
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
|
||||
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
|
||||
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
|
||||
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
|
||||
|
||||
## 新旋转链路
|
||||
|
||||
### R1b：基线在 IMU 中的 2DoF 方向
|
||||
|
||||
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
|
||||
|
||||
angle(b_ENU(t0), b_ENU(t1))
|
||||
= angle(b_IMU, DeltaR_IMU b_IMU)
|
||||
|
||||
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
|
||||
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
|
||||
奇异值。
|
||||
|
||||
### R2V：基线 + Doppler velocity
|
||||
|
||||
筛选条件：
|
||||
|
||||
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
|
||||
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
|
||||
- checksum 有效；
|
||||
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
|
||||
- |IMU gyro_z| ≤3°/s；
|
||||
- 速度方向与横向基线接近正交；
|
||||
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
|
||||
|
||||
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
|
||||
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
|
||||
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
|
||||
|
||||
### R2G：基线 + 水平静止重力
|
||||
|
||||
只允许调用方明确标记的水平静止会话。本次使用：
|
||||
|
||||
- 0819_20260819_072130（flat_static_hdg207）
|
||||
- 0819_20260819_073045（flat_static_hdg082）
|
||||
|
||||
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
|
||||
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
|
||||
|
||||
### R3：稳定性与正式门禁
|
||||
|
||||
R2V/R2G 都输出：
|
||||
|
||||
- 样本和会话数量；
|
||||
- SO(3) RMS/P95；
|
||||
- 旋转向量样本标准差和均值协方差；
|
||||
- leave-one-session；
|
||||
- 10 样本或 10 s block-out。
|
||||
|
||||
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
|
||||
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
|
||||
|
||||
## 首轮实测
|
||||
|
||||
R1b：
|
||||
|
||||
b_IMU = [0.999999976, -0.000213959, -0.000044368]
|
||||
tilt_yz = [-0.0123, -0.0025] deg
|
||||
pairs = 2091
|
||||
RMS / P95 = 0.8086 / 1.7105 deg
|
||||
information singular values = [1.176e-2, 8.476e-4]
|
||||
std = [27.77, 7.47] deg
|
||||
gate = failed
|
||||
|
||||
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
|
||||
数据独立估出了这两个小角。
|
||||
|
||||
R2V：
|
||||
|
||||
qualifying samples / sessions = 41 / 2
|
||||
selected quaternion convention = HI13_q_body_to_ENU
|
||||
RPY = [0.7532, 0.0708, -9.2944] deg
|
||||
RMS / P95 = 1.8436 / 3.4975 deg
|
||||
max leave-one-session = 3.4519 deg
|
||||
gate = failed
|
||||
|
||||
R2G：
|
||||
|
||||
level-static blocks / sessions = 53 / 2
|
||||
RPY = [0.4543, -0.0026, 0.0122] deg
|
||||
RMS / P95 = 0.2542 / 0.2732 deg
|
||||
max leave-one-session = 0.2688 deg
|
||||
gate = passed (level/gravity-prior constrained)
|
||||
|
||||
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
|
||||
|
||||
full_rotation_accepted = false
|
||||
translation_unlocked = false
|
||||
|
||||
## 下一轮数据要求
|
||||
|
||||
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
|
||||
GNSS week/TOW 输出周期。
|
||||
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
|
||||
避免单一磁环境和单一会话支配。
|
||||
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
|
||||
基线的两个微小倾斜角。
|
||||
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
|
||||
quaternion 的跨日期稳定性。
|
||||
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
|
||||
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
|
||||
决定。
|
||||
6. 上述门禁通过前继续冻结杆臂和平移。
|
||||
|
||||
## 实现入口
|
||||
|
||||
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
|
||||
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
|
||||
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
|
||||
- imu_lidar/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
|
||||
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
|
||||
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
|
||||
Reference in New Issue
Block a user