initial

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