From 642f20dd1b068e23146b8ec96c5eac8b63c96b87 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 05 十一月 2025 18:18:25 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/PathPlanningService.java | 62 ++++++++++++++-
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | 129 ++++++++++++++++++++++++++++++-
2 files changed, 178 insertions(+), 13 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
index 894f28e..486c246 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -367,8 +367,9 @@
}
}
- // 鍑忓皯绛夊緟閫夐」
- if (Math.random() < 0.2) {
+ // 蹇呰鏃讹紙鏈夊啿绐侊級鐢熸垚绛夊緟鑺傜偣锛涘綋鍓嶈妭鐐圭殑鎵�鏈夐偦灞呭湪鐭湡鍐呴兘琚樆鎸″垯绛夊緟
+ boolean allNeighborsBlocked = checkIfAllNeighborsBlocked(current, constraintChecker, physicalConfig);
+ if (allNeighborsBlocked) {
long waitTime = timeResolution;
long waitUntilTime = current.timePoint + waitTime;
String waitKey = createSpaceTimeKey(current.code, waitUntilTime);
@@ -392,6 +393,52 @@
}
}
}
+ }
+
+ /**
+ * 妫�鏌ュ綋鍓嶈妭鐐圭殑閭诲眳鏄惁鍦ㄧ煭鏈熷唴閮借闃绘尅
+ */
+ private boolean checkIfAllNeighborsBlocked(SpaceTimeAStarNode current,
+ EnhancedConstraintChecker constraintChecker,
+ CTUPhysicalConfig physicalConfig) {
+ // 鑾峰彇褰撳墠鑺傜偣鐨勬墍鏈夌┖闂撮偦灞�
+ List<Map<String, String>> neighbors = getNeighbors(current.code);
+
+ if (neighbors.isEmpty()) {
+ return true;
+ }
+
+ // 妫�鏌ユ湭鏉ュ嚑涓椂闂寸墖鍐呯殑鍗犵敤鎯呭喌
+ int checkTimeSteps = 3;
+ boolean allBlocked = true;
+
+ for (int step = 1; step <= checkTimeSteps; step++) {
+ long checkTime = current.timePoint + (step * timeResolution);
+
+ // 妫�鏌ユ槸鍚︽湁鑷冲皯涓�涓偦灞呭湪褰撳墠鏃堕棿鐗囧彲鐢�
+ for (Map<String, String> neighbor : neighbors) {
+ String neighborCode = neighbor.get("code");
+ long arrivalTime = checkTime;
+
+ // 瀵归綈鍒版椂闂村垎杈ㄧ巼
+ arrivalTime = alignToTimeResolution(arrivalTime);
+
+ // 妫�鏌ヨ繖涓偦灞呭湪褰撳墠鏃堕棿鐗囨槸鍚﹀彲鐢�
+ if (constraintChecker.isValidMove(current.code, neighborCode,
+ current.timePoint, arrivalTime)) {
+ // 鑷冲皯鏈変竴涓偦灞呭彲鐢紝涓嶉渶瑕佺瓑寰�
+ allBlocked = false;
+ break;
+ }
+ }
+
+ // 濡傛灉鎵惧埌鍙敤閭诲眳锛屼笉闇�瑕佺瓑寰�
+ if (!allBlocked) {
+ break;
+ }
+ }
+
+ return allBlocked;
}
/**
@@ -566,17 +613,85 @@
codeList.add(pathCode);
}
+ codeList = optimizePathByRemovingBacktrack(codeList);
+
PlannedPath plannedPath = new PlannedPath("", "", codeList);
- // 浣跨敤缁熶竴鐨勬椂闂磋绠楀櫒璁＄畻绮剧‘鏃堕棿
- long startTime = pathNodes.get(0).timePoint;
- CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
- timeCalculator.calculatePathTiming(plannedPath, startTime, defaultConfig, 0.0);
-
return plannedPath;
}
/**
+ * 浼樺寲璺緞锛氬幓闄ょ┖闂翠笂鐨勯噸澶嶇偣锛堥伩鍏嶆潵鍥炶蛋锛�
+ * 妫�娴嬪苟绉婚櫎 A->B->A 妯″紡鐨勬潵鍥炶蛋锛岄伩鍏嶄笉蹇呰鐨勭粫琛�
+ *
+ * @param codeList 鍘熷璺緞浠ｇ爜鍒楄〃
+ * @return 浼樺寲鍚庣殑璺緞浠ｇ爜鍒楄〃
+ */
+ private List<PathCode> optimizePathByRemovingBacktrack(List<PathCode> codeList) {
+ if (codeList == null || codeList.size() <= 2) {
+ return codeList;
+ }
+
+ List<PathCode> optimized = new ArrayList<>();
+ int i = 0;
+
+ while (i < codeList.size()) {
+ PathCode current = codeList.get(i);
+
+ // 妫�鏌ユ槸鍚︽槸鏉ュ洖璧版ā寮忥細A->B->A锛堣繛缁笁涓偣锛岀涓�涓拰绗笁涓浉鍚岋級
+ // 濡傛灉鍑虹幇鏉ュ洖璧帮紝璺宠繃B鍜岀浜屼釜A锛岀洿鎺ュ埌涓嬩竴涓笉鍚岀殑鐐�
+ if (i < codeList.size() - 2) {
+ PathCode next = codeList.get(i + 1);
+ PathCode nextNext = codeList.get(i + 2);
+
+ // 妫�娴嬫潵鍥炶蛋锛氬綋鍓嶄綅缃� == 涓嬩笅涓綅缃紝涓斾腑闂翠綅缃笉鍚�
+ if (current.getCode().equals(nextNext.getCode()) &&
+ !current.getCode().equals(next.getCode())) {
+ // 鏉ュ洖璧帮細璺宠繃B鍜岀浜屼釜A锛岀洿鎺ュ鐞嗕笅涓�涓笉鍚岀殑鐐�
+ // 鎵惧埌涓嬩竴涓笌褰撳墠鐐逛笉鍚岀殑浣嶇疆
+ int j = i + 2;
+ while (j < codeList.size() && codeList.get(j).getCode().equals(current.getCode())) {
+ j++;
+ }
+ // 濡傛灉鎵惧埌浜嗕笉鍚岀殑鐐癸紝鎴栬�呭埌杈炬湯灏撅紝璺宠繃鏉ュ洖璧扮殑閮ㄥ垎
+ if (j < codeList.size()) {
+ i = j; // 璺冲埌涓嬩竴涓笉鍚岀殑鐐�
+ continue;
+ } else {
+ // 濡傛灉鍚庨潰閮芥槸鐩稿悓鐐癸紝淇濈暀褰撳墠鐐癸紙鍙兘鏄洰鏍囩偣锛�
+ break;
+ }
+ }
+ }
+
+ // 姝ｅ父娣诲姞褰撳墠鐐�
+ optimized.add(current);
+ i++;
+ }
+
+ // 纭繚鐩爣鐐硅鍖呭惈
+ if (!optimized.isEmpty() && !codeList.isEmpty()) {
+ PathCode lastOriginal = codeList.get(codeList.size() - 1);
+ PathCode lastOptimized = optimized.get(optimized.size() - 1);
+ if (!lastOriginal.getCode().equals(lastOptimized.getCode())) {
+ optimized.add(lastOriginal);
+ }
+ }
+
+ // 閲嶆柊璁＄畻鏂瑰悜锛堝洜涓轰紭鍖栧悗璺宠繃浜嗕腑闂寸偣锛屾柟鍚戦渶瑕佹洿鏂帮級
+ for (int k = 0; k < optimized.size(); k++) {
+ PathCode code = optimized.get(k);
+ if (k < optimized.size() - 1) {
+ PathCode nextCode = optimized.get(k + 1);
+ String direction = calculateDirection(code.getCode(), nextCode.getCode());
+ code.setDirection(direction);
+ }
+ }
+
+ return optimized;
+ }
+
+ /**
* 鍒涘缓鏃剁┖閿�
*
* @param code 浣嶇疆浠ｇ爜
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 1d93b85..54b0e54 100644
--- a/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
+++ b/algo-zkd/src/main/java/com/algo/service/PathPlanningService.java
@@ -170,7 +170,7 @@
System.out.println("姝ラ2: 澶勭悊鏈夊墿浣欒矾寰勭殑CTU");
for (AGVStatus agv : ctuWithRemainingPaths) {
- PlannedPath remainingPath = processRemainingPath(agv);
+ PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
if (remainingPath != null) {
plannedPaths.add(remainingPath);
plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");
@@ -299,6 +299,9 @@
// 7.5. 鏈�缁堥伩璁╂鏌�
performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
+ // 7.6. 閲嶆柊璁＄畻鎵�鏈夎矾寰勭殑鏃堕棿鎴�
+ recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);
+
// 8. 鏈�缁堢鎾炴娴嬪拰瑙ｅ喅
System.out.println("姝ラ6: 鏈�缁堢鎾炴娴�");
List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);
@@ -341,9 +344,10 @@
* 澶勭悊CTU鐨勫墿浣欒矾寰�
*
* @param agv CTU鐘舵��
+ * @param unifiedTimestamp 缁熶竴鏃堕棿鎴筹紙绉掞級
* @return 澶勭悊鍚庣殑璺緞
*/
- private PlannedPath processRemainingPath(AGVStatus agv) {
+ private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
if (!agv.hasRemainingPath()) {
return null;
}
@@ -376,8 +380,8 @@
processedPath.setSegId(segId);
if (timeCalculator != null && !remainingCodes.isEmpty()) {
- // 鑾峰彇AGV鐨勪笅涓�涓矾寰勭偣鍒拌揪鏃堕棿浣滀负璧峰鏃堕棿
- long startTime = agv.getNextPointArrivalTime();
+ // 浣跨敤缁熶竴鏃堕棿鎴充綔涓鸿捣濮嬫椂闂达紙杞崲涓烘绉掞級
+ long startTime = unifiedTimestamp * 1000;
CTUPhysicalConfig config = agv.getPhysicalConfig();
@@ -619,7 +623,6 @@
return;
}
- String actionType = "2"; // 浠诲姟绫诲瀷
String taskId = task.getTaskId();
int backpackLevel = task.getBackpackIndex();
@@ -627,20 +630,23 @@
PathCode pathCode = codeList.get(i);
// 璁剧疆鍩烘湰淇℃伅
- pathCode.setActionType(actionType);
pathCode.setTaskId(taskId);
pathCode.setLev(backpackLevel);
// 璁剧疆鐩爣鐐逛俊鎭�
if (i == codeList.size() - 1) { // 鏈�鍚庝竴涓偣
pathCode.setTargetPoint(true);
+ // 鏍规嵁浠诲姟绫诲瀷璁剧疆actionType鍜宲osType锛�
if (task.isLoaded()) {
+ pathCode.setActionType("2"); // 鏀捐揣
pathCode.setPosType("2"); // 鏀捐揣
} else {
+ pathCode.setActionType("1"); // 鍙栬揣
pathCode.setPosType("1"); // 鍙栬揣
}
} else {
pathCode.setTargetPoint(false);
+ pathCode.setActionType(null);
pathCode.setPosType(null);
}
}
@@ -796,6 +802,50 @@
}
}
+ private void recalculateAllPathTimings(List<PlannedPath> plannedPaths,
+ List<AGVStatus> agvStatusList,
+ long unifiedTimestamp) {
+ if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
+ return;
+ }
+
+ Map<String, AGVStatus> agvStatusMap = new HashMap<>();
+ for (AGVStatus agv : agvStatusList) {
+ if (agv.getAgvId() != null) {
+ agvStatusMap.put(agv.getAgvId(), agv);
+ }
+ }
+
+ long startTimeMs = unifiedTimestamp * 1000;
+ int recalculatedCount = 0;
+
+ // 閬嶅巻鎵�鏈夎矾寰勶紝閲嶆柊璁＄畻鏃堕棿鎴�
+ for (PlannedPath path : plannedPaths) {
+ if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
+ continue;
+ }
+
+ String agvId = path.getAgvId();
+ if (agvId == null) {
+ continue;
+ }
+
+ // 鑾峰彇AGV鐨勭墿鐞嗛厤缃�
+ AGVStatus agvStatus = agvStatusMap.get(agvId);
+ CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null)
+ ? agvStatus.getPhysicalConfig()
+ : new CTUPhysicalConfig();
+
+ double initialSpeed = 0.0;
+
+ // 閲嶆柊璁＄畻璺緞鏃堕棿鎴�
+ timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
+ recalculatedCount++;
+ }
+
+ System.out.println(" 宸查噸鏂拌绠� " + recalculatedCount + " 鏉¤矾寰勭殑鏃堕棿鎴筹紙缁熶竴鏃堕棿鍩哄噯: " + unifiedTimestamp + "绉掞級");
+ }
+
/**
* 璇嗗埆闇�瑕佽琛岀殑AGV
* 妫�鏌ョ┖闂睞GV鏄惁鍗犵敤浜嗗叾浠朅GV鍓╀綑璺緞涓婄殑浣嶇疆
--
Gitblit v1.9.1