agv-control-slam/docs/fixes/TRACKING_ERROR_ANALYSIS.md

路径跟踪偏差问题分析报告

问题描述

现象: AGV实际运行的Trajectory和reference path偏差较大，没有很好地追踪

根本原因分析

经过深入分析代码，发现以下关键问题：

1. 初始状态与路径起点不匹配 ⭐⭐⭐（主要原因）

问题详情:

// qt_gui_demo.cpp:450
AGVModel::State initial_state(0.0, 0.0, 0.0);  // 固定为原点，theta=0
tracker_->setInitialState(initial_state);

路径实际起点（以smooth_path.csv为例）:

x=0, y=0, theta=0.310064 rad (≈17.8度), kappa=0

问题:

• 初始theta设为0，但路径起点theta≈0.31 rad
• 初始朝向偏差17.8度，导致一开始就偏离路径
• 对于CSV路径，起点坐标可能也不是(0,0)

2. 控制参数硬编码，无法调整 ⭐⭐⭐

Pure Pursuit硬编码（path_tracker.cpp:35）:

control_sequence_ = control_generator_.generatePurePursuit(
    reference_path_, initial_state_, dt,
    1.5,  // lookahead_distance 硬编码！
    1.0,  // desired_velocity 硬编码！
    horizon);

Stanley硬编码（path_tracker.cpp:38）:

control_sequence_ = control_generator_.generateStanley(
    reference_path_, initial_state_, dt,
    1.0,  // k_gain 硬编码！
    1.0,  // desired_velocity 硬编码！
    horizon);

问题:

• GUI中有max_vel_spin_参数（默认2.0 m/s），但从未使用
• 前视距离1.5米可能不适合所有速度
• Stanley增益1.0可能需要针对不同路径调整
• 用户无法通过GUI调整这些关键参数

3. Pure Pursuit前视距离不合理 ⭐⭐

理论公式:

lookahead_distance = k * velocity
推荐: k = 1.0 到 2.0

当前问题:

• lookahead固定为1.5米
• 速度硬编码为1.0 m/s → lookahead/v = 1.5
• 如果实际速度是2.0 m/s，lookahead应该是3.0米，但仍用1.5米
• 前视距离太短导致转弯反应过快，太长导致切弯

4. Stanley增益可能不适配 ⭐⭐

Stanley控制律:

delta = heading_error + atan(k * cross_track_error / v)

问题:

• k_gain=1.0是经验值，不一定适合所有场景
• 对于急弯路径，可能需要更大的k（比如2.0-3.0）
• 对于平缓路径，较小的k（0.5-1.0）更平滑

5. 速度设置不一致 ⭐

GUI中设置:

• Max Velocity默认: 2.0 m/s

实际使用:

• desired_velocity硬编码: 1.0 m/s

结果: 用户以为设置了2.0 m/s，实际只用1.0 m/s

影响分析

偏差来源

原因	初始偏差	累积效应	严重度
初始theta不匹配	大（17.8度）	立即偏离	⭐⭐⭐
前视距离不当	中	逐渐偏离	⭐⭐
速度参数错误	小	影响lookahead	⭐⭐
Stanley增益不当	中	震荡或滞后	⭐⭐

实际表现

初始状态不匹配的影响:

时刻0:
  AGV朝向: 0度（向东）
  路径朝向: 17.8度（东北）
  → 立即产生17.8度朝向误差

时刻1:
  AGV会尝试转向路径，但已经偏离
  → 横向误差累积

后续:
  持续追赶路径，但始终有偏差
  → 轨迹呈"追赶"模式而非"跟踪"模式

前视距离不当的影响:

lookahead太小(0.5m):
  → 反应过于敏感
  → 轨迹震荡
  → 频繁调整方向

lookahead太大(3.0m):
  → 反应迟钝
  → 切弯
  → 路径跟踪不精确

合适的lookahead(1.5-2.5m @ 1.0m/s):
  → 平滑跟踪
  → 适度预判

修复方案

修复1: 初始状态匹配路径起点 ⭐⭐⭐（必须修复）

修改位置: examples/qt_gui_demo.cpp:450

修改前:

AGVModel::State initial_state(0.0, 0.0, 0.0);
tracker_->setInitialState(initial_state);

修改后:

// 从路径起点获取初始状态
const auto& path_points = path.getPathPoints();
if (!path_points.empty()) {
    const PathPoint& start = path_points[0];
    AGVModel::State initial_state(start.x, start.y, start.theta);
    tracker_->setInitialState(initial_state);
} else {
    AGVModel::State initial_state(0.0, 0.0, 0.0);
    tracker_->setInitialState(initial_state);
}

修复2: 使用GUI速度参数 ⭐⭐⭐（必须修复）

修改位置: examples/qt_gui_demo.cpp:458-460

修改前:

tracker_->generateControlSequence(algo_str, dt, horizon);

修改后:

double desired_velocity = max_vel_spin_->value();  // 使用GUI参数
tracker_->generateControlSequence(algo_str, dt, horizon, desired_velocity);

需要修改path_tracker.h和path_tracker.cpp添加velocity参数。

修复3: 自适应前视距离 ⭐⭐（推荐修复）

修改位置: src/path_tracker.cpp:35

修改前:

control_sequence_ = control_generator_.generatePurePursuit(
    reference_path_, initial_state_, dt, 1.5, 1.0, horizon);

修改后:

double lookahead = std::max(1.0, desired_velocity * 2.0);  // 速度的2倍
control_sequence_ = control_generator_.generatePurePursuit(
    reference_path_, initial_state_, dt, lookahead, desired_velocity, horizon);

修复4: 添加GUI参数控制 ⭐⭐（推荐修复）

在GUI中添加:

• Lookahead参数（Pure Pursuit）
• K Gain参数（Stanley）

这样用户可以根据路径特性调整参数。

修复5: 改进Stanley增益 ⭐（可选修复）

修改位置: src/path_tracker.cpp:38

修改后:

double k_gain = 2.0;  // 增加到2.0以提高响应性
control_sequence_ = control_generator_.generateStanley(
    reference_path_, initial_state_, dt, k_gain, desired_velocity, horizon);

优先级

修复	优先级	难度	效果
初始状态匹配路径起点	⭐⭐⭐	简单	立即显著改善
使用GUI速度参数	⭐⭐⭐	中等	提高一致性
自适应前视距离	⭐⭐	简单	改善跟踪性能
添加GUI参数	⭐⭐	复杂	提高可调性
改进Stanley增益	⭐	简单	微小改善

预期效果

修复前:

初始状态: (0, 0, 0°)
路径起点: (0, 0, 17.8°)
→ 立即产生17.8度朝向偏差
→ Trajectory始终追赶reference path
→ 横向误差大（0.5-2.0米）

修复后:

初始状态: (0, 0, 17.8°) ← 匹配路径起点
路径起点: (0, 0, 17.8°)
→ 完美对齐
→ Trajectory平滑跟踪reference path
→ 横向误差小（<0.2米）

下一步行动

建议按以下顺序实施修复：

1. 立即修复: 初始状态匹配路径起点
2. 立即修复: 使用GUI速度参数
3. 推荐修复: 自适应前视距离
4. 可选修复: 添加GUI参数控制

---

分析日期: 2025-11-14 问题类型: 控制算法参数设置 严重程度: 高 根本原因: 初始状态不匹配 + 参数硬编码