上传文件至 src/dispatch/rl

src/dispatch/rl/dqn_agent.cpp

@@ -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;
+}

src/dispatch/rl/rl_enhanced_agv_manager.cpp

@@ -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;
+}

src/dispatch/rl/state_encoder.cpp

@@ -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));
+}