initial

include/agv_model.h

@@ -0,0 +1,88 @@
+#ifndef AGV_MODEL_H
+#define AGV_MODEL_H
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @brief 单舵轮AGV运动学模型
+ *
+ * 模型描述：前面一个主舵轮，后面两个从动轮
+ * 状态量：x, y, theta (位置和航向角)
+ * 控制量：v (速度), delta (主舵轮转向角)
+ */
+class AGVModel {
+public:
+    /**
+     * @brief AGV状态结构体
+     */
+    struct State {
+        double x;       // x坐标 (m)
+        double y;       // y坐标 (m)
+        double theta;   // 航向角 (rad)
+
+        State(double x_ = 0.0, double y_ = 0.0, double theta_ = 0.0)
+            : x(x_), y(y_), theta(theta_) {}
+    };
+
+    /**
+     * @brief 控制输入结构体
+     */
+    struct Control {
+        double v;       // 速度 (m/s)
+        double delta;   // 主舵轮转向角 (rad)
+
+        Control(double v_ = 0.0, double delta_ = 0.0)
+            : v(v_), delta(delta_) {}
+    };
+
+    /**
+     * @brief 构造函数
+     * @param wheelbase 轮距（主舵轮到后轴中心的距离）(m)
+     * @param max_velocity 最大速度 (m/s)
+     * @param max_steering_angle 最大转向角 (rad)
+     */
+    AGVModel(double wheelbase = 1.0,
+             double max_velocity = 2.0,
+             double max_steering_angle = M_PI / 4);
+
+    /**
+     * @brief 运动学方程，计算状态导数
+     * @param state 当前状态
+     * @param control 控制输入
+     * @return 状态导数
+     */
+    State derivative(const State& state, const Control& control) const;
+
+    /**
+     * @brief 使用欧拉法更新状态
+     * @param state 当前状态
+     * @param control 控制输入
+     * @param dt 时间步长 (s)
+     * @return 更新后的状态
+     */
+    State update(const State& state, const Control& control, double dt) const;
+
+    /**
+     * @brief 限制控制输入在允许范围内
+     * @param control 控制输入
+     * @return 限制后的控制输入
+     */
+    Control clampControl(const Control& control) const;
+
+    // 获取参数
+    double getWheelbase() const { return wheelbase_; }
+    double getMaxVelocity() const { return max_velocity_; }
+    double getMaxSteeringAngle() const { return max_steering_angle_; }
+
+private:
+    double wheelbase_;              // 轮距 (m)
+    double max_velocity_;           // 最大速度 (m/s)
+    double max_steering_angle_;     // 最大转向角 (rad)
+};
+
+#endif // AGV_MODEL_H

include/can/CANController.h

@@ -0,0 +1,109 @@
+/**
+ * CAN 控制器封装类
+ * 功能：简化 CAN 设备的操作，提供易用的接口
+ */
+
+#ifndef CAN_CONTROLLER_H
+#define CAN_CONTROLLER_H
+
+#include "../lib/ControlCAN.h"
+#include <string>
+#include <vector>
+#include <functional>
+
+/**
+ * CAN 控制器类
+ */
+class CANController {
+public:
+    // 回调函数类型：接收到 CAN 数据时调用
+    using ReceiveCallback = std::function<void(const VCI_CAN_OBJ&)>;
+
+    /**
+     * 构造函数
+     * @param device_type 设备类型（VCI_USBCAN2 = 4）
+     * @param device_index 设备索引（第几个设备，从0开始）
+     * @param can_index CAN 通道索引（0 或 1）
+     */
+    CANController(DWORD device_type = VCI_USBCAN2,
+                  DWORD device_index = 0,
+                  DWORD can_index = 0);
+
+    ~CANController();
+
+    /**
+     * 初始化 CAN 设备
+     * @param baud_t0 波特率定时器0
+     * @param baud_t1 波特率定时器1
+     * @param mode 工作模式：0=正常，1=只听，2=自发自收
+     * @return 成功返回 true
+     */
+    bool Initialize(BYTE baud_t0 = 0x00, BYTE baud_t1 = 0x1C, BYTE mode = 0);
+
+    /**
+     * 关闭 CAN 设备
+     */
+    void Close();
+
+    /**
+     * 发送标准帧
+     * @param can_id CAN ID（11位）
+     * @param data 数据指针
+     * @param len 数据长度（最大8字节）
+     * @return 成功返回 true
+     */
+    bool SendStandardFrame(UINT can_id, const BYTE* data, BYTE len);
+
+    /**
+     * 发送扩展帧
+     * @param can_id CAN ID（29位）
+     * @param data 数据指针
+     * @param len 数据长度（最大8字节）
+     * @return 成功返回 true
+     */
+    bool SendExtendedFrame(UINT can_id, const BYTE* data, BYTE len);
+
+    /**
+     * 接收 CAN 数据（非阻塞）
+     * @param frames 接收缓冲区
+     * @param max_count 最大接收帧数
+     * @return 实际接收到的帧数
+     */
+    DWORD Receive(std::vector<VCI_CAN_OBJ>& frames, DWORD max_count = 2500);
+
+    /**
+     * 获取接收缓冲区中的帧数量
+     */
+    DWORD GetReceiveNum();
+
+    /**
+     * 清空接收缓冲区
+     */
+    bool ClearBuffer();
+
+    /**
+     * 读取设备信息
+     */
+    bool GetDeviceInfo(VCI_BOARD_INFO& info);
+
+    /**
+     * 设置接收回调函数
+     */
+    void SetReceiveCallback(ReceiveCallback callback) {
+        m_callback = callback;
+    }
+
+    /**
+     * 是否已初始化
+     */
+    bool IsInitialized() const { return m_initialized; }
+
+private:
+    DWORD m_device_type;
+    DWORD m_device_index;
+    DWORD m_can_index;
+    bool m_initialized;
+    ReceiveCallback m_callback;
+};
+
+#endif // CAN_CONTROLLER_H

include/control_generator.h

@@ -0,0 +1,117 @@
+#ifndef CONTROL_GENERATOR_H
+#define CONTROL_GENERATOR_H
+
+#include "agv_model.h"
+#include "path_curve.h"
+#include <vector>
+
+/**
+ * @brief 控制序列结构体
+ */
+struct ControlSequence {
+    std::vector<AGVModel::Control> controls;    // 控制量数组
+    std::vector<double> timestamps;              // 时间戳数组
+    std::vector<AGVModel::State> predicted_states; // 预测状态轨迹
+
+    void clear() {
+        controls.clear();
+        timestamps.clear();
+        predicted_states.clear();
+    }
+
+    size_t size() const {
+        return controls.size();
+    }
+};
+
+/**
+ * @brief 控制序列生成器
+ * 根据给定的路径曲线生成控制序列
+ */
+class ControlGenerator {
+public:
+    /**
+     * @brief 构造函数
+     * @param model AGV运动学模型
+     */
+    explicit ControlGenerator(const AGVModel& model);
+
+    /**
+     * @brief 生成控制序列（基本跟踪方法）
+     * @param path 参考路径
+     * @param initial_state 初始状态
+     * @param dt 时间步长 (s)
+     * @param horizon 预测时域
+     * @return 控制序列
+     */
+    ControlSequence generate(const PathCurve& path,
+                            const AGVModel::State& initial_state,
+                            double dt = 0.1,
+                            double horizon = 10.0);
+
+    /**
+     * @brief 使用Pure Pursuit算法生成控制序列
+     * @param path 参考路径
+     * @param initial_state 初始状态
+     * @param dt 时间步长 (s)
+     * @param lookahead_distance 前视距离 (m)
+     * @param desired_velocity 期望速度 (m/s)
+     * @param horizon 预测时域
+     * @return 控制序列
+     */
+    ControlSequence generatePurePursuit(const PathCurve& path,
+                                       const AGVModel::State& initial_state,
+                                       double dt = 0.1,
+                                       double lookahead_distance = 1.5,
+                                       double desired_velocity = 1.0,
+                                       double horizon = 10.0);
+
+    /**
+     * @brief 使用Stanley算法生成控制序列
+     * @param path 参考路径
+     * @param initial_state 初始状态
+     * @param dt 时间步长 (s)
+     * @param k_gain 增益系数
+     * @param desired_velocity 期望速度 (m/s)
+     * @param horizon 预测时域
+     * @return 控制序列
+     */
+    ControlSequence generateStanley(const PathCurve& path,
+                                   const AGVModel::State& initial_state,
+                                   double dt = 0.1,
+                                   double k_gain = 1.0,
+                                   double desired_velocity = 1.0,
+                                   double horizon = 10.0);
+
+private:
+    AGVModel model_;
+
+    /**
+     * @brief Pure Pursuit算法计算单步控制
+     */
+    AGVModel::Control computePurePursuitControl(const AGVModel::State& state,
+                                               const PathPoint& target_point,
+                                               double desired_velocity);
+
+    /**
+     * @brief Stanley算法计算单步控制
+     */
+    AGVModel::Control computeStanleyControl(const AGVModel::State& state,
+                                           const PathPoint& nearest_point,
+                                           double k_gain,
+                                           double desired_velocity);
+
+    /**
+     * @brief 找到前视点
+     */
+    PathPoint findLookaheadPoint(const PathCurve& path,
+                                const AGVModel::State& state,
+                                double lookahead_distance) const;
+
+    /**
+     * @brief 计算角度差（归一化到[-π, π]）
+     */
+    double normalizeAngle(double angle) const;
+};
+
+#endif // CONTROL_GENERATOR_H

include/path_curve.h

@@ -0,0 +1,138 @@
+#ifndef PATH_CURVE_H
+#define PATH_CURVE_H
+
+#include <vector>
+#include <string>
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @brief 路径点结构体
+ */
+struct PathPoint {
+    double x;       // x坐标
+    double y;       // y坐标
+    double theta;   // 切线方向角
+    double kappa;   // 曲率
+
+    PathPoint(double x_ = 0.0, double y_ = 0.0,
+              double theta_ = 0.0, double kappa_ = 0.0)
+        : x(x_), y(y_), theta(theta_), kappa(kappa_) {}
+};
+
+/**
+ * @brief 路径曲线类
+ * 支持多种曲线类型：直线、圆弧、贝塞尔曲线等
+ */
+class PathCurve {
+public:
+    enum CurveType {
+        LINE,           // 直线
+        CIRCLE_ARC,     // 圆弧
+        CUBIC_BEZIER,   // 三次贝塞尔曲线
+        SPLINE          // 样条曲线
+    };
+
+    PathCurve() = default;
+
+    /**
+     * @brief 生成直线路径
+     * @param start 起点
+     * @param end 终点
+     * @param num_points 路径点数量
+     */
+    void generateLine(const PathPoint& start, const PathPoint& end, int num_points = 100);
+
+    /**
+     * @brief 生成圆弧路径
+     * @param center_x 圆心x坐标
+     * @param center_y 圆心y坐标
+     * @param radius 半径
+     * @param start_angle 起始角度 (rad)
+     * @param end_angle 终止角度 (rad)
+     * @param num_points 路径点数量
+     */
+    void generateCircleArc(double center_x, double center_y, double radius,
+                          double start_angle, double end_angle, int num_points = 100);
+
+    /**
+     * @brief 生成三次贝塞尔曲线
+     * @param p0 起点
+     * @param p1 第一个控制点
+     * @param p2 第二个控制点
+     * @param p3 终点
+     * @param num_points 路径点数量
+     */
+    void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
+                            const PathPoint& p2, const PathPoint& p3,
+                            int num_points = 100);
+
+    /**
+     * @brief 从路径点数组生成路径
+     * @param points 路径点数组
+     */
+    void setPathPoints(const std::vector<PathPoint>& points);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 获取路径点
+     */
+    const std::vector<PathPoint>& getPathPoints() const { return path_points_; }
+
+    /**
+     * @brief 获取指定参数t处的路径点（线性插值）
+     * @param t 参数 [0, 1]
+     * @return 插值后的路径点
+     */
+    PathPoint getPointAt(double t) const;
+
+    /**
+     * @brief 获取路径长度
+     */
+    double getPathLength() const;
+
+    /**
+     * @brief 找到最近的路径点索引
+     * @param x 查询点x坐标
+     * @param y 查询点y坐标
+     * @return 最近点的索引
+     */
+    int findNearestPoint(double x, double y) const;
+
+private:
+    std::vector<PathPoint> path_points_;
+
+    // 计算两点间的曲率
+    double computeCurvature(const PathPoint& p1, const PathPoint& p2,
+                           const PathPoint& p3) const;
+};
+
+#endif // PATH_CURVE_H

include/path_curve.h.backup

@@ -0,0 +1,216 @@
+#ifndef PATH_CURVE_H
+#define PATH_CURVE_H
+
+#include <vector>
+#include <string>
+#include <string>
+#include <string>
+#include <string>
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @brief 路径点结构体
+ */
+struct PathPoint {
+    double x;       // x坐标
+    double y;       // y坐标
+    double theta;   // 切线方向角
+    double kappa;   // 曲率
+
+    PathPoint(double x_ = 0.0, double y_ = 0.0,
+              double theta_ = 0.0, double kappa_ = 0.0)
+        : x(x_), y(y_), theta(theta_), kappa(kappa_) {}
+};
+
+/**
+ * @brief 路径曲线类
+ * 支持多种曲线类型：直线、圆弧、贝塞尔曲线等
+ */
+class PathCurve {
+public:
+    enum CurveType {
+        LINE,           // 直线
+        CIRCLE_ARC,     // 圆弧
+        CUBIC_BEZIER,   // 三次贝塞尔曲线
+        SPLINE          // 样条曲线
+    };
+
+    PathCurve() = default;
+
+    /**
+     * @brief 生成直线路径
+     * @param start 起点
+     * @param end 终点
+     * @param num_points 路径点数量
+     */
+    void generateLine(const PathPoint& start, const PathPoint& end, int num_points = 100);
+
+    /**
+     * @brief 生成圆弧路径
+     * @param center_x 圆心x坐标
+     * @param center_y 圆心y坐标
+     * @param radius 半径
+     * @param start_angle 起始角度 (rad)
+     * @param end_angle 终止角度 (rad)
+     * @param num_points 路径点数量
+     */
+    void generateCircleArc(double center_x, double center_y, double radius,
+                          double start_angle, double end_angle, int num_points = 100);
+
+    /**
+     * @brief 生成三次贝塞尔曲线
+     * @param p0 起点
+     * @param p1 第一个控制点
+     * @param p2 第二个控制点
+     * @param p3 终点
+     * @param num_points 路径点数量
+     */
+    void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
+                            const PathPoint& p2, const PathPoint& p3,
+                            int num_points = 100);
+
+    /**
+     * @brief 从路径点数组生成路径
+     * @param points 路径点数组
+     */
+    void setPathPoints(const std::vector<PathPoint>& points);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 获取路径点
+     */
+    const std::vector<PathPoint>& getPathPoints() const { return path_points_; }
+
+    /**
+     * @brief 获取指定参数t处的路径点（线性插值）
+     * @param t 参数 [0, 1]
+     * @return 插值后的路径点
+     */
+    PathPoint getPointAt(double t) const;
+
+    /**
+     * @brief 获取路径长度
+     */
+    double getPathLength() const;
+
+    /**
+     * @brief 找到最近的路径点索引
+     * @param x 查询点x坐标
+     * @param y 查询点y坐标
+     * @return 最近点的索引
+     */
+    int findNearestPoint(double x, double y) const;
+
+private:
+    std::vector<PathPoint> path_points_;
+
+    // 计算两点间的曲率
+    double computeCurvature(const PathPoint& p1, const PathPoint& p2,
+                           const PathPoint& p3) const;
+};
+
+#endif // PATH_CURVE_H

include/path_curve.h.broken

@@ -0,0 +1,190 @@
+#ifndef PATH_CURVE_H
+#define PATH_CURVE_H
+
+#include <vector>
+#include <string>
+#include <string>
+#include <string>
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/**
+ * @brief 路径点结构体
+ */
+struct PathPoint {
+    double x;       // x坐标
+    double y;       // y坐标
+    double theta;   // 切线方向角
+    double kappa;   // 曲率
+
+    PathPoint(double x_ = 0.0, double y_ = 0.0,
+              double theta_ = 0.0, double kappa_ = 0.0)
+        : x(x_), y(y_), theta(theta_), kappa(kappa_) {}
+};
+
+/**
+ * @brief 路径曲线类
+ * 支持多种曲线类型：直线、圆弧、贝塞尔曲线等
+ */
+class PathCurve {
+public:
+    enum CurveType {
+        LINE,           // 直线
+        CIRCLE_ARC,     // 圆弧
+        CUBIC_BEZIER,   // 三次贝塞尔曲线
+        SPLINE          // 样条曲线
+    };
+
+    PathCurve() = default;
+
+    /**
+     * @brief 生成直线路径
+     * @param start 起点
+     * @param end 终点
+     * @param num_points 路径点数量
+     */
+    void generateLine(const PathPoint& start, const PathPoint& end, int num_points = 100);
+
+    /**
+     * @brief 生成圆弧路径
+     * @param center_x 圆心x坐标
+     * @param center_y 圆心y坐标
+     * @param radius 半径
+     * @param start_angle 起始角度 (rad)
+     * @param end_angle 终止角度 (rad)
+     * @param num_points 路径点数量
+     */
+    void generateCircleArc(double center_x, double center_y, double radius,
+                          double start_angle, double end_angle, int num_points = 100);
+
+    /**
+     * @brief 生成三次贝塞尔曲线
+     * @param p0 起点
+     * @param p1 第一个控制点
+     * @param p2 第二个控制点
+     * @param p3 终点
+     * @param num_points 路径点数量
+     */
+    void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
+                            const PathPoint& p2, const PathPoint& p3,
+                            int num_points = 100);
+
+    /**
+     * @brief 从路径点数组生成路径
+     * @param points 路径点数组
+     */
+    void setPathPoints(const std::vector<PathPoint>& points);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 从CSV文件加载路径点
+     * @param filename CSV文件路径
+     * @param has_header 是否包含表头（默认true）
+     * @return 是否加载成功
+     */
+    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 关键路径点
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数
+     */
+    void generateSpline(const std::vector<PathPoint>& key_points,
+                       int num_points = 100,
+                       double tension = 0.5);
+
+    /**
+     * @brief 获取路径点
+     */
+    const std::vector<PathPoint>& getPathPoints() const { return path_points_; }
+
+    /**
+     * @brief 获取指定参数t处的路径点（线性插值）
+     * @param t 参数 [0, 1]
+     * @return 插值后的路径点
+     */
+    PathPoint getPointAt(double t) const;
+
+    /**
+     * @brief 获取路径长度
+     */
+    double getPathLength() const;
+
+    /**
+     * @brief 找到最近的路径点索引
+     * @param x 查询点x坐标
+     * @param y 查询点y坐标
+     * @return 最近点的索引
+     */
+    int findNearestPoint(double x, double y) const;
+
+private:
+    std::vector<PathPoint> path_points_;
+
+    // 计算两点间的曲率
+    double computeCurvature(const PathPoint& p1, const PathPoint& p2,
+                           const PathPoint& p3) const;
+};
+
+#endif // PATH_CURVE_H

include/path_tracker.h

@@ -0,0 +1,75 @@
+#ifndef PATH_TRACKER_H
+#define PATH_TRACKER_H
+
+#include "agv_model.h"
+#include "path_curve.h"
+#include "control_generator.h"
+#include <string>
+#include <fstream>
+
+/**
+ * @brief 路径跟踪器
+ * 整合模型、路径和控制生成，提供完整的路径跟踪功能
+ */
+class PathTracker {
+public:
+    /**
+     * @brief 构造函数
+     * @param model AGV模型
+     */
+    explicit PathTracker(const AGVModel& model);
+
+    /**
+     * @brief 设置参考路径
+     */
+    void setReferencePath(const PathCurve& path);
+
+    /**
+     * @brief 设置初始状态
+     */
+    void setInitialState(const AGVModel::State& state);
+
+    /**
+     * @brief 生成控制序列
+     * @param algorithm 算法类型 (pure_pursuit 或 stanley)
+     * @param dt 时间步长
+     * @param horizon 时域
+     * @param desired_velocity 期望速度 (m/s)
+     * @return 是否成功
+     */
+    bool generateControlSequence(const std::string& algorithm = "pure_pursuit",
+                                 double dt = 0.1,
+                                 double horizon = 10.0,
+                                 double desired_velocity = 1.0);
+
+    /**
+     * @brief 获取控制序列
+     */
+    const ControlSequence& getControlSequence() const { return control_sequence_; }
+
+    /**
+     * @brief 打印控制序列到控制台
+     */
+    void printControlSequence() const;
+
+    /**
+     * @brief 保存控制序列到文件
+     * @param filename 文件名
+     */
+    bool saveControlSequence(const std::string& filename) const;
+
+    /**
+     * @brief 保存轨迹到文件（用于可视化）
+     * @param filename 文件名
+     */
+    bool saveTrajectory(const std::string& filename) const;
+
+private:
+    AGVModel model_;
+    PathCurve reference_path_;
+    AGVModel::State initial_state_;
+    ControlSequence control_sequence_;
+    ControlGenerator control_generator_;
+};
+
+#endif // PATH_TRACKER_H

include/path_tracker.h.backup3

@@ -0,0 +1,73 @@
+#ifndef PATH_TRACKER_H
+#define PATH_TRACKER_H
+
+#include "agv_model.h"
+#include "path_curve.h"
+#include "control_generator.h"
+#include <string>
+#include <fstream>
+
+/**
+ * @brief 路径跟踪器
+ * 整合模型、路径和控制生成，提供完整的路径跟踪功能
+ */
+class PathTracker {
+public:
+    /**
+     * @brief 构造函数
+     * @param model AGV模型
+     */
+    explicit PathTracker(const AGVModel& model);
+
+    /**
+     * @brief 设置参考路径
+     */
+    void setReferencePath(const PathCurve& path);
+
+    /**
+     * @brief 设置初始状态
+     */
+    void setInitialState(const AGVModel::State& state);
+
+    /**
+     * @brief 生成控制序列
+     * @param algorithm 算法类型 (pure_pursuit 或 stanley)
+     * @param dt 时间步长
+     * @param horizon 时域
+     * @return 是否成功
+     */
+    bool generateControlSequence(const std::string& algorithm = "pure_pursuit",
+                                 double dt = 0.1,
+                                 double horizon = 10.0);
+
+    /**
+     * @brief 获取控制序列
+     */
+    const ControlSequence& getControlSequence() const { return control_sequence_; }
+
+    /**
+     * @brief 打印控制序列到控制台
+     */
+    void printControlSequence() const;
+
+    /**
+     * @brief 保存控制序列到文件
+     * @param filename 文件名
+     */
+    bool saveControlSequence(const std::string& filename) const;
+
+    /**
+     * @brief 保存轨迹到文件（用于可视化）
+     * @param filename 文件名
+     */
+    bool saveTrajectory(const std::string& filename) const;
+
+private:
+    AGVModel model_;
+    PathCurve reference_path_;
+    AGVModel::State initial_state_;
+    ControlSequence control_sequence_;
+    ControlGenerator control_generator_;
+};
+
+#endif // PATH_TRACKER_H