上传文件至 src/dispatch/rl
This commit is contained in:
303
src/dispatch/rl/dqn_agent.cpp
Normal file
303
src/dispatch/rl/dqn_agent.cpp
Normal file
@@ -0,0 +1,303 @@
|
||||
#include "dispatch/rl/dqn_agent.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
#ifdef USE_LIBTORCH
|
||||
// LibTorch实现
|
||||
DQNAgent::DQNAgent(const std::string& model_path)
|
||||
: model_loaded_(false), action_dim_(0), model_path_(model_path) {
|
||||
|
||||
device_ = torch::kCPU;
|
||||
|
||||
#ifdef USE_CUDA
|
||||
if (torch::cuda::is_available()) {
|
||||
std::cout << "[DQN] 使用CUDA加速推理" << std::endl;
|
||||
device_ = torch::kCUDA;
|
||||
}
|
||||
#endif
|
||||
|
||||
loadModel(model_path);
|
||||
}
|
||||
|
||||
DQNAgent::~DQNAgent() {
|
||||
// TorchScript模块会自动释放
|
||||
}
|
||||
|
||||
bool DQNAgent::loadModel(const std::string& model_path) {
|
||||
try {
|
||||
std::cout << "[DQN] 加载模型: " << model_path << std::endl;
|
||||
|
||||
// 检查文件是否存在
|
||||
std::ifstream f(model_path);
|
||||
if (!f.good()) {
|
||||
std::cerr << "[DQN] 模型文件不存在: " << model_path << std::endl;
|
||||
return false;
|
||||
}
|
||||
f.close();
|
||||
|
||||
// 加载TorchScript模型
|
||||
model_ = torch::jit::load(model_path);
|
||||
model_.to(device_);
|
||||
model_.eval();
|
||||
|
||||
model_loaded_ = true;
|
||||
|
||||
// 尝试推断动作维度 (通过运行一次前向传播)
|
||||
try {
|
||||
std::vector<float> dummy_state(35, 0.0f);
|
||||
auto output = predict(dummy_state);
|
||||
action_dim_ = output;
|
||||
std::cout << "[DQN] 模型加载成功, 动作维度: " << action_dim_ << std::endl;
|
||||
} catch (...) {
|
||||
action_dim_ = 5; // 默认值
|
||||
std::cout << "[DQN] 模型加载成功 (使用默认动作维度: 5)" << std::endl;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} catch (const c10::Error& e) {
|
||||
std::cerr << "[DQN] 模型加载失败: " << e.what() << std::endl;
|
||||
model_loaded_ = false;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[DQN] 模型加载失败: " << e.what() << std::endl;
|
||||
model_loaded_ = false;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
int DQNAgent::predict(const std::vector<float>& state) {
|
||||
if (!model_loaded_) {
|
||||
std::cerr << "[DQN] 模型未加载, 返回默认动作" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
try {
|
||||
// 转换为torch tensor
|
||||
torch::Tensor input = torch::from_blob(
|
||||
const_cast<float*>(state.data()),
|
||||
{1, 1, static_cast<long>(state.size())},
|
||||
torch::kFloat32
|
||||
).clone().to(device_);
|
||||
|
||||
// 推理
|
||||
std::vector<torch::jit::IValue> inputs;
|
||||
inputs.push_back(input);
|
||||
|
||||
auto outputs = model_.forward(inputs);
|
||||
torch::Tensor output = outputs.toTensor();
|
||||
|
||||
// 获取最佳动作
|
||||
torch::Tensor action = output.argmax(1);
|
||||
return action.item<int>();
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[DQN] 推理错误: " << e.what() << ", 返回默认动作" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<float> DQNAgent::predictBatch(const std::vector<std::vector<float>>& states) {
|
||||
if (!model_loaded_) {
|
||||
std::cerr << "[DQN] 模型未加载" << std::endl;
|
||||
return {};
|
||||
}
|
||||
|
||||
try {
|
||||
std::vector<torch::Tensor> tensors;
|
||||
for (const auto& state : states) {
|
||||
tensors.push_back(torch::from_blob(
|
||||
const_cast<float*>(state.data()),
|
||||
{1, static_cast<long>(state.size())},
|
||||
torch::kFloat32
|
||||
).clone());
|
||||
}
|
||||
|
||||
torch::Tensor batch = torch::cat(tensors, 0).unsqueeze(1).to(device_);
|
||||
|
||||
std::vector<torch::jit::IValue> inputs;
|
||||
inputs.push_back(batch);
|
||||
|
||||
auto outputs = model_.forward(inputs);
|
||||
torch::Tensor q_values = outputs.toTensor();
|
||||
|
||||
return std::vector<float>(
|
||||
q_values.data_ptr<float>(),
|
||||
q_values.data_ptr<float>() + q_values.numel()
|
||||
);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[DQN] 批量推理错误: " << e.what() << std::endl;
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
#else // !USE_LIBTORCH
|
||||
// 无LibTorch时的降级实现
|
||||
DQNAgent::DQNAgent(const std::string& model_path)
|
||||
: model_loaded_(false), action_dim_(0), model_path_(model_path) {
|
||||
std::cout << "[DQN] LibTorch未启用, 使用基于规则的降级策略" << std::endl;
|
||||
}
|
||||
|
||||
DQNAgent::~DQNAgent() = default;
|
||||
|
||||
bool DQNAgent::loadModel(const std::string& model_path) {
|
||||
std::cout << "[DQN] 跳过模型加载 (LibTorch未编译)" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
int DQNAgent::predict(const std::vector<float>& state) {
|
||||
std::cerr << "[DQN] 模型不可用, 返回默认动作" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::vector<float> DQNAgent::predictBatch(const std::vector<std::vector<float>>& states) {
|
||||
std::cerr << "[DQN] 模型不可用" << std::endl;
|
||||
return {};
|
||||
}
|
||||
#endif // USE_LIBTORCH
|
||||
|
||||
// PathPlanningDQNAgent实现
|
||||
PathPlanningDQNAgent::PathPlanningDQNAgent(const std::string& model_path, int num_candidates)
|
||||
: num_candidates_(num_candidates) {
|
||||
agent_ = std::make_unique<DQNAgent>(model_path);
|
||||
}
|
||||
|
||||
int PathPlanningDQNAgent::selectPath(const std::vector<float>& state) {
|
||||
if (agent_->isAvailable()) {
|
||||
return agent_->predict(state);
|
||||
}
|
||||
|
||||
// 降级到规则策略
|
||||
std::cout << "[DQN] 使用规则降级策略选择路径" << std::endl;
|
||||
return 0; // 默认选择第一条路径
|
||||
}
|
||||
|
||||
int PathPlanningDQNAgent::selectPathEncoded(
|
||||
const std::vector<float>& task_features,
|
||||
const std::vector<float>& agv_features,
|
||||
const std::vector<std::vector<float>>& path_features,
|
||||
const std::vector<float>& global_congestion,
|
||||
const std::vector<float>& time_features,
|
||||
const std::vector<float>& urgency_features
|
||||
) {
|
||||
// 组装完整状态 (35维)
|
||||
std::vector<float> state;
|
||||
state.reserve(35);
|
||||
|
||||
// 任务 (3)
|
||||
state.insert(state.end(), task_features.begin(), task_features.end());
|
||||
|
||||
// AGV (4)
|
||||
state.insert(state.end(), agv_features.begin(), agv_features.end());
|
||||
|
||||
// 路径 (20 = 5×4)
|
||||
for (const auto& pf : path_features) {
|
||||
state.insert(state.end(), pf.begin(), pf.end());
|
||||
}
|
||||
// 填充到5条路径
|
||||
while (state.size() < 27) {
|
||||
state.push_back(0.0f);
|
||||
}
|
||||
|
||||
// 全局拥堵 (4)
|
||||
state.insert(state.end(), global_congestion.begin(), global_congestion.end());
|
||||
|
||||
// 时间 (2)
|
||||
state.insert(state.end(), time_features.begin(), time_features.end());
|
||||
|
||||
// 紧急度 (2)
|
||||
state.insert(state.end(), urgency_features.begin(), urgency_features.end());
|
||||
|
||||
return selectPath(state);
|
||||
}
|
||||
|
||||
// TaskAssignmentDQNAgent实现
|
||||
TaskAssignmentDQNAgent::TaskAssignmentDQNAgent(const std::string& model_path, int num_agvs)
|
||||
: num_agvs_(num_agvs) {
|
||||
agent_ = std::make_unique<DQNAgent>(model_path);
|
||||
}
|
||||
|
||||
int TaskAssignmentDQNAgent::selectAGV(const std::vector<float>& state) {
|
||||
if (agent_->isAvailable()) {
|
||||
int action = agent_->predict(state);
|
||||
if (action < num_agvs_) {
|
||||
return action;
|
||||
}
|
||||
}
|
||||
|
||||
// 降级到规则策略: 选择第一个可用AGV
|
||||
std::cout << "[DQN] 使用规则降级策略选择AGV" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// RuleBasedFallback实现
|
||||
int RuleBasedFallback::selectPathByRules(const std::vector<std::vector<float>>& path_features) {
|
||||
if (path_features.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int best_path = 0;
|
||||
float best_score = std::numeric_limits<float>::lowest();
|
||||
|
||||
for (size_t i = 0; i < path_features.size(); ++i) {
|
||||
const auto& features = path_features[i];
|
||||
|
||||
if (features.size() < 4) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算综合得分
|
||||
// features: [length, conflicts, speed, smoothness]
|
||||
// 目标: 最小化长度和冲突, 最大化速度和平滑度
|
||||
float score = -features[0] * 1.0f // 长度惩罚
|
||||
- features[1] * 5.0f // 冲突惩罚 (更高权重)
|
||||
+ features[2] * 2.0f // 速度奖励
|
||||
+ features[3] * 1.0f; // 平滑度奖励
|
||||
|
||||
if (score > best_score) {
|
||||
best_score = score;
|
||||
best_path = static_cast<int>(i);
|
||||
}
|
||||
}
|
||||
|
||||
return best_path;
|
||||
}
|
||||
|
||||
int RuleBasedFallback::selectAGVByRules(const std::vector<std::vector<float>>& agv_features) {
|
||||
if (agv_features.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int best_agv = 0;
|
||||
float best_score = std::numeric_limits<float>::lowest();
|
||||
|
||||
for (size_t i = 0; i < agv_features.size(); ++i) {
|
||||
const auto& features = agv_features[i];
|
||||
|
||||
if (features.size() < 5) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// features: [distance, battery, speed, status, load]
|
||||
// 只考虑状态为可用(status > 0.5)的AGV
|
||||
if (features[3] < 0.5f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算得分
|
||||
float score = -features[0] * 2.0f // 距离惩罚 (距离近优先)
|
||||
+ features[1] * 1.0f // 电量奖励
|
||||
+ features[2] * 0.5f // 速度奖励
|
||||
- features[4] * 1.0f; // 负载惩罚 (负载低优先)
|
||||
|
||||
if (score > best_score) {
|
||||
best_score = score;
|
||||
best_agv = static_cast<int>(i);
|
||||
}
|
||||
}
|
||||
|
||||
return best_agv;
|
||||
}
|
||||
296
src/dispatch/rl/rl_enhanced_agv_manager.cpp
Normal file
296
src/dispatch/rl/rl_enhanced_agv_manager.cpp
Normal file
@@ -0,0 +1,296 @@
|
||||
#include "dispatch/rl/rl_enhanced_agv_manager.h"
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
RLEnhancedAGVManager::RLEnhancedAGVManager(GraphMap* map, ResourceManager* rm)
|
||||
: EnhancedAGVManager(map, rm),
|
||||
use_rl_for_path_(false),
|
||||
use_rl_for_assignment_(false),
|
||||
num_candidates_(5),
|
||||
num_agvs_(10) {
|
||||
|
||||
state_encoder_ = std::make_unique<StateEncoder>(map, rm);
|
||||
std::cout << "[RLManager] RLEnhancedAGVManager 初始化完成" << std::endl;
|
||||
}
|
||||
|
||||
bool RLEnhancedAGVManager::enableRLForPath(const std::string& model_path, int num_candidates) {
|
||||
try {
|
||||
path_dqn_ = std::make_unique<PathPlanningDQNAgent>(model_path, num_candidates);
|
||||
|
||||
if (path_dqn_->isAvailable()) {
|
||||
use_rl_for_path_ = true;
|
||||
num_candidates_ = num_candidates;
|
||||
std::cout << "[RLManager] 路径规划DQN已启用 (模型: " << model_path << ")" << std::endl;
|
||||
return true;
|
||||
} else {
|
||||
std::cerr << "[RLManager] 路径规划DQN加载失败, 将使用传统A*算法" << std::endl;
|
||||
return false;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[RLManager] 启用路径规划DQN时出错: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool RLEnhancedAGVManager::enableRLForAssignment(const std::string& model_path, int num_agvs) {
|
||||
try {
|
||||
assignment_dqn_ = std::make_unique<TaskAssignmentDQNAgent>(model_path, num_agvs);
|
||||
|
||||
if (assignment_dqn_->isAvailable()) {
|
||||
use_rl_for_assignment_ = true;
|
||||
num_agvs_ = num_agvs;
|
||||
std::cout << "[RLManager] 任务分配DQN已启用 (模型: " << model_path << ")" << std::endl;
|
||||
return true;
|
||||
} else {
|
||||
std::cerr << "[RLManager] 任务分配DQN加载失败, 将使用传统策略" << std::endl;
|
||||
return false;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[RLManager] 启用任务分配DQN时出错: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void RLEnhancedAGVManager::disableRL() {
|
||||
use_rl_for_path_ = false;
|
||||
use_rl_for_assignment_ = false;
|
||||
std::cout << "[RLManager] RL功能已禁用" << std::endl;
|
||||
}
|
||||
|
||||
int RLEnhancedAGVManager::assignTasksSmart() {
|
||||
if (use_rl_for_assignment_ && isRLAssignmentAvailable()) {
|
||||
return assignTasksWithRL();
|
||||
}
|
||||
|
||||
// Fallback到父类的传统策略
|
||||
return EnhancedAGVManager::assignTasksSmart();
|
||||
}
|
||||
|
||||
std::vector<Path*> RLEnhancedAGVManager::findPathForTask(AGV* agv, Task* task) {
|
||||
if (!agv || !task) {
|
||||
return {};
|
||||
}
|
||||
|
||||
if (use_rl_for_path_ && isRLPathAvailable()) {
|
||||
return selectOptimalPathWithRL(agv, task);
|
||||
}
|
||||
|
||||
// Fallback到传统A*
|
||||
Point* start = map_->getPoint(task->start_point_id);
|
||||
Point* end = map_->getPoint(task->end_point_id);
|
||||
|
||||
if (!start || !end) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return map_->findPath(start, end, {});
|
||||
}
|
||||
|
||||
std::vector<Path*> RLEnhancedAGVManager::selectOptimalPathWithRL(AGV* agv, Task* task) {
|
||||
rl_stats_.path_decisions++;
|
||||
|
||||
try {
|
||||
// 1. 使用GraphMap生成K条候选路径
|
||||
Point* start = map_->getPoint(task->start_point_id);
|
||||
Point* end = map_->getPoint(task->end_point_id);
|
||||
|
||||
if (!start || !end) {
|
||||
rl_stats_.path_fallbacks++;
|
||||
return findPathForTask(agv, task); // Fallback
|
||||
}
|
||||
|
||||
std::vector<std::vector<Path*>> candidates =
|
||||
map_->findKShortestPaths(start, end, num_candidates_);
|
||||
|
||||
if (candidates.empty()) {
|
||||
rl_stats_.path_fallbacks++;
|
||||
return findPathForTask(agv, task);
|
||||
}
|
||||
|
||||
// 2. 编码状态
|
||||
std::vector<float> state = state_encoder_->encodePathPlanningState(
|
||||
agv, task, candidates
|
||||
);
|
||||
|
||||
// 3. DQN选择路径
|
||||
int selected_idx = path_dqn_->selectPath(state);
|
||||
|
||||
if (selected_idx < 0 || selected_idx >= static_cast<int>(candidates.size())) {
|
||||
selected_idx = 0; // 默认选择最短路径
|
||||
}
|
||||
|
||||
std::cout << "[RLManager] DQN选择路径 " << selected_idx
|
||||
<< " / " << candidates.size()
|
||||
<< " (任务: " << task->start_point_id << " -> " << task->end_point_id << ")"
|
||||
<< std::endl;
|
||||
|
||||
return candidates[selected_idx];
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[RLManager] 路径选择RL推理失败: " << e.what() << std::endl;
|
||||
rl_stats_.path_fallbacks++;
|
||||
return findPathForTask(agv, task); // Fallback
|
||||
}
|
||||
}
|
||||
|
||||
int RLEnhancedAGVManager::selectAGVWithRL(
|
||||
Task* task,
|
||||
const std::vector<AGV*>& available_agvs
|
||||
) {
|
||||
rl_stats_.assignment_decisions++;
|
||||
|
||||
try {
|
||||
// 编码状态
|
||||
std::vector<float> state = state_encoder_->encodeDispatchState(
|
||||
task, available_agvs
|
||||
);
|
||||
|
||||
// DQN选择AGV
|
||||
int selected_idx = assignment_dqn_->selectAGV(state);
|
||||
|
||||
if (selected_idx < 0 || selected_idx >= static_cast<int>(available_agvs.size())) {
|
||||
selected_idx = 0; // 默认选择第一个AGV
|
||||
}
|
||||
|
||||
std::cout << "[RLManager] DQN选择AGV " << selected_idx
|
||||
<< " / " << available_agvs.size()
|
||||
<< " (任务优先级: " << task->priority << ")"
|
||||
<< std::endl;
|
||||
|
||||
return selected_idx;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[RLManager] AGV选择RL推理失败: " << e.what() << std::endl;
|
||||
rl_stats_.assignment_fallbacks++;
|
||||
|
||||
// Fallback到规则策略
|
||||
return RuleBasedFallback::selectAGVByRules(
|
||||
[available_agvs, task]() {
|
||||
std::vector<std::vector<float>> features;
|
||||
for (AGV* agv : available_agvs) {
|
||||
Point* start = map_->getPoint(task->start_point_id);
|
||||
auto agv_feat = state_encoder_->encodeSingleAGVFeatures(agv, start);
|
||||
features.push_back(agv_feat);
|
||||
}
|
||||
return features;
|
||||
}()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
int RLEnhancedAGVManager::assignTasksWithRL() {
|
||||
auto pending_tasks = getPendingTasks();
|
||||
auto available_agvs = getAvailableAGVs();
|
||||
|
||||
if (pending_tasks.empty() || available_agvs.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int assigned_count = 0;
|
||||
|
||||
// 为每个任务选择最优AGV
|
||||
for (Task* task : pending_tasks) {
|
||||
if (available_agvs.empty()) {
|
||||
break;
|
||||
}
|
||||
|
||||
// 使用DQN选择AGV
|
||||
int agv_idx = selectAGVWithRL(task, available_agvs);
|
||||
|
||||
if (agv_idx >= 0 && agv_idx < static_cast<int>(available_agvs.size())) {
|
||||
AGV* selected_agv = available_agvs[agv_idx];
|
||||
|
||||
// 分配任务
|
||||
if (assignSingleTask(selected_agv, task)) {
|
||||
assigned_count++;
|
||||
|
||||
// 从可用列表移除
|
||||
available_agvs.erase(
|
||||
std::remove(available_agvs.begin(), available_agvs.end(), selected_agv),
|
||||
available_agvs.end()
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return assigned_count;
|
||||
}
|
||||
|
||||
bool RLEnhancedAGVManager::assignSingleTask(AGV* agv, Task* task) {
|
||||
if (!agv || !task) {
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
// 查找路径
|
||||
std::vector<Path*> path = findPathForTask(agv, task);
|
||||
|
||||
if (path.empty()) {
|
||||
std::cerr << "[RLManager] 无法为任务找到路径" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 分配任务
|
||||
agv->assignedTask = task;
|
||||
agv->currentPath = path;
|
||||
agv->state = AGVState::MOVING;
|
||||
|
||||
std::cout << "[RLManager] 任务已分配: AGV-" << agv->id
|
||||
<< " -> 任务-" << task->id
|
||||
<< " (路径长度: " << path.size() << " 段)"
|
||||
<< std::endl;
|
||||
|
||||
return true;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "[RLManager] 分配任务失败: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<AGV*> RLEnhancedAGVManager::getAvailableAGVs() {
|
||||
std::vector<AGV*> available;
|
||||
|
||||
for (auto& pair : agvs_) {
|
||||
AGV* agv = pair.second.get();
|
||||
if (agv->state == AGVState::IDLE) {
|
||||
available.push_back(agv);
|
||||
}
|
||||
}
|
||||
|
||||
return available;
|
||||
}
|
||||
|
||||
std::vector<Task*> RLEnhancedAGVManager::getPendingTasks() {
|
||||
std::vector<Task*> pending;
|
||||
|
||||
for (auto& pair : tasks_) {
|
||||
Task* task = pair.second.get();
|
||||
if (!task->assignedAGV && task->state == TaskState::PENDING) {
|
||||
pending.push_back(task);
|
||||
}
|
||||
}
|
||||
|
||||
return pending;
|
||||
}
|
||||
|
||||
void RLEnhancedAGVManager::printRLStatistics() {
|
||||
std::cout << "\n========== RL统计信息 ==========" << std::endl;
|
||||
std::cout << "路径规划:" << std::endl;
|
||||
std::cout << " 决策次数: " << rl_stats_.path_decisions << std::endl;
|
||||
std::cout << " 降级次数: " << rl_stats_.path_fallbacks << std::endl;
|
||||
|
||||
std::cout << "任务分配:" << std::endl;
|
||||
std::cout << " 决策次数: " << rl_stats_.assignment_decisions << std::endl;
|
||||
std::cout << " 降级次数: " << rl_stats_.assignment_fallbacks << std::endl;
|
||||
|
||||
// 计算成功率
|
||||
float path_success_rate = rl_stats_.path_decisions > 0 ?
|
||||
100.0f * (1.0f - float(rl_stats_.path_fallbacks) / rl_stats_.path_decisions) : 0.0f;
|
||||
float assign_success_rate = rl_stats_.assignment_decisions > 0 ?
|
||||
100.0f * (1.0f - float(rl_stats_.assignment_fallbacks) / rl_stats_.assignment_decisions) : 0.0f;
|
||||
|
||||
std::cout << "路径规划成功率: " << path_success_rate << "%" << std::endl;
|
||||
std::cout << "任务分配成功率: " << assign_success_rate << "%" << std::endl;
|
||||
std::cout << "==================================" << std::endl;
|
||||
}
|
||||
240
src/dispatch/rl/state_encoder.cpp
Normal file
240
src/dispatch/rl/state_encoder.cpp
Normal file
@@ -0,0 +1,240 @@
|
||||
#include "dispatch/rl/state_encoder.h"
|
||||
#include "dispatch/resource_manager.h"
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
StateEncoder::StateEncoder(GraphMap* map, ResourceManager* rm)
|
||||
: map_(map), resource_manager_(rm) {
|
||||
initializeMapBounds();
|
||||
}
|
||||
|
||||
void StateEncoder::initializeMapBounds() {
|
||||
// 初始化地图边界
|
||||
map_min_x_ = 0.0f;
|
||||
map_max_x_ = 100.0f;
|
||||
map_min_y_ = 0.0f;
|
||||
map_max_y_ = 100.0f;
|
||||
max_path_length_ = 200.0f; // 默认值
|
||||
|
||||
// TODO: 从实际地图中推断边界
|
||||
}
|
||||
|
||||
std::vector<float> StateEncoder::encodePathPlanningState(
|
||||
AGV* agv,
|
||||
Task* task,
|
||||
const std::vector<std::vector<Path*>>& candidates
|
||||
) {
|
||||
std::vector<float> state(35, 0.0f);
|
||||
|
||||
if (!agv || !task || !map_) {
|
||||
return state;
|
||||
}
|
||||
|
||||
// 获取起终点
|
||||
Point* start = map_->getPoint(task->start_point_id);
|
||||
Point* end = map_->getPoint(task->end_point_id);
|
||||
|
||||
if (!start || !end) {
|
||||
return state;
|
||||
}
|
||||
|
||||
// ========== 任务信息 (3维) ==========
|
||||
state[0] = clip(normalize(start->x, map_min_x_, map_max_x_));
|
||||
state[1] = clip(normalize(start->y, map_min_y_, map_max_y_));
|
||||
state[2] = clip(normalize(end->x, map_min_x_, map_max_x_));
|
||||
|
||||
// ========== AGV状态 (4维) ==========
|
||||
state[3] = clip(normalize(agv->x, map_min_x_, map_max_x_)); // 假设AGV有x,y成员
|
||||
state[4] = clip(normalize(agv->y, map_min_y_, map_max_y_));
|
||||
state[5] = clip(normalize(agv->currentSpeed, 0.0f, agv->maxSpeed)); // 假设AGV有这些成员
|
||||
state[6] = clip(normalize(agv->batteryLevel, 0.0f, 100.0f));
|
||||
|
||||
// ========== 候选路径特征 (20维 = 5条路径 × 4特征) ==========
|
||||
for (size_t i = 0; i < std::min(size_t(5), candidates.size()); ++i) {
|
||||
std::vector<float> path_features = encodeSinglePathFeatures(candidates[i]);
|
||||
int base = 7 + i * 4;
|
||||
|
||||
if (path_features.size() >= 4) {
|
||||
state[base + 0] = path_features[0]; // 归一化长度
|
||||
state[base + 1] = path_features[1]; // 归一化冲突数
|
||||
state[base + 2] = path_features[2]; // 归一化速度
|
||||
state[base + 3] = path_features[3]; // 平滑度
|
||||
}
|
||||
}
|
||||
|
||||
// ========== 全局拥堵 (4维) ==========
|
||||
std::vector<int> zone_occupancy = calculateZoneOccupancy(4);
|
||||
int total_agvs = 10; // TODO: 从实际系统获取
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
state[27 + i] = clip(normalize(float(zone_occupancy[i]), 0.0f, float(total_agvs)));
|
||||
}
|
||||
|
||||
// ========== 时间信息 (2维) ==========
|
||||
state[31] = 0.5f; // 当前时段 (简化)
|
||||
state[32] = 0.0f; // 历史拥堵预测 (简化)
|
||||
|
||||
// ========== 紧急度 (2维) ==========
|
||||
state[33] = clip(normalize(float(task->priority), 1.0f, 10.0f));
|
||||
state[34] = clip(normalize(task->time_remaining, 0.0f, task->deadline));
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
std::vector<float> StateEncoder::encodeSinglePathFeatures(
|
||||
const std::vector<Path*>& path
|
||||
) {
|
||||
std::vector<float> features(4, 0.0f);
|
||||
|
||||
if (path.empty()) {
|
||||
return features;
|
||||
}
|
||||
|
||||
// 使用GraphMap的PathFeatures计算
|
||||
GraphMap::PathFeatures path_features = map_->calculatePathFeatures(path);
|
||||
|
||||
// 归一化
|
||||
features[0] = clip(normalize(path_features.length, 0.0f, max_path_length_));
|
||||
features[1] = clip(normalize(float(path_features.conflicts), 0.0f, 10.0f));
|
||||
features[2] = clip(normalize(path_features.avg_speed, 0.0f, 5.0f));
|
||||
features[3] = clip(path_features.smoothness);
|
||||
|
||||
return features;
|
||||
}
|
||||
|
||||
std::vector<float> StateEncoder::encodeDispatchState(
|
||||
Task* task,
|
||||
const std::vector<AGV*>& available_agvs
|
||||
) {
|
||||
std::vector<float> state;
|
||||
|
||||
if (!task || !map_) {
|
||||
return state;
|
||||
}
|
||||
|
||||
Point* start = map_->getPoint(task->start_point_id);
|
||||
Point* end = map_->getPoint(task->end_point_id);
|
||||
|
||||
if (!start || !end) {
|
||||
return state;
|
||||
}
|
||||
|
||||
double distance = start->distanceTo(*end);
|
||||
|
||||
// ========== 任务特征 (8维) ==========
|
||||
state.push_back(clip(normalize(start->x, map_min_x_, map_max_x_)));
|
||||
state.push_back(clip(normalize(start->y, map_min_y_, map_max_y_)));
|
||||
state.push_back(clip(normalize(end->x, map_min_x_, map_max_x_)));
|
||||
state.push_back(clip(normalize(end->y, map_min_y_, map_max_y_)));
|
||||
state.push_back(clip(normalize(distance, 0.0, max_path_length_)));
|
||||
state.push_back(clip(normalize(task->priority, 1.0f, 10.0f)));
|
||||
state.push_back(clip(normalize(task->deadline, 0.0, 300.0)));
|
||||
state.push_back(0.0f); // 任务类型编码 (简化)
|
||||
|
||||
// ========== 可用AGV池 (50维 = 10 AGV × 5特征) ==========
|
||||
for (size_t i = 0; i < std::min(size_t(10), available_agvs.size()); ++i) {
|
||||
std::vector<float> agv_features = encodeSingleAGVFeatures(available_agvs[i], start);
|
||||
state.insert(state.end(), agv_features.begin(), agv_features.end());
|
||||
}
|
||||
|
||||
// 填充到10个AGV
|
||||
while (state.size() < 58) { // 8 + 50
|
||||
state.push_back(0.0f);
|
||||
}
|
||||
|
||||
// ========== 全局状态 (7维) ==========
|
||||
state.push_back(0.3f); // 等待任务数 (简化)
|
||||
state.push_back(0.2f); // 执行中任务数 (简化)
|
||||
state.push_back(0.5f); # 已完成任务数 (简化)
|
||||
state.push_back(0.7f); # AGV平均利用率 (简化)
|
||||
state.push_back(0.8f); // 平均电量 (简化)
|
||||
state.push_back(0.1f); // 平均等待时间 (简化)
|
||||
state.push_back(0.9f); // 任务完成率 (简化)
|
||||
|
||||
// ========== 资源状态 (20维) ==========
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
state.push_back(0.0f); // 简化: 假设无资源占用
|
||||
}
|
||||
|
||||
// ========== 时间特征 (2维) ==========
|
||||
state.push_back(0.5f); // 时间进度
|
||||
state.push_back(0.8f); # 历史成功率 (简化)
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
std::vector<float> StateEncoder::encodeSingleAGVFeatures(
|
||||
AGV* agv,
|
||||
Point* task_start
|
||||
) {
|
||||
std::vector<float> features(5, 0.0f);
|
||||
|
||||
if (!agv || !task_start) {
|
||||
return features;
|
||||
}
|
||||
|
||||
// 计算到任务起点的距离
|
||||
double dx = agv->x - task_start->x;
|
||||
double dy = agv->y - task_start->y;
|
||||
double distance = std::sqrt(dx * dx + dy * dy);
|
||||
|
||||
features[0] = clip(normalize(distance, 0.0, max_path_length_));
|
||||
features[1] = clip(normalize(agv->batteryLevel, 0.0f, 100.0f));
|
||||
features[2] = clip(normalize(agv->currentSpeed, 0.0f, agv->maxSpeed));
|
||||
features[3] = (agv->status == AGVStatus::IDLE) ? 1.0f : 0.0f; // 假设有status枚举
|
||||
features[4] = clip(normalize(agv->currentLoad, 0.0f, agv->maxLoad));
|
||||
|
||||
return features;
|
||||
}
|
||||
|
||||
std::vector<int> StateEncoder::calculateZoneOccupancy(int num_zones) {
|
||||
std::vector<int> occupancy(num_zones, 0);
|
||||
|
||||
// 简化实现: 假设地图分为4个象限
|
||||
// 实际应用中应该遍历所有AGV并统计它们所在的区域
|
||||
|
||||
// TODO: 从ResourceManager获取所有AGV位置并统计
|
||||
|
||||
return occupancy;
|
||||
}
|
||||
|
||||
float StateEncoder::calculatePathSmoothness(const std::vector<Path*>& path) {
|
||||
if (path.size() < 3) {
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
double curvature_changes = 0.0;
|
||||
|
||||
for (size_t i = 1; i < path.size() - 1; ++i) {
|
||||
// 获取三个连续点
|
||||
Point* p1 = path[i-1]->start;
|
||||
Point* p2 = path[i]->start;
|
||||
Point* p3 = path[i+1]->start;
|
||||
|
||||
if (p1 && p2 && p3) {
|
||||
// 计算向量
|
||||
double v1x = p2->x - p1->x;
|
||||
double v1y = p2->y - p1->y;
|
||||
double v2x = p3->x - p2->x;
|
||||
double v2y = p3->y - p2->y;
|
||||
|
||||
// 归一化
|
||||
double norm1 = std::sqrt(v1x * v1x + v1y * v1y);
|
||||
double norm2 = std::sqrt(v2x * v2x + v2y * v2y);
|
||||
|
||||
if (norm1 > 1e-6 && norm2 > 1e-6) {
|
||||
v1x /= norm1;
|
||||
v1y /= norm1;
|
||||
v2x /= norm2;
|
||||
v2y /= norm2;
|
||||
|
||||
double dot = v1x * v2x + v1y * v2y;
|
||||
dot = std::max(-1.0, std::min(1.0, dot));
|
||||
|
||||
double angle = std::acos(dot);
|
||||
curvature_changes += angle;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 1.0f / (1.0f + static_cast<float>(curvature_changes));
|
||||
}
|
||||
Reference in New Issue
Block a user