From f6df128b575bdb9d0f2512b5fa5a126edfce2799 Mon Sep 17 00:00:00 2001
From: zhang <zc857179121@qq.com>
Date: 星期二, 05 五月 2026 09:38:27 +0800
Subject: [PATCH] 算法集成
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 519 +++++++++++++++++++++++++++++++++++++++++++++-----------
1 files changed, 413 insertions(+), 106 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
index 3a310e6..89fa1f9 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
@@ -1,26 +1,26 @@
package com.zy.acs.manager.core.service.astart;
+import com.fasterxml.jackson.databind.ObjectMapper;
import com.zy.acs.common.utils.RedisSupport;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.common.utils.MapDataUtils;
-import com.zy.acs.manager.core.service.AgvAreaDispatcher;
import com.zy.acs.manager.core.service.LaneBuilder;
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
import com.zy.acs.manager.core.utils.RouteGenerator;
import com.zy.acs.manager.manager.entity.Segment;
-import com.zy.acs.manager.manager.service.AgvService;
import com.zy.acs.manager.manager.service.JamService;
import com.zy.acs.manager.system.service.ConfigService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
+import org.springframework.core.io.ClassPathResource;
import org.springframework.stereotype.Service;
+import javax.annotation.PostConstruct;
+import java.io.InputStream;
import java.util.*;
-/**
- * Created by vincent on 6/12/2024
- */
+
@Slf4j
@Service
public class AStarNavigateService {
@@ -28,10 +28,9 @@
private final RedisSupport redis = RedisSupport.defaultRedisSupport;
public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.TRUE;
-
public static final int WEIGHT_CALC_FACTOR = 1;
- // right left up down
+ // 鍥涙柟鍚�
private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
@Autowired
@@ -39,33 +38,154 @@
@Autowired
private JamService jamService;
@Autowired
- private LaneBuilder laneBuilder;
+ private LaneBuilder laneService;
@Autowired
private ConfigService configService;
- @Autowired
- private AgvAreaDispatcher agvAreaDispatcher;
- @Autowired
- private AgvService agvService;
- public synchronized AStarNavigateNode execute(String agvNo, AStarNavigateNode start, AStarNavigateNode end
- , Boolean lock, List<String> blackList, Segment segment) {
+ /**
+ * 鑺傜偣鍧愭爣缂撳瓨锛歝odeData -> Coordinate
+ * 浠巎son鍔犺浇
+ */
+ private Map<String, NodeCoordinate> nodeCoordinateMap = new HashMap<>();
+
+ public static class NodeCoordinate {
+ private double x;
+ private double y;
+
+ public NodeCoordinate() {}
+
+ public NodeCoordinate(double x, double y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ public double getX() { return x; }
+ public void setX(double x) { this.x = x; }
+ public double getY() { return y; }
+ public void setY(double y) { this.y = y; }
+
+ /**
+ * 璁＄畻涓庡彟涓�涓妭鐐圭殑璺濈锛堢背锛�
+ */
+ public double distanceTo(NodeCoordinate other) {
+ if (other == null) {
+ return 0.0;
+ }
+ double dx = (this.x - other.x) / 1000.0; // 姣背杞背
+ double dy = (this.y - other.y) / 1000.0;
+ return Math.sqrt(dx * dx + dy * dy);
+ }
+ }
+
+ private static class MotionParams {
+ final double vMax; // 鏈�澶ч�熷害 (m/s)
+ final double aAccel; // 鍔犻�熷害 (m/s^2)
+ final double aDecel; // 鍑忛�熷害 (m/s^2)
+ final double controlPeriod; // 鎺у埗鍛ㄦ湡 (s)
+ final double fallbackGridLength; // 鍚庡缃戞牸璺濈 (m)
+
+ MotionParams(double vMax, double aAccel, double aDecel, double controlPeriod, double fallbackGridLength) {
+ this.vMax = vMax;
+ this.aAccel = aAccel;
+ this.aDecel = aDecel;
+ this.controlPeriod = controlPeriod;
+ this.fallbackGridLength = fallbackGridLength;
+ }
+ }
+
+ @PostConstruct
+ public void loadNodeCoordinates() {
+ try {
+ // 鏂囦欢璺緞
+ String coordinateFile = getStringConfig("nodeCoordinateFile", "man_code.json");
+
+ ClassPathResource resource = new ClassPathResource(coordinateFile);
+ InputStream inputStream = resource.getInputStream();
+
+ ObjectMapper objectMapper = new ObjectMapper();
+ Map<String, Object> jsonData = objectMapper.readValue(inputStream, Map.class);
+
+ if (jsonData.containsKey("path_id_to_coordinates")) {
+ Map<String, List<Map<String, Object>>> pathData =
+ (Map<String, List<Map<String, Object>>>) jsonData.get("path_id_to_coordinates");
+
+ for (Map.Entry<String, List<Map<String, Object>>> entry : pathData.entrySet()) {
+ String nodeCode = entry.getKey();
+ List<Map<String, Object>> coordinates = entry.getValue();
+
+ if (coordinates != null && !coordinates.isEmpty()) {
+ Map<String, Object> coord = coordinates.get(0);
+ double x = ((Number) coord.get("x")).doubleValue();
+ double y = ((Number) coord.get("y")).doubleValue();
+ nodeCoordinateMap.put(nodeCode, new NodeCoordinate(x, y));
+ }
+ }
+
+ log.info("success load node coordinates, total {} nodes", nodeCoordinateMap.size());
+ } else {
+ log.warn("path_id_to_coordinates not found");
+ }
+
+ } catch (Exception e) {
+ log.error("load node coordinates failed", e);
+ }
+ }
+
+ private String getStringConfig(String key, String defaultVal) {
+ try {
+ String v = configService.getVal(key, String.class);
+ if (v != null && !v.isEmpty()) {
+ return v;
+ }
+ } catch (Exception e) {
+ log.debug("config {} not available, use default value {}", key, defaultVal);
+ }
+ return defaultVal;
+ }
+
+ public synchronized AStarNavigateNode execute(String agvNo,
+ AStarNavigateNode start,
+ AStarNavigateNode end,
+ Boolean lock,
+ List<String> blackList,
+ Segment segment) {
if (start.getX() == end.getX() && start.getY() == end.getY()) {
return end;
}
-
- // scope code area: 4ms
- Long agvId = agvService.getAgvId(agvNo);
- Boolean withinArea = agvAreaDispatcher.isAgvExistsInAnyArea(agvId);
- List<String> scopeCodeList = new ArrayList<>();
- if (withinArea) {
- scopeCodeList = agvAreaDispatcher.getCodesByAgvId(agvId);
- if (!Cools.isEmpty(scopeCodeList) && !scopeCodeList.contains(start.getCodeData())) {
- withinArea = false;
- }
- }
-
Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
+ // ===== 1. 鍩轰簬鐗╃悊鍙傛暟璁＄畻鏃堕棿姝ョ浉鍏崇殑鍙傛暟 =====
+ // 缃戞牸涓績璺濓紙绫筹級- 榛樿鍚庡鍊硷紝褰撴棤娉曡幏鍙栧疄闄呭潗鏍囨椂浣跨敤
+ double gridLength = getDoubleConfig("navigateGridLength", 0.5);
+ // 鎺у埗鍛ㄦ湡锛堢/鏃堕棿姝ワ級
+ double controlPeriod = getDoubleConfig("navigateControlPeriod", 0.5);
+
+ // AGV 鐗╃悊鐗规��
+ double vMax = getDoubleConfig("agvMaxSpeed", 1.0); // m/s
+ double aAccel = getDoubleConfig("agvAccel", 0.4); // m/s^2
+ double aDecel = getDoubleConfig("agvDecel", 0.4); // m/s^2
+ double tTurn = getDoubleConfig("agvTurn90TimeSec", 4.0); // s
+
+ // 瀹夊叏鏃堕棿闂撮殧锛堢锛夆啋 鏃堕棿姝�
+ double safetyGapTime = getDoubleConfig("navigateSafetyGapTimeSec", 3.0);
+
+ // 杞� 90掳 鐨勬椂闂达紙绉掞級鐩存帴鐢ㄩ厤缃�
+ double turnTimeSec = tTurn;
+ // 杞集鏃堕棿姝ユ垚鏈�
+ final int turnStepCost = Math.max(1, (int) Math.ceil(turnTimeSec / controlPeriod));
+ final int safetyGapStep = Math.max(1, (int) Math.ceil(safetyGapTime / controlPeriod));
+
+ // 杩愬姩瀛﹀弬鏁版墦鍖�
+ final MotionParams motionParams = new MotionParams(vMax, aAccel, aDecel, controlPeriod, gridLength);
+
+ if (log.isDebugEnabled()) {
+ log.debug("A* phys params init: gridLength(fallback)={}, controlPeriod={}, vMax={}, aAccel={}, aDecel={}, " +
+ "tTurn={}, turnStepCost={}, safetyGapStep={}, loaded node coordinates={}",
+ gridLength, controlPeriod, vMax, aAccel, aDecel,
+ tTurn, turnStepCost, safetyGapStep, nodeCoordinateMap.size());
+ }
+
+ // ===== 2. A* 鍒濆鍖� =====
PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
Set<AStarNavigateNode> existNodes = new HashSet<>();
Map<String, Integer> bestGMap = new HashMap<>();
@@ -73,6 +193,8 @@
start.setG(0);
start.setH(calcNodeCost(start, end));
start.setF(start.getG() + start.getH());
+ // 鏃堕棿姝ヤ粠 0 寮�濮�
+ start.setStep(0);
String startKey = start.getX() + "_" + start.getY();
bestGMap.put(startKey, start.getG());
@@ -84,24 +206,30 @@
DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);
+ // ===== 3. 鍩轰簬 dynamicMatrix 浼扮畻鍚� AGV 褰撳墠杩涘害 =====
+ Map<String, Integer> currentSerialMap = buildCurrentSerialMap(dynamicMatrix, agvNo);
+
+ long getNeighborNodesTime = 0;
+ int getNeighborNodesCount = 0;
+
while (!openQueue.isEmpty()) {
- // 鍙栦紭鍏堥槦鍒楅《閮ㄥ厓绱犲苟涓旀妸杩欎釜鍏冪礌浠嶰pen琛ㄤ腑鍒犻櫎锛屽彇F鍊兼渶灏忕殑鑺傜偣
+ // 鍙� F 鍊兼渶灏忕殑鑺傜偣
AStarNavigateNode currentNode = openQueue.poll();
- // 缁堢偣
+ // 宸插埌缁堢偣
if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
-// System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
return currentNode;
}
+
long currentTime = System.currentTimeMillis();
- List<AStarNavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
+ List<AStarNavigateNode> neighbourNodes =
+ this.getNeighborNodes(currentNode, mapMatrix, codeMatrix, existNodes,
+ motionParams, turnStepCost);
+ getNeighborNodesTime += System.currentTimeMillis() - currentTime;
+ getNeighborNodesCount++;
+
for (AStarNavigateNode node : neighbourNodes) {
node.setCodeData(codeMatrix[node.getX()][node.getY()]);
-
- if (withinArea) {
- assert !Cools.isEmpty(scopeCodeList);
- if (!scopeCodeList.contains(node.getCodeData())) { continue; }
- }
boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();
@@ -110,27 +238,50 @@
if (!Cools.isEmpty(blackList) && blackList.contains(node.getCodeData())) {
continue;
}
- // 鐗规畩鎯呭喌锛屽綋blackList鏈変笖鍙湁涓�涓厓绱犱笖涓簊tartNode鏃�
- // 璇存槑blackList宸茬粡鐭ラ亾褰撳墠瀵艰埅璧峰鐐瑰拰鐩爣鐐逛负鐩搁偦鑺傜偣
- // 浣嗘槸褰撳墠blackList鐨勪换鍔℃槸涓嶈绯荤粺璧扮浉閭荤殑鏈�鐭矾寰勶紝鎵�浠ユ墠浼氭湁涓嬮潰鐨勫垽鏂拰continue
+ // 鐗规畩锛歜lackList 鍙湁 start 鑺傜偣锛岄伩鍏嶈蛋鐩存帴鐩搁偦鏈�鐭矾
if (blackList.size() == 1 && blackList.get(0).equals(start.getCodeData())) {
if (isEndNode && currentNode.getCodeData().equals(start.getCodeData())) {
continue;
}
}
- // 鑺傜偣琚崰鐢�
+ // ===== 3.1 鍔ㄦ�侀伩闅滐細鍩轰簬鏃堕棿姝ョ殑鏃剁┖鍐茬獊鍒ゆ柇 =====
DynamicNode dynamicNode = dynamicMatrix[node.getX()][node.getY()];
String vehicle = dynamicNode.getVehicle();
assert !vehicle.equals(DynamicNodeType.BLOCK.val);
+
if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
if (!vehicle.equals(agvNo)) {
- if (lock) {
- continue;
+ boolean conflict = true;
+
+ Integer otherCurrentSerial = currentSerialMap.get(vehicle);
+ int otherSerial = dynamicNode.getSerial();
+
+ if (otherCurrentSerial != null) {
+ int remainStep = otherSerial - otherCurrentSerial;
+ if (remainStep < 0) {
+ remainStep = 0;
+ }
+ // 鎴戜粠璧风偣鍒拌鑺傜偣闇�瑕佺殑鏃堕棿姝�
+ int myStep = node.getStep();
+
+ // 濡傛灉鎴戝埌鏃跺鏂瑰凡缁忕寮�锛屽苟涓旈鐣欏畨鍏ㄩ棿闅旓紝鍒欏厑璁稿叡浜鑺傜偣
+ if (myStep > remainStep + safetyGapStep) {
+ conflict = false;
+ }
}
- // 濡傛灉瀛樺湪杞﹁締锛屽垯澧炲姞鏉冮噸 2 鎴栬�� 3锛屽洜涓烘嫄鐐逛細澧炲姞鏉冮噸 1
- // vehicle宸茬粡涓哄綋鍓峴egment鍋氳繃浜嗛伩璁╋紝涓旈伩璁╀换鍔″凡瀹屾垚锛屽垯鏉冮噸鍊煎鍔�
+ if (conflict) {
+ if (lock) {
+ // 閿佸畾妯″紡涓嬶紝鐩存帴瑙嗕负涓嶅彲璧�
+ continue;
+ } else {
+ // 闈為攣妯″紡锛氬厑璁搁�氳繃锛屼絾澧炲姞浠ｄ环
+ weight += (WEIGHT_CALC_FACTOR * 2);
+ }
+ }
+
+ // 鎷ュ牭鍘嗗彶鍔犳潈锛堜笌鏃堕棿鍐茬獊鍒ゆ柇姝ｄ氦锛�
if (null != segment) {
if (!Cools.isEmpty(jamService.getJamFromSegmentByAvo(segment, vehicle))) {
weight += (WEIGHT_CALC_FACTOR * 3);
@@ -141,22 +292,21 @@
}
}
- // 閬块殰娉�
+ // ===== 3.2 閬块殰娉� waveMatrix 鍒ゆ柇 =====
String waveNode = waveMatrix[node.getX()][node.getY()];
assert !waveNode.equals(WaveNodeType.DISABLE.val);
if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
if (!Cools.isEmpty(otherWaveList)) {
-
if (lock) {
continue;
}
}
}
- // 鍗曞贩閬撹溅杈嗗杞芥暟閲�
- List<int[]> laneCodeIdxList = laneBuilder.getLaneCodeIdxList(node.getCodeData());
+ // ===== 3.3 鍗曞贩閬撳閲忔帶鍒� =====
+ List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
if (!Cools.isEmpty(laneCodeIdxList)) {
Set<String> lanVehicleSet = new HashSet<>();
@@ -175,17 +325,17 @@
}
}
+ // ===== 3.4 杞集棰濆浠ｄ环锛堝湪姝ラ暱鍩虹涓婂姞鏉冿級 =====
if (OPEN_TURN_COST_WEIGHT) {
if (this.isTurning(currentNode, node)) {
weight += WEIGHT_CALC_FACTOR;
node.setTurnCount(currentNode.getTurnCount() + 1);
} else {
- // 鏂瑰悜娌″彉
node.setTurnCount(currentNode.getTurnCount());
}
}
- //杩涜璁＄畻瀵� G, F, H 绛夊��
+ // ===== 3.5 璁＄畻 G/F/H 绛� =====
node.setWeight(weight);
node.setLastDistance(calcNodeCost(currentNode, node));
node.initNode(currentNode, end);
@@ -196,45 +346,52 @@
Integer recordedG = bestGMap.get(key);
if (recordedG == null || node.getG() <= recordedG) {
bestGMap.put(key, node.getG());
-
openQueue.add(node);
}
-
-// openQueue.add(node);
-// existNodes.add(node);
}
}
-// System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
return null;
}
- // 鑾峰彇鍥涘懆鑺傜偣
- private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
+ /**
+ * 鑾峰彇褰撳墠鑺傜偣鐨勫洓鍛ㄩ偦灞呰妭鐐�
+ *
+ * @param currentNode 褰撳墠鑺傜偣
+ * @param mapMatrix 鍦板浘鐭╅樀
+ * @param codeMatrix 鑺傜偣缂栫爜鐭╅樀
+ * @param existNodes 宸插瓨鍦ㄨ妭鐐归泦鍚�
+ * @param motionParams AGV杩愬姩瀛﹀弬鏁�
+ * @param turnStepCost 杞集鏃堕棿姝ユ垚鏈�
+ * @return 閭诲眳鑺傜偣鍒楄〃
+ */
+ private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode,
+ int[][] mapMatrix,
+ String[][] codeMatrix,
+ Set<AStarNavigateNode> existNodes,
+ MotionParams motionParams,
+ int turnStepCost) {
int x = currentNode.getX();
int y = currentNode.getY();
AStarNavigateNode parent = currentNode.getParent();
-// List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
List<AStarNavigateNode> neighbourNodes = new ArrayList<>();
-
List<AStarNavigateNode> possibleNodes = new ArrayList<>();
+
+ // 閬嶅巻鍥涗釜鏂瑰悜锛氬彸銆佸乏銆佷笂銆佷笅
for (int[] d: DIRECTIONS) {
int nx = x + d[0];
int ny = y + d[1];
- // 濡傛灉鐖惰妭鐐逛笉涓虹┖锛屽苟涓� (nx,ny) 绛変簬鐖惰妭鐐瑰潗鏍囷紝鍒欒烦杩�
+ // 涓嶅洖鍒扮埗鑺傜偣
if (parent != null && nx == parent.getX() && ny == parent.getY()) {
continue;
}
possibleNodes.add(new AStarNavigateNode(nx, ny));
}
-// possibleNodes.parallelStream()
-// .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
-// .filter(Objects::nonNull)
-// .forEach(neighbourNodes::add);
-
+ // 鎵╁睍姣忎釜鍙兘鐨勯偦灞呰妭鐐�
for (AStarNavigateNode pn : possibleNodes) {
- AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, existNodes, null, null);
+ AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, codeMatrix,
+ existNodes, null, null, motionParams, turnStepCost);
if (next != null) {
neighbourNodes.add(next);
}
@@ -243,10 +400,32 @@
return neighbourNodes;
}
- private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode, AStarNavigateNode extendNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes, Integer dx, Integer dy) {
+ /**
+ * 鎵╁睍閭诲眳鑺傜偣锛岃绠楀熀浜庡疄闄呰窛绂荤殑鏃堕棿姝ユ垚鏈�
+ *
+ * @param currentNode 褰撳墠鑺傜偣
+ * @param extendNode 寰呮墿灞曡妭鐐�
+ * @param mapMatrix 鍦板浘鐭╅樀
+ * @param codeMatrix 鑺傜偣缂栫爜鐭╅樀
+ * @param existNodes 宸插瓨鍦ㄨ妭鐐归泦鍚�
+ * @param dx x鏂瑰悜澧為噺锛堥�掑綊璋冪敤鏃朵娇鐢級
+ * @param dy y鏂瑰悜澧為噺锛堥�掑綊璋冪敤鏃朵娇鐢級
+ * @param motionParams AGV杩愬姩瀛﹀弬鏁�
+ * @param turnStepCost 杞集鏃堕棿姝ユ垚鏈�
+ * @return 鎵╁睍鍚庣殑鑺傜偣锛岃嫢涓嶅彲杈惧垯杩斿洖null
+ */
+ private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode,
+ AStarNavigateNode extendNode,
+ int[][] mapMatrix,
+ String[][] codeMatrix,
+ Set<AStarNavigateNode> existNodes,
+ Integer dx,
+ Integer dy,
+ MotionParams motionParams,
+ int turnStepCost) {
AStarNavigateNode nextNode;
- if (null == dx || null == dy) {
+ if (dx == null || dy == null) {
dx = extendNode.getX() - currentNode.getX();
dy = extendNode.getY() - currentNode.getY();
nextNode = extendNode;
@@ -256,70 +435,198 @@
int x = nextNode.getX();
int y = nextNode.getY();
- // 鏁扮粍瓒婄晫
- if (x < 0 || x >= mapMatrix.length
- || y < 0 || y >= mapMatrix[0].length) {
+
+ // 鏁扮粍瓒婄晫妫�鏌�
+ if (x < 0 || x >= mapMatrix.length || y < 0 || y >= mapMatrix[0].length) {
return null;
}
- if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
-
- return extendNeighborNodes(currentNode, nextNode, mapMatrix, existNodes, dx, dy);
+ // 閬囧埌涓嶅彲閫氳鑺傜偣锛岀户缁部褰撳墠鏂瑰悜鎵╁睍
+ if (mapMatrix[x][y] == MapNodeType.DISABLE.val) {
+ return extendNeighborNodes(currentNode, nextNode, mapMatrix, codeMatrix,
+ existNodes, dx, dy, motionParams, turnStepCost);
}
assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
-// if (existNodes.contains(nextNode)) {
-// return null;
-// }
-
- // 鍒ゆ柇閫氳繃鎬�
- String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
+ // 鍒ゆ柇閫氶亾閫氳繃鎬�
+ String routeCdaKey = RouteGenerator.generateRouteCdaKey(
+ new int[]{currentNode.getX(), currentNode.getY()},
+ new int[]{nextNode.getX(), nextNode.getY()});
if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
return null;
}
+ // ===== 鍩轰簬瀹為檯鍧愭爣璁＄畻鑺傜偣闂磋窛绂诲拰鏃堕棿姝� =====
+ boolean turning = isTurning(currentNode, nextNode);
+ int deltaStep;
+
+ if (turning) {
+ // 杞集锛氫娇鐢ㄥ浐瀹氱殑杞集鏃堕棿姝�
+ deltaStep = turnStepCost;
+ } else {
+ // 鐩磋锛氭牴鎹疄闄呰窛绂昏绠楁椂闂存
+ String currentCode = codeMatrix[currentNode.getX()][currentNode.getY()];
+ String nextCode = codeMatrix[x][y];
+
+ // 浠庡潗鏍囨槧灏勮幏鍙栧疄闄呰窛绂�
+ double actualDistance = calculateActualDistance(currentCode, nextCode, motionParams.fallbackGridLength);
+
+ // 鍩轰簬瀹為檯璺濈鍜岃繍鍔ㄥ妯″瀷璁＄畻鎵�闇�鏃堕棿
+ double travelTimeSec = calcStraightTravelTimeSec(actualDistance,
+ motionParams.vMax, motionParams.aAccel, motionParams.aDecel);
+
+ // 杞崲涓烘椂闂存锛堣嚦灏戜负1锛�
+ deltaStep = Math.max(1, (int) Math.ceil(travelTimeSec / motionParams.controlPeriod));
+
+ if (log.isTraceEnabled()) {
+ log.trace("鑺傜偣 {} -> {} 璺濈={}m, 鏃堕棿={}s, 鏃堕棿姝�={}",
+ currentCode, nextCode, actualDistance, travelTimeSec, deltaStep);
+ }
+ }
+
+ // 璁剧疆绱鏃堕棿姝�
+ nextNode.setStep(currentNode.getStep() + deltaStep);
+
return nextNode;
}
- //------------------A*鍚彂鍑芥暟------------------//
+ /**
+ * 璁＄畻涓や釜鑺傜偣闂寸殑瀹為檯璺濈
+ * 浼樺厛浣跨敤鍧愭爣鏄犲皠璁＄畻娆у嚑閲屽緱璺濈锛岃嫢鏃犲潗鏍囨暟鎹垯浣跨敤鍚庡缃戞牸璺濈
+ *
+ * @param fromCode 璧峰鑺傜偣缂栫爜
+ * @param toCode 鐩爣鑺傜偣缂栫爜
+ * @param fallbackDistance 鍚庡璺濈锛堢背锛�
+ * @return 瀹為檯璺濈锛堢背锛�
+ */
+ private double calculateActualDistance(String fromCode, String toCode, double fallbackDistance) {
+ NodeCoordinate fromCoord = nodeCoordinateMap.get(fromCode);
+ NodeCoordinate toCoord = nodeCoordinateMap.get(toCode);
- //璁＄畻閫氳繃鐜板湪鐨勭粨鐐圭殑浣嶇疆鍜屾渶缁堢粨鐐圭殑浣嶇疆璁＄畻H鍊�(鏇煎搱椤挎硶锛氬潗鏍囧垎鍒彇宸�肩浉鍔�)
+ if (fromCoord != null && toCoord != null) {
+ return fromCoord.distanceTo(toCoord);
+ } else {
+ if (log.isTraceEnabled() && (fromCoord == null || toCoord == null)) {
+ log.trace("鑺傜偣 {} 鎴� {} 缂哄皯鍧愭爣鏁版嵁锛屼娇鐢ㄥ悗澶囪窛绂� {}m",
+ fromCode, toCode, fallbackDistance);
+ }
+ return fallbackDistance;
+ }
+ }
+
+ //------------------A* 鍚彂鍑芥暟------------------//
+
+ // 鏇煎搱椤胯窛绂�
private int calcNodeCost(AStarNavigateNode node1, AStarNavigateNode node2) {
return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
}
- // 杞集鍒ゆ柇锛氬彧鍏佽鈥滃瀭鐩存垨姘村钩鈥濊繍鍔�
+ /**
+ * 鏍规嵁褰撳墠 dynamicMatrix 棰勪及姣忚締杞︾殑鈥滃綋鍓嶆椂闂存鈥濓紙浣跨敤鏈�灏� serial 浣滀负杩戜技锛夈��
+ */
+ private Map<String, Integer> buildCurrentSerialMap(DynamicNode[][] dynamicMatrix, String selfAgvNo) {
+ Map<String, Integer> result = new HashMap<>();
+ if (dynamicMatrix == null) {
+ return result;
+ }
+ for (int i = 0; i < dynamicMatrix.length; i++) {
+ DynamicNode[] row = dynamicMatrix[i];
+ if (row == null) {
+ continue;
+ }
+ for (int j = 0; j < row.length; j++) {
+ DynamicNode node = row[j];
+ if (node == null) {
+ continue;
+ }
+ String vehicle = node.getVehicle();
+ if (vehicle == null
+ || DynamicNodeType.ACCESS.val.equals(vehicle)
+ || DynamicNodeType.BLOCK.val.equals(vehicle)
+ || vehicle.equals(selfAgvNo)) {
+ continue;
+ }
+ int serial = node.getSerial();
+ Integer recorded = result.get(vehicle);
+ if (recorded == null || serial < recorded) {
+ result.put(vehicle, serial);
+ }
+ }
+ }
+ return result;
+ }
+
+ // 杞集鍒ゆ柇锛氭槸鍚︽敼鍙樹簡鏂瑰悜
private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
- // 绗竴涓偣
if (currNode.getParent() == null) {
return false;
}
AStarNavigateNode parent = currNode.getParent();
- // 濡傛灉涓嬩竴鐐癸紙nextNode锛変笌 parent 鍦ㄥ悓涓�琛屾垨鍚屼竴鍒� => 娌℃湁杞集
- // 娉ㄦ剰锛岃繖瀹為檯涓婄瓑鍚屼簬鈥�(curr->next) 鐨勬柟鍚� == (parent->curr) 鐨勬柟鍚戔�濄��
- boolean sameRowOrCol =
- (nextNode.getX() == parent.getX())
- || (nextNode.getY() == parent.getY());
- return !sameRowOrCol;
+ // 濡傛灉涓嬩竴鐐逛笌 parent 鍦ㄥ悓涓�琛屾垨鍚屼竴鍒� => 娌℃湁杞集
+ if (nextNode.getX() == parent.getX() || nextNode.getY() == parent.getY()) {
+ return false;
+ }
+ return true;
}
- // 杞集鍒ゆ柇锛氬鏋滆繖涓や釜鍚戦噺鐩稿悓锛堜緥濡傞兘绛変簬 (0,1)锛夛紝璇存槑鏂瑰悜鐩稿悓锛涘惁鍒欒鏄庤浆寮��
-// private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
-// // 濡傛灉 currNode 娌℃湁鐖惰妭鐐癸紝璇存槑鏄捣鐐癸紝涓嶇畻杞集
-// if (currNode.getParent() == null) {
-// return false;
-// }
-// // 鍙栧嚭鍧愭爣
-// AStarNavigateNode parent = currNode.getParent();
-// int px = currNode.getX() - parent.getX(); // parent -> curr 鐨剎鍋忕Щ
-// int py = currNode.getY() - parent.getY(); // parent -> curr 鐨剏鍋忕Щ
-//
-// int nx = nextNode.getX() - currNode.getX(); // curr -> next 鐨剎鍋忕Щ
-// int ny = nextNode.getY() - currNode.getY(); // curr -> next 鐨剏鍋忕Щ
-//
-// // 濡傛灉 (px, py) 涓� (nx, ny) 涓嶄竴鏍凤紝灏辫鏄庤浆寮�
-// return (px != nx) || (py != ny);
-// }
+ // --------- 鐗╃悊鍙傛暟 鈫� 鏃堕棿姝� 鐨勬崲绠楄緟鍔╂柟娉� --------- //
+
+ /**
+ * 浠庨厤缃腑蹇冭鍙� double锛屽け璐ュ垯杩斿洖榛樿鍊�
+ */
+ private double getDoubleConfig(String key, double defaultVal) {
+ try {
+ Double v = configService.getVal(key, Double.class);
+ if (v != null && v > 0) {
+ return v;
+ }
+ } catch (Exception e) {
+ log.warn("config {} not available, use default {}", key, defaultVal);
+ }
+ return defaultVal;
+ }
+
+ /**
+ * 璁＄畻涓�鏍肩洿绾跨Щ鍔ㄧ殑鐗╃悊鏃堕棿锛堢锛�
+ * 浣跨敤鏍囧噯鐨勫姞閫�-鍖�閫�-鍑忛�� 鎴� 涓夎閫熷害鏇茬嚎妯″瀷锛�
+ *
+ * - 鍏堢畻杈惧埌 vmax 鎵�闇�鐨勫姞閫熻窛绂� s_acc = vmax^2 / (2 a_acc)
+ * - 鍑忛�熻窛绂� s_dec = vmax^2 / (2 a_dec)
+ * - 鑻� dist >= s_acc + s_dec锛氳兘璺戝埌 vmax锛屽垯 t = t_acc + t_dec + t_cruise
+ * - 鍚﹀垯锛氳揪涓嶅埌 vmax锛岄噰鐢ㄤ笁瑙掑舰閫熷害鏇茬嚎锛屾眰宄板�奸�熷害 v_peak锛屽啀绠� t_acc + t_dec
+ */
+ private double calcStraightTravelTimeSec(double dist,
+ double vMax,
+ double aAcc,
+ double aDec) {
+ if (dist <= 0) {
+ return 0.0;
+ }
+ // 閬垮厤鍙傛暟寮傚父
+ vMax = Math.max(vMax, 0.01);
+ aAcc = Math.max(aAcc, 0.01);
+ aDec = Math.max(aDec, 0.01);
+
+ double sAcc = vMax * vMax / (2.0 * aAcc);
+ double sDec = vMax * vMax / (2.0 * aDec);
+
+ // 鎯呭喌1锛氳窛绂昏冻澶熼暱锛屽彲浠ュ姞閫熷埌 vmax 鍐嶅噺閫�
+ if (dist >= sAcc + sDec) {
+ double tAcc = vMax / aAcc;
+ double tDec = vMax / aDec;
+ double sCruise = dist - sAcc - sDec;
+ double tCruise = sCruise / vMax;
+ return tAcc + tCruise + tDec;
+ } else {
+ // 鎯呭喌2锛氳窛绂昏緝鐭紝杈句笉鍒� vmax锛岄噰鐢ㄤ笁瑙掗�熷害鏇茬嚎
+ // dist = v_peak^2 / (2 aAcc) + v_peak^2 / (2 aDec)
+ double denom = (1.0 / aAcc + 1.0 / aDec);
+ double vPeak = Math.sqrt(2.0 * dist / denom);
+ double tAcc = vPeak / aAcc;
+ double tDec = vPeak / aDec;
+ return tAcc + tDec;
+ }
+ }
}
--
Gitblit v1.9.1