From abbfb9733fe7370ae427cfa726cc311298bda89a Mon Sep 17 00:00:00 2001
From: zhang <zc857179121@qq.com>
Date: 星期一, 03 十一月 2025 16:50:47 +0800
Subject: [PATCH] 在驱赶小车时，找的空闲位置，让它多走3格
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 94 +++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 91 insertions(+), 3 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
index e8f0fef..8caeef7 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/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. 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰勶紝濡傛灉鍗犵敤鍒欑敓鎴愰伩璁╄矾寰�
+ 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(" 鏃犻渶閬胯鐨勭┖闂睞GV");
+ }
+
+ // 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 @@
}
/**
+ * 杩唬妫�鏌ユ墍鏈夌┖闂睞GV鏄惁鍗犵敤宸茶鍒掕矾寰�
+ */
+ 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(" 鏃犻渶閬胯鐨凙GV");
+ } 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 + " 涓伩璁〢GV锛岃凯浠� " + iteration + " 杞�");
+ }
+ }
+
+ /**
* 璇嗗埆闇�瑕佽琛岀殑AGV
* 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤浜嗗叾浠朅GV鍓╀綑璺緞涓婄殑浣嶇疆
*
@@ -802,7 +890,7 @@
}
// 鎵惧埌涓�涓湭琚崰鐢ㄧ殑浣嶇疆
- if (!blockedPositions.contains(neighborPos)) {
+ if (!blockedPositions.contains(neighborPos) && (depth + 1) >= 3) {
System.out.println(" 鎵惧埌閬胯浣嶇疆: " + neighborPos + " (璺濈=" + (depth + 1) + "姝�)");
return neighborPos;
}
@@ -813,7 +901,7 @@
}
}
- System.out.println(" 鏈壘鍒板悎閫傜殑閬胯浣嶇疆 ");
+ System.out.println(" 鏈壘鍒板悎閫傜殑閬胯浣嶇疆");
return null;
}
--
Gitblit v1.9.1