commit for push
This commit is contained in:
CaiXiang
283
docs/custom_path/REFERENCE_PATH_SUMMARY.txt
Normal file
283
docs/custom_path/REFERENCE_PATH_SUMMARY.txt
Normal file
@@ -0,0 +1,283 @@
|
||||
AGV 路径跟踪项目 - 参考路径实现完整分析
|
||||
============================================================
|
||||
|
||||
1. 参考路径相关文件
|
||||
============================================================
|
||||
|
||||
核心文件：
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/include/path_curve.h
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/src/path_curve.cpp
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/include/path_tracker.h
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/src/path_tracker.cpp
|
||||
|
||||
示例文件：
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/examples/demo.cpp
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/examples/generate_data.cpp
|
||||
|
||||
数据结构文件：
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/include/agv_model.h
|
||||
C:/work/AGV/AGV运动规划/agv_path_tracking/include/control_generator.h
|
||||
|
||||
|
||||
2. 路径数据结构
|
||||
============================================================
|
||||
|
||||
2.1 PathPoint 结构体：
|
||||
struct PathPoint {
|
||||
double x; // x坐标（米）
|
||||
double y; // y坐标（米）
|
||||
double theta; // 切线方向角（弧度）
|
||||
double kappa; // 曲率（1/米）
|
||||
};
|
||||
|
||||
2.2 PathCurve 类：
|
||||
class PathCurve {
|
||||
private:
|
||||
std::vector<PathPoint> path_points_; // 路径点序列
|
||||
};
|
||||
|
||||
|
||||
3. 参考路径定义和生成方式
|
||||
============================================================
|
||||
|
||||
3.1 四种生成方法：
|
||||
|
||||
方法 1: 直线路径 - generateLine()
|
||||
原型：void generateLine(const PathPoint& start, const PathPoint& end,
|
||||
int num_points = 100)
|
||||
特点：曲率为 0，所有点方向相同
|
||||
示例：path.generateLine(PathPoint(0,0), PathPoint(10,10), 100)
|
||||
|
||||
方法 2: 圆弧路径 - generateCircleArc()
|
||||
原型：void generateCircleArc(double center_x, double center_y, double radius,
|
||||
double start_angle, double end_angle,
|
||||
int num_points = 100)
|
||||
特点：恒定曲率 κ = ±1/radius
|
||||
示例：path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI/2, 100)
|
||||
|
||||
方法 3: 三次贝塞尔曲线 - generateCubicBezier()
|
||||
原型：void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
|
||||
const PathPoint& p2, const PathPoint& p3,
|
||||
int num_points = 100)
|
||||
特点：平滑曲线，可变曲率
|
||||
公式：P(t) = (1-t)³p0 + 3(1-t)²t·p1 + 3(1-t)t²·p2 + t³·p3
|
||||
示例：PathPoint p0(0,0), p1(3,5), p2(7,5), p3(10,0);
|
||||
path.generateCubicBezier(p0, p1, p2, p3, 100)
|
||||
|
||||
方法 4: 设置路径点数组 - setPathPoints()
|
||||
原型：void setPathPoints(const std::vector<PathPoint>& points)
|
||||
特点：直接从点数组初始化，支持路径组合
|
||||
用途：合并多条曲线（如 S 型曲线）
|
||||
|
||||
3.2 路径查询方法：
|
||||
const std::vector<PathPoint>& getPathPoints() const;
|
||||
- 获取所有路径点
|
||||
|
||||
PathPoint getPointAt(double t) const;
|
||||
- 根据参数 t ∈ [0,1] 线性插值获取路径点
|
||||
|
||||
double getPathLength() const;
|
||||
- 计算路径总长度（米）
|
||||
|
||||
int findNearestPoint(double x, double y) const;
|
||||
- 找到距离 (x,y) 最近的路径点索引
|
||||
|
||||
|
||||
4. 预设的曲线类型
|
||||
============================================================
|
||||
|
||||
enum CurveType {
|
||||
LINE, // 直线（κ = 0）
|
||||
CIRCLE_ARC, // 圆弧（κ = ±1/R，恒定）
|
||||
CUBIC_BEZIER, // 三次贝塞尔（κ 可变）
|
||||
SPLINE // 样条曲线（预留）
|
||||
};
|
||||
|
||||
详细对比：
|
||||
┌──────────────┬──────────────────┬───────────┬─────────────────┐
|
||||
│ 曲线类型 │ 特点 │ 曲率 │ 应用场景 │
|
||||
├──────────────┼──────────────────┼───────────┼─────────────────┤
|
||||
│ LINE │ 直线，无曲率 │ κ = 0 │ 长直线运动 │
|
||||
│ CIRCLE_ARC │ 圆形弧线 │ κ = ±1/R │ 转弯、回转 │
|
||||
│ CUBIC_BEZIER │ 平滑曲线 │ 可变 │ 路径过渡 │
|
||||
│ SPLINE │ 样条曲线（待实现）│ 平滑 │ 复杂曲线拟合 │
|
||||
└──────────────┴──────────────────┴───────────┴─────────────────┘
|
||||
|
||||
|
||||
5. 路径数据格式
|
||||
============================================================
|
||||
|
||||
5.1 CSV 输出格式 - trajectory.csv（轨迹数据）：
|
||||
# AGV Predicted Trajectory
|
||||
# x(m), y(m), theta(rad), theta(deg)
|
||||
0.000000, 0.000000, 0.000000, 0.000000
|
||||
0.070711, 0.070711, 0.785398, 45.000000
|
||||
...
|
||||
10.000000, 10.000000, 0.785398, 45.000000
|
||||
|
||||
字段说明：
|
||||
x(m): X 坐标，单位米
|
||||
y(m): Y 坐标，单位米
|
||||
theta(rad): 朝向角，单位弧度
|
||||
theta(deg): 朝向角，单位度
|
||||
|
||||
5.2 CSV 输出格式 - control_sequence.csv（控制序列）：
|
||||
# AGV Control Sequence
|
||||
# Time(s), Velocity(m/s), Steering(rad), Steering(deg)
|
||||
0.000000, 1.000000, 0.732770, 41.984039
|
||||
0.100000, 1.000000, 0.732933, 41.993384
|
||||
...
|
||||
|
||||
字段说明：
|
||||
Time(s): 时间戳，单位秒
|
||||
Velocity(m/s): 线速度，单位米/秒
|
||||
Steering(rad): 转向角，单位弧度
|
||||
Steering(deg): 转向角，单位度
|
||||
|
||||
|
||||
6. 关键算法
|
||||
============================================================
|
||||
|
||||
6.1 曲率计算 - Menger 公式（三点法）：
|
||||
κ = 4 × Area / (d1 × d2 × d3)
|
||||
|
||||
其中：
|
||||
Area = |叉积| / 2 = |dx1×dy2 - dy1×dx2| / 2
|
||||
d1 = 距离(p1, p2)
|
||||
d2 = 距离(p2, p3)
|
||||
d3 = 距离(p1, p3)
|
||||
|
||||
6.2 路径点插值（参数 t ∈ [0, 1]）：
|
||||
使用线性插值在相邻两点间：
|
||||
x(t) = x1 + α(x2 - x1)
|
||||
y(t) = y1 + α(y2 - y1)
|
||||
θ(t) = θ1 + α(θ2 - θ1)
|
||||
κ(t) = κ1 + α(κ2 - κ1)
|
||||
|
||||
其中 α = t × (size-1) 的小数部分
|
||||
|
||||
6.3 最近点查找：
|
||||
遍历所有路径点，计算到给定点 (x,y) 的欧氏距离，
|
||||
返回距离最小的点的索引。
|
||||
|
||||
|
||||
7. 使用示例
|
||||
============================================================
|
||||
|
||||
示例 1: 生成直线路径并跟踪
|
||||
---
|
||||
PathCurve path;
|
||||
path.generateLine(PathPoint(0,0), PathPoint(10,10), 100);
|
||||
|
||||
PathTracker tracker(agv_model);
|
||||
tracker.setReferencePath(path);
|
||||
tracker.setInitialState(AGVModel::State(0,0,0));
|
||||
tracker.generateControlSequence("pure_pursuit", 0.1, 20.0);
|
||||
tracker.saveTrajectory("trajectory.csv");
|
||||
---
|
||||
|
||||
示例 2: 生成圆弧路径
|
||||
---
|
||||
PathCurve path;
|
||||
path.generateCircleArc(5.0, 0.0, 5.0, M_PI, M_PI/2, 100);
|
||||
std::cout << "Path length: " << path.getPathLength() << " m" << std::endl;
|
||||
---
|
||||
|
||||
示例 3: 生成 S 型曲线（组合两个圆弧）
|
||||
---
|
||||
std::vector<PathPoint> points;
|
||||
|
||||
PathCurve arc1;
|
||||
arc1.generateCircleArc(2.5, 0.0, 2.5, M_PI, M_PI/2, 50);
|
||||
auto p1 = arc1.getPathPoints();
|
||||
points.insert(points.end(), p1.begin(), p1.end());
|
||||
|
||||
PathCurve arc2;
|
||||
arc2.generateCircleArc(2.5, 5.0, 2.5, -M_PI/2, 0, 50);
|
||||
auto p2 = arc2.getPathPoints();
|
||||
points.insert(points.end(), p2.begin(), p2.end());
|
||||
|
||||
PathCurve path;
|
||||
path.setPathPoints(points);
|
||||
---
|
||||
|
||||
示例 4: 贝塞尔曲线路径
|
||||
---
|
||||
PathPoint p0(0.0, 0.0);
|
||||
PathPoint p1(3.0, 5.0);
|
||||
PathPoint p2(7.0, 5.0);
|
||||
PathPoint p3(10.0, 0.0);
|
||||
path.generateCubicBezier(p0, p1, p2, p3, 100);
|
||||
---
|
||||
|
||||
示例 5: 路径查询
|
||||
---
|
||||
const auto& points = path.getPathPoints();
|
||||
std::cout << "Total points: " << points.size() << std::endl;
|
||||
|
||||
// 获取中间点
|
||||
PathPoint mid = path.getPointAt(0.5);
|
||||
std::cout << "Mid point: (" << mid.x << ", " << mid.y << ")" << std::endl;
|
||||
|
||||
// 找最近点
|
||||
int idx = path.findNearestPoint(5.0, 5.0);
|
||||
std::cout << "Nearest point index: " << idx << std::endl;
|
||||
---
|
||||
|
||||
|
||||
8. 参考路径与控制的集成
|
||||
============================================================
|
||||
|
||||
完整工作流程：
|
||||
|
||||
1. 创建 AGV 模型
|
||||
AGVModel agv(1.0, 2.0, M_PI/4);
|
||||
|
||||
2. 创建路径跟踪器
|
||||
PathTracker tracker(agv);
|
||||
|
||||
3. 定义参考路径（4 种方式之一）
|
||||
PathCurve path;
|
||||
path.generateLine(start, end, 100);
|
||||
|
||||
4. 设置参考路径
|
||||
tracker.setReferencePath(path);
|
||||
|
||||
5. 设置初始状态
|
||||
tracker.setInitialState(AGVModel::State(0,0,0));
|
||||
|
||||
6. 生成控制序列（基于路径和控制算法）
|
||||
tracker.generateControlSequence("pure_pursuit", 0.1, 20.0);
|
||||
|
||||
7. 保存输出
|
||||
tracker.saveControlSequence("control_sequence.csv");
|
||||
tracker.saveTrajectory("trajectory.csv");
|
||||
|
||||
8. 可视化
|
||||
python visualize.py
|
||||
|
||||
控制算法使用参考路径的：
|
||||
- 路径点序列
|
||||
- 曲率信息
|
||||
- 方向角信息
|
||||
|
||||
|
||||
9. 执行命令
|
||||
============================================================
|
||||
|
||||
编译：
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
cmake --build . --config Release
|
||||
|
||||
运行交互式演示（可选择路径类型和控制算法）：
|
||||
cd build/Release
|
||||
agv_demo.exe
|
||||
|
||||
自动生成数据：
|
||||
generate_data.exe
|
||||
|
||||
Python 可视化：
|
||||
python visualize.py
|
||||
|
||||
Reference in New Issue
Block a user