From 7dd762215b373851ed313b46bad08cc973816665 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 12 十一月 2025 12:26:23 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/CollisionDetector.java | 193 +++++++++++++++++++++++++++++++++++-------------
1 files changed, 141 insertions(+), 52 deletions(-)
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 79be4b1..1b6e5fe 100644
--- a/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
+++ b/algo-zkd/src/main/java/com/algo/service/CollisionDetector.java
@@ -70,21 +70,21 @@
System.out.println("寮�濮嬬鎾炴娴嬶紝璺緞鏁伴噺: " + plannedPaths.size());
- // 鏋勫缓楂樼簿搴︽椂绌鸿〃
- Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
+ // 鏋勫缓鏃剁┖琛�
+ Map<Long, List<SpaceTimeNode>> spaceTimeTable = buildSpaceTimeTable(plannedPaths);
// 妫�娴嬮《鐐瑰啿绐侊紙鍚屼竴鏃堕棿鍚屼竴浣嶇疆锛�
- List<Conflict> vertexConflicts = detectEnhancedVertexConflicts(spaceTimeTable);
+ List<Conflict> vertexConflicts = detectVertexConflicts(spaceTimeTable);
conflicts.addAll(vertexConflicts);
System.out.println("椤剁偣鍐茬獊鏁伴噺: " + vertexConflicts.size());
// 妫�娴嬭竟鍐茬獊锛圕TU浜ゆ崲浣嶇疆锛�
- List<Conflict> edgeConflicts = detectEnhancedEdgeConflicts(spaceTimeTable);
+ List<Conflict> edgeConflicts = detectEdgeConflicts(spaceTimeTable);
conflicts.addAll(edgeConflicts);
System.out.println("杈瑰啿绐佹暟閲�: " + edgeConflicts.size());
// 妫�娴嬭窡闅忓啿绐�
- List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
+ List<Conflict> followingConflicts = detectFollowingConflicts(spaceTimeTable);
conflicts.addAll(followingConflicts);
System.out.println("璺熼殢鍐茬獊鏁伴噺: " + followingConflicts.size());
@@ -103,8 +103,8 @@
* @param plannedPaths 瑙勫垝璺緞鍒楄〃
* @return 鏃剁┖琛�
*/
- private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
- Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
+ private Map<Long, List<SpaceTimeNode>> buildSpaceTimeTable(List<PlannedPath> plannedPaths) {
+ Map<Long, List<SpaceTimeNode>> spaceTimeTable = new HashMap<>();
for (PlannedPath path : plannedPaths) {
String agvId = path.getAgvId();
@@ -133,7 +133,7 @@
if (coordinates != null) {
// 鍒涘缓鏃堕棿娈靛唴鐨勫涓椂闂寸偣锛堜娇鐢ㄥ疄闄呮椂闂存埑锛�
for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
- EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
+ SpaceTimeNode node = new SpaceTimeNode(
agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
);
@@ -261,36 +261,36 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 椤剁偣鍐茬獊鍒楄〃
*/
- private List<Conflict> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectVertexConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
// 鎸変綅缃垎缁�
- Map<String, List<EnhancedSpaceTimeNode>> positionGroups = new HashMap<>();
- for (EnhancedSpaceTimeNode node : nodes) {
+ Map<String, List<SpaceTimeNode>> positionGroups = new HashMap<>();
+ for (SpaceTimeNode node : nodes) {
positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node);
}
// 妫�鏌ユ瘡涓綅缃槸鍚︽湁澶氫釜CTU
- for (Map.Entry<String, List<EnhancedSpaceTimeNode>> posEntry : positionGroups.entrySet()) {
+ for (Map.Entry<String, List<SpaceTimeNode>> posEntry : positionGroups.entrySet()) {
String position = posEntry.getKey();
- List<EnhancedSpaceTimeNode> ctuNodes = posEntry.getValue();
+ List<SpaceTimeNode> ctuNodes = posEntry.getValue();
if (ctuNodes.size() > 1) {
// 杩涗竴姝ユ鏌ユ椂闂撮噸鍙�
for (int i = 0; i < ctuNodes.size(); i++) {
for (int j = i + 1; j < ctuNodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = ctuNodes.get(i);
- EnhancedSpaceTimeNode node2 = ctuNodes.get(j);
+ SpaceTimeNode node1 = ctuNodes.get(i);
+ SpaceTimeNode node2 = ctuNodes.get(j);
// 妫�鏌ユ椂闂存鏄惁閲嶅彔
if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
node2.arrivalTime, node2.departureTime)) {
Conflict conflict = new Conflict(
- "enhanced_vertex",
+ "vertex",
node1.agvId,
node2.agvId,
(int) (timePoint / 1000), // 杞崲涓虹鐢ㄤ簬鏄剧ず
@@ -316,7 +316,7 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 杈瑰啿绐佸垪琛�
*/
- private List<Conflict> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectEdgeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
if (spaceTimeTable.isEmpty()) {
@@ -332,8 +332,8 @@
// 鍙鏌ヨ繛缁殑鏃堕棿鐐�
if (nextTime - currentTime <= timeResolution * 2) {
- List<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
- List<EnhancedSpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
+ List<SpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
+ List<SpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime));
}
@@ -351,19 +351,19 @@
* @param nextTime 涓嬩竴鏃堕棿
* @return 鍐茬獊鍒楄〃
*/
- private List<Conflict> detectPositionSwapping(List<EnhancedSpaceTimeNode> currentNodes,
- List<EnhancedSpaceTimeNode> nextNodes,
+ private List<Conflict> detectPositionSwapping(List<SpaceTimeNode> currentNodes,
+ List<SpaceTimeNode> nextNodes,
long currentTime, long nextTime) {
List<Conflict> conflicts = new ArrayList<>();
Map<String, String> currentPositions = new HashMap<>();
Map<String, String> nextPositions = new HashMap<>();
- for (EnhancedSpaceTimeNode node : currentNodes) {
+ for (SpaceTimeNode node : currentNodes) {
currentPositions.put(node.agvId, node.position);
}
- for (EnhancedSpaceTimeNode node : nextNodes) {
+ for (SpaceTimeNode node : nextNodes) {
nextPositions.put(node.agvId, node.position);
}
@@ -386,7 +386,7 @@
pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) {
Conflict conflict = new Conflict(
- "enhanced_edge",
+ "edge",
ctu1,
ctu2,
(int) (currentTime / 1000),
@@ -409,17 +409,18 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 璺熼殢鍐茬獊鍒楄〃
*/
- private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectFollowingConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ // 绗竴閮ㄥ垎锛氭鏌ュ悓涓�鏃堕棿鐐圭殑鑺傜偣
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
for (int i = 0; i < nodes.size(); i++) {
for (int j = i + 1; j < nodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = nodes.get(i);
- EnhancedSpaceTimeNode node2 = nodes.get(j);
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
double distance = calculateDistance(node1.coordinates, node2.coordinates);
double minSafeDistance = Math.max(
@@ -429,7 +430,7 @@
if (distance > 0 && distance < minSafeDistance) {
Conflict conflict = new Conflict(
- "enhanced_follow",
+ "follow",
node1.agvId,
node2.agvId,
(int) (timePoint / 1000),
@@ -441,22 +442,18 @@
conflicts.add(conflict);
}
- // 妫�鏌ュ悓涓�浣嶇疆鎴栫浉閭讳綅缃殑鏃堕棿闂撮殧鏄惁瓒冲
- // 濡傛灉涓や釜AGV鍦ㄥ悓涓�浣嶇疆鎴栫浉閭讳綅缃紝瑕佹眰鏈�灏忔椂闂撮棿闅�
- if (distance <= minSafeDistance) {
+ // 妫�鏌ュ悓涓�鏃堕棿鐐圭殑鏃堕棿鑼冨洿閲嶅彔锛堝悓鏃跺崰鐢級
+ if (distance <= minSafeDistance && timeRangeOverlap(
+ node1.arrivalTime, node1.departureTime,
+ node2.arrivalTime, node2.departureTime)) {
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)) {
+ if (timeGap < minTimeGap) {
Conflict conflict = new Conflict(
"time_gap_insufficient",
node1.agvId,
@@ -464,12 +461,104 @@
(int) (timePoint / 1000),
node1.position,
node2.position,
- String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s/%s 鏃堕棿闂撮殧涓嶈冻 (%.2f绉� < %.2f绉�)",
+ 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);
}
+ }
+ }
+ }
+ }
+
+ // 绗簩閮ㄥ垎锛氭鏌ュ悓涓�浣嶇疆杩炵画缁忚繃鐨勬椂闂撮棿闅�
+ Map<String, Map<String, SpaceTimeNode>> positionAgvMap = new HashMap<>();
+ for (List<SpaceTimeNode> nodes : spaceTimeTable.values()) {
+ for (SpaceTimeNode node : nodes) {
+ positionAgvMap.computeIfAbsent(node.position, k -> new HashMap<>())
+ .compute(node.agvId, (k, existing) -> {
+ if (existing == null) {
+ return node;
+ } else {
+ // 鍚堝苟鏃堕棿鑼冨洿
+ long minArrival = Math.min(existing.arrivalTime, node.arrivalTime);
+ long maxDeparture = Math.max(existing.departureTime, node.departureTime);
+ return new SpaceTimeNode(
+ node.agvId, node.position, node.coordinates,
+ minArrival, minArrival, maxDeparture, node.physicalConfig
+ );
+ }
+ });
+ }
+ }
+
+ // 瀵逛簬姣忎釜浣嶇疆锛屾鏌ユ墍鏈夌粡杩囪浣嶇疆鐨凙GV鐨勬椂闂撮棿闅�
+ Set<String> processedPairs = new HashSet<>();
+ for (Map.Entry<String, Map<String, SpaceTimeNode>> positionEntry : positionAgvMap.entrySet()) {
+ String position = positionEntry.getKey();
+ Map<String, SpaceTimeNode> agvNodes = positionEntry.getValue();
+
+ List<SpaceTimeNode> nodes = new ArrayList<>(agvNodes.values());
+ nodes.sort((n1, n2) -> Long.compare(n1.arrivalTime, n2.arrivalTime));
+
+ for (int i = 0; i < nodes.size(); i++) {
+ for (int j = i + 1; j < nodes.size(); j++) {
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
+
+ if (node1.agvId.equals(node2.agvId)) {
+ continue;
+ }
+
+ String pairKey1 = node1.agvId + "_" + node2.agvId + "_" + position;
+ String pairKey2 = node2.agvId + "_" + node1.agvId + "_" + position;
+ if (processedPairs.contains(pairKey1) || processedPairs.contains(pairKey2)) {
+ continue;
+ }
+
+ // 璁＄畻鏃堕棿闂撮殧锛歯ode1绂诲紑鍚庯紝node2鍒拌揪鐨勬椂闂撮棿闅�
+ long timeGap;
+ if (node1.departureTime <= node2.arrivalTime) {
+ timeGap = node2.arrivalTime - node1.departureTime;
+ } else if (node2.departureTime <= node1.arrivalTime) {
+ timeGap = node1.arrivalTime - node2.departureTime;
+ } else {
+ continue;
+ }
+
+ // 璁＄畻鏈�灏忓畨鍏ㄦ椂闂撮棿闅�
+ double minSafeDistance = Math.max(
+ node1.physicalConfig.getMinSafetyDistance(),
+ node2.physicalConfig.getMinSafetyDistance()
+ );
+ double normalSpeed = Math.min(
+ node1.physicalConfig.getNormalSpeed(),
+ node2.physicalConfig.getNormalSpeed()
+ );
+ // 鏈�灏忔椂闂撮棿闅� = 瀹夊叏璺濈/閫熷害 + 棰濆瀹夊叏缂撳啿(7绉�)
+ long minTimeGap = (long) ((minSafeDistance / normalSpeed + 7.0) * 1000);
+
+ // 妫�鏌ユ椂闂撮棿闅旀槸鍚﹁冻澶�
+ if (timeGap < minTimeGap) {
+ String firstAgv = node1.arrivalTime < node2.arrivalTime ? node1.agvId : node2.agvId;
+ String secondAgv = node1.arrivalTime < node2.arrivalTime ? node2.agvId : node1.agvId;
+ long secondArrival = node1.arrivalTime < node2.arrivalTime ? node2.arrivalTime : node1.arrivalTime;
+
+ Conflict conflict = new Conflict(
+ "time_gap_insufficient",
+ firstAgv,
+ secondAgv,
+ (int) (secondArrival / 1000),
+ position,
+ position,
+ String.format("CTU %s 鍜� %s 鍦ㄤ綅缃� %s 杩炵画缁忚繃鏃堕棿闂撮殧涓嶈冻 (%.2f绉� < %.2f绉�, %s绂诲紑鍚�%.2f绉�%s鍒拌揪)",
+ firstAgv, secondAgv, position,
+ timeGap / 1000.0, minTimeGap / 1000.0,
+ firstAgv, timeGap / 1000.0, secondAgv)
+ );
+ conflicts.add(conflict);
+ processedPairs.add(pairKey1);
}
}
}
@@ -484,17 +573,17 @@
* @param spaceTimeTable 鏃剁┖琛�
* @return 鐗╃悊鍐茬獊鍒楄〃
*/
- private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
+ private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
List<Conflict> conflicts = new ArrayList<>();
- for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
+ for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
long timePoint = entry.getKey();
- List<EnhancedSpaceTimeNode> nodes = entry.getValue();
+ List<SpaceTimeNode> nodes = entry.getValue();
for (int i = 0; i < nodes.size(); i++) {
for (int j = i + 1; j < nodes.size(); j++) {
- EnhancedSpaceTimeNode node1 = nodes.get(i);
- EnhancedSpaceTimeNode node2 = nodes.get(j);
+ SpaceTimeNode node1 = nodes.get(i);
+ SpaceTimeNode node2 = nodes.get(j);
if (checkPhysicalCollision(node1, node2)) {
Conflict conflict = new Conflict(
@@ -523,7 +612,7 @@
* @param node2 CTU鑺傜偣2
* @return 鏄惁纰版挒
*/
- private boolean checkPhysicalCollision(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode node2) {
+ private boolean checkPhysicalCollision(SpaceTimeNode node1, SpaceTimeNode node2) {
double distance = calculateDistance(node1.coordinates, node2.coordinates);
// 鑰冭檻CTU鐨勭墿鐞嗗昂瀵�
@@ -566,7 +655,7 @@
/**
* 鏃剁┖鑺傜偣鍐呴儴绫�
*/
- private static class EnhancedSpaceTimeNode {
+ private static class SpaceTimeNode {
final String agvId;
final String position;
final int[] coordinates;
@@ -575,9 +664,9 @@
final long departureTime;
final CTUPhysicalConfig physicalConfig;
- public EnhancedSpaceTimeNode(String agvId, String position, int[] coordinates,
- long timePoint, long arrivalTime, long departureTime,
- CTUPhysicalConfig physicalConfig) {
+ public SpaceTimeNode(String agvId, String position, int[] coordinates,
+ long timePoint, long arrivalTime, long departureTime,
+ CTUPhysicalConfig physicalConfig) {
this.agvId = agvId;
this.position = position;
this.coordinates = coordinates;
--
Gitblit v1.9.1