diff --git a/.gitignore b/.gitignore index 79c3698..004038b 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,6 @@ log/ logs/ data/ files/ +*.csv +*.png + diff --git a/Readme.md b/Readme.md index d8c0f5c..0391cc3 100644 --- a/Readme.md +++ b/Readme.md @@ -1,5 +1,5 @@ # auto drive misson center webots simulation: -该分支修复了原LQR算法中矩阵错误问题，并对参数进行调教， +该分支修复了原LQR算法中矩阵错误问题，并对参数进行调教，增加stanley前轮转向控制算法，并适配webots仿真环境。后续实车测试优先使用该分支 ## 1, `ubuntu20.04`, `ubuntu22.04` ## 2, 安装 `webots` `https://github.com/cyberbotics/webots/tags`， 版本： `ubuntu20.04： R2023a， ubuntu22.04：R2025a` diff --git a/lqr_plot_path_tracking_error.py b/lqr_plot_path_tracking_error.py new file mode 100644 index 0000000..218f62f --- /dev/null +++ b/lqr_plot_path_tracking_error.py @@ -0,0 +1,355 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +读取当前目录下： + mission_center_logger.txt + mission_center_logger1.txt + mission_center_logger2.txt + ... + +从 [path_follow] 日志中提取： + LQR: dis=..., alpha=... + path controller, current pose point:x:..., y:..., yaw:... + +绘制： + 1. 横向误差 dis 随实际行驶路程变化 + 2. 航向误差 alpha 随实际行驶路程变化 + +说明： +- dis：按你日志中的 LQR dis，单位通常是 m +- alpha：按你日志中的 alpha，单位是 rad +- 路程：根据连续 current pose 的 x/y 计算累计行驶距离 +- 日志中同一时刻可能重复打印多次，因此脚本会保留数据， + 但累计距离对相同位置不会增加。 +""" + +import re +from pathlib import Path + +import numpy as np +import matplotlib.pyplot as plt + + +# ============================================================ +# 1. 日志文件 +# ============================================================ + +LOG_PATTERN = "logs/mission_center_logger*.txt" + +files = sorted(Path(".").glob(LOG_PATTERN)) + +if not files: + raise FileNotFoundError( + f"当前目录没有找到 {LOG_PATTERN}" + ) + +print("找到日志文件：") +for f in files: + print(" ", f) + + +# ============================================================ +# 2. 正则表达式 +# ============================================================ + +# LQR: +# [LQR: dis=0.001038186, alpha=-0.003193076, ... +lqr_pattern = re.compile( + r"\[LQR:\s*" + r"dis=([-+0-9.eE]+),\s*" + r"alpha=([-+0-9.eE]+)" +) + +# current pose: +# [path controller, current pose point:x:24.68, y:1.16, yaw:0.026; +pose_pattern = re.compile( + r"current pose point:x:([-+0-9.eE]+),\s*" + r"y:([-+0-9.eE]+),\s*" + r"yaw:([-+0-9.eE]+)" +) + +yaw_command_pattern = re.compile( + r"PP output angular=([-+0-9.eE]+)" +) + +# ============================================================ +# 3. 读取日志 +# ============================================================ + +all_data = [] + +for file in files: + print(f"\n读取：{file}") + + pending_lqr = None + pending_yaw = None + + with file.open("r", encoding="utf-8", errors="ignore") as f: + for line in f: + + # ---- LQR误差 ---- + m_lqr = lqr_pattern.search(line) + if m_lqr: + dis = float(m_lqr.group(1)) + alpha = float(m_lqr.group(2)) + + pending_lqr = { + "dis": dis, + "alpha": alpha, + } + + # ---- 当前车辆位置 ---- + m_pose = pose_pattern.search(line) + m_yaw = yaw_command_pattern.search(line) + if m_yaw: + pending_yaw = float(m_yaw.group(1)) + if m_pose and pending_lqr is not None: + + x = float(m_pose.group(1)) + y = float(m_pose.group(2)) + yaw = float(m_pose.group(3)) + + + all_data.append({ + "file": file.name, + "x": x, + "y": y, + "yaw": yaw, + "dis": pending_lqr["dis"], + "alpha": pending_lqr["alpha"], + "yaw_command": pending_yaw, + }) + + pending_lqr = None + + +if not all_data: + raise RuntimeError( + "没有解析到数据，请检查日志格式是否和示例一致。" + ) + + +print(f"\n总共解析 {len(all_data)} 个数据点") + + +# ============================================================ +# 4. 根据 x/y 计算实际行驶路程 +# ============================================================ + +x = np.array([d["x"] for d in all_data]) +y = np.array([d["y"] for d in all_data]) + +dis = np.array([d["dis"] for d in all_data]) +alpha = np.array([d["alpha"] for d in all_data]) +yaw_command = np.array([d["yaw_command"] for d in all_data]) + +# 连续位置之间的距离 +step_distance = np.sqrt( + np.diff(x) ** 2 + + np.diff(y) ** 2 +) + +# 累计路程 +distance = np.zeros(len(x)) + +if len(x) > 1: + distance[1:] = np.cumsum(step_distance) + + +# ============================================================ +# 5. 如果希望横向误差取绝对值，可以取消下面注释 +# ============================================================ + +# dis = np.abs(dis) + +# 注意： +# 不建议默认取绝对值。 +# 保留正负号可以观察车辆是在路径左侧还是右侧。 + + +# ============================================================ +# 6. 输出基本统计信息 +# ============================================================ + +print("\n========== 误差统计 ==========") + +print( + f"总行驶距离 : {distance[-1]:.3f} m" +) + +print( + f"横向误差最大值 : {np.max(dis):.6f} m" +) + +print( + f"横向误差最小值 : {np.min(dis):.6f} m" +) + +print( + f"横向误差绝对最大: {np.max(np.abs(dis)):.6f} m" +) + +print( + f"横向误差 RMS : {np.sqrt(np.mean(dis ** 2)):.6f} m" +) + +print( + f"航向误差最大值 : {np.max(alpha):.6f} rad " + f"({np.degrees(np.max(alpha)):.3f} deg)" +) + +print( + f"航向误差最小值 : {np.min(alpha):.6f} rad " + f"({np.degrees(np.min(alpha)):.3f} deg)" +) + +print( + f"航向误差绝对最大: {np.max(np.abs(alpha)):.6f} rad " + f"({np.degrees(np.max(np.abs(alpha))):.3f} deg)" +) + +print( + f"航向误差 RMS : {np.sqrt(np.mean(alpha ** 2)):.6f} rad " + f"({np.degrees(np.sqrt(np.mean(alpha ** 2))):.3f} deg)" +) + + +# ============================================================ +# 7. 转换航向误差为角度 +# ============================================================ + +alpha_deg = np.degrees(alpha) + + +# ============================================================ +# 8. 图1：横向误差 +# ============================================================ + +plt.figure(figsize=(12, 5)) + +plt.plot( + distance, + dis, + linewidth=2.0 +) + +plt.axhline( + 0.0, + linestyle="--", + linewidth=0.8 +) + +plt.xlabel("Travel Distance (m)") +plt.ylabel("Lateral Error dis (m)") +plt.title("Lateral Error vs Travel Distance") +plt.grid(True, alpha=0.3) + +plt.tight_layout() + +plt.savefig( + "lateral_error_vs_distance.png", + dpi=200 +) + +plt.show() + + +# ============================================================ +# 9. 图2：航向误差 +# ============================================================ + +plt.figure(figsize=(12, 5)) + +plt.plot( + distance, + alpha_deg, + linewidth=2.0 +) + +plt.axhline( + 0.0, + linestyle="--", + linewidth=0.8 +) + +plt.xlabel("Travel Distance (m)") +plt.ylabel("Heading Error alpha (deg)") +plt.title("Heading Error vs Travel Distance") +plt.grid(True, alpha=0.3) + +plt.tight_layout() + +plt.savefig( + "heading_error_vs_distance.png", + dpi=200 +) + +plt.show() + +# ============================================================ +# 13. 航向误差 rad +# ============================================================ + +plt.figure( + figsize=(12, 5) +) + +plt.plot( + distance, + yaw_command, + linewidth=1.2 +) + +plt.axhline( + 0.0, + linestyle="--", + linewidth=0.8 +) + +plt.xlabel( + "Distance (m)" +) + +plt.ylabel( + "Yaw Command (rad)" +) + +plt.title( + "Recalculated Yaw Command" +) + +plt.grid(True) + +plt.tight_layout() + +plt.show() +# ============================================================ +# 10. 同时保存 CSV，方便后续 MATLAB / Excel 分析 +# ============================================================ + +output = np.column_stack( + ( + distance, + x, + y, + dis, + alpha, + alpha_deg, + yaw_command, + ) +) + +np.savetxt( + "path_tracking_error.csv", + output, + delimiter=",", + header="distance_m,x_m,y_m,lateral_error_m,heading_error_rad,heading_error_deg,yaw_command", + comments="", + fmt="%.8f" +) + +print("\n输出文件：") +print(" lateral_error_vs_distance.png") +print(" heading_error_vs_distance.png") +print(" path_tracking_error.csv") diff --git a/pp_lqr_stanley_error.py b/pp_lqr_stanley_error.py new file mode 100644 index 0000000..6a24435 --- /dev/null +++ b/pp_lqr_stanley_error.py @@ -0,0 +1,163 @@ +import csv +import os +import matplotlib.pyplot as plt +import numpy as np + +files = [ + "U_turn_0.32_lqr.csv", + "U_turn_0.32_pp.csv", + "U_turn_0.32_stanley.csv", +] + +data = {} + +for file in files: + if not os.path.exists(file): + raise FileNotFoundError(f"找不到文件: {file}") + + distance = [] + lateral_error = [] + heading_error = [] + yaw_command = [] + + with open(file, "r", encoding="utf-8-sig", newline="") as f: + reader = csv.DictReader(f) + + required = {"distance_m", "lateral_error_m", "heading_error_rad","yaw_command"} + if not required.issubset(reader.fieldnames or []): + raise ValueError( + f"{file} 缺少必要列，需要: {required}，实际列: {reader.fieldnames}" + ) + + for row in reader: + try: + distance.append(float(row["distance_m"])) + lateral_error.append(abs(float(row["lateral_error_m"]))) + heading_error.append(abs(float(row["heading_error_rad"]))) + yaw_command.append(float(row["yaw_command"])) + except (ValueError, TypeError): + continue + + name = os.path.splitext(os.path.basename(file))[0] + data[name] = { + "distance": distance, + "lateral_error": lateral_error, + "heading_error": heading_error, + "yaw_command": yaw_command, + } + +# 颜色由 matplotlib 自动分配，不手动指定 +names = list(data.keys()) + +# ========================= +# 图1：Lateral Error +# ========================= +plt.figure(figsize=(12, 6)) + +for name in names: + plt.plot( + data[name]["distance"], + data[name]["lateral_error"], + label=name, + linewidth=1.5, + ) + + +plt.xlabel("Distance (m)") +plt.ylabel("Lateral Error (m)") +plt.title("Lateral Error Comparison") +plt.grid(True, alpha=0.3) +plt.legend(["LQR", "Pure Pursuit", "Stanley"]) +plt.tight_layout() + +lateral_output = "U_turn_lateral_error_comparison.png" +plt.savefig(lateral_output, dpi=200) +plt.show() + +# ========================= +# 图2：Heading Error +# ========================= +plt.figure(figsize=(12, 6)) + +for name in names: + plt.plot( + data[name]["distance"], + data[name]["heading_error"], + label=name, + linewidth=1.5, + ) + +plt.axhline(0, linewidth=2.0) +plt.xlabel("Distance (m)") +plt.ylabel("Heading Error (rad)") +plt.title("Heading Error Comparison") +plt.grid(True, alpha=0.3) +plt.legend(["LQR", "Pure Pursuit", "Stanley"]) +plt.tight_layout() + +heading_output = "U_turn_heading_error_comparison.png" +plt.savefig(heading_output, dpi=200) +plt.show() + + +# ========================= +# 图3：Yaw Command +# ========================= +plt.figure(figsize=(12, 6)) + +for name in names: + plt.plot( + data[name]["distance"], + data[name]["yaw_command"], + label=name, + linewidth=1.5, + ) + +plt.axhline(0, linewidth=2.0) +plt.xlabel("Distance (m)") +plt.ylabel("Yaw Command (rad)") +plt.title("Yaw Command Comparison") +plt.grid(True, alpha=0.3) +plt.legend(["LQR", "Pure Pursuit", "Stanley"]) +plt.tight_layout() + +yaw_output = "U_turn_yaw_command_comparison.png" +plt.savefig(yaw_output, dpi=200) +plt.show() +print("读取完成：") +for name in names: + print(f"{name}: {len(data[name]['distance'])} points") + +print("\n已保存：") +print(lateral_output) +print(heading_output) +print(yaw_output) + + +print("\n" + "=" * 100) +print( + f"{'Controller':<20}" + f"{'Lat Max(m)':>12}" + f"{'Lat Mean(m)':>13}" + f"{'Lat RMSE(m)':>14}" + f"{'Lat P95(m)':>13}" + f"{'Head Max(deg)':>15}" + f"{'Head RMSE(deg)':>16}" +) +print("=" * 100) + +for name in names: + lat = np.array(data[name]["lateral_error"]) + head = np.array(data[name]["heading_error"]) + + print( + f"{name:<20}" + f"{np.max(lat):>12.4f}" + f"{np.mean(lat):>13.4f}" + f"{np.sqrt(np.mean(lat**2)):>14.4f}" + f"{np.percentile(lat, 95):>13.4f}" + f"{np.degrees(np.max(head)):>15.4f}" + f"{np.degrees(np.sqrt(np.mean(head**2))):>16.4f}" + ) + +print("=" * 100) diff --git a/pp_stanley_plot_path_tracking_error.py b/pp_stanley_plot_path_tracking_error.py new file mode 100644 index 0000000..e6275c4 --- /dev/null +++ b/pp_stanley_plot_path_tracking_error.py @@ -0,0 +1,778 @@ +import re +import csv +from pathlib import Path + +import numpy as np +import matplotlib.pyplot as plt + + +# ============================================================ +# 参数 +# ============================================================ + +LOG_PATTERN = "logs/mission_center_logger*.txt" + +# 每 5 个完整数据取 1 个 +SAMPLE_STEP = 40 + +CSV_FILE = "recalculated_tracking_error.csv" + + +# ============================================================ +# 1. 查找日志文件 +# ============================================================ + +files = sorted( + Path(".").glob(LOG_PATTERN) +) + +if not files: + + raise FileNotFoundError( + f"当前目录没有找到：{LOG_PATTERN}" + ) + +print("找到日志文件：") + +for file in files: + + print(" ", file) + + +# ============================================================ +# 2. 正则表达式 +# ============================================================ + +# ------------------------------------------------------------ +# current pose +# +# [debug] [path_follow] +# [current_pose_x:11.57, +# current_pose_y:-2.13, +# current_pose_yaw:-3.10] +# ------------------------------------------------------------ + +current_pose_pattern = re.compile( + r"current_pose_x:([-+0-9.eE]+),\s*" + r"current_pose_y:([-+0-9.eE]+),\s*" + r"current_pose_yaw:([-+0-9.eE]+)" +) + + +# ------------------------------------------------------------ +# current path +# +# [debug] [path_follow] +# [current_path_x:11.59, +# current_path_y:-2.12, +# current_path_yaw:-3.10] +# ------------------------------------------------------------ + +current_path_pattern = re.compile( + r"current_path_x:([-+0-9.eE]+),\s*" + r"current_path_y:([-+0-9.eE]+),\s*" + r"current_path_yaw:([-+0-9.eE]+)" +) + + +# ============================================================ +# 3. 函数：角度归一化 +# ============================================================ + +def normalize_angle(angle): + + return ( + angle + np.pi + ) % ( + 2.0 * np.pi + ) - np.pi + + +# ============================================================ +# 4. 函数：根据 current pose + path 重新计算 +# ============================================================ + +def calculate_errors( + current_x, + current_y, + current_yaw, + path_x, + path_y, + path_yaw +): + + # -------------------------------------------------------- + # 世界坐标系下的位置差 + # -------------------------------------------------------- + + dx = path_x - current_x + + dy = path_y - current_y + + + # -------------------------------------------------------- + # 转换到车辆坐标系 + # + # vehicle frame: + # + # x + # ↑ + # | + # | + # ● -----> y + # + # 根据当前 yaw 进行旋转 + # -------------------------------------------------------- + + lon_dis = ( + np.cos(current_yaw) * dx + + + np.sin(current_yaw) * dy + ) + + + dis = ( + -np.sin(current_yaw) * dx + + + np.cos(current_yaw) * dy + ) + + + # -------------------------------------------------------- + # 车辆到路径点的欧氏距离 + # -------------------------------------------------------- + + distance = np.sqrt( + dx * dx + + dy * dy + ) + + + # -------------------------------------------------------- + # 航向误差 + # -------------------------------------------------------- + + alpha = normalize_angle( + path_yaw - current_yaw + ) + + + # -------------------------------------------------------- + # deg + # -------------------------------------------------------- + + alpha_deg = np.degrees(alpha) + + + return ( + distance, + lon_dis, + dis, + alpha, + alpha_deg + ) + + +# ============================================================ +# 5. 读取日志 +# ============================================================ + +all_data = [] + +total_raw_count = 0 + + +for file in files: + + print() + print("=" * 70) + print(f"读取：{file}") + print("=" * 70) + + + # -------------------------------------------------------- + # 每个文件单独计数 + # -------------------------------------------------------- + + file_data_count = 0 + + + # -------------------------------------------------------- + # 临时保存 current pose + # -------------------------------------------------------- + + pending_pose = None + + + with file.open( + "r", + encoding="utf-8", + errors="ignore" + ) as f: + + + for line in f: + + + # ================================================= + # current pose + # ================================================= + + m_pose = current_pose_pattern.search( + line + ) + + + if m_pose: + + pending_pose = { + + "x": float( + m_pose.group(1) + ), + + "y": float( + m_pose.group(2) + ), + + "yaw": float( + m_pose.group(3) + ), + } + + continue + + + # ================================================= + # current path + # ================================================= + + m_path = current_path_pattern.search( + line + ) + + + if ( + m_path is not None + and + pending_pose is not None + ): + + + path_x = float( + m_path.group(1) + ) + + path_y = float( + m_path.group(2) + ) + + path_yaw = float( + m_path.group(3) + ) + + + # ================================================= + # 重新计算 + # ================================================= + + ( + point_distance, + lon_dis, + dis, + alpha, + alpha_deg + ) = calculate_errors( + + pending_pose["x"], + pending_pose["y"], + pending_pose["yaw"], + + path_x, + path_y, + path_yaw + ) + + + # ================================================= + # 每 SAMPLE_STEP 个数据取一个 + # ================================================= + + if file_data_count % SAMPLE_STEP == 0: + + all_data.append({ + + "file": file.name, + + "current_x": + pending_pose["x"], + + "current_y": + pending_pose["y"], + + "current_yaw": + pending_pose["yaw"], + + "path_x": + path_x, + + "path_y": + path_y, + + "path_yaw": + path_yaw, + + "point_distance": + point_distance, + + "lon_dis": + lon_dis, + + "dis": + dis, + + "alpha": + alpha, + + "alpha_deg": + alpha_deg, + }) + + + file_data_count += 1 + + total_raw_count += 1 + + + # ------------------------------------------------ + # 清空 + # ------------------------------------------------ + + pending_pose = None + + + print( + f"原始完整数据：{file_data_count}" + ) + + print( + f"抽样后数据：" + f"{(file_data_count + SAMPLE_STEP - 1) // SAMPLE_STEP}" + ) + + +# ============================================================ +# 6. 检查 +# ============================================================ + +if not all_data: + + raise RuntimeError( + "没有解析到 current_pose + current_path 数据。" + ) + + +print() +print("=" * 70) +print("解析完成") +print("=" * 70) + +print( + f"原始数据：{total_raw_count}" +) + +print( + f"最终数据：{len(all_data)}" +) + + +# ============================================================ +# 7. 提取数据 +# ============================================================ + +current_x = np.array([ + d["current_x"] + for d in all_data +]) + +current_y = np.array([ + d["current_y"] + for d in all_data +]) + +current_yaw = np.array([ + d["current_yaw"] + for d in all_data +]) + + +path_x = np.array([ + d["path_x"] + for d in all_data +]) + +path_y = np.array([ + d["path_y"] + for d in all_data +]) + +path_yaw = np.array([ + d["path_yaw"] + for d in all_data +]) + + +point_distance = np.array([ + d["point_distance"] + for d in all_data +]) + +lon_dis = np.array([ + d["lon_dis"] + for d in all_data +]) + +dis = np.array([ + d["dis"] + for d in all_data +]) + +alpha = np.array([ + d["alpha"] + for d in all_data +]) + +alpha_deg = np.array([ + d["alpha_deg"] + for d in all_data +]) + + +# ============================================================ +# 8. 计算车辆累计行驶距离 +# ============================================================ + +if len(path_x) > 1: + + dx = np.diff( + path_x + ) + + dy = np.diff( + path_y + ) + + step_distance = np.sqrt( + dx ** 2 + + dy ** 2 + ) + +else: + + step_distance = np.array([]) +for i in range(len(step_distance) +): + + if step_distance[i] > 0.5: + step_distance[i] = 0.0 + print( + f"Warning: step_distance[{i}] = {step_distance[i]:.3f} m, which is too large. Setting it to 0.0.") + + +distance_m = np.zeros( + len(path_x) +) + + +if len(path_x) > 1: + + distance_m[1:] = np.cumsum( + step_distance + ) + + +# ============================================================ +# 9. 保存 CSV +# ============================================================ + +print() +print("=" * 70) +print("保存 CSV") +print("=" * 70) + + +with open( + CSV_FILE, + "w", + newline="", + encoding="utf-8-sig" +) as f: + + writer = csv.writer(f) + + + writer.writerow([ + + "distance_m", + + "current_x_m", + "current_y_m", + + "lateral_error_m", + + "heading_error_rad", + + "heading_error_deg", + + ]) + + + for i in range( + len(all_data) + ): + + writer.writerow([ + + f"{distance_m[i]:.8f}", + + f"{current_x[i]:.8f}", + f"{current_y[i]:.8f}", + + + f"{dis[i]:.8f}", + + f"{alpha[i]:.8f}", + + f"{alpha_deg[i]:.8f}", + + ]) + + +print( + f"CSV 已保存：" + f"{Path(CSV_FILE).resolve()}" +) + + +# ============================================================ +# 11. 横向误差 +# ============================================================ + +plt.figure( + figsize=(12, 5) +) + +plt.plot( + distance_m, + dis, + linewidth=1.2 +) + +plt.axhline( + 0.0, + linestyle="--", + linewidth=0.8 +) + +plt.xlabel( + "Distance (m)" +) + +plt.ylabel( + "Lateral Error (m)" +) + +plt.title( + "Recalculated Lateral Error" +) + +plt.grid(True) + +plt.tight_layout() + +plt.show() + + +# ============================================================ +# 13. 航向误差 rad +# ============================================================ + +plt.figure( + figsize=(12, 5) +) + +plt.plot( + distance_m, + alpha, + linewidth=1.2 +) + +plt.axhline( + 0.0, + linestyle="--", + linewidth=0.8 +) + +plt.xlabel( + "Distance (m)" +) + +plt.ylabel( + "Heading Error (rad)" +) + +plt.title( + "Recalculated Heading Error" +) + +plt.grid(True) + +plt.tight_layout() + +plt.show() + + +# ============================================================ +# 15. 车辆轨迹 + 路径点 +# ============================================================ + +plt.figure( + figsize=(8, 8) +) + +plt.plot( + current_x, + current_y, + linewidth=1.2, + label="Current Pose" +) + +plt.plot( + path_x, + path_y, + linestyle="--", + linewidth=1.0, + label="Current Path Point" +) + + +plt.scatter( + current_x[0], + current_y[0], + s=50, + label="Start" +) + +plt.scatter( + current_x[-1], + current_y[-1], + s=50, + label="End" +) + + +plt.xlabel( + "X (m)" +) + +plt.ylabel( + "Y (m)" +) + +plt.title( + "Current Pose and Path" +) + +plt.axis( + "equal" +) + +plt.grid(True) + +plt.legend() + +plt.tight_layout() + + +plt.show() + + +# ============================================================ +# 16. 误差统计 +# ============================================================ + +print() +print("=" * 70) +print("误差统计") +print("=" * 70) + + +print( + f"最大横向误差：" + f"{np.max(np.abs(dis)):.6f} m" +) + +print( + f"平均绝对横向误差：" + f"{np.mean(np.abs(dis)):.6f} m" +) + +print( + f"横向误差 RMSE：" + f"{np.sqrt(np.mean(dis ** 2)):.6f} m" +) + + +print() + +print( + f"最大纵向误差：" + f"{np.max(np.abs(lon_dis)):.6f} m" +) + +print( + f"平均绝对纵向误差：" + f"{np.mean(np.abs(lon_dis)):.6f} m" +) + +print( + f"纵向误差 RMSE：" + f"{np.sqrt(np.mean(lon_dis ** 2)):.6f} m" +) + + +print() + +print( + f"最大航向误差：" + f"{np.max(np.abs(alpha_deg)):.6f} deg" +) + +print( + f"平均绝对航向误差：" + f"{np.mean(np.abs(alpha_deg)):.6f} deg" +) + +print( + f"航向误差 RMSE：" + f"{np.sqrt(np.mean(alpha_deg ** 2)):.6f} deg" +) + + +print() + +print( + f"最大车辆-路径点距离：" + f"{np.max(point_distance):.6f} m" +) + +print( + f"平均车辆-路径点距离：" + f"{np.mean(point_distance):.6f} m" +) + +print( + f"总行驶距离：" + f"{distance_m[-1]:.3f} m" +) + diff --git a/src/auto_drive_misson_center/config/misson_center_params.yaml b/src/auto_drive_misson_center/config/misson_center_params.yaml index c3456df..6b16646 100644 --- a/src/auto_drive_misson_center/config/misson_center_params.yaml +++ b/src/auto_drive_misson_center/config/misson_center_params.yaml @@ -26,7 +26,7 @@ path_follow_handler_node: device_id: "2072868304711761920" - controller_type: "lqr" # "pure_pursuit", "lqr", "pid" + controller_type: "stanley" # "pure_pursuit", "lqr", "pid","stanley" # logger logger_logger_name: "path_follow" diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp index 892ba62..ca80005 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp @@ -188,6 +188,7 @@ protected: logger_->debug("current_point_index_:{}, path_points_ size:{}, distance2:{}, distance1:{}", current_point_index_, path_points_.size(), distance2, distance1); + // 如果current_point_index_+1的距离小于current_point_index_距离，current_point_index_++ if (distance2 < distance1) { @@ -216,7 +217,7 @@ protected: double mission_speed_{0.0}; double turn_radius_{0.3}; double max_allowed_speed_{1.0}; - double wheel_dis_{0.95}; + double wheel_dis_{0.5}; std::vector path_points_; int current_point_index_{0}; bool is_initialized_{false}; diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp index 0bec479..c1c5c47 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp @@ -8,6 +8,7 @@ #include "lqr_control.hpp" #include "pid_control.hpp" #include "pure_pursuit_control.hpp" +#include "stanley_control.hpp" #include "spdlog/logger.h" #include "spdlog/spdlog.h" #include "spdlog/sinks/rotating_file_sink.h" @@ -27,7 +28,7 @@ protected: // 从PathTrackingController的参数映射到原有控制器的init参数 // 注意：原有控制器需要max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv // 这里需要根据需求设置合适的参数值 - float max_linear_vel = mission_speed_; + float max_linear_vel = mission_speed_ ; float max_angular_vel = mission_speed_ / turn_radius_; // 根据转弯半径估算最大角速度 float max_angular_vel_theory = max_allowed_speed_/wheel_dis_; max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory); @@ -173,7 +174,7 @@ public: protected: bool onInitialize() override { - float max_linear_vel = mission_speed_; + float max_linear_vel = mission_speed_ ; float max_angular_vel = mission_speed_ / turn_radius_; float max_angular_vel_theory = max_allowed_speed_/wheel_dis_; max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory); @@ -183,7 +184,7 @@ protected: float look_ahead_dis = 0.4; // 前视距离，需要根据实际情况调整 float kv = 0.6; // 速度系数，需要根据实际情况调整 adapter_look_ahead_dis_ = look_ahead_dis; - logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ ); + //logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ ); controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv); return true; } @@ -258,40 +259,13 @@ private: dis = static_cast(-dx * sin(path_yaw) + dy * cos(path_yaw)); lon_dis = static_cast(sqrt(dx * dx + dy * dy)); kappa = static_cast(path_points_[idx].kappa); - logger_->info( - "PurePursuit parameters:\n" - " current_x:{}, current_y:{}, current_yaw:{}\n" - " current_path_x:{}, current_path_y:{}, current_path_yaw:{}\n" - " lookahead_x:{}, lookahead_y:{}, lookahead_yaw:{}, lookahead_kappa:{}\n" - " dx:{}, dy:{}, target_yaw:{}, alpha:{}\n" - " linear_val:{}, dis:{}, lon_dis:{}\n" - " current_point_index:{}, idx:{}, path_size:{}", - - current_pose.x, - current_pose.y, - current_pose.yaw, - - path_points_[current_point_index_].x, - path_points_[current_point_index_].y, - path_points_[current_point_index_].yaw, - - lookahead_point.x, - lookahead_point.y, - lookahead_point.yaw, - lookahead_point.kappa, - - dx, - dy, - target_yaw, - alpha, - - linear_val, - dis, - lon_dis, - - current_point_index_, - idx, - path_points_.size() + logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}", + current_pose.x, current_pose.y,current_pose.yaw + ); + logger_->debug("current_path_x:{}, current_path_y:{},current_path_yaw:{}", + path_points_[idx].x, + path_points_[idx].y, + path_points_[idx].yaw ); } @@ -335,6 +309,148 @@ private: } }; -} // namespace auto_drive_misson_center +// Stanley控制器适配器 +class StanleyControllerAdapter : public PathTrackingController { +public: + StanleyControllerAdapter(std::shared_ptr logger = nullptr) + : logger_(logger), PathTrackingController(logger), controller_(std::make_shared(logger)) {} + protected: + bool onInitialize() override { + float max_linear_vel = mission_speed_ ; + float max_angular_vel = mission_speed_ / turn_radius_; + float max_angular_vel_theory = max_allowed_speed_/wheel_dis_; + max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory); + float kp = 1.76; + float ki = 0.0; + float kd = 0.8; + float look_ahead_dis = 0.4; // 前视距离，需要根据实际情况调整 + float kv = 0.6; // 速度系数，需要根据实际情况调整 + adapter_look_ahead_dis_ = look_ahead_dis; + //logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ ); + controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv); + return true; + } + bool kappa_cal = false; + ControlOutput onRun(const PathPoint& current_pose) override { + float alpha = 0.0; + float linear_val = 0.0; + float dis = 0.0; + float lon_dis = 0.0; + float kappa = 0.0; + if(!kappa_cal){ + computePathCurvature(); + kappa_cal = true; + } + + // Pure Pursuit使用前视点，计算需要特殊处理 + computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis, lon_dis, kappa); + logger_->debug("Stanley Adapter alpha={}, linear_val={}, dis={}, lon_dis={},kappa={}", alpha, linear_val, dis,lon_dis,kappa); + + float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa); + //float angular_velocity = controller_->update(alpha, linear_val, dis); + logger_->debug("Stanley output angular={}", angular_velocity); + bool completed = isPathCompleted(current_pose); + + return ControlOutput(mission_speed_, angular_velocity, completed); + } + + void onReset() override { + controller_->reset(); + } + +private: + std::shared_ptr controller_; + double adapter_look_ahead_dis_; //保存前视距离 + std::shared_ptr logger_; + + void computeControlParamsWithLookAhead(const PathPoint& current_pose, + float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) { + // 查找前视点 + PathPoint lookahead_point; + if (!findLookAheadPoint(current_pose, lookahead_point)) { + // 如果找不到前视点，使用当前路径点 + int idx = current_point_index_; + if (idx >= static_cast(path_points_.size())) { + idx = path_points_.size() - 1; + } + lookahead_point = path_points_[idx]; + } + + // 计算从当前点到前视点的角度误差 + double dx = lookahead_point.x - current_pose.x; + double dy = lookahead_point.y - current_pose.y; + double target_yaw = atan2(dy, dx); + + // 预瞄角度误差 + alpha = static_cast(target_yaw - current_pose.yaw); + while (alpha > M_PI) alpha -= 2 * M_PI; + while (alpha < -M_PI) alpha += 2 * M_PI; + + // 线速度 + linear_val = static_cast(mission_speed_); + + // 横向误差（用于更新索引） + int idx = current_point_index_; + if (idx >= static_cast(path_points_.size()) - 1) { + idx = path_points_.size() - 1; + } + double dx_path = current_pose.x - path_points_[idx].x; + double dy_path = current_pose.y - path_points_[idx].y; + double path_yaw = path_points_[idx].yaw; + //dis = static_cast(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw)); + dis = static_cast(-dx * sin(path_yaw) + dy * cos(path_yaw)); + lon_dis = static_cast(sqrt(dx * dx + dy * dy)); + kappa = static_cast(path_points_[idx].kappa); + logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}", + current_pose.x, current_pose.y,current_pose.yaw + ); + logger_->debug("current_path_x:{}, current_path_y:{},current_path_yaw:{}", + path_points_[idx].x, + path_points_[idx].y, + path_points_[idx].yaw + ); + + } + + bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) { + if (path_points_.empty()) { + return false; + } + + // 获取前视距离（从控制器参数获取） + // 这里需要从controller_获取lookAheadDis，但由于接口限制，我们使用默认值 + double look_ahead_distance = adapter_look_ahead_dis_; // 需要与初始化时一致 + + // 从当前点开始沿路径向前查找 + int start_idx = current_point_index_; + if (start_idx >= static_cast(path_points_.size())) { + start_idx = path_points_.size() - 1; + } + + double accumulated_dist = 0.0; + for (int i = start_idx; i < static_cast(path_points_.size()) - 1; ++i) { + double dx = path_points_[i + 1].x - path_points_[i].x; + double dy = path_points_[i + 1].y - path_points_[i].y; + double segment_length = sqrt(dx * dx + dy * dy); + + if (accumulated_dist + segment_length >= look_ahead_distance) { + // 在前视距离内插值 + double ratio = (look_ahead_distance - accumulated_dist) / segment_length; + lookahead_point.x = path_points_[i].x + ratio * dx; + lookahead_point.y = path_points_[i].y + ratio * dy; + lookahead_point.yaw = path_points_[i].yaw; + lookahead_point.kappa = path_points_[i].kappa; + return true; + } + accumulated_dist += segment_length; + } + + // 如果路径长度不够，使用最后一个点 + lookahead_point = path_points_.back(); + return true; + } +}; // namespace auto_drive_misson_center + +} #endif // AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_ADAPTER_HPP_ \ No newline at end of file diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp index e3dc44c..2fecdbc 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp @@ -41,13 +41,14 @@ public: { // std::cout<<"PurePursuit update!"<trace("PurePursuit update!"); - logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa); + // logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa); float w_val = (2 * fabs(linear_val) * sin(alpha) / (lon_dis)) + dis; //float w_val = linear_val*kappa; w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val; // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl; logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val); + return w_val; }; diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/stanley_control.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/stanley_control.hpp new file mode 100644 index 0000000..d721719 --- /dev/null +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/stanley_control.hpp @@ -0,0 +1,71 @@ +#ifndef _STANLEY_CONTROLL_H_ +#define _STANLEY_CONTROLL_H_ + +#include +#include +#include +#include "base_following_controller.hpp" +#include "pid_control.hpp" + +using namespace Eigen; + +class Stanley_Controller_C : public BaseFollowingController +{ +public: + Stanley_Controller_C() = delete; + Stanley_Controller_C(std::shared_ptr logger_recorder) + :BaseFollowingController(logger_recorder), logger_(logger_recorder){}; + ~Stanley_Controller_C(){}; + + void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki, \ + const float kd, const float look_ahead_dis, const float kv) + { + // std::cout<<"PurePursuit init!"<info("PurePursuit init! max_linear_vel:{}, max_angular_vel:{}", max_linear_vel, max_angular_vel); + maxLinearVel = max_linear_vel; + maxAngularVel = max_angular_vel; + lookAheadDis = look_ahead_dis; + k_v = kv; + } + float update(const float alpha, const float linear_val, const float dis) + { + // std::cout<<"PurePursuit update!"<trace("PurePursuit update!"); + float w_val = 1.5 * alpha +1.0 * std::atan2(dis,linear_val); + w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val; + // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl; + logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val); + return w_val; + }; + float update(const float alpha, const float linear_val, const float dis, const float lon_dis,const float kappa) + { + // std::cout<<"PurePursuit update!"<trace("PurePursuit update!"); + // logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa); + float w_val = 1.5 * alpha +1.0 * std::atan2(dis,linear_val); + + //float w_val = linear_val*kappa; + w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val; + // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl; + logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val); + + return w_val; + }; + + void reset() + { + //pid_.reset(); + } + + void test(){std::cout<<"pp test!"<debug("pp test!");}; + +private: + float maxLinearVel; + float maxAngularVel; + float lookAheadDis, k_v; + float maxSlowAngularVel, minToleranceAngle; + //PID_Controller_C pid_; + std::shared_ptr logger_; +}; + +#endif \ No newline at end of file diff --git a/src/auto_drive_misson_center/src/path_follow_handler.cpp b/src/auto_drive_misson_center/src/path_follow_handler.cpp index 5a73209..5333167 100644 --- a/src/auto_drive_misson_center/src/path_follow_handler.cpp +++ b/src/auto_drive_misson_center/src/path_follow_handler.cpp @@ -9,7 +9,6 @@ #include #include #include - namespace auto_drive_misson_center { constexpr double PathFollowHandler::POS_LOST_TIMEOUT_SECONDS; @@ -693,7 +692,13 @@ bool PathFollowHandler::loadPathTrackingController(const std::string& controller LOG_INFO("Pure Pursuit path tracking controller loaded"); reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "Pure Pursuit控制器加载成功"); return true; - } else { + } else if (controller_type == "stanley") { + path_controller_ = std::make_shared(logger); + LOG_INFO("STAN path tracking controller loaded"); + reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "STAN控制器加载成功"); + return true; + } + else { LOG_ERROR("load Path Tracking Controller ERROR"); } diff --git a/src/robots_rviz_panel/rviz2.png b/src/robots_rviz_panel/rviz2.png deleted file mode 100644 index 0a93e7c..0000000 Binary files a/src/robots_rviz_panel/rviz2.png and /dev/null differ diff --git a/src/webots_simulation/communication diagram.png b/src/webots_simulation/communication diagram.png deleted file mode 100644 index 9d581e3..0000000 Binary files a/src/webots_simulation/communication diagram.png and /dev/null differ diff --git a/src/webots_simulation/src/PathVisualizer.cpp b/src/webots_simulation/src/PathVisualizer.cpp index 3ffdac1..d5e5615 100644 --- a/src/webots_simulation/src/PathVisualizer.cpp +++ b/src/webots_simulation/src/PathVisualizer.cpp @@ -966,7 +966,7 @@ void PathVisualizer::publishOdomPath(){ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); std::string file_name; - file_name = "1701895373952102400.json"; + file_name = "1699983696942006272.json"; // std::cout << "请输入路径文件路径: "; // std::getline(std::cin, file_name);