最小安全距离添加到6

luxiaotao1123

7 天以前 3ad23ed4281aba7a0db52688d6ee318b9429f33e

 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(
@@ -363,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);
@@ -388,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;
    }

    /**
@@ -562,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      位置代码