优化更新

jianghaiyue

2025-11-05 12f30a09bce1c61b2ae90129124cdc467a59b074

优化更新

 algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java

@@ -158,7 +158,8 @@
        if (fastPath != null) {
            return fastPath;
        }
        return planSpaceTimePath(startCode, endCode, constraints, null, null);
        long defaultStartTime = System.currentTimeMillis();
        return planSpaceTimePath(startCode, endCode, constraints, null, null, defaultStartTime);
    }

    @Override
@@ -174,12 +175,14 @@
     * @param constraints           静态约束条件
     * @param spaceTimeOccupancyMap 时空占用表
     * @param physicalConfig        CTU物理配置
     * @param startTimeMs           起始时间（毫秒）
     * @return 规划的路径
     */
    public PlannedPath planSpaceTimePath(String startCode, String endCode,
                                         List<double[]> constraints,
                                         Map<String, String> spaceTimeOccupancyMap,
                                         CTUPhysicalConfig physicalConfig) {
                                         CTUPhysicalConfig physicalConfig,
                                         long startTimeMs) {
        // 验证输入
        if (!isValidPathPoint(startCode) || !isValidPathPoint(endCode)) {
            System.out.println("无效的路径点: " + startCode + " 或 " + endCode);
@@ -212,7 +215,7 @@
        // 时空A*算法实现
        PlannedPath result = spaceTimeAStarSearch(
                startCode, endCode, startCoord, endCoord,
                constraints, spaceTimeOccupancyMap, physicalConfig
                constraints, spaceTimeOccupancyMap, physicalConfig, startTimeMs
        );

        if (result != null) {
@@ -234,13 +237,15 @@
     * @param constraints    约束条件
     * @param occupancyMap   时空占用表
     * @param physicalConfig 物理配置
     * @param startTimeMs    起始时间（毫秒）
     * @return 规划的路径
     */
    private PlannedPath spaceTimeAStarSearch(String startCode, String endCode,
                                             int[] startCoord, int[] endCoord,
                                             List<double[]> constraints,
                                             Map<String, String> occupancyMap,
                                             CTUPhysicalConfig physicalConfig) {
                                             CTUPhysicalConfig physicalConfig,
                                             long startTimeMs) {

        // 使用优先队列实现开放列表
        PriorityQueue<SpaceTimeAStarNode> openSet = new PriorityQueue<>(
@@ -250,8 +255,7 @@
        Map<String, Double> gScores = new HashMap<>();
        Map<String, SpaceTimeAStarNode> cameFrom = new HashMap<>();

        // 起始时间
        long startTime = System.currentTimeMillis();
        long startTime = startTimeMs;

        // 初始化起始节点
        SpaceTimeAStarNode startNode = new SpaceTimeAStarNode(

 algo-zkd/src/main/java/com/algo/service/PathPlanningService.java

@@ -103,41 +103,70 @@
        this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);

        long startTime = System.currentTimeMillis();
        long unifiedTimestamp = startTime / 1000; // 统一时间戳（秒）
        long unifiedTimestamp = startTime / 1000;

        System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
        System.out.println("统一时间基准: " + unifiedTimestamp + "秒 (" + new Date(startTime) + ")");
        System.out.println("时间基准: " + unifiedTimestamp + "秒 (" + new Date(startTime) + ")");

        // 统计故障车数量
        long faultyCount = agvStatusList.stream().filter(agv -> agv.getError() == 1).count();
        if (faultyCount > 0) {
            System.out.println("检测到 " + faultyCount + " 个故障车(error=1)，将在时空表中占据位置");
            System.out.println("检测到 " + faultyCount + " 个故障车(error)，将在时空表中占据位置");
        }

        // 1. 构建现有剩余路径的时空占用表
        System.out.println("步骤1: 构建时空占用表");
        Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList);
        Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList, unifiedTimestamp);
        System.out.println("时空占用表构建完成，包含 " + spaceTimeOccupancyMap.size() + " 个占用点");

        // 2. 分类处理CTU：有剩余路径的和需要新路径的
        // 2. 初始化规划结果集合
        List<PlannedPath> plannedPaths = new ArrayList<>();
        List<ExecutingTask> executingTasks = new ArrayList<>();
        Map<String, String> plannedAgvIds = new HashMap<>(); // 提前初始化标记故障车

        // 3. 分类处理CTU：有剩余路径的和需要新路径的
        List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>();
        List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>();
        List<AGVStatus> unhandledAgvs = new ArrayList<>(); // 未处理的AGV

        for (AGVStatus agv : agvStatusList) {
            // 故障车已在时空占用表中处理，标记为已处理但不规划路径
            if (agv.getError() == 1) {
                plannedAgvIds.put(agv.getAgvId(), "FAULTY");
                continue;
            }
            
            if (agv.hasRemainingPath()) {
                ctuWithRemainingPaths.add(agv);
            } else if (agv.canAcceptNewTask() || (includeIdleAgv && isAgvIdle(agv))) {
            } else if (agv.canAcceptNewTask()) {
                // 可以接受新任务的AGV（有任务或可以接受新任务）
                ctuNeedingNewPaths.add(agv);
            } else if (includeIdleAgv && isAgvIdle(agv)) {
                // 完全空闲的AGV（无任务，无剩余路径）
                ctuNeedingNewPaths.add(agv);
            } else {
                // 其他情况：不可用、有剩余路径但>1、有任务但不可用等
                unhandledAgvs.add(agv);
            }
        }

        System.out.println("CTU分类完成: 有剩余路径=" + ctuWithRemainingPaths.size() +
                ", 需要新路径=" + ctuNeedingNewPaths.size());
                ", 需要新路径=" + ctuNeedingNewPaths.size() +
                ", 未处理=" + unhandledAgvs.size());
        
        // 记录未处理的AGV信息（debug）
        if (!unhandledAgvs.isEmpty()) {
            for (AGVStatus agv : unhandledAgvs) {
                System.out.println("  - AGV " + agv.getAgvId() + 
                    ": error=" + agv.getError() + 
                    ", hasRemainingPath=" + agv.hasRemainingPath() + 
                    ", canAcceptNewTask=" + agv.canAcceptNewTask() + 
                    ", isIdle=" + isAgvIdle(agv) +
                    ", isAvailable=" + agv.isAvailable());
            }
        }

        // 3. 处理有剩余路径的CTU
        List<PlannedPath> plannedPaths = new ArrayList<>();
        List<ExecutingTask> executingTasks = new ArrayList<>();
        Map<String, String> plannedAgvIds = new HashMap<>();
        // 4. 处理有剩余路径的CTU

        System.out.println("步骤2: 处理有剩余路径的CTU");
        for (AGVStatus agv : ctuWithRemainingPaths) {
@@ -157,13 +186,13 @@
            }
        }

        // 3.5. 检查空闲AGV是否需要让行
        // 4.5. 检查空闲AGV是否需要让行
        System.out.println("检查空闲AGV是否需要让行");
        List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
        if (!yieldingAgvs.isEmpty()) {
            System.out.println("  发现 " + yieldingAgvs.size() + " 个需要让行的空闲AGV");
            for (AGVStatus yieldAgv : yieldingAgvs) {
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -175,12 +204,12 @@
            System.out.println("  无需让行的AGV");
        }

        // 4. 为需要新路径的CTU提取执行中任务
        // 5. 为需要新路径的CTU提取执行中任务
        System.out.println("步骤3: 提取需要新路径的CTU任务");
        List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
        System.out.println("提取到 " + newTasks.size() + " 个新任务");

        // 5. 为新任务规划路径（考虑时空约束）
        // 6. 为新任务规划路径（考虑时空约束）
        System.out.println("步骤4: 为新任务规划路径");
        for (ExecutingTask task : newTasks) {
            String agvId = task.getAgvId();
@@ -208,7 +237,7 @@

            // 规划时空安全的路径
            PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                    currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
                    currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
            );

            if (plannedPath != null) {
@@ -216,7 +245,6 @@
                plannedPath.setAgvId(agvId);
                plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));

                // 增强路径代码信息
                enhancePathCodes(plannedPath, task);

                plannedPaths.add(plannedPath);
@@ -233,11 +261,11 @@
            }
        }

        // 6. 处理空闲CTU
        // 7. 处理空闲CTU
        if (includeIdleAgv) {
            System.out.println("步骤5: 处理空闲CTU");
            
            // 6.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            // 7.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);

            List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
@@ -250,7 +278,7 @@
            if (!yieldingAgvsToHandle.isEmpty()) {
                System.out.println( yieldingAgvsToHandle.size() + " 个需要避让的空闲AGV");
                for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                    if (yieldPath != null) {
                        plannedPaths.add(yieldPath);
                        plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -260,18 +288,18 @@
                System.out.println("  无需避让的空闲AGV");
            }
            
            // 6.2. 然后为剩余的空闲AGV规划待机/充电路径
            // 7.2. 然后为剩余的空闲AGV规划待机/充电路径
            List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap, unifiedTimestamp
            );
            plannedPaths.addAll(idlePaths);
            System.out.println("  空闲CTU路径规划，数量: " + idlePaths.size());
        }

        // 6.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints);
        // 7.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);

        // 7. 最终碰撞检测和解决（针对同时生成的新路径）
        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);

@@ -415,27 +443,30 @@
     * @param constraints  约束条件
     * @param occupancyMap 时空占用表
     * @param agvStatus    CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 规划的路径
     */
    private PlannedPath planPathWithSpaceTimeConstraints(String startPos, String endPos,
                                                         List<double[]> constraints,
                                                         Map<String, String> occupancyMap,
                                                         AGVStatus agvStatus) {
                                                         AGVStatus agvStatus,
                                                         long unifiedTimestamp) {
        // 带时空占用表的路径规划
        long startTimeMs = unifiedTimestamp * 1000; 
        PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
                startPos, 
                endPos, 
                constraints, 
                occupancyMap, 
                agvStatus.getPhysicalConfig()
                agvStatus.getPhysicalConfig(),
                startTimeMs 
        );
        
        if (spacetimePath == null) {
            return null;
        }

        long startTime = System.currentTimeMillis();
        timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
        timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);

        return spacetimePath;
    }
@@ -526,12 +557,14 @@
     * @param plannedAgvIds 已规划CTU映射
     * @param constraints   约束条件
     * @param occupancyMap  时空占用表
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 空闲CTU路径列表
     */
    private List<PlannedPath> planIdleAgvPathsWithConstraints(List<AGVStatus> agvStatusList,
                                                              Map<String, String> plannedAgvIds,
                                                              List<double[]> constraints,
                                                              Map<String, String> occupancyMap) {
                                                              Map<String, String> occupancyMap,
                                                              long unifiedTimestamp) {
        List<PlannedPath> idlePaths = new ArrayList<>();

        for (AGVStatus agvStatus : agvStatusList) {
@@ -549,7 +582,7 @@

                if (targetPos != null && !targetPos.equals(currentPos)) {
                    PlannedPath idlePath = planPathWithSpaceTimeConstraints(
                            currentPos, targetPos, constraints, occupancyMap, agvStatus
                            currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
                    );

                    if (idlePath != null) {
@@ -709,7 +742,8 @@
                                           List<PlannedPath> plannedPaths,
                                           Map<String, String> plannedAgvIds,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<double[]> constraints) {
                                           List<double[]> constraints,
                                           long unifiedTimestamp) {
        final int MAX_ITERATIONS = 5; // 最大迭代次数
        int iteration = 0;
        int totalYieldingCount = 0;
@@ -738,7 +772,7 @@
                    continue;
                }
                
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
@@ -827,7 +861,8 @@
     * @return 让行路径
     */
    private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
                                     Map<String, String> occupancyMap, List<double[]> constraints) {
                                     Map<String, String> occupancyMap, List<double[]> constraints,
                                     long unifiedTimestamp) {
        String currentPos = yieldAgv.getPosition();
        String agvId = yieldAgv.getAgvId();
        
@@ -853,7 +888,7 @@
        
        // 3. 规划避让路径
        PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
            currentPos, targetPos, constraints, occupancyMap, yieldAgv
            currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
        );
        
        if (yieldPath != null) {
@@ -1114,7 +1149,8 @@
    private List<PlannedPath> planTasksInParallel(List<ExecutingTask> tasks,
                                                  List<double[]> constraints,
                                                  Map<String, String> spaceTimeOccupancyMap,
                                                  List<AGVStatus> agvStatusList) {
                                                  List<AGVStatus> agvStatusList,
                                                  long unifiedTimestamp) {

        List<PlannedPath> allPaths = Collections.synchronizedList(new ArrayList<>());

@@ -1135,7 +1171,7 @@

            for (ExecutingTask task : batch) {
                Future<PlannedPath> future = executorService.submit(() -> {
                    return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList);
                    return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList, unifiedTimestamp);
                });
                futures.add(future);
            }
@@ -1165,7 +1201,8 @@
    private PlannedPath planSingleTaskPath(ExecutingTask task,
                                           List<double[]> constraints,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<AGVStatus> agvStatusList) {
                                           List<AGVStatus> agvStatusList,
                                           long unifiedTimestamp) {

        String agvId = task.getAgvId();
        String currentPos = task.getCurrentPosition();
@@ -1181,9 +1218,9 @@
            return null;
        }

        // 规划时空安全的路径
        // 规划时空安全的路径（使用统一时间戳）
        PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus
                currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
        );

        if (plannedPath != null) {

 algo-zkd/src/main/java/com/algo/service/RemainingPathProcessor.java

@@ -40,11 +40,11 @@
     * 2. 对error=1的故障车，全程占据时空表，确保可以生成绕行路线
     *
     * @param agvStatusList CTU状态列表
     * @param unifiedTimestamp 统一时间戳
     * @return 时空占用表，key为"x,y,timeSlot"，value为CTU编号
     */
    public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList) {
        // 重置统一时间戳为当前时间
        this.unifiedTimestamp = System.currentTimeMillis() / 1000;
    public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList, long unifiedTimestamp) {
        this.unifiedTimestamp = unifiedTimestamp;
        
        Map<String, String> occupancyMap = new HashMap<>();

@@ -103,36 +103,20 @@
    
    /**
     * 处理静止AGV的位置占用
     *
     * @param agv          CTU状态
     * @param occupancyMap 时空占用表
     */
    private void processStaticAgvOccupancy(AGVStatus agv, Map<String, String> occupancyMap) {
        
        if (!agv.hasRemainingPath()) {
            System.out.println("  静止AGV " + agv.getAgvId());
            return;
        }
        
        String position = agv.getPosition();
        if (position == null || position.isEmpty()) {
            return;
        if (position != null && !position.isEmpty()) {
            System.out.println("  静止AGV " + agv.getAgvId() + " 有剩余路径，已在剩余路径处理中占用位置");
        }
        
        // 获取位置坐标
        int[] coord = JsonUtils.getCoordinate(position, pathMapping);
        if (coord == null) {
            return;
        }
        
        CTUPhysicalConfig config = agv.getPhysicalConfig();
        
        // 静止AGV占用当前位置的长时间段（假设300秒）
        long occupancyDuration = 300; // 300秒的占用时间

        for (long timeSlot = unifiedTimestamp; timeSlot < unifiedTimestamp + occupancyDuration; timeSlot++) {
            String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
            occupancyMap.put(spaceTimeKey, agv.getAgvId());
            
            occupyAdjacentSpaces(coord, timeSlot, agv.getAgvId(), occupancyMap, config);
        }
        
        System.out.println("  静止AGV " + agv.getAgvId() + " 占用位置 " + position + 
                         " (坐标: " + coord[0] + "," + coord[1] + ") " );
    }

    /**