实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制

Co-authored-by: Cursor <cursoragent@cursor.com>

data_process/plot_trajectory_comparison.py

@@ -76,6 +76,92 @@ def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
     return (angle_degrees + 180.0) % 360.0 - 180.0
 
 
+def build_complete_s_curve(
+    start: np.ndarray,
+    end: np.ndarray,
+    offset_mm: float,
+    samples_per_segment: int = 120,
+) -> tuple[np.ndarray, np.ndarray]:
+    """重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。"""
+    line = end - start
+    length = float(np.linalg.norm(line))
+    if length <= 1e-6:
+        raise ValueError("S型曲线的起点和终点不能重合。")
+
+    forward = line / length
+    left = np.array([-forward[1], forward[0]])
+    controls = [
+        np.array([
+            [0.0, 0.0],
+            [length / 12.0, 0.0],
+            [length / 6.0, offset_mm],
+            [length * 0.25, offset_mm],
+        ]),
+        np.array([
+            [length * 0.25, offset_mm],
+            [length / 3.0, offset_mm],
+            [length * 2.0 / 3.0, -offset_mm],
+            [length * 0.75, -offset_mm],
+        ]),
+        np.array([
+            [length * 0.75, -offset_mm],
+            [length * 5.0 / 6.0, -offset_mm],
+            [length * 11.0 / 12.0, 0.0],
+            [length, 0.0],
+        ]),
+    ]
+
+    local_parts: list[np.ndarray] = []
+    derivative_parts: list[np.ndarray] = []
+    for index, points in enumerate(controls):
+        t = np.linspace(0.0, 1.0, samples_per_segment + 1)
+        if index > 0:
+            t = t[1:]
+        one_minus_t = 1.0 - t
+        local = (
+            one_minus_t[:, None] ** 3 * points[0]
+            + 3.0
+            * one_minus_t[:, None] ** 2
+            * t[:, None]
+            * points[1]
+            + 3.0
+            * one_minus_t[:, None]
+            * t[:, None] ** 2
+            * points[2]
+            + t[:, None] ** 3 * points[3]
+        )
+        derivative = (
+            3.0
+            * one_minus_t[:, None] ** 2
+            * (points[1] - points[0])
+            + 6.0
+            * one_minus_t[:, None]
+            * t[:, None]
+            * (points[2] - points[1])
+            + 3.0
+            * t[:, None] ** 2
+            * (points[3] - points[2])
+        )
+        local_parts.append(local)
+        derivative_parts.append(derivative)
+
+    local_points = np.vstack(local_parts)
+    local_derivatives = np.vstack(derivative_parts)
+    world_points = (
+        start
+        + local_points[:, 0, None] * forward
+        + local_points[:, 1, None] * left
+    )
+    world_derivatives = (
+        local_derivatives[:, 0, None] * forward
+        + local_derivatives[:, 1, None] * left
+    )
+    headings = np.rad2deg(
+        np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0])
+    )
+    return world_points, headings
+
+
 def segmented_savgol(
     values: np.ndarray,
     sample_interval: float,
@@ -166,6 +252,16 @@ def load_and_resample(
         "ReferenceEndY",
         "ReferenceSpeed",
     ]
+    optional_numeric_columns = [
+        "CommandAngularSpeedRadPerSecond",
+        "ReferenceAngularSpeedRadPerSecond",
+        "ReferenceMotionFrameYawDegrees",
+    ]
+    numeric_columns.extend(
+        column
+        for column in optional_numeric_columns
+        if column in raw.columns
+    )
     for column in numeric_columns:
         raw[column] = pd.to_numeric(raw[column], errors="coerce")
 
@@ -220,9 +316,21 @@ def load_and_resample(
     update_command_speed = np.abs(
         updates["CommandSpeed"].to_numpy(dtype=float)
     )
-    update_command_angular = np.abs(
-        updates["CommandAngularSpeed"].to_numpy(dtype=float)
-    )
+    if "CommandAngularSpeedRadPerSecond" in updates.columns:
+        update_command_angular_rad = np.abs(
+            updates[
+                "CommandAngularSpeedRadPerSecond"
+            ].to_numpy(dtype=float)
+        )
+    else:
+        # 旧CSV中的CommandAngularSpeed单位为deg/s。
+        update_command_angular_rad = np.deg2rad(
+            np.abs(
+                updates[
+                    "CommandAngularSpeed"
+                ].to_numpy(dtype=float)
+            )
+        )
 
     # 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
     # 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
@@ -243,8 +351,12 @@ def load_and_resample(
     )
     expected_heading_delta = (
         0.5 *
-        (update_command_angular[1:] + update_command_angular[:-1]) *
-        update_dt
+        (
+            update_command_angular_rad[1:] +
+            update_command_angular_rad[:-1]
+        ) *
+        update_dt *
+        180.0 / np.pi
     )
     heading_threshold = np.maximum(
         5.0,
@@ -347,6 +459,15 @@ def load_and_resample(
             (time_uniform >= start) & (time_uniform <= end)
         )
 
+    if "CommandAngularSpeedRadPerSecond" in raw.columns:
+        angular_command_rad = interpolate_command(
+            "CommandAngularSpeedRadPerSecond"
+        )
+    else:
+        angular_command_rad = np.deg2rad(
+            interpolate_command("CommandAngularSpeed")
+        )
+
     frame = pd.DataFrame({
         "TimeSeconds": time_uniform,
         "DetourXRawMm": x_resampled,
@@ -356,8 +477,8 @@ def load_and_resample(
         "DetourThetaUnwrappedDeg": theta_filtered,
         "DetourThetaDeg": wrap_degrees(theta_filtered),
         "CommandSpeedMps": interpolate_command("CommandSpeed"),
-        "CommandAngularSpeedDegPerSec":
-            interpolate_command("CommandAngularSpeed"),
+        "CommandAngularSpeedRadPerSec":
+            angular_command_rad,
         "InvalidNearLocalizationJump": invalid_near_jump,
     })
 
@@ -367,6 +488,25 @@ def load_and_resample(
         "trajectory_name": str(first["TrajectoryName"]),
         "controller_name": str(first.get("ControllerName", "")),
         "trial_number": str(first.get("TrialNumber", "")),
+        # 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。
+        # DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。
+        "motion_frame_yaw_degrees": float(
+            first["ReferenceMotionFrameYawDegrees"]
+            if (
+                "ReferenceMotionFrameYawDegrees" in raw.columns
+                and pd.notna(
+                    first["ReferenceMotionFrameYawDegrees"]
+                )
+            )
+            else (
+                90.0
+                if "crab" in (
+                    str(first["TrajectoryName"]) +
+                    str(first.get("ControllerName", ""))
+                ).lower()
+                else 0.0
+            )
+        ),
         "reference_start_mm": np.array(
             [first["ReferenceStartX"], first["ReferenceStartY"]],
             dtype=float,
@@ -376,6 +516,12 @@ def load_and_resample(
             dtype=float,
         ),
         "reference_speed_mps": float(first["ReferenceSpeed"]),
+        "reference_angular_speed_rad_per_second": float(
+            first.get(
+                "ReferenceAngularSpeedRadPerSecond",
+                0.0,
+            )
+        ),
         # 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
         "start_heading_degrees": float(first["DetourTheta"]),
         "sample_interval_seconds": sample_interval,
@@ -393,10 +539,13 @@ def build_reference(
     frame: pd.DataFrame,
     metadata: dict[str, Any],
 ) -> dict[str, np.ndarray | float | str]:
-    """根据CSV元数据建立直线、圆弧或原地自转参考及误差。"""
+    """根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。"""
     trajectory_name = str(metadata["trajectory_name"])
     start = np.asarray(metadata["reference_start_mm"], dtype=float)
     end = np.asarray(metadata["reference_end_mm"], dtype=float)
+    motion_frame_yaw_degrees = float(
+        metadata.get("motion_frame_yaw_degrees", 0.0)
+    )
     actual = frame[
         ["DetourXFilteredMm", "DetourYFilteredMm"]
     ].to_numpy(dtype=float)
@@ -410,12 +559,15 @@ def build_reference(
     if radius_match:
         radius = float(radius_match.group("radius"))
         sweep_degrees = float(radius_match.group("sweep"))
-        start_heading = float(metadata["start_heading_degrees"])
-        heading_radians = np.deg2rad(start_heading)
+        start_body_heading = float(metadata["start_heading_degrees"])
+        start_motion_heading = (
+            start_body_heading + motion_frame_yaw_degrees
+        )
+        heading_radians = np.deg2rad(start_motion_heading)
         center = start + radius * np.array(
             [-np.sin(heading_radians), np.cos(heading_radians)]
         )
-        start_radial_degrees = start_heading - 90.0
+        start_radial_degrees = start_motion_heading - 90.0
 
         radial = actual - center
         distance_to_center = np.linalg.norm(radial, axis=1)
@@ -429,7 +581,10 @@ def build_reference(
         ])
         # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
         lateral_error = radius - distance_to_center
-        reference_heading = radial_angle_degrees + 90.0
+        reference_motion_heading = radial_angle_degrees + 90.0
+        reference_heading = (
+            reference_motion_heading - motion_frame_yaw_degrees
+        )
         heading_error = wrap_degrees(
             actual_heading - reference_heading
         )
@@ -450,12 +605,60 @@ def build_reference(
             "ideal_plot_mm": ideal_plot,
             "reference_points_mm": reference_points,
             "reference_heading_degrees": reference_heading,
+            "reference_motion_heading_degrees":
+                reference_motion_heading,
             "lateral_error_mm": lateral_error,
             "heading_error_degrees": heading_error,
             "center_mm": center,
             "radius_mm": radius,
         }
 
+    s_curve_match = re.search(
+        r"SCurve(?P<length>[0-9.]+)m_A(?P<offset>[0-9.]+)mm",
+        trajectory_name,
+        flags=re.IGNORECASE,
+    )
+    if s_curve_match:
+        offset_mm = float(s_curve_match.group("offset"))
+        ideal_plot, ideal_heading = build_complete_s_curve(
+            start,
+            end,
+            offset_mm,
+        )
+        delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :]
+        nearest_indices = np.argmin(
+            np.sum(delta * delta, axis=2),
+            axis=1,
+        )
+        reference_points = ideal_plot[nearest_indices]
+        reference_motion_heading = ideal_heading[nearest_indices]
+        reference_heading = (
+            reference_motion_heading - motion_frame_yaw_degrees
+        )
+        heading_radians = np.deg2rad(reference_motion_heading)
+        left_normals = np.column_stack([
+            -np.sin(heading_radians),
+            np.cos(heading_radians),
+        ])
+        lateral_error = np.sum(
+            (actual - reference_points) * left_normals,
+            axis=1,
+        )
+        heading_error = wrap_degrees(
+            actual_heading - reference_heading
+        )
+        return {
+            "kind": "s_curve",
+            "ideal_plot_mm": ideal_plot,
+            "reference_points_mm": reference_points,
+            "reference_heading_degrees": reference_heading,
+            "reference_motion_heading_degrees":
+                reference_motion_heading,
+            "lateral_error_mm": lateral_error,
+            "heading_error_degrees": heading_error,
+            "offset_mm": offset_mm,
+        }
+
     line = end - start
     length = float(np.linalg.norm(line))
     if length <= 1e-6:
@@ -516,10 +719,17 @@ def build_reference(
     progress = np.clip(displacement @ tangent, 0.0, length)
     reference_points = start + np.outer(progress, tangent)
     lateral_error = (actual - reference_points) @ left_normal
-    reference_heading_scalar = np.rad2deg(
+    reference_motion_heading_scalar = np.rad2deg(
         np.arctan2(tangent[1], tangent[0])
     )
-    reference_heading = np.full(len(frame), reference_heading_scalar)
+    reference_heading_scalar = (
+        reference_motion_heading_scalar -
+        motion_frame_yaw_degrees
+    )
+    reference_heading = np.full(
+        len(frame),
+        reference_heading_scalar,
+    )
     heading_error = wrap_degrees(
         actual_heading - reference_heading
     )
@@ -529,6 +739,10 @@ def build_reference(
         "ideal_plot_mm": ideal_plot,
         "reference_points_mm": reference_points,
         "reference_heading_degrees": reference_heading,
+        "reference_motion_heading_degrees": np.full(
+            len(frame),
+            reference_motion_heading_scalar,
+        ),
         "lateral_error_mm": lateral_error,
         "heading_error_degrees": heading_error,
     }