完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

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

data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py

@@ -208,10 +208,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     has_control_reference = (
         numeric_column(frame, "HasControlReference", 0.0) > 0.5
     )
-    lateral_error = np.where(
-        has_control_reference & np.isfinite(recorded_lateral_error),
-        recorded_lateral_error,
-        derived_lateral_error,
+    recorded_lateral_valid = (
+        has_control_reference & np.isfinite(recorded_lateral_error)
+    )
+    lateral_error = (
+        np.where(recorded_lateral_valid, recorded_lateral_error, np.nan)
+        if np.any(recorded_lateral_valid)
+        else derived_lateral_error
     )
 
     state_yaw = numeric_column(frame, "StateYawRadians")
@@ -230,10 +233,28 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         frame,
         "ControlHeadingErrorRadians",
     )
-    heading_error = np.where(
-        has_control_reference & np.isfinite(recorded_heading_error),
-        recorded_heading_error,
-        derived_heading_error,
+    recorded_heading_valid = (
+        has_control_reference & np.isfinite(recorded_heading_error)
+    )
+    heading_error = (
+        np.where(recorded_heading_valid, recorded_heading_error, np.nan)
+        if np.any(recorded_heading_valid)
+        else derived_heading_error
+    )
+
+    # 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。
+    # 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。
+    projected_reference_yaw = actual_yaw + heading_error
+    reference_x = (
+        actual_x - lateral_error * np.sin(projected_reference_yaw)
+    )
+    reference_y = (
+        actual_y + lateral_error * np.cos(projected_reference_yaw)
+    )
+    valid_reference_position = (
+        has_control_reference
+        & np.isfinite(reference_x)
+        & np.isfinite(reference_y)
     )
 
     cruise_speed = first_finite(
@@ -270,6 +291,8 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
         ideal_speed,
     )
+    if np.any(has_control_reference):
+        reference_speed[~has_control_reference] = np.nan
     actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
@@ -283,6 +306,9 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         "actual_x": actual_x,
         "actual_y": actual_y,
         "valid_position": valid_position,
+        "reference_x": reference_x,
+        "reference_y": reference_y,
+        "valid_reference_position": valid_reference_position,
         "start": start,
         "end": end,
         "length": length_meters,
@@ -336,13 +362,23 @@ def plot_experiment(
 
     valid_position = data["valid_position"]
     fig, axis = plt.subplots(figsize=(9.0, 6.5))
-    axis.plot(
-        [data["start"][0], data["end"][0]],
-        [data["start"][1], data["end"][1]],
-        "--",
-        linewidth=2.0,
-        label="期望4m直线轨迹",
-    )
+    valid_reference_position = data["valid_reference_position"]
+    if np.count_nonzero(valid_reference_position) >= 2:
+        axis.plot(
+            data["reference_x"][valid_reference_position],
+            data["reference_y"][valid_reference_position],
+            "--",
+            linewidth=2.0,
+            label="控制器实际使用的参考轨迹",
+        )
+    else:
+        axis.plot(
+            [data["start"][0], data["end"][0]],
+            [data["start"][1], data["end"][1]],
+            "--",
+            linewidth=2.0,
+            label="参考起终点连线",
+        )
     axis.plot(
         data["actual_x"][valid_position],
         data["actual_y"][valid_position],
@@ -451,7 +487,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
 def main() -> None:
     """解析命令行并批量处理新版控制器实验CSV。"""
     parser = argparse.ArgumentParser(
-        description="绘制新版控制器4m直线实验的四类对比图。"
+        description="绘制新版控制器轨迹实验的四类对比图。"
     )
     parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
     parser.add_argument(