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 | 110 +++++++++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 103 insertions(+), 7 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 8caeef7..7ea0de9 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -864,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<>(); // 璁板綍浣嶇疆鍜岃窛绂�
@@ -889,19 +896,108 @@
continue;
}
- // 鎵惧埌涓�涓湭琚崰鐢ㄧ殑浣嶇疆
- if (!blockedPositions.contains(neighborPos) && (depth + 1) >= 3) {
- 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