From 4618bbf913912f85ee197ccd10644df29ad2f287 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期一, 10 十一月 2025 10:33:52 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/CollisionDetector.java | 108 +++++++++++++++++++++--------------
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | 17 +----
2 files changed, 67 insertions(+), 58 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 486c246..26c28ef 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -621,11 +621,7 @@
}
/**
- * 浼樺寲璺緞锛氬幓闄ょ┖闂翠笂鐨勯噸澶嶇偣锛堥伩鍏嶆潵鍥炶蛋锛�
- * 妫�娴嬪苟绉婚櫎 A->B->A 妯″紡鐨勬潵鍥炶蛋锛岄伩鍏嶄笉蹇呰鐨勭粫琛�
- *
- * @param codeList 鍘熷璺緞浠ｇ爜鍒楄〃
- * @return 浼樺寲鍚庣殑璺緞浠ｇ爜鍒楄〃
+ * 浼樺寲璺緞
*/
private List<PathCode> optimizePathByRemovingBacktrack(List<PathCode> codeList) {
if (codeList == null || codeList.size() <= 2) {
@@ -638,27 +634,20 @@
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; // 璺冲埌涓嬩竴涓笉鍚岀殑鐐�
+ i = j;
continue;
} else {
- // 濡傛灉鍚庨潰閮芥槸鐩稿悓鐐癸紝淇濈暀褰撳墠鐐癸紙鍙兘鏄洰鏍囩偣锛�
break;
}
}
@@ -678,7 +667,7 @@
}
}
- // 閲嶆柊璁＄畻鏂瑰悜锛堝洜涓轰紭鍖栧悗璺宠繃浜嗕腑闂寸偣锛屾柟鍚戦渶瑕佹洿鏂帮級
+ // 閲嶆柊璁＄畻鏂瑰悜
for (int k = 0; k < optimized.size(); k++) {
PathCode code = optimized.get(k);
if (k < optimized.size() - 1) {
diff --git a/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java b/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
index 2398831..79be4b1 100644
--- a/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
+++ b/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
@@ -9,8 +9,7 @@
import java.util.*;
/**
- * 纰版挒妫�娴嬪櫒锛堝寮虹増锛�
- * 鐢ㄤ簬妫�娴婥TU璺緞涔嬮棿鐨勬椂绌哄啿绐侊紝鏀寔鐗╃悊鍙傛暟鍜岄珮绮惧害鏃堕棿妫�娴�
+ * 纰版挒妫�娴嬪櫒
*/
public class CollisionDetector {
@@ -26,13 +25,14 @@
/**
* 鏃堕棿绮惧害锛堟绉掞級
+ * 鎻愰珮鏃堕棿绮惧害浠ユ洿鍑嗙‘鍦版娴嬬鎾�
*/
- private final long timeResolution = 100; // 100姣鐨勬椂闂寸簿搴�
+ private final long timeResolution = 50;
/**
* 绌洪棿鍒嗚鲸鐜囷紙绫筹級
*/
- private final double spaceResolution = 0.1; // 10鍘樼背鐨勭┖闂寸簿搴�
+ private final double spaceResolution = 0.1;
/**
* 鏋勯�犲嚱鏁�
@@ -47,7 +47,7 @@
}
/**
- * 鏋勯�犲嚱鏁帮紙浣跨敤榛樿鍙傛暟锛�
+ * 鏋勯�犲嚱鏁�
*
* @param pathMapping 璺緞鏄犲皠琛�
*/
@@ -56,7 +56,7 @@
}
/**
- * 妫�娴嬭矾寰勫啿绐侊紙澧炲己鐗堬紝鏀寔CTU鐗╃悊鍙傛暟锛�
+ * 妫�娴嬭矾寰勫啿绐�
*
* @param plannedPaths 瑙勫垝璺緞鍒楄〃
* @return 鍐茬獊鍒楄〃
@@ -68,7 +68,7 @@
return conflicts;
}
- System.out.println("寮�濮嬪寮虹増纰版挒妫�娴嬶紝璺緞鏁伴噺: " + plannedPaths.size());
+ System.out.println("寮�濮嬬鎾炴娴嬶紝璺緞鏁伴噺: " + plannedPaths.size());
// 鏋勫缓楂樼簿搴︽椂绌鸿〃
Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
@@ -83,7 +83,7 @@
conflicts.addAll(edgeConflicts);
System.out.println("杈瑰啿绐佹暟閲�: " + edgeConflicts.size());
- // 妫�娴嬭窡闅忓啿绐侊紙CTU璺濈杩囪繎锛岃�冭檻鐗╃悊鍙傛暟锛�
+ // 妫�娴嬭窡闅忓啿绐�
List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
conflicts.addAll(followingConflicts);
System.out.println("璺熼殢鍐茬獊鏁伴噺: " + followingConflicts.size());
@@ -93,15 +93,15 @@
conflicts.addAll(physicalConflicts);
System.out.println("鐗╃悊灏哄鍐茬獊鏁伴噺: " + physicalConflicts.size());
- System.out.println("澧炲己鐗堢鎾炴娴嬪畬鎴愶紝鎬诲啿绐佹暟閲�: " + conflicts.size());
+ System.out.println("纰版挒妫�娴嬪畬鎴愶紝鎬诲啿绐佹暟閲�: " + conflicts.size());
return conflicts;
}
/**
- * 鏋勫缓澧炲己鐨勬椂绌鸿〃锛堥珮绮惧害鏃堕棿鍜岀墿鐞嗗弬鏁帮級
+ * 鏋勫缓鏃剁┖琛�
*
* @param plannedPaths 瑙勫垝璺緞鍒楄〃
- * @return 澧炲己鏃剁┖琛�
+ * @return 鏃剁┖琛�
*/
private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
@@ -114,24 +114,24 @@
continue;
}
- // 鑾峰彇CTU鐨勭墿鐞嗛厤缃紙杩欓噷闇�瑕佷粠鏌愪釜鍦版柟鑾峰彇锛屽彲鑳介渶瑕佹墿灞昉lannedPath绫伙級
+ // 鑾峰彇CTU鐨勭墿鐞嗛厤缃�
CTUPhysicalConfig physicalConfig = getPhysicalConfigForCTU(agvId);
- long currentTime = System.currentTimeMillis(); // 鍩哄噯鏃堕棿
- for (int i = 0; i < codeList.size(); i++) {
- PathCode pathCode = codeList.get(i);
+ for (PathCode pathCode : codeList) {
String position = pathCode.getCode();
+ Long arrivalTime = pathCode.getArrivalTime();
+ Long departureTime = pathCode.getDepartureTime();
+
+ // 濡傛灉鏃堕棿鎴充负绌猴紝璺宠繃
+ if (arrivalTime == null || departureTime == null) {
+ System.out.println("璀﹀憡: AGV " + agvId + " 鍦ㄤ綅缃� " + position + " 鐨勬椂闂存埑涓虹┖锛岃烦杩�");
+ continue;
+ }
int[] coordinates = JsonUtils.getCoordinate(position, pathMapping);
if (coordinates != null) {
- // 璁＄畻绮剧‘鐨勫埌杈炬椂闂�
- long arrivalTime = calculateArrivalTime(currentTime, i, codeList, physicalConfig);
-
- // 璁＄畻鍋滅暀鏃堕棿
- long departureTime = calculateDepartureTime(arrivalTime, pathCode, physicalConfig);
-
- // 鍒涘缓鏃堕棿娈靛唴鐨勫涓椂闂寸偣
+ // 鍒涘缓鏃堕棿娈靛唴鐨勫涓椂闂寸偣锛堜娇鐢ㄥ疄闄呮椂闂存埑锛�
for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
@@ -153,23 +153,13 @@
* @return 鐗╃悊閰嶇疆
*/
private CTUPhysicalConfig getPhysicalConfigForCTU(String agvId) {
- // 杩欓噷搴旇浠庨厤缃鐞嗗櫒鎴栨暟鎹簱鑾峰彇瀹為檯鐨凜TU鐗╃悊鍙傛暟
- // 鏆傛椂杩斿洖榛樿閰嶇疆
+ // 浠嶢GV鐘舵�佷腑鑾峰彇
CTUPhysicalConfig config = new CTUPhysicalConfig();
- config.setMaxSpeed(2.0);
- config.setNormalSpeed(1.5);
- config.setMaxAcceleration(1.0);
- config.setMaxDeceleration(1.5);
- config.setTurnTime90(2.0);
- config.setTurnTime180(4.0);
- config.setMinSafetyDistance(defaultMinDistance);
- config.setMinFollowingDistance(2.0);
- config.setCtuLength(1.2);
- config.setCtuWidth(0.8);
- config.setStartupTime(1.0);
- config.setStopTime(0.5);
- config.setStandardPointDistance(1.0);
- return config;
+ // 鏍囧噯閰嶇疆
+ // maxSpeed=1.0, normalSpeed=1.0, maxAcceleration=0.4, maxDeceleration=0.4
+ // turnTime90=4.0, turnTime180=8.0, minSafetyDistance=3.0, minFollowingDistance=3.0
+ // ctuLength=1.5, ctuWidth=1.0, startupTime=1.0, stopTime=1.5, standardPointDistance=1.0
+ return config; // 浣跨敤榛樿鏋勯�犲嚱鏁扮殑鏍囧噯閰嶇疆
}
/**
@@ -251,8 +241,7 @@
return 0.0;
}
- // 绠�鍖栫殑鏂瑰悜杞崲璁＄畻
- // 瀹為檯瀹炵幇涓渶瑕佹牴鎹叿浣撶殑鏂瑰悜缂栫爜璁＄畻瑙掑害宸�
+ // 鏂瑰悜杞崲璁＄畻
int angleDiff = Math.abs(Integer.parseInt(toDirection) - Integer.parseInt(fromDirection));
angleDiff = Math.min(angleDiff, 4 - angleDiff); // 鑰冭檻鐜舰
@@ -267,7 +256,7 @@
}
/**
- * 妫�娴嬪寮虹殑椤剁偣鍐茬獊
+ * 妫�娴嬮《鐐瑰啿绐�
*
* @param spaceTimeTable 鏃剁┖琛�
* @return 椤剁偣鍐茬獊鍒楄〃
@@ -322,7 +311,7 @@
}
/**
- * 妫�娴嬪寮虹殑杈瑰啿绐�
+ * 妫�娴嬭竟鍐茬獊
*
* @param spaceTimeTable 鏃剁┖琛�
* @return 杈瑰啿绐佸垪琛�
@@ -415,7 +404,7 @@
}
/**
- * 妫�娴嬪寮虹殑璺熼殢鍐茬獊锛堣�冭檻CTU鐗╃悊鍙傛暟锛�
+ * 妫�娴嬭窡闅忓啿绐�
*
* @param spaceTimeTable 鏃剁┖琛�
* @return 璺熼殢鍐茬獊鍒楄〃
@@ -450,6 +439,37 @@
node1.agvId, node2.agvId, timePoint / 1000, distance, minSafeDistance)
);
conflicts.add(conflict);
+ }
+
+ // 妫�鏌ュ悓涓�浣嶇疆鎴栫浉閭讳綅缃殑鏃堕棿闂撮殧鏄惁瓒冲
+ // 濡傛灉涓や釜AGV鍦ㄥ悓涓�浣嶇疆鎴栫浉閭讳綅缃紝瑕佹眰鏈�灏忔椂闂撮棿闅�
+ if (distance <= minSafeDistance) {
+ double normalSpeed = Math.min(
+ node1.physicalConfig.getNormalSpeed(),
+ node2.physicalConfig.getNormalSpeed()
+ );
+ // 鏈�灏忔椂闂撮棿闅� = 瀹夊叏璺濈/閫熷害 + 棰濆瀹夊叏缂撳啿(10绉�)
+ // 鎬绘椂闂撮棿闅旂害12~13绉掞紝鐢ㄤ簬闃叉璁＄畻璇樊
+ long minTimeGap = (long) ((minSafeDistance / normalSpeed + 10.0) * 1000);
+
+ // 妫�鏌ユ椂闂撮棿闅�
+ long timeGap = Math.abs(node1.arrivalTime - node2.arrivalTime);
+ if (timeGap < minTimeGap && timeRangeOverlap(
+ node1.arrivalTime, node1.departureTime,
+ node2.arrivalTime, node2.departureTime)) {
+ Conflict conflict = new Conflict(
+ "time_gap_insufficient",
+ node1.agvId,
+ node2.agvId,
+ (int) (timePoint / 1000),
+ node1.position,
+ node2.position,
+ String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s/%s 鏃堕棿闂撮殧涓嶈冻 (%.2f绉� < %.2f绉�)",
+ node1.agvId, node2.agvId, node1.position, node2.position,
+ timeGap / 1000.0, minTimeGap / 1000.0)
+ );
+ conflicts.add(conflict);
+ }
}
}
}
@@ -544,7 +564,7 @@
}
/**
- * 澧炲己鐨勬椂绌鸿妭鐐瑰唴閮ㄧ被
+ * 鏃剁┖鑺傜偣鍐呴儴绫�
*/
private static class EnhancedSpaceTimeNode {
final String agvId;
--
Gitblit v1.9.1