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

控制器测试横向跟踪，增加记录

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-10 10:10:21 +08:00
parent 88f651e0c2
commit 2f9e4285d3
22 changed files with 769 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt
-17
View File
@@ -1,17 +0,0 @@
第一次：
ok
第二次：
: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
*p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
第三次：
ok
data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py
+203 -37
View File
@@ -1,4 +1,4 @@
"""为新版4m直线控制器实验CSV生成轨迹、横向/航向误差和速度响应图。"""
"""为新版控制器实验CSV生成包含轨迹、误差、速度和转角的六子图总图。"""
from __future__ import annotations
@@ -293,13 +293,87 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
)
if np.any(has_control_reference):
reference_speed[~has_control_reference] = np.nan
actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
velocity_valid = (
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
)
actual_speed[~velocity_valid] = np.nan
state_body_vx[~velocity_valid] = np.nan
has_velocity_diagnostics = (
numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
)
detour_speed = numeric_column(
frame,
"DetourEstimatedBodyVxMetersPerSecond",
)
detour_speed_valid = (
has_velocity_diagnostics
& (
numeric_column(
frame,
"DetourVelocityEstimateValid",
0.0,
)
> 0.5
)
)
detour_speed[~detour_speed_valid] = np.nan
wheel_raw_speed = numeric_column(
frame,
"WheelFeedbackRawBodyVxMetersPerSecond",
)
wheel_filtered_speed = numeric_column(
frame,
"WheelFeedbackFilteredBodyVxMetersPerSecond",
)
wheel_speed_valid = (
has_velocity_diagnostics
& (
numeric_column(
frame,
"WheelFeedbackVelocityEstimateValid",
0.0,
)
> 0.5
)
)
wheel_raw_speed[~wheel_speed_valid] = np.nan
wheel_filtered_speed[~wheel_speed_valid] = np.nan
actual_speed = np.where(
np.isfinite(wheel_filtered_speed),
wheel_filtered_speed,
state_body_vx,
)
command_speed = numeric_column(frame, "CommandSpeed")
has_steering_diagnostics = (
numeric_column(frame, "HasSteeringDiagnostics", 0.0) > 0.5
)
steering_angles_degrees = {}
for wheel_name in (
"LeftFront",
"LeftRear",
"RightFront",
"RightRear",
):
values = numeric_column(
frame,
f"ActualSteer{wheel_name}Degrees",
)
values[~has_steering_diagnostics] = np.nan
steering_angles_degrees[wheel_name] = values
has_gcp_command = (
numeric_column(frame, "HasGcpCommand", 0.0) > 0.5
)
front_gcp_degrees = np.rad2deg(
numeric_column(frame, "CommandFrontGcpAngleRadians")
)
rear_gcp_degrees = np.rad2deg(
numeric_column(frame, "CommandRearGcpAngleRadians")
)
front_gcp_degrees[~has_gcp_command] = np.nan
rear_gcp_degrees[~has_gcp_command] = np.nan
return {
"frame": frame,
"time": time_seconds,
@@ -316,7 +390,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
"heading_error": heading_error,
"reference_speed": reference_speed,
"actual_speed": actual_speed,
"detour_speed": detour_speed,
"wheel_raw_speed": wheel_raw_speed,
"wheel_filtered_speed": wheel_filtered_speed,
"command_speed": command_speed,
"steering_angles_degrees": steering_angles_degrees,
"front_gcp_degrees": front_gcp_degrees,
"rear_gcp_degrees": rear_gcp_degrees,
"controller_name": first_text(
frame,
"ControllerName",
@@ -342,7 +422,7 @@ def save_figure(
show: bool,
) -> None:
"""保存并关闭一张实验图。"""
fig.tight_layout()
fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
fig.savefig(destination, dpi=300, bbox_inches="tight")
if show:
plt.show()
@@ -354,14 +434,16 @@ def plot_experiment(
output_directory: Path,
show: bool,
) -> list[Path]:
"""为单份新版控制器CSV生成四类对比图。"""
"""为单份新版控制器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] = []
fig, axes = plt.subplots(3, 2, figsize=(18.0, 16.0))
fig.suptitle(title, fontsize=16)
# 1. 期望轨迹与实际轨迹。
axis = axes[0, 0]
valid_position = data["valid_position"]
fig, axis = plt.subplots(figsize=(9.0, 6.5))
valid_reference_position = data["valid_reference_position"]
if np.count_nonzero(valid_reference_position) >= 2:
axis.plot(
@@ -390,47 +472,42 @@ def plot_experiment(
axis.set_aspect("equal", adjustable="box")
axis.set_xlabel("世界坐标X / m")
axis.set_ylabel("世界坐标Y / m")
axis.set_title(f"期望轨迹与实际轨迹对比\n{title}")
axis.set_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)
axis.legend(fontsize=8)
# 2. 横向误差。
lateral_mm = data["lateral_error"] * 1000.0
lateral_rmse_mm = finite_rmse(lateral_mm)
fig, axis = plt.subplots(figsize=(10.0, 5.5))
axis = axes[0, 1]
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"
"横向误差（轨迹在车辆左侧为正）\n"
f"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)
# 3. 航向误差。
heading_degrees = np.rad2deg(data["heading_error"])
heading_rmse_degrees = finite_rmse(heading_degrees)
fig, axis = plt.subplots(figsize=(10.0, 5.5))
axis = axes[1, 0]
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}°"
f"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)
# 4. 参考、命令、Detour估计和轮速解算速度。
speed_error = data["actual_speed"] - data["reference_speed"]
speed_rmse = finite_rmse(speed_error)
fig, axis = plt.subplots(figsize=(10.0, 5.8))
axis = axes[1, 1]
axis.plot(
data["time"],
data["reference_speed"],
@@ -444,29 +521,118 @@ def plot_experiment(
linewidth=1.3,
label="纵向控制器下发速度",
)
axis.plot(
data["time"],
data["actual_speed"],
linewidth=1.5,
label="状态估计实际车体纵向速度",
)
if np.any(np.isfinite(data["detour_speed"])):
axis.plot(
data["time"],
data["detour_speed"],
":",
linewidth=1.2,
label="Detour估计Vx",
)
if np.any(np.isfinite(data["wheel_filtered_speed"])):
axis.plot(
data["time"],
data["wheel_filtered_speed"],
linewidth=1.5,
label="轮速解算滤波Vx（控制使用）",
)
else:
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.set_title(
"参考速度、控制命令与观测速度\n"
f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
)
axis.grid(True, alpha=0.3)
axis.legend()
destination = output_directory / f"{csv_path.stem}_speed_response.png"
axis.legend(fontsize=8)
# 5. 四个舵轮的实际机械转角。
axis = axes[2, 0]
wheel_labels = {
"LeftFront": "左前轮",
"LeftRear": "左后轮",
"RightFront": "右前轮",
"RightRear": "右后轮",
}
steering_data_available = False
for wheel_name, wheel_label in wheel_labels.items():
wheel_angles = data["steering_angles_degrees"][wheel_name]
if np.any(np.isfinite(wheel_angles)):
steering_data_available = True
axis.plot(
data["time"],
wheel_angles,
linewidth=1.2,
label=wheel_label,
)
if steering_data_available:
axis.axhline(0.0, color="black", linewidth=0.8)
axis.legend(fontsize=8, ncol=2)
else:
axis.text(
0.5,
0.5,
"CSV不含四舵轮转角诊断数据",
ha="center",
va="center",
transform=axis.transAxes,
)
axis.set_xlabel("时间 / s")
axis.set_ylabel("实际舵角 / °")
axis.set_title("四个舵轮实际反馈转角")
axis.grid(True, alpha=0.3)
# 6. 经过角速度限制后实际发送的前、后虚拟GCP转角。
axis = axes[2, 1]
gcp_data_available = (
np.any(np.isfinite(data["front_gcp_degrees"]))
or np.any(np.isfinite(data["rear_gcp_degrees"]))
)
if gcp_data_available:
axis.plot(
data["time"],
data["front_gcp_degrees"],
linewidth=1.4,
label="前GCP",
)
axis.plot(
data["time"],
data["rear_gcp_degrees"],
linewidth=1.4,
label="后GCP",
)
axis.axhline(0.0, color="black", linewidth=0.8)
axis.legend(fontsize=8)
else:
axis.text(
0.5,
0.5,
"CSV不含前后GCP转角数据",
ha="center",
va="center",
transform=axis.transAxes,
)
axis.set_xlabel("时间 / s")
axis.set_ylabel("GCP命令角 / °")
axis.set_title("前后虚拟GCP实际发送转角")
axis.grid(True, alpha=0.3)
destination = output_directory / f"{csv_path.stem}_summary_6plots.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
print(f"已生成六子图总图：{destination}")
return [destination]
def discover_csv_files(arguments: list[str]) -> list[Path]:
@@ -487,7 +653,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
def main() -> None:
"""解析命令行并批量处理新版控制器实验CSV。"""
parser = argparse.ArgumentParser(
description="绘制新版控制器轨迹实验的四类对比图。"
description="绘制新版控制器轨迹实验的六子图总图。"
)
parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
parser.add_argument(