重构RTK-IMU标定链路并完成机械先验工程验证

imu_lidar/rtk_attitude.py

@@ -1,4 +1,4 @@
-"""GNHPR attitude conventions and SO(3) interpolation."""
+"""GNHPR dual-antenna baseline conventions and SO(3) interpolation."""
 
 from __future__ import annotations
 
@@ -9,12 +9,14 @@ from scipy.spatial.transform import Rotation, Slerp
 
 @dataclass(frozen=True)
 class GnhprConvention:
-    """Interpretation of GNHPR angles as ``R_ENU_RTK``.
+    """Interpretation of the GNHPR ANT1-to-ANT2 baseline.
 
-    Heading is normally clockwise from north. With ENU and an x-forward RTK
-    frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
-    nose-up, which is the negative mathematical Y rotation in an FLU frame.
-    Alternative signs are retained for empirical protocol validation.
+    Heading is clockwise from north. In this vehicle the RTK ``+X`` axis is
+    the baseline from the main/left antenna (ANT1) to the secondary/right
+    antenna (ANT2), so it points to vehicle right rather than vehicle forward.
+    GNHPR pitch is the elevation of that baseline. A two-antenna receiver
+    cannot observe rotation about the baseline; the reported roll is therefore
+    not treated as a third independent attitude measurement.
     """
 
     name: str
@@ -23,7 +25,7 @@ class GnhprConvention:
     roll_sign: float = 1.0
 
 
-EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
+EXPECTED_GNHPR = GnhprConvention("ant1_to_ant2__north_cw__elevation_up")
 GNHPR_CANDIDATES = (
     EXPECTED_GNHPR,
     GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
@@ -38,7 +40,12 @@ def gnhpr_to_rotation_enu_rtk(
     roll_deg: np.ndarray,
     convention: GnhprConvention = EXPECTED_GNHPR,
 ) -> np.ndarray:
-    """Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
+    """Build a zero-roll mathematical completion of the baseline frame.
+
+    Only the first column (the ANT1-to-ANT2 unit vector) is physically observed
+    by GNHPR. The remaining columns are a convenient gauge completion and must
+    not be used as a measured full vehicle attitude.
+    """
 
     heading = np.asarray(heading_deg, dtype=float).reshape(-1)
     pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
@@ -47,11 +54,36 @@ def gnhpr_to_rotation_enu_rtk(
         raise ValueError("heading, pitch and roll must have equal length")
     yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
     pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
-    roll_rad = np.deg2rad(convention.roll_sign * roll)
+    # A dual-antenna baseline has no independent roll observation. Keep the
+    # argument for wire-format compatibility, but never inject it into SO(3).
+    roll_rad = np.zeros_like(roll)
     angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
     return Rotation.from_euler('ZYX', angles).as_matrix()
 
 
+def gnhpr_to_baseline_enu(
+    heading_deg: np.ndarray,
+    pitch_deg: np.ndarray,
+    convention: GnhprConvention = EXPECTED_GNHPR,
+) -> np.ndarray:
+    """Return ANT1-to-ANT2 unit vectors expressed in ENU.
+
+    The result is independent of the unobservable GNHPR roll field.
+    """
+
+    heading = np.asarray(heading_deg, dtype=float).reshape(-1)
+    pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
+    if heading.size != pitch.size:
+        raise ValueError("heading and pitch must have equal length")
+    azimuth = np.deg2rad(heading)
+    elevation = np.deg2rad(-convention.pitch_sign * pitch)
+    horizontal = np.cos(elevation)
+    east = np.sin(azimuth) * horizontal
+    north = np.cos(azimuth) * horizontal
+    up = np.sin(elevation)
+    return np.column_stack([east, north, up])
+
+
 def interpolate_rotations(
     source_t_s: np.ndarray,
     rotations: np.ndarray,