打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -0,0 +1,177 @@
+"""绘制控制器参考速度与Detour差分实际速度对比图。"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+
+from plot_trajectory_comparison import (
+    configure_matplotlib,
+    discover_csv_files,
+    load_and_resample,
+    output_path,
+    segmented_savgol,
+    shade_localization_jump_windows,
+)
+
+
+def calculate_actual_speed_mps(
+    frame,
+    filter_window_seconds: float,
+) -> np.ndarray:
+    """使用Savitzky-Golay求位置导数并计算Detour实际合速度。"""
+    time = frame["TimeSeconds"].to_numpy(dtype=float)
+    dt = float(np.median(np.diff(time)))
+    # 直接对固定频率重采样后的位置做SG求导，避免“先平滑再求导”
+    # 造成两次滤波和过度削弱速度峰值。
+    x_mm = frame["DetourXRawMm"].to_numpy(dtype=float)
+    y_mm = frame["DetourYRawMm"].to_numpy(dtype=float)
+    vx_mm_per_second = segmented_savgol(
+        x_mm,
+        dt,
+        filter_window_seconds,
+        derivative=1,
+    )
+    vy_mm_per_second = segmented_savgol(
+        y_mm,
+        dt,
+        filter_window_seconds,
+        derivative=1,
+    )
+
+    speed = np.hypot(
+        vx_mm_per_second,
+        vy_mm_per_second,
+    ) / 1000.0
+    speed[
+        frame["InvalidNearLocalizationJump"].to_numpy(dtype=bool)
+    ] = np.nan
+    return speed
+
+
+def plot_speed(
+    csv_path: Path,
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成单份CSV的参考/实际速度响应图。"""
+    frame, metadata = load_and_resample(
+        csv_path,
+        frequency_hz,
+        filter_window_seconds,
+    )
+    time = frame["TimeSeconds"].to_numpy(dtype=float)
+    command_speed = frame["CommandSpeedMps"].to_numpy(dtype=float)
+    actual_speed = calculate_actual_speed_mps(
+        frame,
+        filter_window_seconds,
+    )
+    is_in_place_rotation = (
+        str(metadata["trajectory_name"])
+        .lower()
+        .startswith("rotate")
+    )
+    # 原地自转CSV中的ReferenceSpeed历史上保存的是角速度上限deg/s，
+    # 不能作为线速度m/s使用；其参考线速度应为0。
+    configured_speed = (
+        0.0
+        if is_in_place_rotation
+        else float(metadata["reference_speed_mps"])
+    )
+
+    moving = (
+        (command_speed > max(0.02, configured_speed * 0.1)) &
+        np.isfinite(actual_speed)
+    )
+    if np.any(moving):
+        speed_rmse = float(
+            np.sqrt(
+                np.mean(
+                    (actual_speed[moving] - command_speed[moving]) ** 2
+                )
+            )
+        )
+    else:
+        speed_rmse = float("nan")
+
+    fig, ax = plt.subplots(figsize=(10.0, 5.8))
+    ax.plot(
+        time,
+        command_speed,
+        linewidth=1.8,
+        label="控制器参考/下发线速度",
+    )
+    ax.plot(
+        time,
+        actual_speed,
+        linewidth=1.5,
+        label="Detour差分实际线速度（SG求导）",
+    )
+    ax.axhline(
+        configured_speed,
+        linestyle=":",
+        linewidth=1.3,
+        color="tab:green",
+        label=(
+            "原地自转参考线速度 0 m/s"
+            if is_in_place_rotation
+            else f"配置巡航速度 {configured_speed:.3f} m/s"
+        ),
+    )
+    shade_localization_jump_windows(ax, metadata)
+    ax.set_xlabel("时间 / s")
+    ax.set_ylabel("线速度 / (m/s)")
+    ax.set_title(
+        f"参考速度与实际速度对比\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}，"
+        f"运动段RMSE={speed_rmse:.4f} m/s"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "speed_response",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制参考速度与Detour差分实际速度对比图。"
+    )
+    parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
+    parser.add_argument("--frequency", type=float, default=20.0)
+    parser.add_argument("--window", type=float, default=0.55)
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    args = parser.parse_args()
+
+    for csv_path in discover_csv_files(args.files):
+        destination = plot_speed(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()