更新版本

jianghaiyue

17 小时以前 d808837cd368c3772962be591aa6532bcc0cf3e4

 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{" +