89 lines
2.4 KiB
C++
89 lines
2.4 KiB
C++
#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
|