261 lines
6.7 KiB
Markdown
261 lines
6.7 KiB
Markdown
# 路径跟踪偏差问题分析报告
|
||
|
||
## 问题描述
|
||
**现象**: 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
|
||
**问题类型**: 控制算法参数设置
|
||
**严重程度**: 高
|
||
**根本原因**: 初始状态不匹配 + 参数硬编码
|