118 lines
3.7 KiB
C++
118 lines
3.7 KiB
C++
#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
|