agv/agv-control-slam

Fork 0

Files

CaiXiang af65c2425d initial

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

8.1 KiB

Raw Blame History

AGV 自定义路径功能使用指南

概述

本指南介绍如何使用新增的自定义路径功能，包括：

从CSV文件加载路径
保存路径到CSV文件
样条插值生成平滑曲线

功能特性

1. CSV 文件加载/保存

支持两种CSV格式：

完整格式：x, y, theta, kappa
简化格式：x, y （theta和kappa会自动计算）

2. 样条插值

使用 Catmull-Rom 样条插值，只需提供少量关键点，自动生成平滑的路径曲线。

安装步骤

第一步：修改头文件

在 include/path_curve.h 中添加以下方法声明（在 setPathPoints 方法之后）：

// 在第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 头文件：

#include <string>

第二步：添加实现文件到编译

在 src/CMakeLists.txt 或主 CMakeLists.txt 中添加：

add_library(path_curve_lib
    src/path_curve.cpp
    src/path_curve_custom.cpp  # 新增
    # ... 其他文件
)

第三步：重新编译

cd build
cmake ..
cmake --build .

使用示例

示例 1：从CSV加载路径

#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：使用样条插值

#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：手动创建并保存路径

#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 文件格式

完整格式示例

# 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

简化格式示例

# 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格式），直接加载使用：

PathCurve map_path;
map_path.loadFromCSV("map_waypoints.csv");

用例 2：平滑路径优化

将粗糙的路径点用样条插值平滑化：

// 加载原始路径
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：交互式路径定义

通过用户输入定义路径：

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. 验证加载的路径

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. 检查路径连续性

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 - 实现代码

总结

使用自定义路径功能的基本步骤：

准备路径数据 - CSV文件或关键点数组
创建PathCurve对象 - 使用loadFromCSV或generateSpline
验证路径 - 检查点数和长度
应用到跟踪器 - 使用tracker.setReferencePath()
运行仿真 - 生成控制序列并保存结果

现在你可以摆脱预设曲线的限制，使用任意自定义路径进行AGV路径跟踪！

8.1 KiB Raw Blame History Unescape Escape