优化更新

jianghaiyue

2025-11-03 79acef0dba1f030bc53a90a7489ff145340587db

优化更新

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

@@ -228,12 +228,40 @@
        // 6. 处理空闲CTU
        if (includeIdleAgv) {
            System.out.println("步骤5: 处理空闲CTU");
            
            // 6.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);

            List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
            for (AGVStatus yieldAgv : yieldingIdleAgvs) {
                if (!plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
                    yieldingAgvsToHandle.add(yieldAgv);
                }
            }
            
            if (!yieldingAgvsToHandle.isEmpty()) {
                System.out.println( yieldingAgvsToHandle.size() + " 个需要避让的空闲AGV");
                for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                    if (yieldPath != null) {
                        plannedPaths.add(yieldPath);
                        plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    }
                }
            } else {
                System.out.println("  无需避让的空闲AGV");
            }
            
            // 6.2. 然后为剩余的空闲AGV规划待机/充电路径
            List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap
            );
            plannedPaths.addAll(idlePaths);
            System.out.println("空闲CTU路径规划完成，数量: " + idlePaths.size());
            System.out.println("  空闲CTU路径规划，数量: " + idlePaths.size());
        }

        // 6.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints);

        // 7. 最终碰撞检测和解决（针对同时生成的新路径）
        System.out.println("步骤6: 最终碰撞检测");
@@ -665,6 +693,66 @@
    }

    /**
     * 迭代检查所有空闲AGV是否占用已规划路径
     */
    private void performFinalYieldingCheck(List<AGVStatus> agvStatusList, 
                                           List<PlannedPath> plannedPaths,
                                           Map<String, String> plannedAgvIds,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<double[]> constraints) {
        final int MAX_ITERATIONS = 5; // 最大迭代次数
        int iteration = 0;
        int totalYieldingCount = 0;
        
        while (iteration < MAX_ITERATIONS) {
            iteration++;
            
            // 识别需要避让的空闲AGV
            List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
            
            if (yieldingAgvs.isEmpty()) {
                if (iteration == 1) {
                    System.out.println("  无需避让的AGV");
                } else {
                    System.out.println("  第" + iteration + "轮检查：无新的避让需求");
                }
                break;
            }
            
            System.out.println("  第" + iteration + "轮检查：" + yieldingAgvs.size() + " 个需要避让的AGV");
            
            // 规划避让路径
            int successCount = 0;
            for (AGVStatus yieldAgv : yieldingAgvs) {
                if (plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
                    continue;
                }
                
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    successCount++;
                    totalYieldingCount++;
                }
            }
            
            if (successCount == 0) {
                System.out.println("  第" + iteration + "轮检查：无法为需要避让的AGV规划路径，停止迭代");
                break;
            }
        }
        
        if (iteration >= MAX_ITERATIONS) {
            System.out.println("  达到最大迭代次数(" + MAX_ITERATIONS + ")，可能存在复杂的避让场景");
        }
        
        if (totalYieldingCount > 0) {
            System.out.println("  避让检查完成：共处理 " + totalYieldingCount + " 个避让AGV，迭代 " + iteration + " 轮");
        }
    }

    /**
     * 识别需要让行的AGV
     * 检查空闲AGV是否占用了其他AGV剩余路径上的位置
     *
@@ -813,7 +901,7 @@
            }
        }
        
        System.out.println("  未找到合适的避让位置 ");
        System.out.println("  未找到合适的避让位置");
        return null;
    }