更新版本

jianghaiyue

16 小时以前 d808837cd368c3772962be591aa6532bcc0cf3e4

更新版本

 algo-zkd/man_code.json

New file
Diff too large

 algo-zkd/src/main/java/com/algo/config/BaseDataConfig.java

@@ -1,4 +1,4 @@
//package com.algo.config;
package com.algo.config;//package com.algo.config;
//
//import com.algo.expose.BaseDataService;
//import com.algo.expose.impl.BaseDataServiceImpl;

 algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java

@@ -71,16 +71,16 @@
     */
    private double standardPointDistance;

    // 默认构造函数 - 使用推荐的默认值
    // 默认构造函数 - 使用基于实际场地的物理参数
    public CTUPhysicalConfig() {
        this.maxSpeed = 2.0;              // 2米/秒
        this.normalSpeed = 1.5;           // 1.5米/秒
        this.maxAcceleration = 1.0;       // 1米/秒²
        this.maxDeceleration = 1.5;       // 1.5米/秒²
        this.turnTime90 = 2.0;            // 90度转向需要2秒
        this.turnTime180 = 4.0;           // 180度转向需要4秒
        this.maxSpeed = 1.0;              // 1000mm/s = 1.0米/秒
        this.normalSpeed = 1.0;           // 正常运行速度100%最大速度
        this.maxAcceleration = 0.4;       // 400 mm/s^2 = 0.4米/秒²
        this.maxDeceleration = 0.4;       // 相同的加减速度
        this.turnTime90 = 4.0;            // 每90度转向需要4秒
        this.turnTime180 = 8.0;           // 180度转向需要8秒
        this.minSafetyDistance = 3.0;     // 最小安全距离3米
        this.minFollowingDistance = 2.0;  // 最小跟随距离2米
        this.minFollowingDistance = 3.0;  // 最小跟随距离3米
        this.ctuLength = 1.5;             // CTU长度1.5米
        this.ctuWidth = 1.0;              // CTU宽度1米
        this.startupTime = 1.0;           // 启动时间1秒
@@ -301,6 +301,151 @@
        return distance >= minDistance;
    }

    /**
     * 计算精确的移动时间，考虑加速度、减速度和实际距离
     *
     * @param distance       实际移动距离（米）
     * @param startSpeed     起始速度（米/秒）
     * @param endSpeed       结束速度（米/秒）
     * @param targetSpeed    目标巡航速度（米/秒）
     * @return 移动时间（秒）
     */
    public double calculatePreciseMovementTime(double distance, double startSpeed, double endSpeed, double targetSpeed) {
        if (distance <= 0) return 0.0;
        
        // 如果距离很短，直接计算平均速度
        if (distance < 0.5) { // 小于0.5米
            double avgSpeed = Math.max(0.1, (startSpeed + endSpeed) / 2.0);
            return distance / avgSpeed;
        }
        
        double totalTime = 0.0;
        double currentSpeed = startSpeed;
        double remainingDistance = distance;
        
        // 阶段1: 加速到目标速度
        if (currentSpeed < targetSpeed) {
            double accelerationTime = (targetSpeed - currentSpeed) / maxAcceleration;
            double accelerationDistance = currentSpeed * accelerationTime + 0.5 * maxAcceleration * accelerationTime * accelerationTime;
            
            if (accelerationDistance < remainingDistance) {
                totalTime += accelerationTime;
                remainingDistance -= accelerationDistance;
                currentSpeed = targetSpeed;
            } else {
                // 距离不足以完全加速，直接计算
                return calculateTimeForShortDistance(distance, startSpeed, endSpeed);
            }
        }
        
        // 阶段2: 减速到目标结束速度
        double decelerationDistance = 0.0;
        double decelerationTime = 0.0;
        if (currentSpeed > endSpeed) {
            decelerationTime = (currentSpeed - endSpeed) / maxDeceleration;
            decelerationDistance = endSpeed * decelerationTime + 0.5 * maxDeceleration * decelerationTime * decelerationTime;
        }
        
        // 阶段3: 匀速巡航
        if (remainingDistance > decelerationDistance) {
            double cruisingDistance = remainingDistance - decelerationDistance;
            double cruisingTime = cruisingDistance / currentSpeed;
            totalTime += cruisingTime;
        }
        
        // 添加减速时间
        totalTime += decelerationTime;
        
        return totalTime;
    }
    
    /**
     * 计算短距离移动时间（无法完整加速减速的情况）
     */
    private double calculateTimeForShortDistance(double distance, double startSpeed, double endSpeed) {
        // 使用运动学公式: s = v0*t + 0.5*a*t^2
        // 其中 v_f = v0 + a*t，求解 t
        double avgAcceleration = (endSpeed > startSpeed) ? maxAcceleration : -maxDeceleration;
        
        // 如果加速度很小或为0，使用平均速度
        if (Math.abs(avgAcceleration) < 0.01) {
            return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
        }
        
        // 求解二次方程: 0.5*a*t^2 + v0*t - s = 0
        double a = 0.5 * avgAcceleration;
        double b = startSpeed;
        double c = -distance;
        
        double discriminant = b * b - 4 * a * c;
        if (discriminant < 0) {
            // 无解，使用平均速度
            return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
        }
        
        double t1 = (-b + Math.sqrt(discriminant)) / (2 * a);
        double t2 = (-b - Math.sqrt(discriminant)) / (2 * a);
        
        // 选择正的时间值
        double time = (t1 > 0) ? t1 : t2;
        return Math.max(0.1, time); // 确保时间为正
    }
    
    /**
     * 基于实际坐标计算两点间的精确距离（米）
     *
     * @param coord1 起点坐标 [x_mm, y_mm]
     * @param coord2 终点坐标 [x_mm, y_mm]
     * @return 距离（米）
     */
    public static double calculateRealDistance(double[] coord1, double[] coord2) {
        if (coord1 == null || coord2 == null || coord1.length < 2 || coord2.length < 2) {
            return 0.0;
        }
        
        // 坐标单位为毫米，转换为米
        double dx = (coord2[0] - coord1[0]) / 1000.0;
        double dy = (coord2[1] - coord1[1]) / 1000.0;
        
        return Math.sqrt(dx * dx + dy * dy);
    }
    
    /**
     * 计算方向改变角度
     *
     * @param coord1 起点坐标
     * @param coord2 中间点坐标  
     * @param coord3 终点坐标
     * @return 转向角度（度）
     */
    public static double calculateTurnAngle(double[] coord1, double[] coord2, double[] coord3) {
        if (coord1 == null || coord2 == null || coord3 == null) {
            return 0.0;
        }
        
        // 计算两个向量
        double[] vec1 = {coord2[0] - coord1[0], coord2[1] - coord1[1]};
        double[] vec2 = {coord3[0] - coord2[0], coord3[1] - coord2[1]};
        
        // 计算向量长度
        double len1 = Math.sqrt(vec1[0] * vec1[0] + vec1[1] * vec1[1]);
        double len2 = Math.sqrt(vec2[0] * vec2[0] + vec2[1] * vec2[1]);
        
        if (len1 < 1e-6 || len2 < 1e-6) {
            return 0.0;
        }
        
        // 计算夹角
        double dot = vec1[0] * vec2[0] + vec1[1] * vec2[1];
        double cosAngle = dot / (len1 * len2);
        
        // 限制cosAngle在[-1, 1]范围内
        cosAngle = Math.max(-1.0, Math.min(1.0, cosAngle));
        
        double angle = Math.acos(cosAngle);
        return Math.toDegrees(angle);
    }

    @Override
    public String toString() {
        return "CTUPhysicalConfig{" +

 algo-zkd/src/main/java/com/algo/model/PathCode.java

@@ -1,6 +1,5 @@
package com.algo.model;

import com.fasterxml.jackson.annotation.JsonProperty;

/**
 * 路径代码模型
@@ -41,8 +40,22 @@
    /**
     * 是否为目标点
     */
    @JsonProperty("isTargetPoint")
    private boolean isTargetPoint;

    /**
     * 到达时间（毫秒时间戳）
     */
    private Long arrivalTime;

    /**
     * 离开时间（毫秒时间戳）
     */
    private Long departureTime;

    /**
     * 累计时间（从路径起点开始的累计时间，毫秒）
     */
    private Long cumulativeTime;

    // 构造函数
    public PathCode() {
@@ -121,6 +134,30 @@
        isTargetPoint = targetPoint;
    }

    public Long getArrivalTime() {
        return arrivalTime;
    }

    public void setArrivalTime(Long arrivalTime) {
        this.arrivalTime = arrivalTime;
    }

    public Long getDepartureTime() {
        return departureTime;
    }

    public void setDepartureTime(Long departureTime) {
        this.departureTime = departureTime;
    }

    public Long getCumulativeTime() {
        return cumulativeTime;
    }

    public void setCumulativeTime(Long cumulativeTime) {
        this.cumulativeTime = cumulativeTime;
    }

    @Override
    public String toString() {
        return "PathCode{" +
@@ -131,6 +168,9 @@
                ", posType='" + posType + '\'' +
                ", lev=" + lev +
                ", isTargetPoint=" + isTargetPoint +
                ", arrivalTime=" + arrivalTime +
                ", departureTime=" + departureTime +
                ", cumulativeTime=" + cumulativeTime +
                '}';
    }
} 

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

@@ -4,12 +4,12 @@
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;

import java.util.*;

/**
 * A*路径规划器实现
 * 使用3D A*算法进行CTU时空路径规划，支持物理约束
 */
public class AStarPathPlanner implements PathPlanner {

@@ -22,6 +22,11 @@
     * 邻接表
     */
    private final Map<String, List<Map<String, String>>> adjacencyList;

    /**
     * 环境连通性数据
     */
    private final JsonUtils.EnvironmentConnectivity environmentConnectivity;

    /**
     * 时间精度（毫秒）
@@ -39,7 +44,7 @@
    private final int maxSearchDepth = 15000;

    /**
     * 距离缓存 - 优化：缓存距离计算结果
     * 距离缓存
     */
    private final Map<String, Double> distanceCache = new HashMap<>();

@@ -47,6 +52,16 @@
     * 快速路径缓存
     */
    private final Map<String, List<String>> fastPathCache = new HashMap<>();
    
    /**
     * 实际坐标映射 
     */
    private Map<String, double[]> realCoordinateMapping;
    
    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;

    /**
     * 构造函数
@@ -57,13 +72,38 @@
        this.pathMapping = pathMapping;
        this.adjacencyList = new HashMap<>();

        // 直接构建邻接表
        buildAdjacencyList();
        // 加载环境连通性数据
        this.environmentConnectivity = JsonUtils.loadEnvironmentConnectivity("environment.json");

        // 构建环境感知的邻接表
        buildEnvironmentAwareAdjacencyList();
        
        // 加载实际坐标映射
        loadRealCoordinateMapping();
        
        // 初始化时间计算器
        this.timeCalculator = new PathTimeCalculator(pathMapping, realCoordinateMapping);

        // 预计算常用距离
        precomputeCommonDistances();

        System.out.println("A*路径规划器初始化完成，邻接表包含 " + adjacencyList.size() + " 个节点");
    }
    
    /**
     * 加载实际坐标映射
     */
    private void loadRealCoordinateMapping() {
        try {
            this.realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
            if (realCoordinateMapping == null || realCoordinateMapping.isEmpty()) {
                System.out.println("未能加载实际坐标映射，使用网格坐标");
                this.realCoordinateMapping = new HashMap<>();
            }
        } catch (Exception e) {
            System.err.println("加载实际坐标映射失败: " + e.getMessage());
            this.realCoordinateMapping = new HashMap<>();
        }
    }

    @Override
@@ -278,7 +318,7 @@
            }
        }

        // 减少等待选项 - 只在必要时等待（20%概率）
        // 减少等待选项
        if (Math.random() < 0.2) {
            long waitTime = timeResolution;
            long waitUntilTime = current.timePoint + waitTime;
@@ -308,27 +348,61 @@
    /**
     * 时空启发式函数
     *
     * @param coord1         当前坐标
     * @param coord2         目标坐标
     * @param coord1         当前坐标（网格）
     * @param coord2         目标坐标（网格）
     * @param currentTime    当前时间
     * @param physicalConfig 物理配置
     * @return 启发式值
     */
    private double spaceTimeHeuristic(int[] coord1, int[] coord2, long currentTime, CTUPhysicalConfig physicalConfig) {
        // 空间距离
        double spatialDistance = Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
        String pathId1 = findPathIdByCoordinate(coord1);
        String pathId2 = findPathIdByCoordinate(coord2);

        // 时间成本估计
        double timeEstimate = spatialDistance * physicalConfig.getStandardPointDistance() / physicalConfig.getNormalSpeed();
        if (pathId1 != null && pathId2 != null) {
            double[] realCoord1 = JsonUtils.getRealCoordinate(pathId1, realCoordinateMapping);
            double[] realCoord2 = JsonUtils.getRealCoordinate(pathId2, realCoordinateMapping);
            
            if (realCoord1 != null && realCoord2 != null) {
                // 使用实际距离计算
                double realDistance = CTUPhysicalConfig.calculateRealDistance(realCoord1, realCoord2);
                
                // 时间成本估计（假设平均速度）
                double timeEstimate = realDistance / physicalConfig.getNormalSpeed();

        // 考虑转向成本
                double turnPenalty = realDistance > 0.5 ? physicalConfig.getTurnTime90() / 2.0 : 0;
                
                return timeEstimate + turnPenalty;
            }
        }
        
        // 前方案：使用网格坐标
        double spatialDistance = Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
        double timeEstimate = spatialDistance * physicalConfig.getStandardPointDistance() / physicalConfig.getNormalSpeed();
        double turnPenalty = spatialDistance > 1 ? physicalConfig.getTurnTime90() : 0;

        return timeEstimate + turnPenalty;
    }

    /**
     * 计算移动时间
     * 根据网格坐标查找路径ID
     */
    private String findPathIdByCoordinate(int[] coord) {
        if (coord == null) return null;
        
        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
            Map<String, Integer> coordMap = entry.getValue();
            if (coordMap != null && 
                coord[0] == coordMap.getOrDefault("x", -1) && 
                coord[1] == coordMap.getOrDefault("y", -1)) {
                return entry.getKey();
            }
        }
        return null;
    }

    /**
     * 计算精确的移动时间，基于实际坐标和物理参数
     *
     * @param fromCode       起始代码
     * @param toCode         目标代码
@@ -337,14 +411,70 @@
     * @return 移动时间（毫秒）
     */
    private long calculateTravelTime(String fromCode, String toCode, String direction, CTUPhysicalConfig physicalConfig) {
        // 基本移动时间
        // 获取实际坐标
        double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
        double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
        
        if (fromCoord == null || toCoord == null) {
            // fallback到网格坐标计算
            return calculateFallbackTravelTime(fromCode, toCode, physicalConfig);
        }
        
        // 计算实际距离（米）
        double realDistance = CTUPhysicalConfig.calculateRealDistance(fromCoord, toCoord);
        
        if (realDistance == 0.0) {
            return 100; // 最小时间100ms
        }
        
        // 假设起始和结束速度（可以根据上下文进一步优化）
        double startSpeed = 0.0; // 假设从静止开始
        double endSpeed = 0.0;   // 假设结束时停止
        double targetSpeed = physicalConfig.getNormalSpeed();
        
        // 计算精确的移动时间
        double movementTime = physicalConfig.calculatePreciseMovementTime(
            realDistance, startSpeed, endSpeed, targetSpeed
        );
        
        // 计算转向时间
        double turnTime = calculateTurnTime(fromCode, toCode, physicalConfig);
        
        return Math.max(100, (long) ((movementTime + turnTime) * 1000));
    }
    
    /**
     * 计算转向时间
     */
    private double calculateTurnTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
        
        double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
        double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
        
        if (fromCoord == null || toCoord == null) {
            return physicalConfig.getTurnTime90() / 4.0; // 平均转向时间
        }
        
        // 计算方向变化
        double dx = toCoord[0] - fromCoord[0];
        double dy = toCoord[1] - fromCoord[1];
        
        // 直线运动无需转向
        if (Math.abs(dx) < 1.0 && Math.abs(dy) < 1.0) {
            return 0.0;
        }
        
        return physicalConfig.getTurnTime90() / 2.0;
    }
    
    /**
     * 备用时间计算方法（当无法获取实际坐标时使用）
     */
    private long calculateFallbackTravelTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
        double distance = physicalConfig.getStandardPointDistance();
        double speed = physicalConfig.getNormalSpeed();
        long moveTime = (long) ((distance / speed) * 1000);

        // 转向时间
        long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4); // 假设平均转向时间

        long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4);
        return moveTime + turnTime;
    }

@@ -384,14 +514,17 @@
            }

            PathCode pathCode = new PathCode(node.code, direction);

            // 添加时间信息
            // pathCode.setArrivalTime(node.timePoint);

            codeList.add(pathCode);
        }

        return new PlannedPath("", "", codeList);
        PlannedPath plannedPath = new PlannedPath("", "", codeList);
        
        // 使用统一的时间计算器计算精确时间
        long startTime = pathNodes.get(0).timePoint;
        CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
        timeCalculator.calculatePathTiming(plannedPath, startTime, defaultConfig, 0.0);
        
        return plannedPath;
    }

    /**
@@ -472,6 +605,15 @@

    @Override
    public double calculateDistance(String startCode, String endCode) {
        // 优先使用实际坐标
        double[] startRealCoord = JsonUtils.getRealCoordinate(startCode, realCoordinateMapping);
        double[] endRealCoord = JsonUtils.getRealCoordinate(endCode, realCoordinateMapping);
        
        if (startRealCoord != null && endRealCoord != null) {
            return CTUPhysicalConfig.calculateRealDistance(startRealCoord, endRealCoord);
        }
        
        // 前方案：使用网格坐标
        int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
        int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);

@@ -493,9 +635,9 @@
    }

    /**
     * 构建邻接表
     * 构建环境感知的邻接表
     */
    private void buildAdjacencyList() {
    private void buildEnvironmentAwareAdjacencyList() {
        // 创建坐标到编号的临时映射
        Map<String, String> tempCoordToCode = new HashMap<>();
        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
@@ -523,6 +665,12 @@
            int x = coordMap.get("x");
            int y = coordMap.get("y");

            // 环境感知：检查当前节点是否可通行
            if (!environmentConnectivity.isTraversable(x, y)) {
                adjacencyList.put(code, new ArrayList<>()); // 不可通行的节点没有邻居
                continue;
            }

            List<Map<String, String>> neighbors = new ArrayList<>();

            // 检查四个方向的邻居
@@ -532,7 +680,9 @@
                String coordKey = newX + "," + newY;

                String neighborCode = tempCoordToCode.get(coordKey);
                if (neighborCode != null) {
                
                // 环境感知：只有当邻居节点也可通行时才添加连接
                if (neighborCode != null && environmentConnectivity.isTraversable(newX, newY)) {
                    Map<String, String> neighbor = new HashMap<>();
                    neighbor.put("code", neighborCode);
                    neighbor.put("direction", directionAngles[i]);
@@ -542,6 +692,8 @@

            adjacencyList.put(code, neighbors);
        }
        
        System.out.println("环境感知邻接表构建完成，过滤了不可通行的连接");
    }

    /**
@@ -673,7 +825,7 @@
    }

    /**
     * 快速路径规划 - 先尝试简化空间路径规划
     * 快速路径规划 
     * 对于近距离路径，直接返回结果避免复杂的时空计算
     */
    private PlannedPath tryFastPathPlanning(String startCode, String endCode, List<double[]> constraints) {
@@ -722,7 +874,7 @@
        openSet.offer(startNode);
        gScores.put(startCode, 0.0);

        // 简化的约束检查器
        // 约束检查器
        FastConstraintChecker constraintChecker = new FastConstraintChecker(constraints);

        int searchDepth = 0;

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

@@ -4,6 +4,8 @@
import com.algo.model.ExecutingTask;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.model.CTUPhysicalConfig;
import com.algo.util.PathTimeCalculator;

import java.util.*;

@@ -19,12 +21,24 @@
    private final CollisionDetector collisionDetector;

    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;

    /**
     * 构造函数
     *
     * @param collisionDetector 碰撞检测器
     */
    public CollisionResolver(CollisionDetector collisionDetector) {
        this.collisionDetector = collisionDetector;
    }
    
    /**
     * 设置时间计算器
     */
    public void setTimeCalculator(PathTimeCalculator timeCalculator) {
        this.timeCalculator = timeCalculator;
    }

    /**
@@ -243,7 +257,7 @@
    }

    /**
     * 为路径添加延迟
     * 为路径添加延迟（增强版，正确重计算时间窗）
     *
     * @param path       路径
     * @param timeStep   延迟开始的时间步
@@ -264,6 +278,9 @@
        // 获取延迟位置的路径代码
        PathCode delayCode = codeList.get(timeStep);

        // 计算延迟时间（每个延迟步骤1秒）
        long delayDuration = delaySteps * 1000L;

        // 创建延迟步骤
        List<PathCode> delaySteps_list = new ArrayList<>();
        for (int i = 0; i < delaySteps; i++) {
@@ -273,14 +290,69 @@
            waitCode.setPosType(delayCode.getPosType());
            waitCode.setLev(delayCode.getLev());
            waitCode.setTargetPoint(false); // 等待步骤不是目标点
            
            // 设置等待步骤的时间信息
            if (delayCode.getArrivalTime() != null) {
                long waitStartTime = delayCode.getArrivalTime() + (i * 1000L);
                waitCode.setArrivalTime(waitStartTime);
                waitCode.setDepartureTime(waitStartTime + 1000L);
            }
            
            delaySteps_list.add(waitCode);
        }

        // 插入延迟步骤
        codeList.addAll(timeStep, delaySteps_list);

        // 更新原路径点的时间（向后推迟）
        if (delayCode.getArrivalTime() != null) {
            delayCode.setArrivalTime(delayCode.getArrivalTime() + delayDuration);
            if (delayCode.getDepartureTime() != null) {
                delayCode.setDepartureTime(delayCode.getDepartureTime() + delayDuration);
            }
        }

        // 更新路径
        path.setCodeList(codeList);
        
        // 🔧 关键修复：重新计算后续路径点的时间窗
        if (timeCalculator != null) {
            CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
            timeCalculator.recalculatePathTimingFromIndex(path, timeStep + delaySteps, defaultConfig);
        } else {
            // 备用方案：手动更新后续时间
            updateSubsequentPathTiming(path, timeStep + delaySteps, delayDuration);
        }
    }
    
    /**
     * 备用方案：手动更新后续路径点的时间
     */
    private void updateSubsequentPathTiming(PlannedPath path, int fromIndex, long timeOffset) {
        List<PathCode> codeList = path.getCodeList();
        
        for (int i = fromIndex; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);
            
            if (pathCode.getArrivalTime() != null) {
                pathCode.setArrivalTime(pathCode.getArrivalTime() + timeOffset);
            }
            
            if (pathCode.getDepartureTime() != null) {
                pathCode.setDepartureTime(pathCode.getDepartureTime() + timeOffset);
            }
            
            if (pathCode.getCumulativeTime() != null) {
                pathCode.setCumulativeTime(pathCode.getCumulativeTime() + timeOffset);
            }
        }
    }
    
    /**
     * 创建默认物理配置
     */
    private CTUPhysicalConfig createDefaultPhysicalConfig() {
        return new CTUPhysicalConfig(); // 使用默认构造函数
    }

    /**

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

@@ -3,6 +3,7 @@
import com.algo.config.EnvDataConfig;
import com.algo.model.*;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

@@ -44,6 +45,11 @@
     */
    private RemainingPathProcessor remainingPathProcessor;

    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;


    /**
     * 线程池
@@ -73,6 +79,13 @@

        // 初始化剩余路径处理器
        this.remainingPathProcessor = new RemainingPathProcessor(envDataConfig.getPathMapping());
        
        // 初始化时间计算器
        Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
        this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
        
        // 为碰撞解决器设置时间计算器
        this.collisionResolver.setTimeCalculator(timeCalculator);
    }

    /**
@@ -320,7 +333,7 @@
                                                         List<double[]> constraints,
                                                         Map<String, String> occupancyMap,
                                                         AGVStatus agvStatus) {
        // 首先尝试基本路径规划
        // 尝试基本路径规划
        PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
        if (basicPath == null) {
            return null;
@@ -331,8 +344,8 @@
                basicPath, occupancyMap, agvStatus.getPhysicalConfig()
        );

        // 设置路径的时间信息
        enhancePathWithTimeInfo(basicPath, safeStartTime, agvStatus.getPhysicalConfig());
        // 使用统一的时间计算器设置精确的时间信息
        timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);

        return basicPath;
    }

 algo-zkd/src/main/java/com/algo/util/JsonUtils.java

@@ -5,18 +5,17 @@
import java.io.*;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;

/**
 * JSON文件读取工具类
 * 用于读取环境配置和路径映射文件
 * JSON文件读取
 */
public class JsonUtils {

    /**
     * 读取JSON文件内容
     *
     * @param filePath 文件路径
     * @return JSON字符串内容
     * @throws IOException 文件读取异常
@@ -33,8 +32,7 @@
    }

    /**
     * 解析路径映射JSON内容
     * 正确解析path_mapping.json的实际格式：{"path_id_to_coordinates": {...}}
     * 解析路径映射JSON
     *
     * @param jsonContent JSON内容
     * @return 路径映射Map，key为路径编号，value为坐标信息
@@ -43,7 +41,6 @@
        Map<String, Map<String, Integer>> pathMapping = new HashMap<>();

        try {
            // 找到path_id_to_coordinates部分
            String pathIdSection = extractJsonSection(jsonContent, "path_id_to_coordinates");
            if (pathIdSection == null) {
                System.err.println("未找到path_id_to_coordinates部分");
@@ -101,8 +98,7 @@
    }

    /**
     * 加载和解析路径映射文件
     * 正确解析path_mapping.json的实际格式：{"path_id_to_coordinates": {...}}
     * 路径映射
     *
     * @param filePath 文件地址
     * @return 路径映射Map，key为路径编号，value为坐标信息
@@ -112,7 +108,6 @@
        Map<String, Map<String, Integer>> pathMapping = new HashMap<>();
        ObjectMapper objectMapper = new ObjectMapper();
        try {
            // 先解析为顶层Map<String, Object>
            Map<String, Object> topLevelMap = objectMapper.readValue(
                    new File(filePath),
                    Map.class
@@ -123,10 +118,9 @@
                return pathMapping;
            }

            // 处理 path_id_to_coordinates（将坐标列表转换为第一个坐标的path_id映射）
            // 处理 path_id_to_coordinates
            Map<String, Object> pathIdCoords = (Map<String, Object>) topLevelMap.get("path_id_to_coordinates");
            for (Map.Entry<String, Object> entry : pathIdCoords.entrySet()) {
                // 保存方式: 路径ID:{"x": , "y":}
                String pathId = entry.getKey();
                Object coordsObj = entry.getValue();
                if (coordsObj instanceof List) {
@@ -155,8 +149,7 @@
    }

    /**
     * 解析环境配置JSON内容
     * 解析environment.json中的stations信息
     * 环境配置JSON
     *
     * @param jsonContent JSON内容
     * @return 环境配置Map
@@ -165,7 +158,6 @@
        Map<String, Object> config = new HashMap<>();

        try {
            // 解析width
            if (jsonContent.contains("\"width\":")) {
                String widthStr = jsonContent.substring(jsonContent.indexOf("\"width\":") + 8);
                widthStr = widthStr.substring(0, widthStr.indexOf(",")).trim();
@@ -176,7 +168,6 @@
                }
            }

            // 解析height
            if (jsonContent.contains("\"height\":")) {
                String heightStr = jsonContent.substring(jsonContent.indexOf("\"height\":") + 9);
                heightStr = heightStr.substring(0, heightStr.indexOf(",")).trim();
@@ -187,7 +178,6 @@
                }
            }

            // 解析stations信息
            Map<String, Map<String, Object>> stations = parseStations(jsonContent);
            config.put("stations", stations);
            config.put("stationCount", stations.size());
@@ -202,7 +192,6 @@
    }

    /**
     * 加载和解析环境配置文件
     * 解析environment.json中的stations信息
     *
     * @param filePath 文件地址
@@ -219,21 +208,18 @@
                    Map.class
            );

            // 解析width
            if (topLevelMap.containsKey("width")) {
                environmentMap.put("width", Integer.parseInt(topLevelMap.get("width").toString()));
            } else {
                environmentMap.put("width", 78);
            }

            // 解析height
            if (topLevelMap.containsKey("width")) {
                environmentMap.put("height", Integer.parseInt(topLevelMap.get("height").toString()));
            } else {
                environmentMap.put("height", 50);
            }

            // 解析stations信息
            if (topLevelMap.containsKey("stations")) {
                Map<String, Map<String, Object>> stations = new HashMap<>();
                Map<String, Object> stationMap = (Map<String, Object>) topLevelMap.get("stations");
@@ -292,14 +278,12 @@
            for (String line : lines) {
                line = line.trim();

                // 查找工作站ID
                if (line.startsWith("\"") && line.contains("\":{")) {
                    int endIndex = line.indexOf("\":{");
                    currentStation = line.substring(1, endIndex);
                    stations.put(currentStation, new HashMap<>());
                }

                // 解析capacity
                if (currentStation != null && line.contains("\"capacity\":")) {
                    String capacityStr = line.substring(line.indexOf("\"capacity\":") + 11);
                    capacityStr = capacityStr.substring(0, capacityStr.indexOf(",")).trim();
@@ -311,7 +295,6 @@
                    }
                }

                // 解析load_position和unload_position
                if (currentStation != null && line.contains("\"load_position\":")) {
                    List<Integer> loadPos = parsePosition(stationsSection, currentStation, "load_position");
                    if (loadPos != null) {
@@ -372,7 +355,7 @@
    }

    /**
     * 提取JSON中的特定部分
     * 提取JSON中特定部分
     *
     * @param jsonContent JSON内容
     * @param sectionName 部分名称
@@ -421,6 +404,64 @@
            return new int[]{coordMap.get("x"), coordMap.get("y")};
        }
        return null;
    }
    
    /**
     * 加载实际物理坐标映射
     *
     * @param filePath 文件路径
     * @return 实际坐标映射 Map<pathId, double[]{x_mm, y_mm}>
     */
    public static Map<String, double[]> loadRealCoordinateMapping(String filePath) {
        Map<String, double[]> coordinateMapping = new HashMap<>();
        ObjectMapper objectMapper = new ObjectMapper();
        
        try {
            Map<String, Object> topLevelMap = objectMapper.readValue(new File(filePath), Map.class);
            
            if (!topLevelMap.containsKey("path_id_to_coordinates")) {
                System.err.println("未找到path_id_to_coordinates部分");
                return coordinateMapping;
            }
            
            Map<String, Object> pathIdCoords = (Map<String, Object>) topLevelMap.get("path_id_to_coordinates");
            for (Map.Entry<String, Object> entry : pathIdCoords.entrySet()) {
                String pathId = entry.getKey();
                Object coordsObj = entry.getValue();
                
                if (coordsObj instanceof List) {
                    List<?> coordsList = (List<?>) coordsObj;
                    if (!coordsList.isEmpty()) {
                        Map<?, ?> coordMap = (Map<?, ?>) coordsList.get(0);
                        double x = ((Number) coordMap.get("x")).doubleValue();
                        double y = ((Number) coordMap.get("y")).doubleValue();
                        coordinateMapping.put(pathId, new double[]{x, y});
                    }
                }
            }
            
            System.out.println("成功加载实际坐标映射，包含 " + coordinateMapping.size() + " 个路径点");
            
        } catch (FileNotFoundException e) {
            System.err.println("实际坐标文件不存在: " + e.getMessage());
        } catch (IOException e) {
            System.err.println("实际坐标文件读取错误: " + e.getMessage());
        } catch (Exception e) {
            System.err.println("加载实际坐标文件失败: " + e.getMessage());
        }
        
        return coordinateMapping;
    }
    
    /**
     * 获取路径点的实际坐标
     *
     * @param pathId             路径点ID
     * @param realCoordinateMap  实际坐标映射
     * @return 坐标数组 [x_mm, y_mm]，如果未找到返回null
     */
    public static double[] getRealCoordinate(String pathId, Map<String, double[]> realCoordinateMap) {
        return realCoordinateMap.get(pathId);
    }

    /**
@@ -483,4 +524,111 @@
        int dy = coord1[1] - coord2[1];
        return Math.sqrt(dx * dx + dy * dy);
    }

    /**
     * 加载环境路径连通性数据（environment.json中的paths和obstacles）
     *
     * @param filePath 环境文件路径
     * @return 环境连通性数据
     */
    public static EnvironmentConnectivity loadEnvironmentConnectivity(String filePath) {
        EnvironmentConnectivity connectivity = new EnvironmentConnectivity();
        ObjectMapper objectMapper = new ObjectMapper();
        
        try {
            Map<String, Object> topLevelMap = objectMapper.readValue(new File(filePath), Map.class);
            
            // 加载可通行路径
            if (topLevelMap.containsKey("paths")) {
                List<Map<String, Object>> pathsList = (List<Map<String, Object>>) topLevelMap.get("paths");
                for (Map<String, Object> pathPoint : pathsList) {
                    Integer x = (Integer) pathPoint.get("x");
                    Integer y = (Integer) pathPoint.get("y");
                    if (x != null && y != null) {
                        connectivity.addValidPath(x, y);
                    }
                }
            }
            
            // 加载障碍物
            if (topLevelMap.containsKey("obstacles")) {
                List<Map<String, Object>> obstaclesList = (List<Map<String, Object>>) topLevelMap.get("obstacles");
                for (Map<String, Object> obstacle : obstaclesList) {
                    Integer x = (Integer) obstacle.get("x");
                    Integer y = (Integer) obstacle.get("y");
                    if (x != null && y != null) {
                        connectivity.addObstacle(x, y);
                    }
                }
            }
            
            System.out.println("成功加载环境连通性数据：" + 
                connectivity.getValidPathsCount() + " 个可通行点，" + 
                connectivity.getObstaclesCount() + " 个障碍物点");
                
        } catch (FileNotFoundException e) {
            System.err.println("环境文件不存在: " + e.getMessage());
        } catch (IOException e) {
            System.err.println("环境文件读取错误: " + e.getMessage());
        } catch (Exception e) {
            System.err.println("加载环境连通性数据失败: " + e.getMessage());
        }
        
        return connectivity;
    }

    /**
     * 环境连通性数据类
     */
    public static class EnvironmentConnectivity {
        private final Set<String> validPaths = new HashSet<>();
        private final Set<String> obstacles = new HashSet<>();
        
        public void addValidPath(int x, int y) {
            validPaths.add(x + "," + y);
        }
        
        public void addObstacle(int x, int y) {
            obstacles.add(x + "," + y);
        }
        
        public boolean isValidPath(int x, int y) {
            return validPaths.contains(x + "," + y);
        }
        
        public boolean isObstacle(int x, int y) {
            return obstacles.contains(x + "," + y);
        }
        
        public boolean isTraversable(int x, int y) {
            return isValidPath(x, y) && !isObstacle(x, y);
        }
        
        public int getValidPathsCount() {
            return validPaths.size();
        }
        
        public int getObstaclesCount() {
            return obstacles.size();
        }
        
        /**
         * 获取有效的相邻节点
         */
        public List<int[]> getValidNeighbors(int x, int y) {
            List<int[]> neighbors = new ArrayList<>();
            int[][] directions = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}}; // 上下左右
            
            for (int[] dir : directions) {
                int newX = x + dir[0];
                int newY = y + dir[1];
                
                if (isTraversable(newX, newY)) {
                    neighbors.add(new int[]{newX, newY});
                }
            }
            
            return neighbors;
        }
    }
} 

 algo-zkd/src/main/java/com/algo/util/PathTimeCalculator.java

New file
@@ -0,0 +1,318 @@
package com.algo.util;

import com.algo.model.CTUPhysicalConfig;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;

import java.util.List;
import java.util.Map;

/**
 * 路径时间计算器
 * 统一处理路径中每个点的时间计算，确保在各个模块中计算的一致性
 * 考虑加减速、转向、停留等所有时间因素
 */
public class PathTimeCalculator {

    /**
     * 路径映射表（网格坐标）
     */
    private final Map<String, Map<String, Integer>> pathMapping;
    
    /**
     * 实际坐标映射表（物理坐标，毫米）
     */
    private final Map<String, double[]> realCoordinateMapping;

    public PathTimeCalculator(Map<String, Map<String, Integer>> pathMapping, 
                             Map<String, double[]> realCoordinateMapping) {
        this.pathMapping = pathMapping;
        this.realCoordinateMapping = realCoordinateMapping;
    }

    /**
     * 为路径计算时间信息
     *
     * @param path         规划路径
     * @param startTime    起始时间（毫秒时间戳）
     * @param config       CTU物理配置
     * @param initialSpeed 初始速度（米/秒）
     */
    public void calculatePathTiming(PlannedPath path, long startTime, 
                                   CTUPhysicalConfig config, double initialSpeed) {
        List<PathCode> codeList = path.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return;
        }

        long currentTime = startTime;
        double currentSpeed = initialSpeed;
        
        for (int i = 0; i < codeList.size(); i++) {
            PathCode currentCode = codeList.get(i);
            
            // 设置到达时间
            currentCode.setArrivalTime(currentTime);
            
            // 计算在当前位置的停留时间
            double stayDuration = calculateStayDuration(currentCode, config);
            
            // 如果不是最后一个点，计算转向时间
            if (i < codeList.size() - 1) {
                PathCode nextCode = codeList.get(i + 1);
                double turnTime = calculateTurnTime(currentCode, nextCode, config);
                stayDuration += turnTime;
            }
            
            // 设置离开时间
            long departureTime = currentTime + (long)(stayDuration * 1000);
            currentCode.setDepartureTime(departureTime);
            
            // 计算累计时间
            currentCode.setCumulativeTime(departureTime - startTime);
            
            // 如果有下一个点，计算移动时间和速度变化
            if (i < codeList.size() - 1) {
                PathCode nextCode = codeList.get(i + 1);
                
                // 计算精确的移动时间（考虑加减速）
                MovementTimeResult movement = calculatePreciseMovementTime(
                    currentCode, nextCode, currentSpeed, config
                );
                
                currentTime = departureTime + movement.travelTime;
                currentSpeed = movement.endSpeed;
            }
        }
    }

    /**
     * 计算两点间移动时间
     */
    private MovementTimeResult calculatePreciseMovementTime(PathCode fromCode, PathCode toCode, 
                                                          double startSpeed, CTUPhysicalConfig config) {
        // 获取实际距离
        double realDistance = getRealDistance(fromCode.getCode(), toCode.getCode());
        
        if (realDistance <= 0) {
            // 备用：使用网格距离
            realDistance = getGridDistance(fromCode.getCode(), toCode.getCode()) 
                         * config.getStandardPointDistance();
        }
        
        // 确定目标速度（考虑是否为目标点）
        double targetSpeed = toCode.isTargetPoint() ? 0.0 : config.getNormalSpeed();
        
        // 使用物理配置的精确计算方法
        double movementTime = config.calculatePreciseMovementTime(
            realDistance, startSpeed, targetSpeed, config.getNormalSpeed()
        );
        
        return new MovementTimeResult((long)(movementTime * 1000), targetSpeed);
    }

    /**
     * 计算在路径点的停留时间
     */
    private double calculateStayDuration(PathCode pathCode, CTUPhysicalConfig config) {
        double stayTime = 0.1; // 基础停留时间100ms
        
        // 根据动作类型增加停留时间
        if (pathCode.getActionType() != null) {
            switch (pathCode.getActionType()) {
                case "1": // 取货
                    stayTime += 10.0;
                    break;
                case "2": // 放货
                    stayTime += 10.0;
                    break;
                case "3": // 充电
                    stayTime += 600.0;
                    break;
                default:
                    stayTime += 0.5;
                    break;
            }
        } else if (pathCode.isTargetPoint()) {
            // 目标点停留时间
            if ("1".equals(pathCode.getPosType())) {
                stayTime += 10.0; // 取货
            } else if ("2".equals(pathCode.getPosType())) {
                stayTime += 10.0; // 放货
            } else {
                stayTime += 10.0; // 其他目标点
            }
        }
        
        return stayTime;
    }

    /**
     * 计算转向时间
     */
    private double calculateTurnTime(PathCode fromCode, PathCode toCode, CTUPhysicalConfig config) {
        if (fromCode.getDirection().equals(toCode.getDirection())) {
            return 0.0;
        }
        
        // 使用配置的转向时间计算方法
        return config.getTurnTime(fromCode.getDirection(), toCode.getDirection());
    }

    /**
     * 获取实际物理距离
     */
    private double getRealDistance(String fromCode, String toCode) {
        if (realCoordinateMapping == null) {
            return 0.0;
        }
        
        double[] fromCoord = realCoordinateMapping.get(fromCode);
        double[] toCoord = realCoordinateMapping.get(toCode);
        
        if (fromCoord != null && toCoord != null) {
            return CTUPhysicalConfig.calculateRealDistance(fromCoord, toCoord);
        }
        
        return 0.0;
    }

    /**
     * 获取网格距离
     */
    private double getGridDistance(String fromCode, String toCode) {
        int[] fromCoord = JsonUtils.getCoordinate(fromCode, pathMapping);
        int[] toCoord = JsonUtils.getCoordinate(toCode, pathMapping);
        
        if (fromCoord != null && toCoord != null) {
            return JsonUtils.calculateEuclideanDistance(fromCoord, toCoord);
        }
        
        return 1.0; // 默认距离
    }

    /**
     * 计算路径中指定位置之后的时间（用于冲突解决）
     *
     * @param path      路径
     * @param fromIndex 开始重新计算的索引
     * @param config    物理配置
     */
    public void recalculatePathTimingFromIndex(PlannedPath path, int fromIndex, CTUPhysicalConfig config) {
        List<PathCode> codeList = path.getCodeList();
        if (codeList == null || fromIndex >= codeList.size()) {
            return;
        }
        
        // 获取重新计算起点的时间和速度
        PathCode startCode = codeList.get(fromIndex);
        long startTime = startCode.getDepartureTime() != null ? 
                        startCode.getDepartureTime() : startCode.getArrivalTime();
        
        // 估算当前速度
        double currentSpeed = config.getNormalSpeed();
        
        // 从指定位置开始重新计算
        long currentTime = startTime;
        
        for (int i = fromIndex + 1; i < codeList.size(); i++) {
            PathCode currentCode = codeList.get(i);
            PathCode previousCode = codeList.get(i - 1);
            
            // 计算移动时间
            MovementTimeResult movement = calculatePreciseMovementTime(
                previousCode, currentCode, currentSpeed, config
            );
            
            currentTime += movement.travelTime;
            currentSpeed = movement.endSpeed;
            
            // 设置到达时间
            currentCode.setArrivalTime(currentTime);
            
            // 计算停留时间
            double stayDuration = calculateStayDuration(currentCode, config);
            
            // 如果不是最后一个点，增加转向时间
            if (i < codeList.size() - 1) {
                PathCode nextCode = codeList.get(i + 1);
                double turnTime = calculateTurnTime(currentCode, nextCode, config);
                stayDuration += turnTime;
            }
            
            // 设置离开时间
            long departureTime = currentTime + (long)(stayDuration * 1000);
            currentCode.setDepartureTime(departureTime);
            
            // 更新累计时间
            long pathStartTime = codeList.get(0).getArrivalTime();
            currentCode.setCumulativeTime(departureTime - pathStartTime);
            
            currentTime = departureTime;
        }
    }

    /**
     * 计算路径的总执行时间
     */
    public long calculateTotalPathTime(PlannedPath path) {
        List<PathCode> codeList = path.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return 0;
        }
        
        PathCode firstCode = codeList.get(0);
        PathCode lastCode = codeList.get(codeList.size() - 1);
        
        if (firstCode.getArrivalTime() != null && lastCode.getDepartureTime() != null) {
            return lastCode.getDepartureTime() - firstCode.getArrivalTime();
        }
        
        return 0;
    }

    /**
     * 检查路径在指定时间窗口是否与占用表冲突
     */
    public boolean isPathTimeConflictFree(PlannedPath path, Map<String, String> occupancyMap) {
        List<PathCode> codeList = path.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return true;
        }
        
        for (PathCode pathCode : codeList) {
            if (pathCode.getArrivalTime() == null || pathCode.getDepartureTime() == null) {
                continue;
            }
            
            int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
            if (coord == null) continue;
            
            // 检查整个停留时间段
            long startTimeSlot = pathCode.getArrivalTime() / 1000;
            long endTimeSlot = pathCode.getDepartureTime() / 1000;
            
            for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
                String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
                if (occupancyMap.containsKey(spaceTimeKey)) {
                    return false;
                }
            }
        }
        
        return true;
    }

    /**
     * 移动时间计算结果
     */
    private static class MovementTimeResult {
        final long travelTime; // 移动时间（毫秒）
        final double endSpeed; // 结束速度（米/秒）
        
        public MovementTimeResult(long travelTime, double endSpeed) {
            this.travelTime = travelTime;
            this.endSpeed = endSpeed;
        }
    }
}