initial

docs/fixes/TRACKING_ERROR_ANALYSIS.md

@@ -0,0 +1,260 @@
+# 路径跟踪偏差问题分析报告
+
+## 问题描述
+**现象**: AGV实际运行的Trajectory和reference path偏差较大，没有很好地追踪
+
+## 根本原因分析
+
+经过深入分析代码，发现以下关键问题：
+
+### 1. 初始状态与路径起点不匹配 ⭐⭐⭐（主要原因）
+
+**问题详情**:
+```cpp
+// 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）:
+```cpp
+control_sequence_ = control_generator_.generatePurePursuit(
+    reference_path_, initial_state_, dt,
+    1.5,  // lookahead_distance 硬编码！
+    1.0,  // desired_velocity 硬编码！
+    horizon);
+```
+
+**Stanley硬编码**（path_tracker.cpp:38）:
+```cpp
+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`
+
+**修改前**:
+```cpp
+AGVModel::State initial_state(0.0, 0.0, 0.0);
+tracker_->setInitialState(initial_state);
+```
+
+**修改后**:
+```cpp
+// 从路径起点获取初始状态
+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`
+
+**修改前**:
+```cpp
+tracker_->generateControlSequence(algo_str, dt, horizon);
+```
+
+**修改后**:
+```cpp
+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`
+
+**修改前**:
+```cpp
+control_sequence_ = control_generator_.generatePurePursuit(
+    reference_path_, initial_state_, dt, 1.5, 1.0, horizon);
+```
+
+**修改后**:
+```cpp
+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`
+
+**修改后**:
+```cpp
+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
+**问题类型**: 控制算法参数设置
+**严重程度**: 高
+**根本原因**: 初始状态不匹配 + 参数硬编码