Stanley control algorithm add
For real vehicle test
This commit is contained in:
liuhaoyan
parent
e02964f617
commit
e301e7cf67
3
.gitignore
vendored
3
.gitignore
vendored
@ -11,3 +11,6 @@ log/
|
||||
logs/
|
||||
data/
|
||||
files/
|
||||
*.csv
|
||||
*.png
|
||||
|
||||
|
||||
@ -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`
|
||||
|
||||
355
lqr_plot_path_tracking_error.py
Normal file
355
lqr_plot_path_tracking_error.py
Normal file
@ -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")
|
||||
163
pp_lqr_stanley_error.py
Normal file
163
pp_lqr_stanley_error.py
Normal file
@ -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)
|
||||
778
pp_stanley_plot_path_tracking_error.py
Normal file
778
pp_stanley_plot_path_tracking_error.py
Normal file
@ -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"
|
||||
)
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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<PathPoint> path_points_;
|
||||
int current_point_index_{0};
|
||||
bool is_initialized_{false};
|
||||
|
||||
@ -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<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
|
||||
lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
|
||||
kappa = static_cast<float>(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<spdlog::logger> logger = nullptr)
|
||||
: logger_(logger), PathTrackingController(logger), controller_(std::make_shared<Stanley_Controller_C>(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<Stanley_Controller_C> controller_;
|
||||
double adapter_look_ahead_dis_; //保存前视距离
|
||||
std::shared_ptr<spdlog::logger> 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<int>(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<float>(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<float>(mission_speed_);
|
||||
|
||||
// 横向误差（用于更新索引）
|
||||
int idx = current_point_index_;
|
||||
if (idx >= static_cast<int>(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<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
|
||||
dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
|
||||
lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
|
||||
kappa = static_cast<float>(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<int>(path_points_.size())) {
|
||||
start_idx = path_points_.size() - 1;
|
||||
}
|
||||
|
||||
double accumulated_dist = 0.0;
|
||||
for (int i = start_idx; i < static_cast<int>(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_
|
||||
@ -41,13 +41,14 @@ public:
|
||||
{
|
||||
// std::cout<<"PurePursuit update!"<<std::endl;
|
||||
logger_->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;
|
||||
};
|
||||
|
||||
|
||||
@ -0,0 +1,71 @@
|
||||
#ifndef _STANLEY_CONTROLL_H_
|
||||
#define _STANLEY_CONTROLL_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include <Eigen/Dense>
|
||||
#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<spdlog::logger> 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!"<<std::endl;
|
||||
logger_->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!"<<std::endl;
|
||||
logger_->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!"<<std::endl;
|
||||
logger_->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!"<<std::endl; logger_->debug("pp test!");};
|
||||
|
||||
private:
|
||||
float maxLinearVel;
|
||||
float maxAngularVel;
|
||||
float lookAheadDis, k_v;
|
||||
float maxSlowAngularVel, minToleranceAngle;
|
||||
//PID_Controller_C pid_;
|
||||
std::shared_ptr<spdlog::logger> logger_;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -9,7 +9,6 @@
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
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<StanleyControllerAdapter>(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");
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 565 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 104 KiB |
@ -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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user