最小安全距离添加到6

luxiaotao1123

7 天以前 3ad23ed4281aba7a0db52688d6ee318b9429f33e

 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,74 @@
            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;
    }

    /**
     * 优化路径
     */
    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);
            
            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())) {
                    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      位置代码