From 434e0ff84a7df875f223e9118f244e04f28310ca Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期二, 04 十一月 2025 14:38:56 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 142 +++++++++++++++++++++++++++++++++++++++++------
1 files changed, 124 insertions(+), 18 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..61d625d 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -103,8 +103,16 @@
this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);
long startTime = System.currentTimeMillis();
+ long unifiedTimestamp = startTime / 1000; // 缁熶竴鏃堕棿鎴筹紙绉掞級
System.out.println("寮�濮嬩负 " + agvStatusList.size() + " 涓狢TU瑙勫垝");
+ System.out.println("缁熶竴鏃堕棿鍩哄噯: " + unifiedTimestamp + "绉� (" + new Date(startTime) + ")");
+
+ // 缁熻鏁呴殰杞︽暟閲�
+ long faultyCount = agvStatusList.stream().filter(agv -> agv.getError() == 1).count();
+ if (faultyCount > 0) {
+ System.out.println("妫�娴嬪埌 " + faultyCount + " 涓晠闅滆溅(error=1)锛屽皢鍦ㄦ椂绌鸿〃涓崰鎹綅缃�");
+ }
// 1. 鏋勫缓鐜版湁鍓╀綑璺緞鐨勬椂绌哄崰鐢ㄨ〃
System.out.println("姝ラ1: 鏋勫缓鏃剁┖鍗犵敤琛�");
@@ -413,21 +421,23 @@
List<double[]> constraints,
Map<String, String> occupancyMap,
AGVStatus agvStatus) {
- // 灏濊瘯鍩烘湰璺緞瑙勫垝
- PlannedPath basicPath = pathPlanner.planPath(startPos, endPos, constraints);
- if (basicPath == null) {
+ // 甯︽椂绌哄崰鐢ㄨ〃鐨勮矾寰勮鍒�
+ PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
+ startPos,
+ endPos,
+ constraints,
+ occupancyMap,
+ agvStatus.getPhysicalConfig()
+ );
+
+ if (spacetimePath == null) {
return null;
}
- // 鎵惧埌瀹夊叏鐨勮捣濮嬫椂闂�
- long safeStartTime = remainingPathProcessor.findSafeStartTime(
- basicPath, occupancyMap, agvStatus.getPhysicalConfig()
- );
+ long startTime = System.currentTimeMillis();
+ timeCalculator.calculatePathTiming(spacetimePath, startTime, agvStatus.getPhysicalConfig(), 0.0);
- // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁剧疆绮剧‘鐨勬椂闂翠俊鎭�
- timeCalculator.calculatePathTiming(basicPath, safeStartTime, agvStatus.getPhysicalConfig(), 0.0);
-
- return basicPath;
+ return spacetimePath;
}
/**
@@ -864,8 +874,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 +906,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