优化更新

jianghaiyue

3 天以前 7dd762215b373851ed313b46bad08cc973816665

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

@@ -70,21 +70,21 @@

        System.out.println("开始碰撞检测，路径数量: " + plannedPaths.size());

        // 构建高精度时空表
        Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
        // 构建时空表
        Map<Long, List<SpaceTimeNode>> spaceTimeTable = buildSpaceTimeTable(plannedPaths);

        // 检测顶点冲突（同一时间同一位置）
        List<Conflict> vertexConflicts = detectEnhancedVertexConflicts(spaceTimeTable);
        List<Conflict> vertexConflicts = detectVertexConflicts(spaceTimeTable);
        conflicts.addAll(vertexConflicts);
        System.out.println("顶点冲突数量: " + vertexConflicts.size());

        // 检测边冲突（CTU交换位置）
        List<Conflict> edgeConflicts = detectEnhancedEdgeConflicts(spaceTimeTable);
        List<Conflict> edgeConflicts = detectEdgeConflicts(spaceTimeTable);
        conflicts.addAll(edgeConflicts);
        System.out.println("边冲突数量: " + edgeConflicts.size());

        // 检测跟随冲突
        List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
        List<Conflict> followingConflicts = detectFollowingConflicts(spaceTimeTable);
        conflicts.addAll(followingConflicts);
        System.out.println("跟随冲突数量: " + followingConflicts.size());

@@ -103,8 +103,8 @@
     * @param plannedPaths 规划路径列表
     * @return 时空表
     */
    private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
        Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
    private Map<Long, List<SpaceTimeNode>> buildSpaceTimeTable(List<PlannedPath> plannedPaths) {
        Map<Long, List<SpaceTimeNode>> spaceTimeTable = new HashMap<>();

        for (PlannedPath path : plannedPaths) {
            String agvId = path.getAgvId();
@@ -133,7 +133,7 @@
                if (coordinates != null) {
                    // 创建时间段内的多个时间点（使用实际时间戳）
                    for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
                        EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
                        SpaceTimeNode node = new SpaceTimeNode(
                                agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
                        );

@@ -261,36 +261,36 @@
     * @param spaceTimeTable 时空表
     * @return 顶点冲突列表
     */
    private List<Conflict> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
    private List<Conflict> detectVertexConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<EnhancedSpaceTimeNode> nodes = entry.getValue();
            List<SpaceTimeNode> nodes = entry.getValue();

            // 按位置分组
            Map<String, List<EnhancedSpaceTimeNode>> positionGroups = new HashMap<>();
            for (EnhancedSpaceTimeNode node : nodes) {
            Map<String, List<SpaceTimeNode>> positionGroups = new HashMap<>();
            for (SpaceTimeNode node : nodes) {
                positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node);
            }

            // 检查每个位置是否有多个CTU
            for (Map.Entry<String, List<EnhancedSpaceTimeNode>> posEntry : positionGroups.entrySet()) {
            for (Map.Entry<String, List<SpaceTimeNode>> posEntry : positionGroups.entrySet()) {
                String position = posEntry.getKey();
                List<EnhancedSpaceTimeNode> ctuNodes = posEntry.getValue();
                List<SpaceTimeNode> ctuNodes = posEntry.getValue();

                if (ctuNodes.size() > 1) {
                    // 进一步检查时间重叠
                    for (int i = 0; i < ctuNodes.size(); i++) {
                        for (int j = i + 1; j < ctuNodes.size(); j++) {
                            EnhancedSpaceTimeNode node1 = ctuNodes.get(i);
                            EnhancedSpaceTimeNode node2 = ctuNodes.get(j);
                            SpaceTimeNode node1 = ctuNodes.get(i);
                            SpaceTimeNode node2 = ctuNodes.get(j);

                            // 检查时间段是否重叠
                            if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
                                    node2.arrivalTime, node2.departureTime)) {
                                Conflict conflict = new Conflict(
                                        "enhanced_vertex",
                                        "vertex",
                                        node1.agvId,
                                        node2.agvId,
                                        (int) (timePoint / 1000), // 转换为秒用于显示
@@ -316,7 +316,7 @@
     * @param spaceTimeTable 时空表
     * @return 边冲突列表
     */
    private List<Conflict> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
    private List<Conflict> detectEdgeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        if (spaceTimeTable.isEmpty()) {
@@ -332,8 +332,8 @@

            // 只检查连续的时间点
            if (nextTime - currentTime <= timeResolution * 2) {
                List<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
                List<EnhancedSpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
                List<SpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
                List<SpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);

                conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime));
            }
@@ -351,19 +351,19 @@
     * @param nextTime     下一时间
     * @return 冲突列表
     */
    private List<Conflict> detectPositionSwapping(List<EnhancedSpaceTimeNode> currentNodes,
                                                  List<EnhancedSpaceTimeNode> nextNodes,
    private List<Conflict> detectPositionSwapping(List<SpaceTimeNode> currentNodes,
                                                  List<SpaceTimeNode> nextNodes,
                                                  long currentTime, long nextTime) {
        List<Conflict> conflicts = new ArrayList<>();

        Map<String, String> currentPositions = new HashMap<>();
        Map<String, String> nextPositions = new HashMap<>();

        for (EnhancedSpaceTimeNode node : currentNodes) {
        for (SpaceTimeNode node : currentNodes) {
            currentPositions.put(node.agvId, node.position);
        }

        for (EnhancedSpaceTimeNode node : nextNodes) {
        for (SpaceTimeNode node : nextNodes) {
            nextPositions.put(node.agvId, node.position);
        }

@@ -386,7 +386,7 @@
                        pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) {

                    Conflict conflict = new Conflict(
                            "enhanced_edge",
                            "edge",
                            ctu1,
                            ctu2,
                            (int) (currentTime / 1000),
@@ -409,17 +409,18 @@
     * @param spaceTimeTable 时空表
     * @return 跟随冲突列表
     */
    private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
    private List<Conflict> detectFollowingConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
        // 第一部分：检查同一时间点的节点
        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<EnhancedSpaceTimeNode> nodes = entry.getValue();
            List<SpaceTimeNode> nodes = entry.getValue();

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    EnhancedSpaceTimeNode node1 = nodes.get(i);
                    EnhancedSpaceTimeNode node2 = nodes.get(j);
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    double distance = calculateDistance(node1.coordinates, node2.coordinates);
                    double minSafeDistance = Math.max(
@@ -429,7 +430,7 @@

                    if (distance > 0 && distance < minSafeDistance) {
                        Conflict conflict = new Conflict(
                                "enhanced_follow",
                                "follow",
                                node1.agvId,
                                node2.agvId,
                                (int) (timePoint / 1000),
@@ -441,22 +442,18 @@
                        conflicts.add(conflict);
                    }

                    // 检查同一位置或相邻位置的时间间隔是否足够
                    // 如果两个AGV在同一位置或相邻位置，要求最小时间间隔
                    if (distance <= minSafeDistance) {
                    // 检查同一时间点的时间范围重叠（同时占用）
                    if (distance <= minSafeDistance && timeRangeOverlap(
                            node1.arrivalTime, node1.departureTime,
                            node2.arrivalTime, node2.departureTime)) {
                        double normalSpeed = Math.min(
                                node1.physicalConfig.getNormalSpeed(),
                                node2.physicalConfig.getNormalSpeed()
                        );
                        // 最小时间间隔 = 安全距离/速度 + 额外安全缓冲(10秒)
                        // 总时间间隔约12~13秒，用于防止计算误差
                        long minTimeGap = (long) ((minSafeDistance / normalSpeed + 10.0) * 1000);
                        
                        // 检查时间间隔
                        long timeGap = Math.abs(node1.arrivalTime - node2.arrivalTime);
                        if (timeGap < minTimeGap && timeRangeOverlap(
                                node1.arrivalTime, node1.departureTime,
                                node2.arrivalTime, node2.departureTime)) {
                        if (timeGap < minTimeGap) {
                            Conflict conflict = new Conflict(
                                    "time_gap_insufficient",
                                    node1.agvId,
@@ -464,12 +461,104 @@
                                    (int) (timePoint / 1000),
                                    node1.position,
                                    node2.position,
                                    String.format("CTU %s 和 %s 在位置 %s/%s 时间间隔不足 (%.2f秒 < %.2f秒)",
                                    String.format("CTU %s 和 %s 在位置 %s/%s 同时占用且时间间隔不足 (%.2f秒 < %.2f秒)",
                                            node1.agvId, node2.agvId, node1.position, node2.position,
                                            timeGap / 1000.0, minTimeGap / 1000.0)
                            );
                            conflicts.add(conflict);
                        }
                    }
                }
            }
        }

        // 第二部分：检查同一位置连续经过的时间间隔
        Map<String, Map<String, SpaceTimeNode>> positionAgvMap = new HashMap<>();
        for (List<SpaceTimeNode> nodes : spaceTimeTable.values()) {
            for (SpaceTimeNode node : nodes) {
                positionAgvMap.computeIfAbsent(node.position, k -> new HashMap<>())
                        .compute(node.agvId, (k, existing) -> {
                            if (existing == null) {
                                return node;
                            } else {
                                // 合并时间范围
                                long minArrival = Math.min(existing.arrivalTime, node.arrivalTime);
                                long maxDeparture = Math.max(existing.departureTime, node.departureTime);
                                return new SpaceTimeNode(
                                        node.agvId, node.position, node.coordinates,
                                        minArrival, minArrival, maxDeparture, node.physicalConfig
                                );
                            }
                        });
            }
        }

        // 对于每个位置，检查所有经过该位置的AGV的时间间隔
        Set<String> processedPairs = new HashSet<>(); 
        for (Map.Entry<String, Map<String, SpaceTimeNode>> positionEntry : positionAgvMap.entrySet()) {
            String position = positionEntry.getKey();
            Map<String, SpaceTimeNode> agvNodes = positionEntry.getValue();

            List<SpaceTimeNode> nodes = new ArrayList<>(agvNodes.values());
            nodes.sort((n1, n2) -> Long.compare(n1.arrivalTime, n2.arrivalTime));

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    if (node1.agvId.equals(node2.agvId)) {
                        continue;
                    }

                    String pairKey1 = node1.agvId + "_" + node2.agvId + "_" + position;
                    String pairKey2 = node2.agvId + "_" + node1.agvId + "_" + position;
                    if (processedPairs.contains(pairKey1) || processedPairs.contains(pairKey2)) {
                        continue; 
                    }

                    // 计算时间间隔：node1离开后，node2到达的时间间隔
                    long timeGap;
                    if (node1.departureTime <= node2.arrivalTime) {
                        timeGap = node2.arrivalTime - node1.departureTime;
                    } else if (node2.departureTime <= node1.arrivalTime) {
                        timeGap = node1.arrivalTime - node2.departureTime;
                    } else {
                        continue;
                    }

                    // 计算最小安全时间间隔
                    double minSafeDistance = Math.max(
                            node1.physicalConfig.getMinSafetyDistance(),
                            node2.physicalConfig.getMinSafetyDistance()
                    );
                    double normalSpeed = Math.min(
                            node1.physicalConfig.getNormalSpeed(),
                            node2.physicalConfig.getNormalSpeed()
                    );
                    // 最小时间间隔 = 安全距离/速度 + 额外安全缓冲(7秒)
                    long minTimeGap = (long) ((minSafeDistance / normalSpeed + 7.0) * 1000);

                    // 检查时间间隔是否足够
                    if (timeGap < minTimeGap) {
                        String firstAgv = node1.arrivalTime < node2.arrivalTime ? node1.agvId : node2.agvId;
                        String secondAgv = node1.arrivalTime < node2.arrivalTime ? node2.agvId : node1.agvId;
                        long secondArrival = node1.arrivalTime < node2.arrivalTime ? node2.arrivalTime : node1.arrivalTime;

                        Conflict conflict = new Conflict(
                                "time_gap_insufficient",
                                firstAgv,
                                secondAgv,
                                (int) (secondArrival / 1000),
                                position,
                                position,
                                String.format("CTU %s 和 %s 在位置 %s 连续经过时间间隔不足 (%.2f秒 < %.2f秒, %s离开后%.2f秒%s到达)",
                                        firstAgv, secondAgv, position,
                                        timeGap / 1000.0, minTimeGap / 1000.0,
                                        firstAgv, timeGap / 1000.0, secondAgv)
                        );
                        conflicts.add(conflict);
                        processedPairs.add(pairKey1); 
                    }
                }
            }
@@ -484,17 +573,17 @@
     * @param spaceTimeTable 时空表
     * @return 物理冲突列表
     */
    private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
    private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<EnhancedSpaceTimeNode> nodes = entry.getValue();
            List<SpaceTimeNode> nodes = entry.getValue();

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    EnhancedSpaceTimeNode node1 = nodes.get(i);
                    EnhancedSpaceTimeNode node2 = nodes.get(j);
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    if (checkPhysicalCollision(node1, node2)) {
                        Conflict conflict = new Conflict(
@@ -523,7 +612,7 @@
     * @param node2 CTU节点2
     * @return 是否碰撞
     */
    private boolean checkPhysicalCollision(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode node2) {
    private boolean checkPhysicalCollision(SpaceTimeNode node1, SpaceTimeNode node2) {
        double distance = calculateDistance(node1.coordinates, node2.coordinates);

        // 考虑CTU的物理尺寸
@@ -566,7 +655,7 @@
    /**
     * 时空节点内部类
     */
    private static class EnhancedSpaceTimeNode {
    private static class SpaceTimeNode {
        final String agvId;
        final String position;
        final int[] coordinates;
@@ -575,9 +664,9 @@
        final long departureTime;
        final CTUPhysicalConfig physicalConfig;

        public EnhancedSpaceTimeNode(String agvId, String position, int[] coordinates,
                                     long timePoint, long arrivalTime, long departureTime,
                                     CTUPhysicalConfig physicalConfig) {
        public SpaceTimeNode(String agvId, String position, int[] coordinates,
                             long timePoint, long arrivalTime, long departureTime,
                             CTUPhysicalConfig physicalConfig) {
            this.agvId = agvId;
            this.position = position;
            this.coordinates = coordinates;