最小安全距离添加到6

luxiaotao1123

7 天以前 3ad23ed4281aba7a0db52688d6ee318b9429f33e

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

@@ -5,7 +5,10 @@
import com.algo.model.PlannedPath;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.ObjectMapper;

import java.io.File;
import java.util.*;

/**
@@ -41,7 +44,7 @@
    /**
     * 最大搜索深度
     */
    private final int maxSearchDepth = 15000;
    private final int maxSearchDepth = 50000;

    /**
     * 距离缓存
@@ -70,13 +73,22 @@
     */
    public AStarPathPlanner(Map<String, Map<String, Integer>> pathMapping) {
        this.pathMapping = pathMapping;
        this.adjacencyList = new HashMap<>();

        // 加载环境连通性数据
        this.environmentConnectivity = JsonUtils.loadEnvironmentConnectivity("environment.json");

        // 构建环境感知的邻接表
        buildEnvironmentAwareAdjacencyList();
        // 加载邻接表
        Map<String, List<Map<String, String>>> loadedAdjacency = loadAdjacencyListFromJson("adjacency.json");
        
        if (loadedAdjacency != null && !loadedAdjacency.isEmpty()) {
            // 成功加载预生成的邻接表
            this.adjacencyList = loadedAdjacency;
            System.out.println("从 adjacency.json 加载邻接表: " + adjacencyList.size() + " 个节点");
        } else {
            this.adjacencyList = new HashMap<>();
            buildEnvironmentAwareAdjacencyList();
            System.out.println("使用网格相邻性构建邻接表: " + adjacencyList.size() + " 个节点");
        }
        
        // 加载实际坐标映射
        loadRealCoordinateMapping();
@@ -88,6 +100,39 @@
        precomputeCommonDistances();

        System.out.println("A*路径规划器初始化完成，邻接表包含 " + adjacencyList.size() + " 个节点");
    }
    
    /**
     * 从 JSON 文件加载预生成的邻接表
     */
    private Map<String, List<Map<String, String>>> loadAdjacencyListFromJson(String filePath) {
        try {
            File file = new File(filePath);
            
            if (!file.exists()) {
                System.out.println("路径邻接表文件不存在: " + filePath + "，使用默认构建方法");
                return null;
            }
            
            ObjectMapper objectMapper = new ObjectMapper();
            
            TypeReference<Map<String, List<Map<String, String>>>> typeRef = 
                new TypeReference<Map<String, List<Map<String, String>>>>() {};
            
            Map<String, List<Map<String, String>>> adjacency = objectMapper.readValue(file, typeRef);
            
            if (adjacency == null || adjacency.isEmpty()) {
                System.out.println("邻接表文件为空: " + filePath);
                return null;
            }
            
            return adjacency;
            
        } catch (Exception e) {
            System.err.println("加载邻接表失败: " + e.getMessage());
            e.printStackTrace();
            return null;
        }
    }
    
    /**
@@ -113,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
@@ -129,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);
@@ -167,7 +215,7 @@
        // 时空A*算法实现
        PlannedPath result = spaceTimeAStarSearch(
                startCode, endCode, startCoord, endCoord,
                constraints, spaceTimeOccupancyMap, physicalConfig
                constraints, spaceTimeOccupancyMap, physicalConfig, startTimeMs
        );

        if (result != null) {
@@ -189,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<>(
@@ -205,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(
@@ -318,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);
@@ -343,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;
    }

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