完善轨迹跟踪测试并添加实验数据记录与绘图分析工具

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

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[
+        "CommandAngularSpeedDegPerSec"
+    ].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("命令角速度 / (°/s)")
+    ax.set_title(
+        f"角速度指令曲线\n"
+        f"{metadata['controller_name']} - "
+        f"{metadata['trajectory_name']}，"
+        f"范围=[{minimum:.3f}, {maximum:.3f}]°/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,706 @@
+"""绘制理想轨迹与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 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",
+    ]
+    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)
+    )
+    update_command_angular = 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[1:] + update_command_angular[:-1]) *
+        update_dt
+    )
+    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)
+        )
+
+    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"),
+        "CommandAngularSpeedDegPerSec":
+            interpolate_command("CommandAngularSpeed"),
+        "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", "")),
+        "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"]),
+        # 圆弧构造时使用了测试开始处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元数据建立直线、圆弧或原地自转参考及误差。"""
+    trajectory_name = str(metadata["trajectory_name"])
+    start = np.asarray(metadata["reference_start_mm"], dtype=float)
+    end = np.asarray(metadata["reference_end_mm"], dtype=float)
+    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_heading = float(metadata["start_heading_degrees"])
+        heading_radians = np.deg2rad(start_heading)
+        center = start + radius * np.array(
+            [-np.sin(heading_radians), np.cos(heading_radians)]
+        )
+        start_radial_degrees = start_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_heading = radial_angle_degrees + 90.0
+        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,
+            "lateral_error_mm": lateral_error,
+            "heading_error_degrees": heading_error,
+            "center_mm": center,
+            "radius_mm": radius,
+        }
+
+    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_heading_scalar = np.rad2deg(
+        np.arctan2(tangent[1], tangent[0])
+    )
+    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,
+        "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()