agv/agv-control-slam

Fork 0

Files

CaiXiang af65c2425d initial

2025-11-14 16:09:58 +08:00

5.5 KiB

Raw Blame History

Trajectory不完整问题修复报告

问题描述

现象: trajectory路径只有一段，无法完整追踪reference path

用户反馈: "要能完整的追踪reference path,现在trajectory路径只有一段"

根本原因分析

经过深入分析代码，发现问题的根本原因：

1. Horizon（时间范围）参数过小 ⭐⭐⭐（主要原因）

问题详情:

默认 horizon = 10.0 秒
默认速度 desired_velocity = 1.0 m/s
在10秒内，AGV只能行驶10米
如果参考路径长度 > 10米（例如20米），轨迹就会在路径中途停止

位置: examples/qt_gui_demo.cpp:294

horizon_spin_ = createParamRow("Horizon (s):", 1.0, 30.0, 10.0, control_layout);
//                                                    ^^^^  ^^^^
//                                                    最大值  默认值

分析:

路径长度示例: smooth_path.csv 约 20 米
默认设置: horizon = 10秒, velocity = 1.0 m/s
结果: 10秒 × 1.0 m/s = 10米 < 20米路径
→ 轨迹只覆盖路径的前一半

2. 终止阈值过于严格

问题详情:

终止条件: distance_to_end < 0.1 米
0.1米的阈值太小，可能导致永远无法满足终止条件
AGV可能在终点附近"徘徊"，消耗时间但无法达到精确的0.1米范围

位置:

src/control_generator.cpp:58 (Pure Pursuit算法)
src/control_generator.cpp:114 (Stanley算法)

已应用的修复

修复1: 增加Horizon参数范围

文件: examples/qt_gui_demo.cpp

修改前:

horizon_spin_ = createParamRow("Horizon (s):", 1.0, 30.0, 10.0, control_layout);

修改后:

horizon_spin_ = createParamRow("Horizon (s):", 1.0, 100.0, 50.0, control_layout);
//                                                    ^^^^^  ^^^^
//                                                    新最大值 新默认值

效果:

最大值: 30秒 → 100秒 （可支持更长路径）
默认值: 10秒 → 50秒 （默认可走50米）
用户可以根据路径长度调整horizon参数

修复2: 放宽终止阈值

文件: src/control_generator.cpp

Pure Pursuit算法 (第50-62行):

修改前:

// 检查是否接近路径终点
if (distance_to_end < 0.1) {
    break; // 已到达终点附近
}

修改后:

// 修复: 检查是否接近路径终点（阈值放宽以确保完整追踪）
if (distance_to_end < 0.5) {
    break; // 已到达终点附近
}

Stanley算法 (第108-120行): 同样的修改

效果:

终止阈值: 0.1米 → 0.5米
更容易到达终止条件
确保路径能够完整追踪

修改文件清单

✅ examples/qt_gui_demo.cpp - 增加horizon范围
✅ src/control_generator.cpp - 放宽终止阈值

备份文件

examples/qt_gui_demo.cpp.backup
src/control_generator.cpp.backup2

修复效果对比

修复前

路径长度: 20米
Horizon: 10秒
速度: 1.0 m/s
轨迹长度: 10米 ✗（只覆盖一半）

修复后

路径长度: 20米
Horizon: 50秒（默认）
速度: 1.0 m/s
轨迹长度: 20米 ✓（完整覆盖）

使用建议

如何设置合适的Horizon值

计算公式：

horizon (秒) = 路径长度(米) / 期望速度(m/s) × 1.5

示例：

路径长度 = 20米
期望速度 = 1.0 m/s
建议horizon = 20 / 1.0 × 1.5 = 30秒

GUI界面操作

在GUI中找到 "Horizon (s):" 参数框
根据路径长度调整（范围：1-100秒）
默认50秒适用于大多数情况
如果轨迹仍不完整，可以继续增加horizon值

下一步操作

重新编译项目

cd build
cmake --build . --config Release

测试验证

运行新编译的 agv_qt_gui.exe
加载一个较长的CSV路径（如 smooth_path.csv）
设置 Horizon = 50秒（默认值）
点击 "Generate Control"
观察 trajectory 是否完整覆盖 reference path

预期结果

✓ Trajectory应该完整追踪整条reference path
✓ 轨迹应该接近路径终点（0.5米范围内）
✓ 不会提前终止

技术说明

Horizon参数的含义

Horizon: 控制序列生成的时间范围
循环条件: while (current_time < horizon)
AGV会根据控制算法生成从0到horizon时间内的所有控制指令

终止条件

现在有两个终止条件（满足任一即停止）：

时间达到horizon: current_time >= horizon
到达路径终点: distance_to_end < 0.5米

性能影响

Horizon增大会增加计算量（更多控制步数）
50秒 @ 0.1秒步长 = 500个控制步
计算时间仍然很快（< 1秒）

其他改进建议（可选）

自动计算Horizon（未实现）

可以添加自动计算功能：

double path_length = path.getPathLength();
double auto_horizon = path_length / desired_velocity * 1.5;
horizon = std::max(auto_horizon, horizon);

显示路径完成度（未实现）

在GUI中显示：

Path Coverage: 85% (17.0m / 20.0m)

总结

问题: Trajectory只追踪路径的一部分原因: Horizon时间太短（10秒只能走10米）修复:

增加Horizon默认值：10秒 → 50秒
增加Horizon最大值：30秒 → 100秒
放宽终止阈值：0.1米 → 0.5米

结果: 现在可以完整追踪长达50米的路径（默认设置）

修复日期: 2025-11-14 修复状态: ✅ 代码已修复，等待重新编译测试 影响范围: 轨迹生成功能（Pure Pursuit和Stanley算法） 风险评估: 低风险（仅修改参数范围和阈值）

5.5 KiB Raw Blame History Unescape Escape