328 lines
8.1 KiB
Markdown
328 lines
8.1 KiB
Markdown
# AGV 自定义路径功能使用指南
|
||
|
||
## 概述
|
||
|
||
本指南介绍如何使用新增的自定义路径功能，包括：
|
||
1. **从CSV文件加载路径**
|
||
2. **保存路径到CSV文件**
|
||
3. **样条插值生成平滑曲线**
|
||
|
||
## 功能特性
|
||
|
||
### 1. CSV 文件加载/保存
|
||
|
||
支持两种CSV格式：
|
||
- **完整格式**：`x, y, theta, kappa`
|
||
- **简化格式**：`x, y` （theta和kappa会自动计算）
|
||
|
||
### 2. 样条插值
|
||
|
||
使用 Catmull-Rom 样条插值，只需提供少量关键点，自动生成平滑的路径曲线。
|
||
|
||
## 安装步骤
|
||
|
||
### 第一步：修改头文件
|
||
|
||
在 `include/path_curve.h` 中添加以下方法声明（在 `setPathPoints` 方法之后）：
|
||
|
||
```cpp
|
||
// 在第77行之后添加：
|
||
|
||
/**
|
||
* @brief 从CSV文件加载路径点
|
||
* @param filename CSV文件路径
|
||
* @param has_header 是否包含表头（默认true）
|
||
* @return 是否加载成功
|
||
*
|
||
* CSV格式支持以下两种：
|
||
* 1. 完整格式：x, y, theta, kappa
|
||
* 2. 简化格式：x, y （theta和kappa将自动计算）
|
||
*/
|
||
bool loadFromCSV(const std::string& filename, bool has_header = true);
|
||
|
||
/**
|
||
* @brief 将路径点保存到CSV文件
|
||
* @param filename CSV文件路径
|
||
* @return 是否保存成功
|
||
*/
|
||
bool saveToCSV(const std::string& filename) const;
|
||
|
||
/**
|
||
* @brief 使用样条插值生成路径
|
||
* @param key_points 关键路径点（只需指定x和y）
|
||
* @param num_points 生成的路径点总数
|
||
* @param tension 张力参数（0.0-1.0，控制曲线平滑度，默认0.5）
|
||
*
|
||
* 使用 Catmull-Rom 样条插值，生成经过所有关键点的平滑曲线
|
||
*/
|
||
void generateSpline(const std::vector<PathPoint>& key_points,
|
||
int num_points = 100,
|
||
double tension = 0.5);
|
||
```
|
||
|
||
同时在文件开头添加 string 头文件：
|
||
```cpp
|
||
#include <string>
|
||
```
|
||
|
||
### 第二步：添加实现文件到编译
|
||
|
||
在 `src/CMakeLists.txt` 或主 `CMakeLists.txt` 中添加：
|
||
```cmake
|
||
add_library(path_curve_lib
|
||
src/path_curve.cpp
|
||
src/path_curve_custom.cpp # 新增
|
||
# ... 其他文件
|
||
)
|
||
```
|
||
|
||
### 第三步：重新编译
|
||
|
||
```bash
|
||
cd build
|
||
cmake ..
|
||
cmake --build .
|
||
```
|
||
|
||
## 使用示例
|
||
|
||
### 示例 1：从CSV加载路径
|
||
|
||
```cpp
|
||
#include "path_curve.h"
|
||
#include "path_tracker.h"
|
||
|
||
int main() {
|
||
// 创建路径对象
|
||
PathCurve path;
|
||
|
||
// 从CSV文件加载
|
||
if (path.loadFromCSV("custom_path.csv", true)) {
|
||
std::cout << "路径加载成功！" << std::endl;
|
||
std::cout << "路径点数量: " << path.getPathPoints().size() << std::endl;
|
||
std::cout << "路径长度: " << path.getPathLength() << " m" << std::endl;
|
||
|
||
// 使用加载的路径进行跟踪
|
||
AGVModel agv(2.0, 1.0, M_PI/4);
|
||
PathTracker tracker(agv);
|
||
tracker.setReferencePath(path);
|
||
|
||
// 设置初始状态并生成控制序列
|
||
const auto& points = path.getPathPoints();
|
||
AGVModel::State initial(points[0].x, points[0].y, points[0].theta);
|
||
tracker.setInitialState(initial);
|
||
tracker.generateControlSequence("pure_pursuit", 0.1, 20.0);
|
||
|
||
// 保存结果
|
||
tracker.saveTrajectory("output_trajectory.csv");
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
```
|
||
|
||
### 示例 2：使用样条插值
|
||
|
||
```cpp
|
||
#include "path_curve.h"
|
||
|
||
int main() {
|
||
PathCurve path;
|
||
|
||
// 定义关键点
|
||
std::vector<PathPoint> key_points;
|
||
key_points.push_back(PathPoint(0.0, 0.0));
|
||
key_points.push_back(PathPoint(5.0, 2.0));
|
||
key_points.push_back(PathPoint(10.0, 5.0));
|
||
key_points.push_back(PathPoint(15.0, 3.0));
|
||
key_points.push_back(PathPoint(20.0, 0.0));
|
||
|
||
// 生成样条曲线（200个点）
|
||
path.generateSpline(key_points, 200, 0.5);
|
||
|
||
std::cout << "样条曲线生成完成" << std::endl;
|
||
std::cout << "路径长度: " << path.getPathLength() << " m" << std::endl;
|
||
|
||
// 保存到文件
|
||
path.saveToCSV("spline_path.csv");
|
||
|
||
return 0;
|
||
}
|
||
```
|
||
|
||
### 示例 3：手动创建并保存路径
|
||
|
||
```cpp
|
||
#include "path_curve.h"
|
||
|
||
int main() {
|
||
PathCurve path;
|
||
|
||
// 创建路径（例如：贝塞尔曲线）
|
||
PathPoint p0(0, 0), p1(5, 10), p2(15, 10), p3(20, 0);
|
||
path.generateCubicBezier(p0, p1, p2, p3, 150);
|
||
|
||
// 保存到CSV
|
||
if (path.saveToCSV("my_custom_path.csv")) {
|
||
std::cout << "路径已保存" << std::endl;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
```
|
||
|
||
## CSV 文件格式
|
||
|
||
### 完整格式示例
|
||
|
||
```csv
|
||
# Custom Path Data
|
||
# x(m), y(m), theta(rad), kappa(1/m)
|
||
0.0, 0.0, 0.0, 0.0
|
||
1.0, 0.5, 0.1, 0.05
|
||
2.0, 1.2, 0.15, 0.03
|
||
```
|
||
|
||
### 简化格式示例
|
||
|
||
```csv
|
||
# Custom Path - Simple Format
|
||
# x(m), y(m)
|
||
0.0, 0.0
|
||
2.0, 1.0
|
||
4.0, 3.0
|
||
6.0, 4.0
|
||
```
|
||
|
||
## 参数说明
|
||
|
||
### loadFromCSV 参数
|
||
|
||
- `filename`: CSV文件路径（相对或绝对路径）
|
||
- `has_header`: 是否包含注释/表头行（默认true）
|
||
|
||
### generateSpline 参数
|
||
|
||
- `key_points`: 关键点数组（最少2个点）
|
||
- `num_points`: 生成的总点数（推荐100-500）
|
||
- `tension`: 张力系数（0.0 = 最平滑，1.0 = 最紧密，推荐0.5）
|
||
|
||
## 常见用例
|
||
|
||
### 用例 1：地图路径规划
|
||
|
||
从地图软件导出路径点（CSV格式），直接加载使用：
|
||
|
||
```cpp
|
||
PathCurve map_path;
|
||
map_path.loadFromCSV("map_waypoints.csv");
|
||
```
|
||
|
||
### 用例 2：平滑路径优化
|
||
|
||
将粗糙的路径点用样条插值平滑化：
|
||
|
||
```cpp
|
||
// 加载原始路径
|
||
PathCurve rough_path;
|
||
rough_path.loadFromCSV("raw_path.csv");
|
||
|
||
// 提取关键点（每隔10个点取一个）
|
||
std::vector<PathPoint> key_points;
|
||
const auto& points = rough_path.getPathPoints();
|
||
for (size_t i = 0; i < points.size(); i += 10) {
|
||
key_points.push_back(points[i]);
|
||
}
|
||
|
||
// 生成平滑路径
|
||
PathCurve smooth_path;
|
||
smooth_path.generateSpline(key_points, 300, 0.3);
|
||
smooth_path.saveToCSV("smooth_path.csv");
|
||
```
|
||
|
||
### 用例 3：交互式路径定义
|
||
|
||
通过用户输入定义路径：
|
||
|
||
```cpp
|
||
std::vector<PathPoint> user_points;
|
||
int n;
|
||
std::cout << "输入路径点数量: ";
|
||
std::cin >> n;
|
||
|
||
for (int i = 0; i < n; i++) {
|
||
double x, y;
|
||
std::cout << "点" << (i+1) << " x: "; std::cin >> x;
|
||
std::cout << "点" << (i+1) << " y: "; std::cin >> y;
|
||
user_points.push_back(PathPoint(x, y));
|
||
}
|
||
|
||
PathCurve user_path;
|
||
user_path.generateSpline(user_points, 200);
|
||
user_path.saveToCSV("user_defined_path.csv");
|
||
```
|
||
|
||
## 与现有路径类型的对比
|
||
|
||
| 路径类型 | 定义方式 | 灵活性 | 适用场景 |
|
||
|---------|---------|--------|---------|
|
||
| **直线** | 起点+终点 | 低 | 简单直线移动 |
|
||
| **圆弧** | 圆心+半径+角度 | 低 | 固定半径转弯 |
|
||
| **贝塞尔曲线** | 4个控制点 | 中 | S型曲线 |
|
||
| **CSV加载** | 外部文件 | 高 | 复杂预定义路径 |
|
||
| **样条插值** | 关键点数组 | 极高 | 任意平滑曲线 |
|
||
|
||
## 调试技巧
|
||
|
||
### 1. 验证加载的路径
|
||
|
||
```cpp
|
||
const auto& points = path.getPathPoints();
|
||
for (size_t i = 0; i < points.size(); i += 10) {
|
||
std::cout << "Point " << i << ": ("
|
||
<< points[i].x << ", " << points[i].y << ")" << std::endl;
|
||
}
|
||
```
|
||
|
||
### 2. 检查路径连续性
|
||
|
||
```cpp
|
||
double max_distance = 0.0;
|
||
for (size_t i = 1; i < points.size(); i++) {
|
||
double dx = points[i].x - points[i-1].x;
|
||
double dy = points[i].y - points[i-1].y;
|
||
double dist = sqrt(dx*dx + dy*dy);
|
||
max_distance = std::max(max_distance, dist);
|
||
}
|
||
std::cout << "最大点间距: " << max_distance << " m" << std::endl;
|
||
```
|
||
|
||
## 常见问题
|
||
|
||
**Q: CSV文件格式不正确怎么办？**
|
||
A: 确保每行格式为 `数字, 数字, ...`，使用逗号分隔，可以有注释行（以#开头）。
|
||
|
||
**Q: 样条插值生成的路径不平滑？**
|
||
A: 尝试增加点数（num_points），或减小tension参数。
|
||
|
||
**Q: 路径跟踪效果不理想？**
|
||
A: 检查路径曲率（kappa值），确保不超过AGV的最大转向能力。
|
||
|
||
## 完整示例程序
|
||
|
||
项目中提供了完整的示例：
|
||
- `examples/custom_path.csv` - 示例CSV路径文件
|
||
- `src/path_curve_custom.cpp` - 实现代码
|
||
|
||
## 总结
|
||
|
||
使用自定义路径功能的基本步骤：
|
||
|
||
1. **准备路径数据** - CSV文件或关键点数组
|
||
2. **创建PathCurve对象** - 使用loadFromCSV或generateSpline
|
||
3. **验证路径** - 检查点数和长度
|
||
4. **应用到跟踪器** - 使用tracker.setReferencePath()
|
||
5. **运行仿真** - 生成控制序列并保存结果
|
||
|
||
现在你可以摆脱预设曲线的限制，使用任意自定义路径进行AGV路径跟踪！
|