First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善Detour状态估计与轨迹跟踪验证

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process/plot_new_controller_experiment.py
+290 -11
View File
@@ -14,6 +14,10 @@ import pandas as pd
SCRIPT_DIR = Path(__file__).resolve().parent
LATERAL_JUMP_THRESHOLD_METERS = 0.03
JUMP_INSET_CONTEXT_SAMPLES = 6
MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND = 1.20
POSITION_JUMP_MARGIN_METERS = 0.03
def configure_matplotlib() -> None:
@@ -57,6 +61,16 @@ def first_text(frame: pd.DataFrame, name: str, default: str) -> str:
return values.iloc[0] if not values.empty else default
def text_column(frame: pd.DataFrame, name: str) -> np.ndarray:
"""读取用于诊断标注的原始文本列，缺失值转换为空字符串。"""
if name not in frame.columns:
return np.full(len(frame), "", dtype=object)
return frame[name].fillna("").astype(str).to_numpy(
dtype=object,
copy=True,
)
def fill_reference_series(
values: np.ndarray,
fallback: np.ndarray,
@@ -169,6 +183,11 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
raw_x_meters = numeric_column(frame, "DetourX") / 1000.0
raw_y_meters = numeric_column(frame, "DetourY") / 1000.0
valid_raw_position = (
np.isfinite(raw_x_meters) & np.isfinite(raw_y_meters)
)
detour_tick_raw = text_column(frame, "DetourTickRaw")
detour_l_step = numeric_column(frame, "DetourLStep")
actual_x = np.where(processed_valid, state_x, raw_x_meters)
actual_y = np.where(processed_valid, state_y, raw_y_meters)
valid_position = np.isfinite(actual_x) & np.isfinite(actual_y)
@@ -257,6 +276,67 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
& np.isfinite(reference_y)
)
# 同时检查Detour是否超出车辆物理运动边界，以及控制状态的横向误差
# 是否发生离散突变。后者能覆盖状态层延迟接受持续定位偏移的情况。
raw_delta_x = np.full(len(frame), np.nan, dtype=float)
raw_delta_y = np.full(len(frame), np.nan, dtype=float)
sample_delta_time = np.full(len(frame), np.nan, dtype=float)
raw_delta_x[1:] = np.diff(raw_x_meters)
raw_delta_y[1:] = np.diff(raw_y_meters)
sample_delta_time[1:] = np.diff(time_seconds)
detour_position_step = np.hypot(raw_delta_x, raw_delta_y)
detour_tick_numeric = numeric_column(frame, "DetourTickRaw")
detour_tick_delta_seconds = np.full(len(frame), np.nan, dtype=float)
detour_tick_delta_seconds[1:] = (
np.diff(detour_tick_numeric) / 10_000_000.0
)
source_delta_time = sample_delta_time.copy()
valid_tick_delta = (
np.isfinite(detour_tick_delta_seconds)
& (detour_tick_delta_seconds > 0.0)
& (detour_tick_delta_seconds <= 0.5)
)
source_delta_time[valid_tick_delta] = (
detour_tick_delta_seconds[valid_tick_delta]
)
consecutive_raw_position_valid = np.zeros(len(frame), dtype=bool)
consecutive_raw_position_valid[1:] = (
valid_raw_position[1:] & valid_raw_position[:-1]
)
maximum_plausible_position_step = (
MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND
* source_delta_time
+ POSITION_JUMP_MARGIN_METERS
)
raw_detour_jump = (
consecutive_raw_position_valid
& np.isfinite(detour_position_step)
& np.isfinite(source_delta_time)
& (source_delta_time > 0.0)
& (source_delta_time <= 0.5)
& (detour_position_step > maximum_plausible_position_step)
)
state_lateral_step = np.full(len(frame), np.nan, dtype=float)
state_lateral_step[1:] = np.diff(lateral_error)
state_lateral_jump = (
np.isfinite(state_lateral_step)
& np.isfinite(sample_delta_time)
& (sample_delta_time > 0.0)
& (sample_delta_time <= 0.5)
& (np.abs(state_lateral_step) >= LATERAL_JUMP_THRESHOLD_METERS)
)
suspected_jump = raw_detour_jump | state_lateral_jump
jump_indices = np.flatnonzero(suspected_jump)
jump_magnitude = np.zeros(len(frame), dtype=float)
jump_magnitude[raw_detour_jump] = detour_position_step[raw_detour_jump]
jump_magnitude[state_lateral_jump] = np.maximum(
jump_magnitude[state_lateral_jump],
np.abs(state_lateral_step[state_lateral_jump]),
)
cruise_speed = first_finite(
numeric_column(frame, "ReferenceSpeed"),
0.30,
@@ -420,6 +500,17 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
"actual_x": actual_x,
"actual_y": actual_y,
"valid_position": valid_position,
"raw_x": raw_x_meters,
"raw_y": raw_y_meters,
"valid_raw_position": valid_raw_position,
"detour_tick_raw": detour_tick_raw,
"detour_l_step": detour_l_step,
"detour_position_step": detour_position_step,
"state_lateral_step": state_lateral_step,
"raw_detour_jump": raw_detour_jump,
"state_lateral_jump": state_lateral_jump,
"jump_magnitude": jump_magnitude,
"jump_indices": jump_indices,
"reference_x": reference_x,
"reference_y": reference_y,
"valid_reference_position": valid_reference_position,
@@ -484,7 +575,9 @@ def plot_experiment(
# 1. 期望轨迹与实际轨迹。
axis = axes[0, 0]
valid_position = data["valid_position"]
valid_raw_position = data["valid_raw_position"]
valid_reference_position = data["valid_reference_position"]
jump_indices = data["jump_indices"]
if np.count_nonzero(valid_reference_position) >= 2:
axis.plot(
data["reference_x"][valid_reference_position],
@@ -501,26 +594,166 @@ def plot_experiment(
linewidth=2.0,
label="参考起终点连线",
)
axis.plot(
data["raw_x"][valid_raw_position],
data["raw_y"][valid_raw_position],
":",
color="tab:gray",
linewidth=1.2,
alpha=0.85,
label="Detour原始轨迹",
)
axis.plot(
data["actual_x"][valid_position],
data["actual_y"][valid_position],
color="tab:orange",
linewidth=1.5,
label="状态估计后的实际轨迹",
label="控制使用的状态轨迹",
)
if jump_indices.size:
axis.scatter(
data["raw_x"][jump_indices],
data["raw_y"][jump_indices],
color="red",
marker="x",
s=65,
linewidths=1.8,
zorder=8,
label="疑似定位/状态突变",
)
axis.scatter(*data["start"], color="green", s=45, label="起点")
axis.scatter(*data["end"], color="red", s=45, label="终点")
axis.set_aspect("equal", adjustable="box")
# 诊断图优先展示厘米级横向变化；横纵轴独立缩放，避免4m行程
# 将数厘米的定位阶跃压缩成几乎不可见的一条细线。
axis.set_aspect("auto")
axis.set_xlabel("世界坐标X / m")
axis.set_ylabel("世界坐标Y / m")
axis.set_title("期望轨迹与实际轨迹对比")
axis.set_title("期望轨迹与状态轨迹对比（横纵轴独立缩放）")
axis.grid(True, alpha=0.3)
axis.legend(fontsize=8)
axis.legend(fontsize=7, loc="upper left")
if jump_indices.size:
strongest_jump_index = int(
jump_indices[
np.argmax(
np.abs(
data["jump_magnitude"][jump_indices]
)
)
]
)
context_start = max(
0,
strongest_jump_index - JUMP_INSET_CONTEXT_SAMPLES,
)
context_end = min(
len(data["time"]),
strongest_jump_index + JUMP_INSET_CONTEXT_SAMPLES + 1,
)
context = np.arange(context_start, context_end)
inset = axis.inset_axes([0.54, 0.08, 0.43, 0.43])
inset.set_zorder(10)
inset.set_facecolor("white")
context_reference_valid = (
data["valid_reference_position"][context]
)
if np.count_nonzero(context_reference_valid) >= 2:
reference_context = context[context_reference_valid]
inset.plot(
data["reference_x"][reference_context],
data["reference_y"][reference_context],
"--",
linewidth=1.2,
color="tab:blue",
)
context_raw_valid = data["valid_raw_position"][context]
raw_context = context[context_raw_valid]
inset.plot(
data["raw_x"][raw_context],
data["raw_y"][raw_context],
":",
linewidth=1.0,
color="tab:gray",
)
context_state_valid = data["valid_position"][context]
state_context = context[context_state_valid]
inset.plot(
data["actual_x"][state_context],
data["actual_y"][state_context],
linewidth=1.2,
color="tab:orange",
)
inset.scatter(
data["raw_x"][strongest_jump_index],
data["raw_y"][strongest_jump_index],
color="red",
marker="x",
s=45,
linewidths=1.5,
zorder=8,
)
jump_descriptions = []
if data["raw_detour_jump"][strongest_jump_index]:
jump_descriptions.append(
"Detour位移="
f"{data['detour_position_step'][strongest_jump_index] * 1000.0:.1f}mm"
)
if data["state_lateral_jump"][strongest_jump_index]:
jump_descriptions.append(
"状态横向Δ="
f"{data['state_lateral_step'][strongest_jump_index] * 1000.0:+.1f}mm"
)
diagnostic_parts = []
detour_tick = data["detour_tick_raw"][strongest_jump_index]
if detour_tick:
diagnostic_parts.append(f"tick={detour_tick}")
detour_l_step = data["detour_l_step"][strongest_jump_index]
if np.isfinite(detour_l_step):
diagnostic_parts.append(f"l_step={detour_l_step:g}")
diagnostic_suffix = (
"\n" + " ".join(diagnostic_parts)
if diagnostic_parts
else ""
)
inset.set_title(
f"最大疑似突变：t={data['time'][strongest_jump_index]:.3f}s\n"
f"{'，'.join(jump_descriptions)}"
f"{diagnostic_suffix}",
fontsize=7,
)
inset.set_aspect("auto")
inset.tick_params(labelsize=6)
inset.grid(True, alpha=0.25)
# 2. 横向误差。
lateral_mm = data["lateral_error"] * 1000.0
lateral_rmse_mm = finite_rmse(lateral_mm)
axis = axes[0, 1]
axis.plot(data["time"], lateral_mm, linewidth=1.5)
if jump_indices.size:
for jump_index in jump_indices:
axis.axvline(
data["time"][jump_index],
color="red",
linewidth=0.8,
alpha=0.35,
)
valid_jump_error = (
data["state_lateral_jump"][jump_indices]
& np.isfinite(lateral_mm[jump_indices])
)
visible_jump_indices = jump_indices[valid_jump_error]
if visible_jump_indices.size:
axis.scatter(
data["time"][visible_jump_indices],
lateral_mm[visible_jump_indices],
color="red",
marker="x",
s=45,
linewidths=1.5,
zorder=7,
label="控制状态横向突变",
)
axis.axhline(0.0, color="black", linewidth=0.8)
axis.set_xlabel("时间 / s")
axis.set_ylabel("横向误差 / mm")
@@ -529,6 +762,11 @@ def plot_experiment(
f"RMSE={lateral_rmse_mm:.2f}mm"
)
axis.grid(True, alpha=0.3)
if jump_indices.size and np.any(
data["state_lateral_jump"][jump_indices]
& np.isfinite(lateral_mm[jump_indices])
):
axis.legend(fontsize=8)
# 3. 航向误差。
heading_degrees = np.rad2deg(data["heading_error"])
@@ -677,21 +915,58 @@ def plot_experiment(
f"航向RMSE={heading_rmse_degrees:.4f}°, "
f"速度RMSE={speed_rmse:.5f}m/s"
)
if jump_indices.size:
strongest_jump_index = int(
jump_indices[
np.argmax(
np.abs(
data["jump_magnitude"][jump_indices]
)
)
]
)
print(
f" 检出{jump_indices.size}个疑似定位/状态突变，"
f"最大幅值={data['jump_magnitude'][strongest_jump_index] * 1000.0:.2f}mm，"
f"时刻={data['time'][strongest_jump_index]:.3f}s"
)
print(f"已生成六子图总图：{destination}")
return [destination]
def discover_csv_files(arguments: list[str]) -> list[Path]:
"""读取命令行文件；未指定时扫描脚本目录及data子目录中的CSV。"""
"""读取命令行文件或目录；目录中只选取非计时CSV。"""
if arguments:
files = [Path(item).expanduser().resolve() for item in arguments]
files = []
for item in arguments:
path = Path(item).expanduser().resolve()
if path.is_dir():
files.extend(
sorted(
candidate
for candidate in path.glob("*.csv")
if not candidate.stem.endswith("_timing")
)
)
else:
files.append(path)
else:
files = sorted(SCRIPT_DIR.glob("*.csv"))
files.extend(sorted((SCRIPT_DIR / "data").glob("*.csv")))
files = [path for path in files if path.is_file()]
files = sorted(
path
for path in SCRIPT_DIR.glob("*.csv")
if not path.stem.endswith("_timing")
)
files.extend(
sorted(
path
for path in (SCRIPT_DIR / "data").glob("*.csv")
if not path.stem.endswith("_timing")
)
)
files = list(dict.fromkeys(path for path in files if path.is_file()))
if not files:
raise FileNotFoundError(
"没有找到CSV；请传入文件路径，或将文件放到脚本目录/data中。"
"没有找到轨迹CSV；请传入文件、目录，或将文件放到脚本目录/data中。"
)
return files
@@ -701,7 +976,11 @@ def main() -> None:
parser = argparse.ArgumentParser(
description="绘制新版控制器轨迹实验的六子图总图。"
)
parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
parser.add_argument(
"csv",
nargs="*",
help="需要处理的轨迹CSV文件或包含轨迹CSV的目录。",
)
parser.add_argument(
"--output-dir",
help="图片输出目录；默认使用脚本目录/plots。",