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/AStarPathPlanner.java | 136 ++++++++++++++++++++++++++++++++++++++++----
1 files changed, 122 insertions(+), 14 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 cdc4431..26c28ef 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -44,7 +44,7 @@
/**
* 鏈�澶ф悳绱㈡繁搴�
*/
- private final int maxSearchDepth = 15000;
+ private final int maxSearchDepth = 50000;
/**
* 璺濈缂撳瓨
@@ -158,7 +158,8 @@
if (fastPath != null) {
return fastPath;
}
- return planSpaceTimePath(startCode, endCode, constraints, null, null);
+ long defaultStartTime = System.currentTimeMillis();
+ return planSpaceTimePath(startCode, endCode, constraints, null, null, defaultStartTime);
}
@Override
@@ -174,12 +175,14 @@
* @param constraints 闈欐�佺害鏉熸潯浠�
* @param spaceTimeOccupancyMap 鏃剁┖鍗犵敤琛�
* @param physicalConfig CTU鐗╃悊閰嶇疆
+ * @param startTimeMs 璧峰鏃堕棿锛堟绉掞級
* @return 瑙勫垝鐨勮矾寰�
*/
public PlannedPath planSpaceTimePath(String startCode, String endCode,
List<double[]> constraints,
Map<String, String> spaceTimeOccupancyMap,
- CTUPhysicalConfig physicalConfig) {
+ CTUPhysicalConfig physicalConfig,
+ long startTimeMs) {
// 楠岃瘉杈撳叆
if (!isValidPathPoint(startCode) || !isValidPathPoint(endCode)) {
System.out.println("鏃犳晥鐨勮矾寰勭偣: " + startCode + " 鎴� " + endCode);
@@ -212,7 +215,7 @@
// 鏃剁┖A*绠楁硶瀹炵幇
PlannedPath result = spaceTimeAStarSearch(
startCode, endCode, startCoord, endCoord,
- constraints, spaceTimeOccupancyMap, physicalConfig
+ constraints, spaceTimeOccupancyMap, physicalConfig, startTimeMs
);
if (result != null) {
@@ -234,13 +237,15 @@
* @param constraints 绾︽潫鏉′欢
* @param occupancyMap 鏃剁┖鍗犵敤琛�
* @param physicalConfig 鐗╃悊閰嶇疆
+ * @param startTimeMs 璧峰鏃堕棿锛堟绉掞級
* @return 瑙勫垝鐨勮矾寰�
*/
private PlannedPath spaceTimeAStarSearch(String startCode, String endCode,
int[] startCoord, int[] endCoord,
List<double[]> constraints,
Map<String, String> occupancyMap,
- CTUPhysicalConfig physicalConfig) {
+ CTUPhysicalConfig physicalConfig,
+ long startTimeMs) {
// 浣跨敤浼樺厛闃熷垪瀹炵幇寮�鏀惧垪琛�
PriorityQueue<SpaceTimeAStarNode> openSet = new PriorityQueue<>(
@@ -250,8 +255,7 @@
Map<String, Double> gScores = new HashMap<>();
Map<String, SpaceTimeAStarNode> cameFrom = new HashMap<>();
- // 璧峰鏃堕棿
- long startTime = System.currentTimeMillis();
+ long startTime = startTimeMs;
// 鍒濆鍖栬捣濮嬭妭鐐�
SpaceTimeAStarNode startNode = new SpaceTimeAStarNode(
@@ -363,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);
@@ -388,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;
}
/**
@@ -562,17 +613,74 @@
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;
}
/**
+ * 浼樺寲璺緞
+ */
+ 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);
+
+ 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())) {
+ 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 浣嶇疆浠ｇ爜
--
Gitblit v1.9.1