优化更新

jianghaiyue

2025-11-05 642f20dd1b068e23146b8ec96c5eac8b63c96b87

优化更新

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

@@ -367,8 +367,9 @@
            }
        }

        // 减少等待选项
        if (Math.random() < 0.2) {
        // 必要时（有冲突）生成等待节点；当前节点的所有邻居在短期内都被阻挡则等待
        boolean allNeighborsBlocked = checkIfAllNeighborsBlocked(current, constraintChecker, physicalConfig);
        if (allNeighborsBlocked) {
            long waitTime = timeResolution;
            long waitUntilTime = current.timePoint + waitTime;
            String waitKey = createSpaceTimeKey(current.code, waitUntilTime);
@@ -392,6 +393,52 @@
                }
            }
        }
    }

    /**
     * 检查当前节点的邻居是否在短期内都被阻挡
     */
    private boolean checkIfAllNeighborsBlocked(SpaceTimeAStarNode current,
                                               EnhancedConstraintChecker constraintChecker,
                                               CTUPhysicalConfig physicalConfig) {
        // 获取当前节点的所有空间邻居
        List<Map<String, String>> neighbors = getNeighbors(current.code);
        
        if (neighbors.isEmpty()) {
            return true;
        }
        
        // 检查未来几个时间片内的占用情况
        int checkTimeSteps = 3;
        boolean allBlocked = true;
        
        for (int step = 1; step <= checkTimeSteps; step++) {
            long checkTime = current.timePoint + (step * timeResolution);
            
            // 检查是否有至少一个邻居在当前时间片可用
            for (Map<String, String> neighbor : neighbors) {
                String neighborCode = neighbor.get("code");
                long arrivalTime = checkTime;
                
                // 对齐到时间分辨率
                arrivalTime = alignToTimeResolution(arrivalTime);
                
                // 检查这个邻居在当前时间片是否可用
                if (constraintChecker.isValidMove(current.code, neighborCode, 
                        current.timePoint, arrivalTime)) {
                    // 至少有一个邻居可用，不需要等待
                    allBlocked = false;
                    break;
                }
            }
            
            // 如果找到可用邻居，不需要等待
            if (!allBlocked) {
                break;
            }
        }
        
        return allBlocked;
    }

    /**
@@ -566,17 +613,85 @@
            codeList.add(pathCode);
        }

        codeList = optimizePathByRemovingBacktrack(codeList);

        PlannedPath plannedPath = new PlannedPath("", "", codeList);
        
        // 使用统一的时间计算器计算精确时间
        long startTime = pathNodes.get(0).timePoint;
        CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
        timeCalculator.calculatePathTiming(plannedPath, startTime, defaultConfig, 0.0);
        
        return plannedPath;
    }

    /**
     * 优化路径：去除空间上的重复点（避免来回走）
     * 检测并移除 A->B->A 模式的来回走，避免不必要的绕行
     *
     * @param codeList 原始路径代码列表
     * @return 优化后的路径代码列表
     */
    private List<PathCode> optimizePathByRemovingBacktrack(List<PathCode> codeList) {
        if (codeList == null || codeList.size() <= 2) {
            return codeList;
        }

        List<PathCode> optimized = new ArrayList<>();
        int i = 0;
        
        while (i < codeList.size()) {
            PathCode current = codeList.get(i);
            
            // 检查是否是来回走模式：A->B->A（连续三个点，第一个和第三个相同）
            // 如果出现来回走，跳过B和第二个A，直接到下一个不同的点
            if (i < codeList.size() - 2) {
                PathCode next = codeList.get(i + 1);
                PathCode nextNext = codeList.get(i + 2);
                
                // 检测来回走：当前位置 == 下下个位置，且中间位置不同
                if (current.getCode().equals(nextNext.getCode()) && 
                    !current.getCode().equals(next.getCode())) {
                    // 来回走：跳过B和第二个A，直接处理下一个不同的点
                    // 找到下一个与当前点不同的位置
                    int j = i + 2;
                    while (j < codeList.size() && codeList.get(j).getCode().equals(current.getCode())) {
                        j++;
                    }
                    // 如果找到了不同的点，或者到达末尾，跳过来回走的部分
                    if (j < codeList.size()) {
                        i = j; // 跳到下一个不同的点
                        continue;
                    } else {
                        // 如果后面都是相同点，保留当前点（可能是目标点）
                        break;
                    }
                }
            }
            
            // 正常添加当前点
            optimized.add(current);
            i++;
        }
        
        // 确保目标点被包含
        if (!optimized.isEmpty() && !codeList.isEmpty()) {
            PathCode lastOriginal = codeList.get(codeList.size() - 1);
            PathCode lastOptimized = optimized.get(optimized.size() - 1);
            if (!lastOriginal.getCode().equals(lastOptimized.getCode())) {
                optimized.add(lastOriginal);
            }
        }
        
        // 重新计算方向（因为优化后跳过了中间点，方向需要更新）
        for (int k = 0; k < optimized.size(); k++) {
            PathCode code = optimized.get(k);
            if (k < optimized.size() - 1) {
                PathCode nextCode = optimized.get(k + 1);
                String direction = calculateDirection(code.getCode(), nextCode.getCode());
                code.setDirection(direction);
            }
        }
        
        return optimized;
    }

    /**
     * 创建时空键
     *
     * @param code      位置代码

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

@@ -170,7 +170,7 @@

        System.out.println("步骤2: 处理有剩余路径的CTU");
        for (AGVStatus agv : ctuWithRemainingPaths) {
            PlannedPath remainingPath = processRemainingPath(agv);
            PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
            if (remainingPath != null) {
                plannedPaths.add(remainingPath);
                plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");
@@ -299,6 +299,9 @@
        // 7.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);

        // 7.6. 重新计算所有路径的时间戳
        recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);

        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -341,9 +344,10 @@
     * 处理CTU的剩余路径
     *
     * @param agv CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 处理后的路径
     */
    private PlannedPath processRemainingPath(AGVStatus agv) {
    private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
        if (!agv.hasRemainingPath()) {
            return null;
        }
@@ -376,8 +380,8 @@
        processedPath.setSegId(segId);

        if (timeCalculator != null && !remainingCodes.isEmpty()) {
            // 获取AGV的下一个路径点到达时间作为起始时间
            long startTime = agv.getNextPointArrivalTime();
            // 使用统一时间戳作为起始时间（转换为毫秒）
            long startTime = unifiedTimestamp * 1000;
            
            CTUPhysicalConfig config = agv.getPhysicalConfig();
            
@@ -619,7 +623,6 @@
            return;
        }

        String actionType = "2"; // 任务类型
        String taskId = task.getTaskId();
        int backpackLevel = task.getBackpackIndex();

@@ -627,20 +630,23 @@
            PathCode pathCode = codeList.get(i);

            // 设置基本信息
            pathCode.setActionType(actionType);
            pathCode.setTaskId(taskId);
            pathCode.setLev(backpackLevel);

            // 设置目标点信息
            if (i == codeList.size() - 1) { // 最后一个点
                pathCode.setTargetPoint(true);
                // 根据任务类型设置actionType和posType（
                if (task.isLoaded()) {
                    pathCode.setActionType("2"); // 放货
                    pathCode.setPosType("2"); // 放货
                } else {
                    pathCode.setActionType("1"); // 取货
                    pathCode.setPosType("1"); // 取货
                }
            } else {
                pathCode.setTargetPoint(false);
                pathCode.setActionType(null); 
                pathCode.setPosType(null);
            }
        }
@@ -796,6 +802,50 @@
        }
    }

    private void recalculateAllPathTimings(List<PlannedPath> plannedPaths, 
                                           List<AGVStatus> agvStatusList, 
                                           long unifiedTimestamp) {
        if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
            return;
        }

        Map<String, AGVStatus> agvStatusMap = new HashMap<>();
        for (AGVStatus agv : agvStatusList) {
            if (agv.getAgvId() != null) {
                agvStatusMap.put(agv.getAgvId(), agv);
            }
        }

        long startTimeMs = unifiedTimestamp * 1000;
        int recalculatedCount = 0;

        // 遍历所有路径，重新计算时间戳
        for (PlannedPath path : plannedPaths) {
            if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
                continue;
            }

            String agvId = path.getAgvId();
            if (agvId == null) {
                continue;
            }

            // 获取AGV的物理配置
            AGVStatus agvStatus = agvStatusMap.get(agvId);
            CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null) 
                    ? agvStatus.getPhysicalConfig() 
                    : new CTUPhysicalConfig(); 

            double initialSpeed = 0.0;

            // 重新计算路径时间戳
            timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
            recalculatedCount++;
        }

        System.out.println("  已重新计算 " + recalculatedCount + " 条路径的时间戳（统一时间基准: " + unifiedTimestamp + "秒）");
    }

    /**
     * 识别需要让行的AGV
     * 检查空闲AGV是否占用了其他AGV剩余路径上的位置