Initial commit from MyParking project

data_process/电机响应处理/README.md

@@ -0,0 +1,28 @@
+# 舵轮转向响应处理
+
+此工具读取 M 层 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDiagnostic` 生成的 `*_snapshot.csv`，用于分析正常、蟹行、自转模式切换时四个舵轮的响应。
+
+首次使用时安装依赖：
+
+```powershell
+pip install -r requirements.txt
+```
+
+在本目录运行：
+
+```powershell
+python .\plot_steering_response.py
+```
+
+默认选择 `MyParking\logs\wheel-speed` 中最新的快照 CSV，并在同级 `plots` 文件夹生成：
+
+- `*_steering_angles.png`：四轮目标角、实际角和 ±120°机械限位；
+- `*_steering_error_pid.png`：四轮转角误差与 PID 输出；
+- `*_motor_command_feedback.png`：八个电机的最终命令速度与 CAN 反馈速度；
+- `*_mode_speed_limit.png`：正常/蟹行/自转模式变化和 `SendThresSpeed`。
+
+也可以指定一个文件或局部时间范围：
+
+```powershell
+python .\plot_steering_response.py --input "D:\\xxx_snapshot.csv" --from-seconds 2 --to-seconds 15
+```

data_process/电机响应处理/plot_steering_response.py

@@ -0,0 +1,238 @@
+"""可视化停车机器人四舵轮的模式切换与转向响应。
+
+默认读取 MyParking/logs/wheel-speed 中最新的 *_snapshot.csv，输出四张 PNG：
+1. 四个舵轮的目标角、实际角和模式切换时刻；
+2. 四个舵轮的转角误差；
+3. 四个转向 PID 输出；
+4. 八个电机的最终命令速度与 CAN 反馈速度。
+
+示例：
+    python plot_steering_response.py
+    python plot_steering_response.py --input "D:\\logs\\xxx_snapshot.csv"
+"""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+
+SCRIPT_DIRECTORY = Path(__file__).resolve().parent
+PROJECT_DIRECTORY = SCRIPT_DIRECTORY.parent.parent
+DEFAULT_LOG_DIRECTORY = PROJECT_DIRECTORY / "logs" / "wheel-speed"
+
+WHEELS = (
+    ("LeftFront", "左前", "tab:blue"),
+    ("LeftRear", "左后", "tab:orange"),
+    ("RightFront", "右前", "tab:green"),
+    ("RightRear", "右后", "tab:red"),
+)
+
+MOTORS = (
+    ("LFL", "左前左"), ("LFR", "左前右"),
+    ("LRL", "左后左"), ("LRR", "左后右"),
+    ("RFL", "右前左"), ("RFR", "右前右"),
+    ("RRL", "右后左"), ("RRR", "右后右"),
+)
+
+MODE_NAMES = {0: "正常", 1: "蟹行", 2: "自转"}
+
+
+def parse_arguments() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="绘制四舵轮模式切换响应图")
+    parser.add_argument("--input", type=Path, help="指定 *_snapshot.csv；缺省时取最新文件")
+    parser.add_argument("--output", type=Path, help="图片输出目录；缺省时写入本次日志同级 plots")
+    parser.add_argument("--from-seconds", type=float, default=0.0, help="从第几秒开始显示")
+    parser.add_argument("--to-seconds", type=float, help="显示到第几秒结束")
+    return parser.parse_args()
+
+
+def find_snapshot(path: Path | None) -> Path:
+    if path is not None:
+        if not path.is_file():
+            raise FileNotFoundError(f"找不到快照文件：{path}")
+        return path
+
+    candidates = sorted(
+        DEFAULT_LOG_DIRECTORY.glob("*_snapshot.csv"),
+        key=lambda item: item.stat().st_mtime,
+        reverse=True,
+    )
+    if not candidates:
+        raise FileNotFoundError(
+            f"{DEFAULT_LOG_DIRECTORY} 中没有 *_snapshot.csv。\n"
+            "请先在 M 层点击 StartWheelSpeedDiagnostic，完成模式切换后点击 StopWheelSpeedDiagnostic。"
+        )
+    return candidates[0]
+
+
+def load_snapshot(path: Path) -> pd.DataFrame:
+    frame = pd.read_csv(path, comment="#")
+    required = {"ElapsedMs", "ManualControlMode", "SendThresSpeed"}
+    missing = required.difference(frame.columns)
+    if missing:
+        raise ValueError(f"CSV 缺少字段：{', '.join(sorted(missing))}。请部署最新 MedullaAdapter.dll 后重新记录。")
+
+    frame = frame.apply(pd.to_numeric, errors="coerce")
+    frame = frame.dropna(subset=["ElapsedMs"]).sort_values("ElapsedMs")
+    if frame.empty:
+        raise ValueError("CSV 中没有有效数据行。")
+    frame["ElapsedSeconds"] = (frame["ElapsedMs"] - frame["ElapsedMs"].iloc[0]) / 1000.0
+    return frame
+
+
+def crop(frame: pd.DataFrame, start: float, end: float | None) -> pd.DataFrame:
+    result = frame[frame["ElapsedSeconds"] >= start]
+    if end is not None:
+        result = result[result["ElapsedSeconds"] <= end]
+    if result.empty:
+        raise ValueError("所选时间范围内没有数据。")
+    return result
+
+
+def require_columns(frame: pd.DataFrame, names: list[str]) -> None:
+    missing = [name for name in names if name not in frame.columns]
+    if missing:
+        raise ValueError("CSV 缺少字段：" + ", ".join(missing))
+
+
+def add_mode_markers(axis: plt.Axes, frame: pd.DataFrame) -> None:
+    modes = frame["ManualControlMode"].round().astype("Int64")
+    changes = modes.ne(modes.shift())
+    for _, row in frame.loc[changes].iterrows():
+        mode = int(row["ManualControlMode"])
+        axis.axvline(row["ElapsedSeconds"], color="0.55", linestyle="--", linewidth=0.8, alpha=0.75)
+        axis.text(
+            row["ElapsedSeconds"], 0.99, MODE_NAMES.get(mode, f"模式{mode}"),
+            transform=axis.get_xaxis_transform(), rotation=90,
+            va="top", ha="right", fontsize=8, color="0.35",
+        )
+
+
+def save_steering_angle_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    required = []
+    for key, _, _ in WHEELS:
+        required.extend([f"TargetTh{key}", f"ActualTh{key}"])
+    require_columns(frame, required)
+
+    figure, axes = plt.subplots(2, 2, figsize=(14, 8), sharex=True)
+    for axis, (key, label, color) in zip(axes.flat, WHEELS):
+        time = frame["ElapsedSeconds"]
+        axis.plot(time, frame[f"TargetTh{key}"], label="目标角", color=color, linewidth=1.8)
+        axis.plot(time, frame[f"ActualTh{key}"], label="实际角", color="0.15", linewidth=1.1)
+        axis.axhline(120, color="tab:red", linestyle=":", linewidth=0.8, label="机械限位 ±120°")
+        axis.axhline(-120, color="tab:red", linestyle=":", linewidth=0.8)
+        add_mode_markers(axis, frame)
+        axis.set_title(f"{label}舵轮")
+        axis.set_ylabel("转角 (deg)")
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", fontsize=8)
+    for axis in axes[1]:
+        axis.set_xlabel("时间 (s)")
+    figure.suptitle("四舵轮目标转角与实际转角")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_steering_angles.png", dpi=180)
+    plt.close(figure)
+
+
+def save_error_and_pid_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    error_columns = [f"ErrorTh{key}" for key, _, _ in WHEELS]
+    pid_columns = [f"PidOut{key}" for key, _, _ in WHEELS]
+    require_columns(frame, error_columns + pid_columns)
+
+    figure, axes = plt.subplots(2, 1, figsize=(14, 9), sharex=True)
+    time = frame["ElapsedSeconds"]
+    for key, label, color in WHEELS:
+        axes[0].plot(time, frame[f"ErrorTh{key}"], label=label, color=color, linewidth=1.2)
+        axes[1].plot(time, frame[f"PidOut{key}"], label=label, color=color, linewidth=1.2)
+    axes[0].axhline(2, color="0.4", linestyle=":", linewidth=0.9, label="到位阈值 ±2°")
+    axes[0].axhline(-2, color="0.4", linestyle=":", linewidth=0.9)
+    for axis in axes:
+        add_mode_markers(axis, frame)
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", ncol=3, fontsize=9)
+    axes[0].set_ylabel("目标角 - 实际角 (deg)")
+    axes[1].set_ylabel("转向 PID 输出 (m/s)")
+    axes[1].set_xlabel("时间 (s)")
+    figure.suptitle("转角误差与转向 PID 输出")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_steering_error_pid.png", dpi=180)
+    plt.close(figure)
+
+
+def save_motor_speed_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    command_columns = [f"Pid{name}" for name, _ in MOTORS]
+    feedback_columns = [f"Actual{name}" for name, _ in MOTORS]
+    require_columns(frame, command_columns + feedback_columns)
+
+    figure, axes = plt.subplots(4, 2, figsize=(15, 12), sharex=True)
+    time = frame["ElapsedSeconds"]
+    for axis, (name, label) in zip(axes.flat, MOTORS):
+        axis.plot(time, frame[f"Pid{name}"], label="最终命令", color="tab:blue", linewidth=1.2)
+        axis.plot(time, frame[f"Actual{name}"], label="CAN反馈", color="tab:orange", linewidth=1.0)
+        add_mode_markers(axis, frame)
+        axis.set_title(f"{label}电机 ({name})")
+        axis.set_ylabel("速度 (m/s)")
+        axis.grid(alpha=0.25)
+        axis.legend(loc="best", fontsize=8)
+    for axis in axes[-1]:
+        axis.set_xlabel("时间 (s)")
+    figure.suptitle("八个电机最终速度命令与 CAN 实际速度反馈")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_motor_command_feedback.png", dpi=180)
+    plt.close(figure)
+
+
+def save_summary_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None:
+    require_columns(frame, ["SendThresSpeed"])
+    figure, axes = plt.subplots(2, 1, figsize=(14, 7), sharex=True)
+    time = frame["ElapsedSeconds"]
+    axes[0].step(time, frame["ManualControlMode"], where="post", color="tab:purple", linewidth=1.5)
+    axes[0].set_yticks([0, 1, 2], ["正常", "蟹行", "自转"])
+    axes[0].set_ylabel("控制模式")
+    axes[0].grid(alpha=0.25)
+    axes[1].plot(time, frame["SendThresSpeed"], color="tab:brown", linewidth=1.4, label="SendThresSpeed")
+    axes[1].set_ylabel("速度限幅 (m/s)")
+    axes[1].set_xlabel("时间 (s)")
+    axes[1].grid(alpha=0.25)
+    axes[1].legend(loc="best")
+    figure.suptitle("模式切换与整车下发速度限幅")
+    figure.tight_layout()
+    figure.savefig(output / f"{prefix}_mode_speed_limit.png", dpi=180)
+    plt.close(figure)
+
+
+def print_parameter_summary(frame: pd.DataFrame) -> None:
+    parameter_names = [
+        "DiffSteerKp", "DiffSteerKi", "DiffSteerKd", "DiffSteerMaxI",
+        "DiffSteerDeadZone", "DiffSteerThresh", "DiffSteerSpeedAcc",
+    ]
+    if not set(parameter_names).issubset(frame.columns):
+        return
+    print("本次记录的转向 PID 参数：")
+    print("  " + ", ".join(f"{name}={frame[name].iloc[0]:.6g}" for name in parameter_names))
+
+
+def main() -> None:
+    arguments = parse_arguments()
+    snapshot_path = find_snapshot(arguments.input)
+    frame = crop(load_snapshot(snapshot_path), arguments.from_seconds, arguments.to_seconds)
+    output_directory = arguments.output or snapshot_path.parent / "plots"
+    output_directory.mkdir(parents=True, exist_ok=True)
+    prefix = snapshot_path.name.removesuffix("_snapshot.csv")
+
+    save_steering_angle_plot(frame, output_directory, prefix)
+    save_error_and_pid_plot(frame, output_directory, prefix)
+    save_motor_speed_plot(frame, output_directory, prefix)
+    save_summary_plot(frame, output_directory, prefix)
+    print_parameter_summary(frame)
+    print(f"已读取：{snapshot_path}")
+    print(f"已生成四张图：{output_directory}")
+
+
+if __name__ == "__main__":
+    main()

data_process/电机响应处理/requirements.txt

@@ -0,0 +1,3 @@
+matplotlib>=3.7
+numpy>=1.24
+pandas>=2.0

data_process/轨迹测试处理/plot_angular_command.py

@@ -0,0 +1,100 @@
+"""绘制控制器下发角速度命令曲线。"""
+
+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,
+    shade_localization_jump_windows,
+)
+
+
+def plot_angular_command(
+    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)
+    angular_command = frame[
+        "CommandAngularSpeedRadPerSec"
+    ].to_numpy(dtype=float)
+    maximum = float(np.max(angular_command))
+    minimum = float(np.min(angular_command))
+
+    fig, ax = plt.subplots(figsize=(10.0, 5.5))
+    ax.plot(
+        time,
+        angular_command,
+        color="tab:red",
+        linewidth=1.6,
+        label="CommandAngularSpeed",
+    )
+    ax.axhline(0.0, color="black", linewidth=0.8)
+    shade_localization_jump_windows(ax, metadata)
+    ax.set_xlabel("时间 / s")
+    ax.set_ylabel("命令角速度 / (rad/s)")
+    ax.set_title(
+        f"角速度指令曲线\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}，"
+        f"范围=[{minimum:.3f}, {maximum:.3f}]rad/s"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "angular_command",
+    )
+    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="绘制控制器下发角速度命令曲线。"
+    )
+    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_angular_command(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/轨迹测试处理/plot_speed_response.py

@@ -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()

data_process/轨迹测试处理/plot_tracking_errors.py

@@ -0,0 +1,172 @@
+"""绘制横向误差和航向误差随时间变化图。"""
+
+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 (
+    build_reference,
+    configure_matplotlib,
+    discover_csv_files,
+    load_and_resample,
+    output_path,
+    shade_localization_jump_windows,
+)
+
+
+def plot_errors(
+    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,
+    )
+    reference = build_reference(frame, metadata)
+    time = frame["TimeSeconds"].to_numpy()
+    lateral = np.asarray(reference["lateral_error_mm"])
+    heading = np.asarray(reference["heading_error_degrees"])
+    invalid = frame[
+        "InvalidNearLocalizationJump"
+    ].to_numpy(dtype=bool)
+    lateral_for_statistics = lateral.copy()
+    heading_for_statistics = heading.copy()
+    lateral_for_statistics[invalid] = np.nan
+    heading_for_statistics[invalid] = np.nan
+
+    lateral_rmse = float(
+        np.sqrt(np.nanmean(lateral_for_statistics**2))
+    )
+    heading_rmse = float(
+        np.sqrt(np.nanmean(heading_for_statistics**2))
+    )
+    lateral_max = float(
+        np.nanmax(np.abs(lateral_for_statistics))
+    )
+    heading_max = float(
+        np.nanmax(np.abs(heading_for_statistics))
+    )
+    is_in_place_rotation = (
+        reference["kind"] == "in_place_rotation"
+    )
+
+    fig, axes = plt.subplots(
+        2,
+        1,
+        figsize=(10.0, 7.0),
+        sharex=True,
+    )
+    axes[0].plot(time, lateral, linewidth=1.5)
+    axes[0].axhline(0.0, color="black", linewidth=0.8)
+    if is_in_place_rotation:
+        axes[0].set_ylabel("旋转中心位置漂移 / mm")
+        axes[0].set_title(
+            f"原地自转位置漂移：RMS={lateral_rmse:.2f} mm，"
+            f"最大值={lateral_max:.2f} mm"
+        )
+    else:
+        axes[0].set_ylabel("横向误差 / mm")
+        axes[0].set_title(
+            f"横向误差：RMSE={lateral_rmse:.2f} mm，"
+            f"最大绝对值={lateral_max:.2f} mm"
+        )
+    shade_localization_jump_windows(axes[0], metadata)
+    axes[0].grid(True, alpha=0.3)
+
+    axes[1].plot(
+        time,
+        heading,
+        color="tab:orange",
+        linewidth=1.5,
+    )
+    axes[1].axhline(0.0, color="black", linewidth=0.8)
+    axes[1].set_xlabel("时间 / s")
+    axes[1].set_ylabel(
+        "目标角度剩余误差 / °"
+        if is_in_place_rotation
+        else "航向误差 / °"
+    )
+    axes[1].set_title(
+        (
+            f"目标角度剩余误差：RMSE={heading_rmse:.2f}°，"
+            f"最大绝对值={heading_max:.2f}°"
+        )
+        if is_in_place_rotation
+        else (
+            f"航向误差：RMSE={heading_rmse:.2f}°，"
+            f"最大绝对值={heading_max:.2f}°"
+        )
+    )
+    shade_localization_jump_windows(axes[1], metadata)
+    axes[1].grid(True, alpha=0.3)
+    if metadata["localization_jump_events"]:
+        axes[1].legend(loc="best")
+
+    fig.suptitle(
+        f"横向/航向误差随时间变化\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}"
+    )
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "tracking_errors",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+
+    if is_in_place_rotation:
+        print(
+            f"{csv_path.name}: position drift RMS="
+            f"{lateral_rmse:.3f} mm, "
+            f"target-angle error RMS={heading_rmse:.3f} deg"
+        )
+    else:
+        print(
+            f"{csv_path.name}: lateral RMSE="
+            f"{lateral_rmse:.3f} mm, "
+            f"heading RMSE={heading_rmse:.3f} deg"
+        )
+    return destination
+
+
+def main() -> None:
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="绘制横向误差和航向误差随时间变化图。"
+    )
+    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_errors(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/轨迹测试处理/plot_trajectory_comparison.py

@@ -0,0 +1,920 @@
+"""绘制理想轨迹与Detour实际轨迹对比图。
+
+不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
+本文件也提供其余三个绘图脚本共用的数据预处理函数。
+"""
+
+from __future__ import annotations
+
+import argparse
+import re
+from pathlib import Path
+from typing import Any
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+from scipy.signal import savgol_filter
+
+
+SCRIPT_DIR = Path(__file__).resolve().parent
+REQUIRED_COLUMNS = {
+    "ElapsedSeconds",
+    "TrajectoryName",
+    "DetourX",
+    "DetourY",
+    "DetourTheta",
+    "CommandSpeed",
+    "CommandAngularSpeed",
+    "ReferenceStartX",
+    "ReferenceStartY",
+    "ReferenceEndX",
+    "ReferenceEndY",
+    "ReferenceSpeed",
+}
+
+
+def configure_matplotlib() -> None:
+    """配置中文字体和图片输出风格。"""
+    matplotlib.rcParams["font.sans-serif"] = [
+        "Microsoft YaHei",
+        "SimHei",
+        "Arial Unicode MS",
+        "DejaVu Sans",
+    ]
+    matplotlib.rcParams["axes.unicode_minus"] = False
+    matplotlib.rcParams["figure.dpi"] = 120
+
+
+def _odd_window_length(
+    sample_count: int,
+    sample_interval: float,
+    window_seconds: float,
+    polynomial_order: int = 2,
+) -> int | None:
+    """计算不超过数据长度的Savitzky-Golay奇数窗口。"""
+    requested = max(
+        polynomial_order + 2,
+        int(round(window_seconds / sample_interval)),
+    )
+    if requested % 2 == 0:
+        requested += 1
+
+    maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
+    window = min(requested, maximum)
+    minimum = polynomial_order + 2
+    if minimum % 2 == 0:
+        minimum += 1
+
+    return window if window >= minimum else None
+
+
+def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
+    """将角度差归一化到[-180°, 180°)。"""
+    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,
+    window_seconds: float,
+    derivative: int = 0,
+    polynomial_order: int = 2,
+) -> np.ndarray:
+    """对含NaN断点的数据逐段执行SG滤波或求导。"""
+    values = np.asarray(values, dtype=float)
+    result = np.full_like(values, np.nan)
+    finite_indices = np.flatnonzero(np.isfinite(values))
+    if finite_indices.size == 0:
+        return result
+
+    breaks = np.flatnonzero(np.diff(finite_indices) > 1)
+    starts = np.r_[0, breaks + 1]
+    ends = np.r_[breaks + 1, finite_indices.size]
+
+    for start_index, end_index in zip(starts, ends):
+        indices = finite_indices[start_index:end_index]
+        segment = values[indices]
+        window = _odd_window_length(
+            len(segment),
+            sample_interval,
+            window_seconds,
+            polynomial_order,
+        )
+        if window is not None:
+            result[indices] = savgol_filter(
+                segment,
+                window,
+                polynomial_order,
+                deriv=derivative,
+                delta=sample_interval,
+                mode="interp",
+            )
+        elif derivative == 0:
+            result[indices] = segment
+        elif len(segment) >= 2:
+            result[indices] = np.gradient(segment, sample_interval)
+
+    return result
+
+
+def shade_localization_jump_windows(
+    axis,
+    metadata: dict[str, Any],
+) -> None:
+    """在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
+    for index, (start, end) in enumerate(
+        metadata["jump_exclusion_windows"]
+    ):
+        axis.axvspan(
+            start,
+            end,
+            color="tab:red",
+            alpha=0.12,
+            label="Detour定位跳变排除窗口" if index == 0 else None,
+        )
+
+
+def load_and_resample(
+    csv_path: Path,
+    frequency_hz: float = 20.0,
+    filter_window_seconds: float = 0.55,
+) -> tuple[pd.DataFrame, dict[str, Any]]:
+    """压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
+    if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
+        raise ValueError("重采样频率必须是正有限值。")
+
+    raw = pd.read_csv(csv_path)
+    missing = REQUIRED_COLUMNS.difference(raw.columns)
+    if missing:
+        raise ValueError(
+            f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
+        )
+
+    numeric_columns = [
+        "ElapsedSeconds",
+        "DetourX",
+        "DetourY",
+        "DetourTheta",
+        "CommandSpeed",
+        "CommandAngularSpeed",
+        "ReferenceStartX",
+        "ReferenceStartY",
+        "ReferenceEndX",
+        "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")
+
+    raw = (
+        raw.dropna(subset=[
+            "ElapsedSeconds",
+            "DetourX",
+            "DetourY",
+            "DetourTheta",
+        ])
+        .sort_values("ElapsedSeconds")
+        .drop_duplicates("ElapsedSeconds", keep="last")
+        .reset_index(drop=True)
+    )
+    if len(raw) < 5:
+        raise ValueError(f"{csv_path.name}有效数据不足5行。")
+
+    time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
+    time_raw = time_raw - time_raw[0]
+    raw["ElapsedSeconds"] = time_raw
+    duration = float(time_raw[-1])
+    sample_interval = 1.0 / frequency_hz
+    time_uniform = np.arange(
+        0.0,
+        duration + sample_interval * 0.5,
+        sample_interval,
+    )
+
+    def interpolate_command(column: str) -> np.ndarray:
+        values = raw[column].to_numpy(dtype=float)
+        return np.interp(time_uniform, time_raw, values)
+
+    # 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
+    # 速度估计前先保留真正发生位姿更新的样本。
+    x_all = raw["DetourX"].to_numpy(dtype=float)
+    y_all = raw["DetourY"].to_numpy(dtype=float)
+    theta_all = raw["DetourTheta"].to_numpy(dtype=float)
+    position_change = np.hypot(np.diff(x_all), np.diff(y_all))
+    heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
+    update_mask = np.r_[
+        True,
+        (position_change > 1e-6) | (heading_change > 1e-6),
+    ]
+    updates = raw.loc[update_mask].copy().reset_index(drop=True)
+    if len(updates) < 3:
+        raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")
+
+    update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
+    update_x = updates["DetourX"].to_numpy(dtype=float)
+    update_y = updates["DetourY"].to_numpy(dtype=float)
+    update_theta = updates["DetourTheta"].to_numpy(dtype=float)
+    update_command_speed = np.abs(
+        updates["CommandSpeed"].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，防止把普通定位噪声误判为跳变。
+    update_dt = np.diff(update_time)
+    update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
+    expected_distance = (
+        0.5 *
+        (update_command_speed[1:] + update_command_speed[:-1]) *
+        update_dt *
+        1000.0
+    )
+    distance_threshold = np.maximum(
+        30.0,
+        expected_distance * 3.0 + 15.0,
+    )
+    update_heading_delta = np.abs(
+        wrap_degrees(np.diff(update_theta))
+    )
+    expected_heading_delta = (
+        0.5 *
+        (
+            update_command_angular_rad[1:] +
+            update_command_angular_rad[:-1]
+        ) *
+        update_dt *
+        180.0 / np.pi
+    )
+    heading_threshold = np.maximum(
+        5.0,
+        expected_heading_delta * 3.0 + 2.0,
+    )
+    jump_before_current = (
+        (update_distance > distance_threshold) |
+        (update_heading_delta > heading_threshold)
+    )
+    jump_at_update = np.r_[False, jump_before_current]
+    segment_ids = np.cumsum(jump_at_update.astype(int))
+
+    jump_events: list[dict[str, float]] = []
+    for current_index in np.flatnonzero(jump_at_update):
+        previous_index = current_index - 1
+        jump_events.append({
+            "time_seconds": float(update_time[current_index]),
+            "distance_mm": float(update_distance[previous_index]),
+            "heading_change_degrees":
+                float(update_heading_delta[previous_index]),
+            "before_x_mm": float(update_x[previous_index]),
+            "before_y_mm": float(update_y[previous_index]),
+            "after_x_mm": float(update_x[current_index]),
+            "after_y_mm": float(update_y[current_index]),
+        })
+
+    # 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
+    # 也防止SG滤波把坐标修正涂抹成车辆高速运动。
+    x_resampled = np.full_like(time_uniform, np.nan)
+    y_resampled = np.full_like(time_uniform, np.nan)
+    theta_resampled = np.full_like(time_uniform, np.nan)
+    update_theta_unwrapped = np.rad2deg(
+        np.unwrap(np.deg2rad(update_theta))
+    )
+    maximum_segment_id = int(segment_ids[-1])
+    for segment_id in range(maximum_segment_id + 1):
+        segment_mask = segment_ids == segment_id
+        segment_time = update_time[segment_mask]
+        if segment_time.size == 0:
+            continue
+
+        interval_start = (
+            0.0 if segment_id == 0 else float(segment_time[0])
+        )
+        interval_end = (
+            duration
+            if segment_id == maximum_segment_id
+            else float(segment_time[-1])
+        )
+        uniform_mask = (
+            (time_uniform >= interval_start) &
+            (time_uniform <= interval_end)
+        )
+        x_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_x[segment_mask],
+        )
+        y_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_y[segment_mask],
+        )
+        theta_resampled[uniform_mask] = np.interp(
+            time_uniform[uniform_mask],
+            segment_time,
+            update_theta_unwrapped[segment_mask],
+        )
+
+    x_filtered = segmented_savgol(
+        x_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+    y_filtered = segmented_savgol(
+        y_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+    theta_filtered = segmented_savgol(
+        theta_resampled,
+        sample_interval,
+        filter_window_seconds,
+    )
+
+    exclusion_half_width = max(
+        0.30,
+        filter_window_seconds * 0.5,
+    )
+    jump_exclusion_windows = [
+        (
+            max(0.0, event["time_seconds"] - exclusion_half_width),
+            min(duration, event["time_seconds"] + exclusion_half_width),
+        )
+        for event in jump_events
+    ]
+    invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
+    for start, end in jump_exclusion_windows:
+        invalid_near_jump |= (
+            (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,
+        "DetourYRawMm": y_resampled,
+        "DetourXFilteredMm": x_filtered,
+        "DetourYFilteredMm": y_filtered,
+        "DetourThetaUnwrappedDeg": theta_filtered,
+        "DetourThetaDeg": wrap_degrees(theta_filtered),
+        "CommandSpeedMps": interpolate_command("CommandSpeed"),
+        "CommandAngularSpeedRadPerSec":
+            angular_command_rad,
+        "InvalidNearLocalizationJump": invalid_near_jump,
+    })
+
+    first = raw.iloc[0]
+    metadata: dict[str, Any] = {
+        "csv_path": csv_path,
+        "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,
+        ),
+        "reference_end_mm": np.array(
+            [first["ReferenceEndX"], first["ReferenceEndY"]],
+            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,
+        "filter_window_seconds": filter_window_seconds,
+        "raw_sample_count": len(raw),
+        "detour_update_count": len(updates),
+        "held_sample_count": int(len(raw) - len(updates)),
+        "localization_jump_events": jump_events,
+        "jump_exclusion_windows": jump_exclusion_windows,
+    }
+    return frame, metadata
+
+
+def build_reference(
+    frame: pd.DataFrame,
+    metadata: dict[str, Any],
+) -> dict[str, np.ndarray | float | str]:
+    """根据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)
+    actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)
+
+    radius_match = re.search(
+        r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
+        trajectory_name,
+        flags=re.IGNORECASE,
+    )
+    if radius_match:
+        radius = float(radius_match.group("radius"))
+        sweep_degrees = float(radius_match.group("sweep"))
+        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_motion_heading - 90.0
+
+        radial = actual - center
+        distance_to_center = np.linalg.norm(radial, axis=1)
+        radial_angle_degrees = np.rad2deg(
+            np.arctan2(radial[:, 1], radial[:, 0])
+        )
+        radial_angle_radians = np.deg2rad(radial_angle_degrees)
+        reference_points = center + radius * np.column_stack([
+            np.cos(radial_angle_radians),
+            np.sin(radial_angle_radians),
+        ])
+        # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
+        lateral_error = radius - distance_to_center
+        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
+        )
+
+        plot_angles = np.deg2rad(
+            np.linspace(
+                start_radial_degrees,
+                start_radial_degrees + sweep_degrees,
+                361,
+            )
+        )
+        ideal_plot = center + radius * np.column_stack([
+            np.cos(plot_angles),
+            np.sin(plot_angles),
+        ])
+        return {
+            "kind": "left_arc",
+            "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:
+        rotation_match = re.search(
+            r"Rotate(?P<angle>[+-]?[0-9.]+)",
+            trajectory_name,
+            flags=re.IGNORECASE,
+        )
+        if rotation_match:
+            relative_angle_degrees = float(
+                rotation_match.group("angle")
+            )
+            target_heading_degrees = (
+                float(metadata["start_heading_degrees"]) +
+                relative_angle_degrees
+            )
+            reference_points = np.repeat(
+                start[np.newaxis, :],
+                len(frame),
+                axis=0,
+            )
+            position_drift = np.linalg.norm(
+                actual - start,
+                axis=1,
+            )
+            reference_heading = np.full(
+                len(frame),
+                target_heading_degrees,
+            )
+            heading_error = wrap_degrees(
+                actual_heading - reference_heading
+            )
+            ideal_plot = np.repeat(
+                start[np.newaxis, :],
+                2,
+                axis=0,
+            )
+            return {
+                "kind": "in_place_rotation",
+                "ideal_plot_mm": ideal_plot,
+                "reference_points_mm": reference_points,
+                "reference_heading_degrees": reference_heading,
+                # 对原地自转，该字段表示偏离初始旋转中心的距离。
+                "lateral_error_mm": position_drift,
+                "heading_error_degrees": heading_error,
+                "rotation_center_mm": start,
+                "relative_angle_degrees": relative_angle_degrees,
+                "target_heading_degrees": target_heading_degrees,
+            }
+
+        raise ValueError(
+            f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
+        )
+
+    tangent = line / length
+    left_normal = np.array([-tangent[1], tangent[0]])
+    displacement = actual - start
+    progress = np.clip(displacement @ tangent, 0.0, length)
+    reference_points = start + np.outer(progress, tangent)
+    lateral_error = (actual - reference_points) @ left_normal
+    reference_motion_heading_scalar = np.rad2deg(
+        np.arctan2(tangent[1], tangent[0])
+    )
+    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
+    )
+    ideal_plot = np.linspace(start, end, 361)
+    return {
+        "kind": "line",
+        "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,
+    }
+
+
+def discover_csv_files(arguments: list[str]) -> list[Path]:
+    """解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
+    if arguments:
+        files = [Path(item).expanduser().resolve() for item in arguments]
+    else:
+        files = sorted(SCRIPT_DIR.glob("*.csv"))
+    if not files:
+        raise FileNotFoundError("没有找到可处理的CSV文件。")
+    return files
+
+
+def output_path(
+    csv_path: Path,
+    output_directory: str | None,
+    suffix: str,
+) -> Path:
+    """构造图片输出路径并创建目录。"""
+    directory = (
+        Path(output_directory).expanduser().resolve()
+        if output_directory
+        else csv_path.parent / "plots"
+    )
+    directory.mkdir(parents=True, exist_ok=True)
+    return directory / f"{csv_path.stem}_{suffix}.png"
+
+
+def plot_trajectory(
+    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,
+    )
+    reference = build_reference(frame, metadata)
+
+    actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
+    actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
+    ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0
+
+    fig, ax = plt.subplots(figsize=(8.0, 7.0))
+    ax.plot(
+        ideal_m[:, 0],
+        ideal_m[:, 1],
+        "--",
+        linewidth=2.2,
+        label="理想轨迹",
+    )
+    ax.plot(
+        actual_x_m,
+        actual_y_m,
+        linewidth=1.8,
+        label="Detour实际轨迹（滤波后）",
+    )
+    if reference["kind"] == "in_place_rotation":
+        ax.scatter(
+            [ideal_m[0, 0]],
+            [ideal_m[0, 1]],
+            marker="*",
+            s=100,
+            label="理想旋转中心",
+            zorder=5,
+        )
+    else:
+        ax.scatter(
+            [ideal_m[0, 0]],
+            [ideal_m[0, 1]],
+            marker="o",
+            s=55,
+            label="起点",
+            zorder=5,
+        )
+        ax.scatter(
+            [ideal_m[-1, 0]],
+            [ideal_m[-1, 1]],
+            marker="x",
+            s=65,
+            label="终点",
+            zorder=5,
+        )
+    for event_index, event in enumerate(
+        metadata["localization_jump_events"]
+    ):
+        before = np.array([
+            event["before_x_mm"],
+            event["before_y_mm"],
+        ]) / 1000.0
+        after = np.array([
+            event["after_x_mm"],
+            event["after_y_mm"],
+        ]) / 1000.0
+        ax.scatter(
+            [before[0], after[0]],
+            [before[1], after[1]],
+            marker="x",
+            color="tab:red",
+            s=55,
+            zorder=6,
+            label="Detour定位跳变前/后"
+            if event_index == 0 else None,
+        )
+        ax.annotate(
+            f"定位跳变 {event['distance_mm']:.1f} mm\n"
+            f"t={event['time_seconds']:.2f} s",
+            xy=(after[0], after[1]),
+            xytext=(8, 8),
+            textcoords="offset points",
+            color="tab:red",
+            fontsize=9,
+        )
+    ax.set_aspect("equal", adjustable="box")
+    ax.set_xlabel("世界坐标 X / m")
+    ax.set_ylabel("世界坐标 Y / m")
+    ax.set_title(
+        f"理想轨迹与实际轨迹对比\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}"
+    )
+    ax.grid(True, alpha=0.3)
+    ax.legend()
+    fig.tight_layout()
+
+    destination = output_path(
+        csv_path,
+        output_directory,
+        "trajectory_comparison",
+    )
+    fig.savefig(destination, dpi=300, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    print(
+        f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
+        f"有效Detour更新{metadata['detour_update_count']}帧，"
+        f"保持重复{metadata['held_sample_count']}帧，"
+        f"定位跳变{len(metadata['localization_jump_events'])}次"
+    )
+    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_trajectory(
+            csv_path,
+            args.frequency,
+            args.window,
+            args.output_dir,
+            args.show,
+        )
+        print(f"已生成：{destination}")
+
+
+if __name__ == "__main__":
+    main()

data_process/轨迹测试处理/requirements.txt

@@ -0,0 +1,4 @@
+numpy>=1.26
+pandas>=2.2
+matplotlib>=3.8
+scipy>=1.12

data_process/轨迹测试处理/run_all_plots.py

@@ -0,0 +1,191 @@
+"""一次运行四个轨迹实验绘图脚本。"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import subprocess
+import sys
+from concurrent.futures import ThreadPoolExecutor, as_completed
+from pathlib import Path
+
+
+SCRIPT_NAMES = (
+    "plot_trajectory_comparison.py",
+    "plot_tracking_errors.py",
+    "plot_speed_response.py",
+    "plot_angular_command.py",
+)
+
+
+def build_command(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> list[str]:
+    """为一个绘图脚本构造与统一入口一致的命令行参数。"""
+    command = [
+        sys.executable,
+        str(script_path),
+        *files,
+        "--frequency",
+        str(frequency_hz),
+        "--window",
+        str(filter_window_seconds),
+    ]
+
+    if output_directory:
+        command.extend(["--output-dir", output_directory])
+
+    if show:
+        command.append("--show")
+
+    return command
+
+
+def run_script(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> tuple[str, int, str, str]:
+    """运行一个绘图脚本并返回名称、退出码及标准输出和错误。"""
+    result = subprocess.run(
+        build_command(
+            script_path,
+            files,
+            frequency_hz,
+            filter_window_seconds,
+            output_directory,
+            show,
+        ),
+        cwd=script_path.parent,
+        capture_output=True,
+        text=True,
+        encoding="utf-8",
+        errors="replace",
+        env={
+            **os.environ,
+            "PYTHONIOENCODING": "utf-8",
+        },
+        check=False,
+    )
+
+    return (
+        script_path.name,
+        result.returncode,
+        result.stdout.strip(),
+        result.stderr.strip(),
+    )
+
+
+def main() -> None:
+    """并行执行四类实验图的生成任务。"""
+    parser = argparse.ArgumentParser(
+        description="一次生成轨迹、误差、速度响应和角速度指令四类图。"
+    )
+    parser.add_argument(
+        "files",
+        nargs="*",
+        help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。",
+    )
+    parser.add_argument(
+        "--frequency",
+        type=float,
+        default=20.0,
+        help="固定重采样频率，默认20 Hz。",
+    )
+    parser.add_argument(
+        "--window",
+        type=float,
+        default=0.55,
+        help="Savitzky-Golay滤波窗口，默认0.55 s。",
+    )
+    parser.add_argument(
+        "--output-dir",
+        help="图片输出目录；省略时由各绘图脚本使用默认目录。",
+    )
+    parser.add_argument(
+        "--show",
+        action="store_true",
+        help="生成后请求显示图片。",
+    )
+    args = parser.parse_args()
+
+    if args.frequency <= 0.0:
+        parser.error("--frequency必须大于0。")
+
+    if args.window <= 0.0:
+        parser.error("--window必须大于0。")
+
+    script_directory = Path(__file__).resolve().parent
+    script_paths = [
+        script_directory / name
+        for name in SCRIPT_NAMES
+    ]
+    missing_scripts = [
+        str(path)
+        for path in script_paths
+        if not path.is_file()
+    ]
+    if missing_scripts:
+        parser.error(
+            "缺少绘图脚本：" + "，".join(missing_scripts)
+        )
+
+    print("开始并行生成四类实验图……")
+    failures: list[str] = []
+
+    with ThreadPoolExecutor(
+        max_workers=len(script_paths)
+    ) as executor:
+        futures = [
+            executor.submit(
+                run_script,
+                script_path,
+                args.files,
+                args.frequency,
+                args.window,
+                args.output_dir,
+                args.show,
+            )
+            for script_path in script_paths
+        ]
+
+        for future in as_completed(futures):
+            script_name, return_code, stdout, stderr = (
+                future.result()
+            )
+            print(f"\n[{script_name}]")
+            if stdout:
+                print(stdout)
+            if stderr:
+                print(stderr, file=sys.stderr)
+
+            if return_code == 0:
+                print("执行成功。")
+            else:
+                failures.append(script_name)
+                print(
+                    f"执行失败，退出码={return_code}。",
+                    file=sys.stderr,
+                )
+
+    if failures:
+        print(
+            "\n以下脚本执行失败：" +
+            "，".join(failures),
+            file=sys.stderr,
+        )
+        raise SystemExit(1)
+
+    print("\n四类实验图均已生成。")
+
+
+if __name__ == "__main__":
+    main()