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