From 5bcf58fa72ec1a03791fb238399986b44bc0ec26 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期二, 04 十一月 2025 09:07:57 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 200 ++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 192 insertions(+), 8 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..7ea0de9 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鍓╀綑璺緞涓婄殑浣嶇疆
*
@@ -776,8 +864,15 @@
* 瀵绘壘璁╄鐨勭洰鏍囦綅缃�
*/
private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
- // 浣跨敤BFS鎼滅储鏈�杩戠殑绌洪棽浣嶇疆
- final int MAX_SEARCH_DEPTH = 10;
+ // 浣跨敤BFS鎼滅储婊¤冻瀹夊叏璺濈鐨勭┖闂蹭綅缃�
+ final int MAX_SEARCH_DEPTH = 15;
+
+ // 鑾峰彇瀹夊叏璺濈鍙傛暟锛堜粠鐗╃悊閰嶇疆涓幏鍙栵紝榛樿3.0锛�
+ CTUPhysicalConfig config = agv.getPhysicalConfig();
+ double minSafetyDistance = config != null ? config.getMinSafetyDistance() : 3.0;
+
+ // 灏嗗畨鍏ㄨ窛绂昏浆鎹负缃戞牸姝ユ暟锛堝亣璁炬瘡涓妭鐐归棿璺濅负1锛�
+ int safetySteps = (int) Math.ceil(minSafetyDistance);
Queue<String> queue = new LinkedList<>();
Map<String, Integer> visited = new HashMap<>(); // 璁板綍浣嶇疆鍜岃窛绂�
@@ -801,19 +896,108 @@
continue;
}
- // 鎵惧埌涓�涓湭琚崰鐢ㄧ殑浣嶇疆
- if (!blockedPositions.contains(neighborPos)) {
- System.out.println(" 鎵惧埌閬胯浣嶇疆: " + neighborPos + " (璺濈=" + (depth + 1) + "姝�)");
+ int distanceToPos = depth + 1;
+ visited.put(neighborPos, distanceToPos);
+
+ // 妫�鏌ヨ浣嶇疆鏄惁婊¤冻瑕佹眰
+ if (!blockedPositions.contains(neighborPos) &&
+ isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, safetySteps)) {
+ System.out.println(" 鎵惧埌瀹夊叏閬胯浣嶇疆: " + neighborPos);
return neighborPos;
}
// 鍔犲叆闃熷垪缁х画鎼滅储
queue.offer(neighborPos);
- visited.put(neighborPos, depth + 1);
}
}
- System.out.println(" 鏈壘鍒板悎閫傜殑閬胯浣嶇疆 ");
+ System.out.println(" 鏈壘鍒版弧瓒冲畨鍏ㄨ窛绂荤殑閬胯浣嶇疆锛岄檷浣庡畨鍏ㄨ姹傞噸璇�");
+
+ // 濡傛灉鎵句笉鍒版弧瓒冲畨鍏ㄨ窛绂荤殑浣嶇疆锛岄檷浣庤姹傚啀娆℃悳绱紙鑷冲皯淇濊瘉2姝ヨ窛绂伙級
+ return findYieldTargetPositionWithReducedSafety(currentPos, blockedPositions, 2);
+ }
+
+ /**
+ * 妫�鏌ュ�欓�変綅缃笌鎵�鏈夎鍗犵敤浣嶇疆鏄惁婊¤冻瀹夊叏璺濈
+ */
+ private boolean isSafeDistanceFromBlockedPositions(String candidatePos,
+ Set<String> blockedPositions,
+ int minSteps) {
+ // 鑾峰彇鍊欓�変綅缃殑鍧愭爣
+ int[] candidateCoord = JsonUtils.getCoordinate(candidatePos, envDataConfig.getPathMapping());
+ if (candidateCoord == null) {
+ return false;
+ }
+
+ // 妫�鏌ヤ笌姣忎釜琚崰鐢ㄤ綅缃殑璺濈
+ for (String blockedPos : blockedPositions) {
+ int[] blockedCoord = JsonUtils.getCoordinate(blockedPos, envDataConfig.getPathMapping());
+ if (blockedCoord == null) {
+ continue;
+ }
+
+ // 璁＄畻鏇煎搱椤胯窛绂�
+ int distance = Math.abs(candidateCoord[0] - blockedCoord[0]) +
+ Math.abs(candidateCoord[1] - blockedCoord[1]);
+
+ if (distance < minSteps) {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ /**
+ * 浣跨敤闄嶄綆鐨勫畨鍏ㄨ姹傛煡鎵捐琛屼綅缃紙澶囬�夛級
+ *
+ * @param currentPos 褰撳墠浣嶇疆
+ * @param blockedPositions 琚崰鐢ㄧ殑浣嶇疆闆嗗悎
+ * @param minSteps 鏈�灏忔鏁拌姹�
+ * @return 璁╄鐩爣浣嶇疆
+ */
+ private String findYieldTargetPositionWithReducedSafety(String currentPos,
+ Set<String> blockedPositions,
+ int minSteps) {
+ final int MAX_SEARCH_DEPTH = 10;
+
+ Queue<String> queue = new LinkedList<>();
+ Map<String, Integer> visited = new HashMap<>();
+
+ queue.offer(currentPos);
+ visited.put(currentPos, 0);
+
+ while (!queue.isEmpty()) {
+ String pos = queue.poll();
+ int depth = visited.get(pos);
+
+ if (depth >= MAX_SEARCH_DEPTH) {
+ break;
+ }
+
+ List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
+ for (Map<String, String> neighbor : neighbors) {
+ String neighborPos = neighbor.get("code");
+
+ if (neighborPos == null || visited.containsKey(neighborPos)) {
+ continue;
+ }
+
+ visited.put(neighborPos, depth + 1);
+
+ // 浣跨敤闄嶄綆鐨勫畨鍏ㄨ姹�
+ if (!blockedPositions.contains(neighborPos) &&
+ isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, minSteps)) {
+ System.out.println(" 鎵惧埌闄嶇骇瀹夊叏閬胯浣嶇疆: " + neighborPos +
+ " (璺濈=" + (depth + 1) + "姝�, 鏈�灏忓畨鍏ㄨ窛绂�=" + minSteps + "姝�)");
+ return neighborPos;
+ }
+
+ queue.offer(neighborPos);
+ }
+ }
+
+ System.out.println(" 鏃犳硶鎵惧埌閬胯浣嶇疆");
return null;
}
--
Gitblit v1.9.1