打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,487 @@
|
||||
"""为新版4m直线控制器实验CSV生成轨迹、横向/航向误差和速度响应图。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
SCRIPT_DIR = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def configure_matplotlib() -> None:
|
||||
"""配置可显示中文和负号的Matplotlib字体。"""
|
||||
plt.rcParams["font.sans-serif"] = [
|
||||
"Microsoft YaHei",
|
||||
"SimHei",
|
||||
"Noto Sans CJK SC",
|
||||
"Arial Unicode MS",
|
||||
"DejaVu Sans",
|
||||
]
|
||||
plt.rcParams["axes.unicode_minus"] = False
|
||||
|
||||
|
||||
def numeric_column(
|
||||
frame: pd.DataFrame,
|
||||
name: str,
|
||||
default: float = np.nan,
|
||||
) -> np.ndarray:
|
||||
"""将CSV列安全转换为浮点数组，缺失列使用指定默认值。"""
|
||||
if name not in frame.columns:
|
||||
return np.full(len(frame), default, dtype=float)
|
||||
return pd.to_numeric(frame[name], errors="coerce").to_numpy(
|
||||
dtype=float,
|
||||
copy=True,
|
||||
)
|
||||
|
||||
|
||||
def first_finite(values: np.ndarray, default: float) -> float:
|
||||
"""读取数组中的第一个有限值。"""
|
||||
finite = values[np.isfinite(values)]
|
||||
return float(finite[0]) if finite.size else default
|
||||
|
||||
|
||||
def first_text(frame: pd.DataFrame, name: str, default: str) -> str:
|
||||
"""读取文本元数据列中的第一个非空值。"""
|
||||
if name not in frame.columns:
|
||||
return default
|
||||
values = frame[name].dropna().astype(str)
|
||||
values = values[values.str.strip() != ""]
|
||||
return values.iloc[0] if not values.empty else default
|
||||
|
||||
|
||||
def fill_reference_series(
|
||||
values: np.ndarray,
|
||||
fallback: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""前后填充后台采样得到的控制参考值，缺失时使用解析速度曲线。"""
|
||||
series = pd.Series(values, dtype=float)
|
||||
filled = series.ffill().bfill().to_numpy(
|
||||
dtype=float,
|
||||
copy=True,
|
||||
)
|
||||
missing = ~np.isfinite(filled)
|
||||
filled[missing] = fallback[missing]
|
||||
return filled
|
||||
|
||||
|
||||
def planned_motion(
|
||||
time_seconds: np.ndarray,
|
||||
length_meters: float,
|
||||
cruise_speed_mps: float,
|
||||
acceleration_mps2: float,
|
||||
deceleration_mps2: float,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""计算从静止出发并在终点静止的梯形或三角形理想时间速度轨迹。"""
|
||||
acceleration_distance = (
|
||||
cruise_speed_mps**2 / (2.0 * acceleration_mps2)
|
||||
)
|
||||
deceleration_distance = (
|
||||
cruise_speed_mps**2 / (2.0 * deceleration_mps2)
|
||||
)
|
||||
|
||||
if acceleration_distance + deceleration_distance <= length_meters:
|
||||
peak_speed = cruise_speed_mps
|
||||
else:
|
||||
peak_speed = np.sqrt(
|
||||
2.0
|
||||
* length_meters
|
||||
/ (1.0 / acceleration_mps2 + 1.0 / deceleration_mps2)
|
||||
)
|
||||
acceleration_distance = (
|
||||
peak_speed**2 / (2.0 * acceleration_mps2)
|
||||
)
|
||||
deceleration_distance = (
|
||||
peak_speed**2 / (2.0 * deceleration_mps2)
|
||||
)
|
||||
|
||||
acceleration_time = peak_speed / acceleration_mps2
|
||||
deceleration_time = peak_speed / deceleration_mps2
|
||||
cruise_distance = max(
|
||||
0.0,
|
||||
length_meters - acceleration_distance - deceleration_distance,
|
||||
)
|
||||
cruise_time = cruise_distance / peak_speed
|
||||
deceleration_start_time = acceleration_time + cruise_time
|
||||
finish_time = deceleration_start_time + deceleration_time
|
||||
|
||||
progress = np.zeros_like(time_seconds, dtype=float)
|
||||
speed = np.zeros_like(time_seconds, dtype=float)
|
||||
|
||||
accelerating = time_seconds <= acceleration_time
|
||||
progress[accelerating] = (
|
||||
0.5 * acceleration_mps2 * time_seconds[accelerating] ** 2
|
||||
)
|
||||
speed[accelerating] = acceleration_mps2 * time_seconds[accelerating]
|
||||
|
||||
cruising = (
|
||||
(time_seconds > acceleration_time)
|
||||
& (time_seconds <= deceleration_start_time)
|
||||
)
|
||||
progress[cruising] = (
|
||||
acceleration_distance
|
||||
+ peak_speed * (time_seconds[cruising] - acceleration_time)
|
||||
)
|
||||
speed[cruising] = peak_speed
|
||||
|
||||
decelerating = (
|
||||
(time_seconds > deceleration_start_time)
|
||||
& (time_seconds <= finish_time)
|
||||
)
|
||||
remaining_time = finish_time - time_seconds[decelerating]
|
||||
progress[decelerating] = (
|
||||
length_meters
|
||||
- 0.5 * deceleration_mps2 * remaining_time**2
|
||||
)
|
||||
speed[decelerating] = deceleration_mps2 * remaining_time
|
||||
|
||||
finished = time_seconds > finish_time
|
||||
progress[finished] = length_meters
|
||||
speed[finished] = 0.0
|
||||
return progress, speed
|
||||
|
||||
|
||||
def load_experiment(csv_path: Path) -> dict[str, object]:
|
||||
"""读取新版CSV并构造绘图所需的统一SI单位数据。"""
|
||||
frame = pd.read_csv(csv_path, encoding="utf-8-sig")
|
||||
if frame.empty:
|
||||
raise ValueError("CSV没有任何采样行。")
|
||||
|
||||
time_seconds = numeric_column(frame, "ElapsedSeconds")
|
||||
valid_time = np.isfinite(time_seconds)
|
||||
frame = frame.loc[valid_time].reset_index(drop=True)
|
||||
time_seconds = time_seconds[valid_time]
|
||||
if time_seconds.size < 2:
|
||||
raise ValueError("CSV中的有效时间采样不足2帧。")
|
||||
time_seconds = time_seconds - time_seconds[0]
|
||||
|
||||
state_x = numeric_column(frame, "StateXMeters")
|
||||
state_y = numeric_column(frame, "StateYMeters")
|
||||
has_processed = numeric_column(frame, "HasProcessedState", 0.0) > 0.5
|
||||
processed_valid = has_processed & np.isfinite(state_x) & np.isfinite(state_y)
|
||||
|
||||
raw_x_meters = numeric_column(frame, "DetourX") / 1000.0
|
||||
raw_y_meters = numeric_column(frame, "DetourY") / 1000.0
|
||||
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)
|
||||
if np.count_nonzero(valid_position) < 2:
|
||||
raise ValueError("CSV中没有足够的有效车辆位置。")
|
||||
|
||||
start = np.array(
|
||||
[
|
||||
first_finite(numeric_column(frame, "ReferenceStartX"), np.nan),
|
||||
first_finite(numeric_column(frame, "ReferenceStartY"), np.nan),
|
||||
],
|
||||
dtype=float,
|
||||
) / 1000.0
|
||||
end = np.array(
|
||||
[
|
||||
first_finite(numeric_column(frame, "ReferenceEndX"), np.nan),
|
||||
first_finite(numeric_column(frame, "ReferenceEndY"), np.nan),
|
||||
],
|
||||
dtype=float,
|
||||
) / 1000.0
|
||||
if not np.all(np.isfinite(start)) or not np.all(np.isfinite(end)):
|
||||
raise ValueError("CSV缺少有效的参考起点或终点。")
|
||||
|
||||
line = end - start
|
||||
length_meters = float(np.linalg.norm(line))
|
||||
if length_meters <= 1e-6:
|
||||
raise ValueError("参考直线长度必须大于0。")
|
||||
tangent = line / length_meters
|
||||
left_normal = np.array([-tangent[1], tangent[0]])
|
||||
displacement = np.column_stack([actual_x, actual_y]) - start
|
||||
# 与C# TrajectoryProjector保持一致：轨迹位于车辆左侧时为正。
|
||||
derived_lateral_error = -(displacement @ left_normal)
|
||||
recorded_lateral_error = numeric_column(
|
||||
frame,
|
||||
"ControlLateralErrorMeters",
|
||||
)
|
||||
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,
|
||||
)
|
||||
|
||||
state_yaw = numeric_column(frame, "StateYawRadians")
|
||||
raw_yaw = np.deg2rad(numeric_column(frame, "DetourTheta"))
|
||||
actual_yaw = np.where(
|
||||
has_processed & np.isfinite(state_yaw),
|
||||
state_yaw,
|
||||
raw_yaw,
|
||||
)
|
||||
reference_yaw = np.arctan2(tangent[1], tangent[0])
|
||||
derived_heading_error = np.arctan2(
|
||||
np.sin(reference_yaw - actual_yaw),
|
||||
np.cos(reference_yaw - actual_yaw),
|
||||
)
|
||||
recorded_heading_error = numeric_column(
|
||||
frame,
|
||||
"ControlHeadingErrorRadians",
|
||||
)
|
||||
heading_error = np.where(
|
||||
has_control_reference & np.isfinite(recorded_heading_error),
|
||||
recorded_heading_error,
|
||||
derived_heading_error,
|
||||
)
|
||||
|
||||
cruise_speed = first_finite(
|
||||
numeric_column(frame, "ReferenceSpeed"),
|
||||
0.30,
|
||||
)
|
||||
acceleration = first_finite(
|
||||
numeric_column(
|
||||
frame,
|
||||
"ReferenceAccelerationMetersPerSecondSquared",
|
||||
),
|
||||
0.20,
|
||||
)
|
||||
deceleration = first_finite(
|
||||
numeric_column(
|
||||
frame,
|
||||
"ReferenceDecelerationMetersPerSecondSquared",
|
||||
),
|
||||
0.20,
|
||||
)
|
||||
if acceleration <= 0.0:
|
||||
acceleration = 0.20
|
||||
if deceleration <= 0.0:
|
||||
deceleration = 0.20
|
||||
|
||||
_, ideal_speed = planned_motion(
|
||||
time_seconds,
|
||||
length_meters,
|
||||
cruise_speed,
|
||||
acceleration,
|
||||
deceleration,
|
||||
)
|
||||
reference_speed = fill_reference_series(
|
||||
numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
|
||||
ideal_speed,
|
||||
)
|
||||
actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
|
||||
velocity_valid = (
|
||||
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
|
||||
)
|
||||
actual_speed[~velocity_valid] = np.nan
|
||||
command_speed = numeric_column(frame, "CommandSpeed")
|
||||
|
||||
return {
|
||||
"frame": frame,
|
||||
"time": time_seconds,
|
||||
"actual_x": actual_x,
|
||||
"actual_y": actual_y,
|
||||
"valid_position": valid_position,
|
||||
"start": start,
|
||||
"end": end,
|
||||
"length": length_meters,
|
||||
"lateral_error": lateral_error,
|
||||
"heading_error": heading_error,
|
||||
"reference_speed": reference_speed,
|
||||
"actual_speed": actual_speed,
|
||||
"command_speed": command_speed,
|
||||
"controller_name": first_text(
|
||||
frame,
|
||||
"ControllerName",
|
||||
"NewController",
|
||||
),
|
||||
"trajectory_name": first_text(
|
||||
frame,
|
||||
"TrajectoryName",
|
||||
"Trajectory",
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def finite_rmse(values: np.ndarray) -> float:
|
||||
"""计算忽略无效样本后的均方根值。"""
|
||||
finite = values[np.isfinite(values)]
|
||||
return float(np.sqrt(np.mean(finite**2))) if finite.size else np.nan
|
||||
|
||||
|
||||
def save_figure(
|
||||
fig: plt.Figure,
|
||||
destination: Path,
|
||||
show: bool,
|
||||
) -> None:
|
||||
"""保存并关闭一张实验图。"""
|
||||
fig.tight_layout()
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
|
||||
|
||||
def plot_experiment(
|
||||
csv_path: Path,
|
||||
output_directory: Path,
|
||||
show: bool,
|
||||
) -> list[Path]:
|
||||
"""为单份新版控制器CSV生成四类对比图。"""
|
||||
data = load_experiment(csv_path)
|
||||
output_directory.mkdir(parents=True, exist_ok=True)
|
||||
title = f"{data['controller_name']} - {data['trajectory_name']}"
|
||||
destinations: list[Path] = []
|
||||
|
||||
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直线轨迹",
|
||||
)
|
||||
axis.plot(
|
||||
data["actual_x"][valid_position],
|
||||
data["actual_y"][valid_position],
|
||||
linewidth=1.5,
|
||||
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")
|
||||
axis.set_xlabel("世界坐标X / m")
|
||||
axis.set_ylabel("世界坐标Y / m")
|
||||
axis.set_title(f"期望轨迹与实际轨迹对比\n{title}")
|
||||
axis.grid(True, alpha=0.3)
|
||||
axis.legend()
|
||||
destination = output_directory / f"{csv_path.stem}_trajectory.png"
|
||||
save_figure(fig, destination, show)
|
||||
destinations.append(destination)
|
||||
|
||||
lateral_mm = data["lateral_error"] * 1000.0
|
||||
lateral_rmse_mm = finite_rmse(lateral_mm)
|
||||
fig, axis = plt.subplots(figsize=(10.0, 5.5))
|
||||
axis.plot(data["time"], lateral_mm, linewidth=1.5)
|
||||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||||
axis.set_xlabel("时间 / s")
|
||||
axis.set_ylabel("横向误差 / mm")
|
||||
axis.set_title(
|
||||
f"横向误差（轨迹在车辆左侧为正）\n{title}，RMSE={lateral_rmse_mm:.2f}mm"
|
||||
)
|
||||
axis.grid(True, alpha=0.3)
|
||||
destination = output_directory / f"{csv_path.stem}_lateral_error.png"
|
||||
save_figure(fig, destination, show)
|
||||
destinations.append(destination)
|
||||
|
||||
heading_degrees = np.rad2deg(data["heading_error"])
|
||||
heading_rmse_degrees = finite_rmse(heading_degrees)
|
||||
fig, axis = plt.subplots(figsize=(10.0, 5.5))
|
||||
axis.plot(data["time"], heading_degrees, linewidth=1.5)
|
||||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||||
axis.set_xlabel("时间 / s")
|
||||
axis.set_ylabel("航向角偏差 / °")
|
||||
axis.set_title(
|
||||
"航向角偏差：参考轨迹航向－实际车体航向（逆时针为正）\n"
|
||||
f"{title}，RMSE={heading_rmse_degrees:.3f}°"
|
||||
)
|
||||
axis.grid(True, alpha=0.3)
|
||||
destination = output_directory / f"{csv_path.stem}_heading_error.png"
|
||||
save_figure(fig, destination, show)
|
||||
destinations.append(destination)
|
||||
|
||||
speed_error = data["actual_speed"] - data["reference_speed"]
|
||||
speed_rmse = finite_rmse(speed_error)
|
||||
fig, axis = plt.subplots(figsize=(10.0, 5.8))
|
||||
axis.plot(
|
||||
data["time"],
|
||||
data["reference_speed"],
|
||||
linewidth=1.8,
|
||||
label="控制器实际参考速度",
|
||||
)
|
||||
axis.plot(
|
||||
data["time"],
|
||||
data["command_speed"],
|
||||
"--",
|
||||
linewidth=1.3,
|
||||
label="纵向控制器下发速度",
|
||||
)
|
||||
axis.plot(
|
||||
data["time"],
|
||||
data["actual_speed"],
|
||||
linewidth=1.5,
|
||||
label="状态估计实际车体纵向速度",
|
||||
)
|
||||
axis.set_xlabel("时间 / s")
|
||||
axis.set_ylabel("速度 / (m/s)")
|
||||
axis.set_title(f"参考速度与实际速度对比\n{title}，RMSE={speed_rmse:.4f}m/s")
|
||||
axis.grid(True, alpha=0.3)
|
||||
axis.legend()
|
||||
destination = output_directory / f"{csv_path.stem}_speed_response.png"
|
||||
save_figure(fig, destination, show)
|
||||
destinations.append(destination)
|
||||
|
||||
print(
|
||||
f"{csv_path.name}: 横向RMSE={lateral_rmse_mm:.3f}mm, "
|
||||
f"航向RMSE={heading_rmse_degrees:.4f}°, "
|
||||
f"速度RMSE={speed_rmse:.5f}m/s"
|
||||
)
|
||||
for destination in destinations:
|
||||
print(f"已生成：{destination}")
|
||||
return destinations
|
||||
|
||||
|
||||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||||
"""读取命令行文件；未指定时扫描脚本目录及data子目录中的CSV。"""
|
||||
if arguments:
|
||||
files = [Path(item).expanduser().resolve() for item in arguments]
|
||||
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()]
|
||||
if not files:
|
||||
raise FileNotFoundError(
|
||||
"没有找到CSV；请传入文件路径，或将文件放到脚本目录/data中。"
|
||||
)
|
||||
return files
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""解析命令行并批量处理新版控制器实验CSV。"""
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制新版控制器4m直线实验的四类对比图。"
|
||||
)
|
||||
parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
help="图片输出目录；默认使用脚本目录/plots。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show",
|
||||
action="store_true",
|
||||
help="保存图片后同时显示窗口。",
|
||||
)
|
||||
arguments = parser.parse_args()
|
||||
configure_matplotlib()
|
||||
|
||||
output_directory = (
|
||||
Path(arguments.output_dir).expanduser().resolve()
|
||||
if arguments.output_dir
|
||||
else SCRIPT_DIR / "plots"
|
||||
)
|
||||
failed = 0
|
||||
for csv_path in discover_csv_files(arguments.csv):
|
||||
try:
|
||||
plot_experiment(csv_path, output_directory, arguments.show)
|
||||
except Exception as exception:
|
||||
failed += 1
|
||||
print(f"处理失败：{csv_path}：{exception}")
|
||||
|
||||
if failed:
|
||||
raise SystemExit(f"共有{failed}个CSV处理失败。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,3 @@
|
||||
numpy>=1.26
|
||||
pandas>=2.2
|
||||
matplotlib>=3.8
|
||||
Reference in New Issue
Block a user