优化更新

jianghaiyue

5 天以前 4618bbf913912f85ee197ccd10644df29ad2f287

优化更新

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

@@ -621,11 +621,7 @@
    }

    /**
     * 优化路径：去除空间上的重复点（避免来回走）
     * 检测并移除 A->B->A 模式的来回走，避免不必要的绕行
     *
     * @param codeList 原始路径代码列表
     * @return 优化后的路径代码列表
     * 优化路径
     */
    private List<PathCode> optimizePathByRemovingBacktrack(List<PathCode> codeList) {
        if (codeList == null || codeList.size() <= 2) {
@@ -638,27 +634,20 @@
        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; // 跳到下一个不同的点
                        i = j;
                        continue;
                    } else {
                        // 如果后面都是相同点，保留当前点（可能是目标点）
                        break;
                    }
                }
@@ -678,7 +667,7 @@
            }
        }
        
        // 重新计算方向（因为优化后跳过了中间点，方向需要更新）
        // 重新计算方向
        for (int k = 0; k < optimized.size(); k++) {
            PathCode code = optimized.get(k);
            if (k < optimized.size() - 1) {

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

@@ -9,8 +9,7 @@
import java.util.*;

/**
 * 碰撞检测器（增强版）
 * 用于检测CTU路径之间的时空冲突，支持物理参数和高精度时间检测
 * 碰撞检测器
 */
public class CollisionDetector {

@@ -26,13 +25,14 @@

    /**
     * 时间精度（毫秒）
     * 提高时间精度以更准确地检测碰撞
     */
    private final long timeResolution = 100; // 100毫秒的时间精度
    private final long timeResolution = 50; 

    /**
     * 空间分辨率（米）
     */
    private final double spaceResolution = 0.1; // 10厘米的空间精度
    private final double spaceResolution = 0.1; 

    /**
     * 构造函数
@@ -47,7 +47,7 @@
    }

    /**
     * 构造函数（使用默认参数）
     * 构造函数
     *
     * @param pathMapping 路径映射表
     */
@@ -56,7 +56,7 @@
    }

    /**
     * 检测路径冲突（增强版，支持CTU物理参数）
     * 检测路径冲突
     *
     * @param plannedPaths 规划路径列表
     * @return 冲突列表
@@ -68,7 +68,7 @@
            return conflicts;
        }

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

        // 构建高精度时空表
        Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
@@ -83,7 +83,7 @@
        conflicts.addAll(edgeConflicts);
        System.out.println("边冲突数量: " + edgeConflicts.size());

        // 检测跟随冲突（CTU距离过近，考虑物理参数）
        // 检测跟随冲突
        List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
        conflicts.addAll(followingConflicts);
        System.out.println("跟随冲突数量: " + followingConflicts.size());
@@ -93,15 +93,15 @@
        conflicts.addAll(physicalConflicts);
        System.out.println("物理尺寸冲突数量: " + physicalConflicts.size());

        System.out.println("增强版碰撞检测完成，总冲突数量: " + conflicts.size());
        System.out.println("碰撞检测完成，总冲突数量: " + conflicts.size());
        return conflicts;
    }

    /**
     * 构建增强的时空表（高精度时间和物理参数）
     * 构建时空表
     *
     * @param plannedPaths 规划路径列表
     * @return 增强时空表
     * @return 时空表
     */
    private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
        Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
@@ -114,24 +114,24 @@
                continue;
            }

            // 获取CTU的物理配置（这里需要从某个地方获取，可能需要扩展PlannedPath类）
            // 获取CTU的物理配置
            CTUPhysicalConfig physicalConfig = getPhysicalConfigForCTU(agvId);

            long currentTime = System.currentTimeMillis(); // 基准时间

            for (int i = 0; i < codeList.size(); i++) {
                PathCode pathCode = codeList.get(i);
            for (PathCode pathCode : codeList) {
                String position = pathCode.getCode();
                Long arrivalTime = pathCode.getArrivalTime();
                Long departureTime = pathCode.getDepartureTime();

                // 如果时间戳为空，跳过
                if (arrivalTime == null || departureTime == null) {
                    System.out.println("警告: AGV " + agvId + " 在位置 " + position + " 的时间戳为空，跳过");
                    continue;
                }

                int[] coordinates = JsonUtils.getCoordinate(position, pathMapping);
                if (coordinates != null) {
                    // 计算精确的到达时间
                    long arrivalTime = calculateArrivalTime(currentTime, i, codeList, physicalConfig);

                    // 计算停留时间
                    long departureTime = calculateDepartureTime(arrivalTime, pathCode, physicalConfig);

                    // 创建时间段内的多个时间点
                    // 创建时间段内的多个时间点（使用实际时间戳）
                    for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
                        EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
                                agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
@@ -153,23 +153,13 @@
     * @return 物理配置
     */
    private CTUPhysicalConfig getPhysicalConfigForCTU(String agvId) {
        // 这里应该从配置管理器或数据库获取实际的CTU物理参数
        // 暂时返回默认配置
        // 从AGV状态中获取
        CTUPhysicalConfig config = new CTUPhysicalConfig();
        config.setMaxSpeed(2.0);
        config.setNormalSpeed(1.5);
        config.setMaxAcceleration(1.0);
        config.setMaxDeceleration(1.5);
        config.setTurnTime90(2.0);
        config.setTurnTime180(4.0);
        config.setMinSafetyDistance(defaultMinDistance);
        config.setMinFollowingDistance(2.0);
        config.setCtuLength(1.2);
        config.setCtuWidth(0.8);
        config.setStartupTime(1.0);
        config.setStopTime(0.5);
        config.setStandardPointDistance(1.0);
        return config;
        // 标准配置
        // maxSpeed=1.0, normalSpeed=1.0, maxAcceleration=0.4, maxDeceleration=0.4
        // turnTime90=4.0, turnTime180=8.0, minSafetyDistance=3.0, minFollowingDistance=3.0
        // ctuLength=1.5, ctuWidth=1.0, startupTime=1.0, stopTime=1.5, standardPointDistance=1.0
        return config; // 使用默认构造函数的标准配置
    }

    /**
@@ -251,8 +241,7 @@
            return 0.0;
        }

        // 简化的方向转换计算
        // 实际实现中需要根据具体的方向编码计算角度差
        // 方向转换计算
        int angleDiff = Math.abs(Integer.parseInt(toDirection) - Integer.parseInt(fromDirection));
        angleDiff = Math.min(angleDiff, 4 - angleDiff); // 考虑环形

@@ -267,7 +256,7 @@
    }

    /**
     * 检测增强的顶点冲突
     * 检测顶点冲突
     *
     * @param spaceTimeTable 时空表
     * @return 顶点冲突列表
@@ -322,7 +311,7 @@
    }

    /**
     * 检测增强的边冲突
     * 检测边冲突
     *
     * @param spaceTimeTable 时空表
     * @return 边冲突列表
@@ -415,7 +404,7 @@
    }

    /**
     * 检测增强的跟随冲突（考虑CTU物理参数）
     * 检测跟随冲突
     *
     * @param spaceTimeTable 时空表
     * @return 跟随冲突列表
@@ -450,6 +439,37 @@
                                        node1.agvId, node2.agvId, timePoint / 1000, distance, minSafeDistance)
                        );
                        conflicts.add(conflict);
                    }

                    // 检查同一位置或相邻位置的时间间隔是否足够
                    // 如果两个AGV在同一位置或相邻位置，要求最小时间间隔
                    if (distance <= minSafeDistance) {
                        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)) {
                            Conflict conflict = new Conflict(
                                    "time_gap_insufficient",
                                    node1.agvId,
                                    node2.agvId,
                                    (int) (timePoint / 1000),
                                    node1.position,
                                    node2.position,
                                    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);
                        }
                    }
                }
            }
@@ -544,7 +564,7 @@
    }

    /**
     * 增强的时空节点内部类
     * 时空节点内部类
     */
    private static class EnhancedSpaceTimeNode {
        final String agvId;