改为车头向前整链：主从装反机械初值、双天线 pitch/roll 姿态与默认 HeadingOffsetDeg=-90
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -1,16 +1,17 @@
|
||||
# 双天线RTK—3D LiDAR直接手眼标定
|
||||
|
||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **RTK 基线导航系**。
|
||||
它**不是** `base_link` 车体外参；求解阶段不使用车体航向偏置，也不使用 RTK 到后轮轴的 XY 杆臂。
|
||||
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。
|
||||
求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
|
||||
|
||||
当前交付标定（2026-08 室外车，27 站）约定如下：
|
||||
|
||||
| 项 | 值 |
|
||||
|---|---|
|
||||
| RTK 坐标系 | **基线系**（`HeadingOffsetDeg = 0`） |
|
||||
| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
|
||||
| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
|
||||
| 机械平移初值 | `(0.414179474, 0.210859360, 0.004000001) m` |
|
||||
| 机械旋转初值 | yaw ≈ **90°**（雷达 X 朝车头、双天线基线左右装） |
|
||||
| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
|
||||
| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
|
||||
| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
|
||||
| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
|
||||
| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
|
||||
|
||||
@@ -19,7 +20,7 @@
|
||||
|
||||
---
|
||||
|
||||
## 1. 输出坐标约定（基线系）
|
||||
## 1. 输出坐标约定（车头向前）
|
||||
|
||||
统一约定 `T_A_B` 把 B 系点变换到 A 系：
|
||||
|
||||
@@ -27,22 +28,16 @@
|
||||
p_RTK = T_RTK_lidar · p_lidar
|
||||
```
|
||||
|
||||
本仓库默认 RTK 导航系（**基线系 / baseline_raw_heading**）：
|
||||
本仓库默认 RTK 导航系（**车头向前 / vehicle_forward_heading_offset**）：
|
||||
|
||||
- 原点：GGA 位置参考点（通常为 ANT1 相位中心，须结合接收机配置确认）；
|
||||
- X 轴：`rawHeading` 双天线基线在水平面的投影；
|
||||
- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
|
||||
- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
|
||||
- Y 轴：左；
|
||||
- Z 轴：上；
|
||||
- ENU 航向：`yaw = 90° - rawHeading`（`heading_offset = 0`）；
|
||||
- roll、pitch：轨迹中固定为 0。
|
||||
- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
|
||||
|
||||
> 不要把基线系结果当成“车头向前系”。若下游需要车头向前，应另乘确认过的固定航向偏置，或显式使用 `-HeadingOffsetDeg 90` **整链重跑**，不要事后只改 JSON 里的 yaw。
|
||||
|
||||
若下游需要 `T_body_lidar`，须另有已确认的 `T_body_rtk`：
|
||||
|
||||
```text
|
||||
T_body_lidar = T_body_rtk · T_RTK_lidar
|
||||
```
|
||||
> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。
|
||||
> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
|
||||
|
||||
机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)
|
||||
**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
|
||||
@@ -54,7 +49,7 @@ T_body_lidar = T_body_rtk · T_RTK_lidar
|
||||
```text
|
||||
原始雷达 + RTK（+ 可选 IMU）
|
||||
→ combined/（按站关联的多传感器 NPZ）
|
||||
→ 每站选一帧静态点云 + yaw-only RTK pose（基线系）
|
||||
→ 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
|
||||
→ Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
|
||||
→ 留出点、正反向、旋转共轭不变量等精筛
|
||||
→ 双后端共识边 → consensus B
|
||||
@@ -137,10 +132,10 @@ $Data = "D:\data\rtk_lidar_run" # 含 combined/
|
||||
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
|
||||
-CombinedRoot "$Data\combined" `
|
||||
-WorkRoot "$Data\prepared_baseline_h19165" `
|
||||
-OutputRoot "$Data\outputs_baseline_h19165" `
|
||||
-WorkRoot "$Data\prepared_vehicle_h19165" `
|
||||
-OutputRoot "$Data\outputs_vehicle_h19165" `
|
||||
-RtkReferenceHeightAboveGroundM 1.9165 `
|
||||
-HeadingOffsetDeg 0 `
|
||||
-HeadingOffsetDeg -90 `
|
||||
-ExpectedStations 27 `
|
||||
-MinStations 20 `
|
||||
-GroundZMin -2.5 `
|
||||
@@ -153,7 +148,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rt
|
||||
| 参数 | 本次取值 | 说明 |
|
||||
|---|---|---|
|
||||
| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
|
||||
| `-HeadingOffsetDeg` | **0** | 基线系；非 0 时才变成车头向前系 |
|
||||
| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`0` 才是基线系 |
|
||||
| `-GroundZMin/Max` | **-2.5 / -1.5** | 约 2 m 车顶雷达；旧默认 `[-1.4,-0.4]` 会拟合到墙 |
|
||||
| `-ExpectedStations` | **27** | 本批站数 |
|
||||
| `-MinStations` | **20** | 远程旧脚本曾写死 30，会跑不了本批 |
|
||||
@@ -230,32 +225,34 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p
|
||||
|
||||
---
|
||||
|
||||
## 7. 当前标定结果（基线系，h = 1.9165 m）
|
||||
## 7. 当前标定结果（车头向前，h = 1.9165 m）
|
||||
|
||||
结果目录：`D:\data\rtk_lidar_run\outputs_baseline_h19165\`
|
||||
交付文件：`final_T_RTK_lidar.json` / `summary.json`
|
||||
> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。
|
||||
> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。
|
||||
> 旧 `outputs_baseline_h19165` 仍不可用。
|
||||
|
||||
结果目录：`D:\data\rtk_lidar_run\outputs_vehicle_h19165\`
|
||||
文件：`final_T_RTK_lidar.json` / `summary.json`
|
||||
|
||||
```text
|
||||
translation_m = [0.412305582, 0.217309210, 0.104057606]
|
||||
RPY_deg_xyz = [0.465513, 0.743343, 89.460018]
|
||||
translation_m = [0.217822250, -0.411347802, 0.106542337]
|
||||
RPY_deg_xyz = [0.066239, 0.809662, -0.551322]
|
||||
|
||||
T_RTK_lidar ≈
|
||||
0.009424 -0.999922 0.008247 0.412306
|
||||
0.999871 0.009529 0.012896 0.217309
|
||||
-0.012973 0.008124 0.999883 0.104058
|
||||
0.999854 0.009639 0.014119 0.217822
|
||||
-0.009621 0.999953 -0.001292 -0.411348
|
||||
-0.014131 0.001156 0.999899 0.106542
|
||||
0 0 0 1
|
||||
```
|
||||
|
||||
| 指标 | 值 |
|
||||
|---|---:|
|
||||
| 有效站点 / 共识对 | 27 / 20 |
|
||||
| 平移残差 RMS / 中位 / P95 / max | 0.070 / 0.042 / 0.121 / **0.186** m |
|
||||
| 旋转残差 RMS / 中位 / max | 0.978 / 0.585 / **2.73** ° |
|
||||
| 双后端差 | 3.2 mm / 0.17° |
|
||||
| Jacobian 条件数 | 6.88 |
|
||||
| bootstrap σ(x,y,z) | 4.7 / 6.3 / 0.8 mm |
|
||||
| 相对机械初值 | 旋转差 ≈ 1.03°（无近 180° 冲突） |
|
||||
| `frame_mode` | `baseline_raw_heading` |
|
||||
| 平移残差 RMS | ≈ 0.071 m |
|
||||
| 旋转残差 RMS | ≈ 0.982 ° |
|
||||
| 双后端差 | ≈ 3.1 mm / 0.12° |
|
||||
| `frame_mode` | `vehicle_forward_heading_offset` |
|
||||
| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
|
||||
|
||||
与机械平移初值 XY 相差约数毫米；z 由天线高度约束，CAD 的 4 mm 不能代替实测 1.9165 m。
|
||||
|
||||
|
||||
Reference in New Issue
Block a user