package com.algo.service;
|
|
import com.algo.model.CTUPhysicalConfig;
|
import com.algo.model.Conflict;
|
import com.algo.model.PathCode;
|
import com.algo.model.PlannedPath;
|
import com.algo.util.JsonUtils;
|
|
import java.util.*;
|
|
/**
|
* 碰撞检测器
|
*/
|
public class CollisionDetector {
|
|
/**
|
* 路径映射表
|
*/
|
private final Map<String, Map<String, Integer>> pathMapping;
|
|
/**
|
* 默认最小安全距离
|
*/
|
private final double defaultMinDistance;
|
|
/**
|
* 时间精度（毫秒）
|
* 提高时间精度以更准确地检测碰撞
|
*/
|
private final long timeResolution = 50;
|
|
/**
|
* 空间分辨率（米）
|
*/
|
private final double spaceResolution = 0.1;
|
|
/**
|
* 构造函数
|
*
|
* @param pathMapping 路径映射表
|
* @param defaultMinDistance 默认最小安全距离
|
*/
|
public CollisionDetector(Map<String, Map<String, Integer>> pathMapping,
|
double defaultMinDistance) {
|
this.pathMapping = pathMapping;
|
this.defaultMinDistance = defaultMinDistance;
|
}
|
|
/**
|
* 构造函数
|
*
|
* @param pathMapping 路径映射表
|
*/
|
public CollisionDetector(Map<String, Map<String, Integer>> pathMapping) {
|
this(pathMapping, 3.0);
|
}
|
|
/**
|
* 检测路径冲突
|
*
|
* @param plannedPaths 规划路径列表
|
* @return 冲突列表
|
*/
|
public List<Conflict> detectConflicts(List<PlannedPath> plannedPaths) {
|
List<Conflict> conflicts = new ArrayList<>();
|
|
if (plannedPaths == null || plannedPaths.size() < 2) {
|
return conflicts;
|
}
|
|
System.out.println("开始碰撞检测，路径数量: " + plannedPaths.size());
|
|
// 构建高精度时空表
|
Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);
|
|
// 检测顶点冲突（同一时间同一位置）
|
List<Conflict> vertexConflicts = detectEnhancedVertexConflicts(spaceTimeTable);
|
conflicts.addAll(vertexConflicts);
|
System.out.println("顶点冲突数量: " + vertexConflicts.size());
|
|
// 检测边冲突（CTU交换位置）
|
List<Conflict> edgeConflicts = detectEnhancedEdgeConflicts(spaceTimeTable);
|
conflicts.addAll(edgeConflicts);
|
System.out.println("边冲突数量: " + edgeConflicts.size());
|
|
// 检测跟随冲突
|
List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(spaceTimeTable);
|
conflicts.addAll(followingConflicts);
|
System.out.println("跟随冲突数量: " + followingConflicts.size());
|
|
// 检测物理尺寸冲突
|
List<Conflict> physicalConflicts = detectPhysicalSizeConflicts(spaceTimeTable);
|
conflicts.addAll(physicalConflicts);
|
System.out.println("物理尺寸冲突数量: " + physicalConflicts.size());
|
|
System.out.println("碰撞检测完成，总冲突数量: " + conflicts.size());
|
return conflicts;
|
}
|
|
/**
|
* 构建时空表
|
*
|
* @param plannedPaths 规划路径列表
|
* @return 时空表
|
*/
|
private Map<Long, List<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
|
Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();
|
|
for (PlannedPath path : plannedPaths) {
|
String agvId = path.getAgvId();
|
List<PathCode> codeList = path.getCodeList();
|
|
if (codeList == null || codeList.isEmpty()) {
|
continue;
|
}
|
|
// 获取CTU的物理配置
|
CTUPhysicalConfig physicalConfig = getPhysicalConfigForCTU(agvId);
|
|
|
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) {
|
// 创建时间段内的多个时间点（使用实际时间戳）
|
for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
|
EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
|
agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
|
);
|
|
spaceTimeTable.computeIfAbsent(timePoint, k -> new ArrayList<>()).add(node);
|
}
|
}
|
}
|
}
|
|
return spaceTimeTable;
|
}
|
|
/**
|
* 获取CTU的物理配置
|
*
|
* @param agvId CTU编号
|
* @return 物理配置
|
*/
|
private CTUPhysicalConfig getPhysicalConfigForCTU(String agvId) {
|
// 从AGV状态中获取
|
CTUPhysicalConfig config = new CTUPhysicalConfig();
|
// 标准配置
|
// 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; // 使用默认构造函数的标准配置
|
}
|
|
/**
|
* 计算到达时间
|
*
|
* @param startTime 起始时间
|
* @param pathIndex 路径索引
|
* @param codeList 路径代码列表
|
* @param config 物理配置
|
* @return 到达时间
|
*/
|
private long calculateArrivalTime(long startTime, int pathIndex, List<PathCode> codeList, CTUPhysicalConfig config) {
|
if (pathIndex == 0) {
|
return startTime;
|
}
|
|
long travelTime = 0;
|
|
for (int i = 1; i <= pathIndex; i++) {
|
PathCode currentCode = codeList.get(i);
|
PathCode previousCode = codeList.get(i - 1);
|
|
// 计算移动时间
|
double distance = config.getStandardPointDistance();
|
double speed = config.getNormalSpeed();
|
travelTime += (long) ((distance / speed) * 1000); // 转换为毫秒
|
|
// 如果有方向变化，增加转向时间
|
// 添加null检查
|
if (currentCode.getDirection() != null && previousCode.getDirection() != null) {
|
if (!currentCode.getDirection().equals(previousCode.getDirection())) {
|
double turnTime = calculateTurnTime(previousCode.getDirection(), currentCode.getDirection(), config);
|
travelTime += (long) (turnTime * 1000);
|
}
|
}
|
|
// 考虑加速和减速时间
|
if (i == 1) {
|
travelTime += (long) (config.getStartupTime() * 1000);
|
}
|
}
|
|
return startTime + travelTime;
|
}
|
|
/**
|
* 计算离开时间
|
*
|
* @param arrivalTime 到达时间
|
* @param pathCode 路径代码
|
* @param config 物理配置
|
* @return 离开时间
|
*/
|
private long calculateDepartureTime(long arrivalTime, PathCode pathCode, CTUPhysicalConfig config) {
|
long stayTime = 500; // 默认停留时间500毫秒
|
|
// 根据路径代码类型确定停留时间
|
if (pathCode.isTargetPoint()) {
|
if ("1".equals(pathCode.getPosType())) { // 取货
|
stayTime = 3000; // 取货需要3秒
|
} else if ("2".equals(pathCode.getPosType())) { // 放货
|
stayTime = 2000; // 放货需要2秒
|
}
|
}
|
|
return arrivalTime + stayTime;
|
}
|
|
/**
|
* 计算转向时间
|
*
|
* @param fromDirection 起始方向
|
* @param toDirection 目标方向
|
* @param config 物理配置
|
* @return 转向时间（秒）
|
*/
|
private double calculateTurnTime(String fromDirection, String toDirection, CTUPhysicalConfig config) {
|
if (fromDirection.equals(toDirection)) {
|
return 0.0;
|
}
|
|
// 方向转换计算
|
int angleDiff = Math.abs(Integer.parseInt(toDirection) - Integer.parseInt(fromDirection));
|
angleDiff = Math.min(angleDiff, 4 - angleDiff); // 考虑环形
|
|
switch (angleDiff) {
|
case 1:
|
return config.getTurnTime90();
|
case 2:
|
return config.getTurnTime180();
|
default:
|
return config.getTurnTime90() * angleDiff;
|
}
|
}
|
|
/**
|
* 检测顶点冲突
|
*
|
* @param spaceTimeTable 时空表
|
* @return 顶点冲突列表
|
*/
|
private List<Conflict> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
|
List<Conflict> conflicts = new ArrayList<>();
|
|
for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
|
long timePoint = entry.getKey();
|
List<EnhancedSpaceTimeNode> nodes = entry.getValue();
|
|
// 按位置分组
|
Map<String, List<EnhancedSpaceTimeNode>> positionGroups = new HashMap<>();
|
for (EnhancedSpaceTimeNode node : nodes) {
|
positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node);
|
}
|
|
// 检查每个位置是否有多个CTU
|
for (Map.Entry<String, List<EnhancedSpaceTimeNode>> posEntry : positionGroups.entrySet()) {
|
String position = posEntry.getKey();
|
List<EnhancedSpaceTimeNode> 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);
|
|
// 检查时间段是否重叠
|
if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
|
node2.arrivalTime, node2.departureTime)) {
|
Conflict conflict = new Conflict(
|
"enhanced_vertex",
|
node1.agvId,
|
node2.agvId,
|
(int) (timePoint / 1000), // 转换为秒用于显示
|
position,
|
position,
|
String.format("CTU %s 和 %s 在时间 %d 占用同一位置 %s（时间重叠）",
|
node1.agvId, node2.agvId, timePoint / 1000, position)
|
);
|
conflicts.add(conflict);
|
}
|
}
|
}
|
}
|
}
|
}
|
|
return conflicts;
|
}
|
|
/**
|
* 检测边冲突
|
*
|
* @param spaceTimeTable 时空表
|
* @return 边冲突列表
|
*/
|
private List<Conflict> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
|
List<Conflict> conflicts = new ArrayList<>();
|
|
if (spaceTimeTable.isEmpty()) {
|
return conflicts;
|
}
|
|
List<Long> sortedTimes = new ArrayList<>(spaceTimeTable.keySet());
|
Collections.sort(sortedTimes);
|
|
for (int i = 0; i < sortedTimes.size() - 1; i++) {
|
long currentTime = sortedTimes.get(i);
|
long nextTime = sortedTimes.get(i + 1);
|
|
// 只检查连续的时间点
|
if (nextTime - currentTime <= timeResolution * 2) {
|
List<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
|
List<EnhancedSpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);
|
|
conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime));
|
}
|
}
|
|
return conflicts;
|
}
|
|
/**
|
* 检测位置交换
|
*
|
* @param currentNodes 当前时间的节点
|
* @param nextNodes 下一时间的节点
|
* @param currentTime 当前时间
|
* @param nextTime 下一时间
|
* @return 冲突列表
|
*/
|
private List<Conflict> detectPositionSwapping(List<EnhancedSpaceTimeNode> currentNodes,
|
List<EnhancedSpaceTimeNode> 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) {
|
currentPositions.put(node.agvId, node.position);
|
}
|
|
for (EnhancedSpaceTimeNode node : nextNodes) {
|
nextPositions.put(node.agvId, node.position);
|
}
|
|
for (Map.Entry<String, String> entry1 : currentPositions.entrySet()) {
|
String ctu1 = entry1.getKey();
|
String pos1Current = entry1.getValue();
|
|
for (Map.Entry<String, String> entry2 : currentPositions.entrySet()) {
|
String ctu2 = entry2.getKey();
|
String pos2Current = entry2.getValue();
|
|
if (ctu1.compareTo(ctu2) >= 0) {
|
continue;
|
}
|
|
String pos1Next = nextPositions.get(ctu1);
|
String pos2Next = nextPositions.get(ctu2);
|
|
if (pos1Next != null && pos2Next != null &&
|
pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) {
|
|
Conflict conflict = new Conflict(
|
"enhanced_edge",
|
ctu1,
|
ctu2,
|
(int) (currentTime / 1000),
|
pos1Current,
|
pos2Current,
|
String.format("CTU %s 和 %s 在时间 %d-%d 交换位置 %s<->%s",
|
ctu1, ctu2, currentTime / 1000, nextTime / 1000, pos1Current, pos2Current)
|
);
|
conflicts.add(conflict);
|
}
|
}
|
}
|
|
return conflicts;
|
}
|
|
/**
|
* 检测跟随冲突
|
*
|
* @param spaceTimeTable 时空表
|
* @return 跟随冲突列表
|
*/
|
private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
|
List<Conflict> conflicts = new ArrayList<>();
|
|
for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
|
long timePoint = entry.getKey();
|
List<EnhancedSpaceTimeNode> 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);
|
|
double distance = calculateDistance(node1.coordinates, node2.coordinates);
|
double minSafeDistance = Math.max(
|
node1.physicalConfig.getMinFollowingDistance(),
|
node2.physicalConfig.getMinFollowingDistance()
|
);
|
|
if (distance > 0 && distance < minSafeDistance) {
|
Conflict conflict = new Conflict(
|
"enhanced_follow",
|
node1.agvId,
|
node2.agvId,
|
(int) (timePoint / 1000),
|
node1.position,
|
node2.position,
|
String.format("CTU %s 和 %s 在时间 %d 距离过近 (%.2f < %.2f)",
|
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);
|
}
|
}
|
}
|
}
|
}
|
|
return conflicts;
|
}
|
|
/**
|
* 检测物理尺寸冲突
|
*
|
* @param spaceTimeTable 时空表
|
* @return 物理冲突列表
|
*/
|
private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
|
List<Conflict> conflicts = new ArrayList<>();
|
|
for (Map.Entry<Long, List<EnhancedSpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
|
long timePoint = entry.getKey();
|
List<EnhancedSpaceTimeNode> 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);
|
|
if (checkPhysicalCollision(node1, node2)) {
|
Conflict conflict = new Conflict(
|
"physical_size",
|
node1.agvId,
|
node2.agvId,
|
(int) (timePoint / 1000),
|
node1.position,
|
node2.position,
|
String.format("CTU %s 和 %s 在时间 %d 发生物理尺寸冲突",
|
node1.agvId, node2.agvId, timePoint / 1000)
|
);
|
conflicts.add(conflict);
|
}
|
}
|
}
|
}
|
|
return conflicts;
|
}
|
|
/**
|
* 检查两个CTU是否发生物理碰撞
|
*
|
* @param node1 CTU节点1
|
* @param node2 CTU节点2
|
* @return 是否碰撞
|
*/
|
private boolean checkPhysicalCollision(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode node2) {
|
double distance = calculateDistance(node1.coordinates, node2.coordinates);
|
|
// 考虑CTU的物理尺寸
|
double ctu1Radius = Math.max(node1.physicalConfig.getCtuLength(), node1.physicalConfig.getCtuWidth()) / 2;
|
double ctu2Radius = Math.max(node2.physicalConfig.getCtuLength(), node2.physicalConfig.getCtuWidth()) / 2;
|
|
double minClearance = ctu1Radius + ctu2Radius + spaceResolution;
|
|
return distance > 0 && distance < minClearance;
|
}
|
|
/**
|
* 检查时间范围是否重叠
|
*
|
* @param start1 开始时间1
|
* @param end1 结束时间1
|
* @param start2 开始时间2
|
* @param end2 结束时间2
|
* @return 是否重叠
|
*/
|
private boolean timeRangeOverlap(long start1, long end1, long start2, long end2) {
|
return Math.max(start1, start2) < Math.min(end1, end2);
|
}
|
|
/**
|
* 计算两个坐标之间的距离
|
*
|
* @param coord1 坐标1
|
* @param coord2 坐标2
|
* @return 距离值
|
*/
|
private double calculateDistance(int[] coord1, int[] coord2) {
|
if (coord1 == null || coord2 == null || coord1.length != 2 || coord2.length != 2) {
|
return -1;
|
}
|
|
return Math.sqrt(Math.pow(coord1[0] - coord2[0], 2) + Math.pow(coord1[1] - coord2[1], 2));
|
}
|
|
/**
|
* 时空节点内部类
|
*/
|
private static class EnhancedSpaceTimeNode {
|
final String agvId;
|
final String position;
|
final int[] coordinates;
|
final long timePoint;
|
final long arrivalTime;
|
final long departureTime;
|
final CTUPhysicalConfig physicalConfig;
|
|
public EnhancedSpaceTimeNode(String agvId, String position, int[] coordinates,
|
long timePoint, long arrivalTime, long departureTime,
|
CTUPhysicalConfig physicalConfig) {
|
this.agvId = agvId;
|
this.position = position;
|
this.coordinates = coordinates;
|
this.timePoint = timePoint;
|
this.arrivalTime = arrivalTime;
|
this.departureTime = departureTime;
|
this.physicalConfig = physicalConfig;
|
}
|
}
|
}
|
