package com.algo.model;
|
|
/**
|
* CTU物理参数配置类
|
* 包含CTU运动的各种物理参数，用于路径规划和碰撞检测
|
*/
|
public class CTUPhysicalConfig {
|
|
/**
|
* 最大移动速度 (米/秒)
|
*/
|
private double maxSpeed;
|
|
/**
|
* 正常移动速度 (米/秒)
|
*/
|
private double normalSpeed;
|
|
/**
|
* 最大加速度 (米/秒²)
|
*/
|
private double maxAcceleration;
|
|
/**
|
* 最大减速度 (米/秒²)
|
*/
|
private double maxDeceleration;
|
|
/**
|
* 转向时间 (秒) - 90度转向所需时间
|
*/
|
private double turnTime90;
|
|
/**
|
* 转向时间 (秒) - 180度转向所需时间
|
*/
|
private double turnTime180;
|
|
/**
|
* CTU之间的最小安全距离 (米)
|
*/
|
private double minSafetyDistance;
|
|
/**
|
* CTU之间的最小跟随距离 (米)
|
*/
|
private double minFollowingDistance;
|
|
/**
|
* CTU的物理长度 (米)
|
*/
|
private double ctuLength;
|
|
/**
|
* CTU的物理宽度 (米)
|
*/
|
private double ctuWidth;
|
|
/**
|
* 启动时间 (秒) - 从静止到正常速度所需时间
|
*/
|
private double startupTime;
|
|
/**
|
* 停止时间 (秒) - 从正常速度到静止所需时间
|
*/
|
private double stopTime;
|
|
/**
|
* 路径点之间的标准距离 (米)
|
*/
|
private double standardPointDistance;
|
|
// 默认构造函数 - 使用基于实际场地的物理参数
|
public CTUPhysicalConfig() {
|
this.maxSpeed = 1.0; // 1000mm/s = 1.0米/秒
|
this.normalSpeed = 1.0; // 正常运行速度100%最大速度
|
this.maxAcceleration = 0.4; // 400 mm/s^2 = 0.4米/秒²
|
this.maxDeceleration = 0.4; // 相同的加减速度
|
this.turnTime90 = 4.0; // 每90度转向需要4秒
|
this.turnTime180 = 8.0; // 180度转向需要8秒
|
this.minSafetyDistance = 3.0; // 最小安全距离3米
|
this.minFollowingDistance = 3.0; // 最小跟随距离3米
|
this.ctuLength = 1.5; // CTU长度1.5米
|
this.ctuWidth = 1.0; // CTU宽度1米
|
this.startupTime = 1.0; // 启动时间1秒
|
this.stopTime = 1.5; // 停止时间1.5秒
|
this.standardPointDistance = 1.0; // 标准路径点距离1米
|
}
|
|
// 完整参数的构造函数
|
public CTUPhysicalConfig(double maxSpeed, double normalSpeed, double maxAcceleration,
|
double maxDeceleration, double turnTime90, double turnTime180,
|
double minSafetyDistance, double minFollowingDistance,
|
double ctuLength, double ctuWidth, double startupTime,
|
double stopTime, double standardPointDistance) {
|
this.maxSpeed = maxSpeed;
|
this.normalSpeed = normalSpeed;
|
this.maxAcceleration = maxAcceleration;
|
this.maxDeceleration = maxDeceleration;
|
this.turnTime90 = turnTime90;
|
this.turnTime180 = turnTime180;
|
this.minSafetyDistance = minSafetyDistance;
|
this.minFollowingDistance = minFollowingDistance;
|
this.ctuLength = ctuLength;
|
this.ctuWidth = ctuWidth;
|
this.startupTime = startupTime;
|
this.stopTime = stopTime;
|
this.standardPointDistance = standardPointDistance;
|
}
|
|
// Getter和Setter方法
|
public double getMaxSpeed() {
|
return maxSpeed;
|
}
|
|
public void setMaxSpeed(double maxSpeed) {
|
this.maxSpeed = maxSpeed;
|
}
|
|
public double getNormalSpeed() {
|
return normalSpeed;
|
}
|
|
public void setNormalSpeed(double normalSpeed) {
|
this.normalSpeed = normalSpeed;
|
}
|
|
public double getMaxAcceleration() {
|
return maxAcceleration;
|
}
|
|
public void setMaxAcceleration(double maxAcceleration) {
|
this.maxAcceleration = maxAcceleration;
|
}
|
|
public double getMaxDeceleration() {
|
return maxDeceleration;
|
}
|
|
public void setMaxDeceleration(double maxDeceleration) {
|
this.maxDeceleration = maxDeceleration;
|
}
|
|
public double getTurnTime90() {
|
return turnTime90;
|
}
|
|
public void setTurnTime90(double turnTime90) {
|
this.turnTime90 = turnTime90;
|
}
|
|
public double getTurnTime180() {
|
return turnTime180;
|
}
|
|
public void setTurnTime180(double turnTime180) {
|
this.turnTime180 = turnTime180;
|
}
|
|
public double getMinSafetyDistance() {
|
return minSafetyDistance;
|
}
|
|
public void setMinSafetyDistance(double minSafetyDistance) {
|
this.minSafetyDistance = minSafetyDistance;
|
}
|
|
public double getMinFollowingDistance() {
|
return minFollowingDistance;
|
}
|
|
public void setMinFollowingDistance(double minFollowingDistance) {
|
this.minFollowingDistance = minFollowingDistance;
|
}
|
|
public double getCtuLength() {
|
return ctuLength;
|
}
|
|
public void setCtuLength(double ctuLength) {
|
this.ctuLength = ctuLength;
|
}
|
|
public double getCtuWidth() {
|
return ctuWidth;
|
}
|
|
public void setCtuWidth(double ctuWidth) {
|
this.ctuWidth = ctuWidth;
|
}
|
|
public double getStartupTime() {
|
return startupTime;
|
}
|
|
public void setStartupTime(double startupTime) {
|
this.startupTime = startupTime;
|
}
|
|
public double getStopTime() {
|
return stopTime;
|
}
|
|
public void setStopTime(double stopTime) {
|
this.stopTime = stopTime;
|
}
|
|
public double getStandardPointDistance() {
|
return standardPointDistance;
|
}
|
|
public void setStandardPointDistance(double standardPointDistance) {
|
this.standardPointDistance = standardPointDistance;
|
}
|
|
/**
|
* 计算从当前速度加速到目标速度所需的时间
|
*
|
* @param currentSpeed 当前速度
|
* @param targetSpeed 目标速度
|
* @return 所需时间（秒）
|
*/
|
public double getAccelerationTime(double currentSpeed, double targetSpeed) {
|
if (targetSpeed > currentSpeed) {
|
return (targetSpeed - currentSpeed) / maxAcceleration;
|
} else {
|
return (currentSpeed - targetSpeed) / maxDeceleration;
|
}
|
}
|
|
/**
|
* 计算从当前速度加速/减速到目标速度所需的距离
|
*
|
* @param currentSpeed 当前速度
|
* @param targetSpeed 目标速度
|
* @return 所需距离（米）
|
*/
|
public double getAccelerationDistance(double currentSpeed, double targetSpeed) {
|
double time = getAccelerationTime(currentSpeed, targetSpeed);
|
return (currentSpeed + targetSpeed) * time / 2.0;
|
}
|
|
/**
|
* 根据路径点距离计算移动时间
|
*
|
* @param distance 距离（米）
|
* @param currentSpeed 当前速度
|
* @return 移动时间（秒）
|
*/
|
public double getTravelTime(double distance, double currentSpeed) {
|
if (currentSpeed <= 0) {
|
currentSpeed = normalSpeed;
|
}
|
return distance / currentSpeed;
|
}
|
|
/**
|
* 根据方向变化计算转向时间
|
*
|
* @param fromDirection 起始方向角度
|
* @param toDirection 目标方向角度
|
* @return 转向时间（秒）
|
*/
|
public double getTurnTime(String fromDirection, String toDirection) {
|
try {
|
double from = Double.parseDouble(fromDirection);
|
double to = Double.parseDouble(toDirection);
|
|
// 计算角度差
|
double angleDiff = Math.abs(to - from);
|
if (angleDiff > 180) {
|
angleDiff = 360 - angleDiff;
|
}
|
|
// 根据角度差计算转向时间
|
if (angleDiff <= 5) { // 基本无转向
|
return 0.0;
|
} else if (angleDiff <= 90) {
|
return turnTime90 * (angleDiff / 90.0);
|
} else if (angleDiff <= 180) {
|
return turnTime90 + (turnTime180 - turnTime90) * ((angleDiff - 90) / 90.0);
|
} else {
|
return turnTime180;
|
}
|
} catch (NumberFormatException e) {
|
// 如果方向不是数字，返回默认转向时间
|
return turnTime90;
|
}
|
}
|
|
/**
|
* 检查两个CTU之间的距离是否满足安全要求
|
*
|
* @param distance 距离（米）
|
* @param isFollowing 是否为跟随情况
|
* @return true如果距离安全
|
*/
|
public boolean isSafeDistance(double distance, boolean isFollowing) {
|
double minDistance = isFollowing ? minFollowingDistance : minSafetyDistance;
|
return distance >= minDistance;
|
}
|
|
/**
|
* 计算精确的移动时间，考虑加速度、减速度和实际距离
|
*
|
* @param distance 实际移动距离（米）
|
* @param startSpeed 起始速度（米/秒）
|
* @param endSpeed 结束速度（米/秒）
|
* @param targetSpeed 目标巡航速度（米/秒）
|
* @return 移动时间（秒）
|
*/
|
public double calculatePreciseMovementTime(double distance, double startSpeed, double endSpeed, double targetSpeed) {
|
if (distance <= 0) return 0.0;
|
|
// 如果距离很短，直接计算平均速度
|
if (distance < 0.5) { // 小于0.5米
|
double avgSpeed = Math.max(0.1, (startSpeed + endSpeed) / 2.0);
|
return distance / avgSpeed;
|
}
|
|
double totalTime = 0.0;
|
double currentSpeed = startSpeed;
|
double remainingDistance = distance;
|
|
// 阶段1: 加速到目标速度
|
if (currentSpeed < targetSpeed) {
|
double accelerationTime = (targetSpeed - currentSpeed) / maxAcceleration;
|
double accelerationDistance = currentSpeed * accelerationTime + 0.5 * maxAcceleration * accelerationTime * accelerationTime;
|
|
if (accelerationDistance < remainingDistance) {
|
totalTime += accelerationTime;
|
remainingDistance -= accelerationDistance;
|
currentSpeed = targetSpeed;
|
} else {
|
// 距离不足以完全加速，直接计算
|
return calculateTimeForShortDistance(distance, startSpeed, endSpeed);
|
}
|
}
|
|
// 阶段2: 减速到目标结束速度
|
double decelerationDistance = 0.0;
|
double decelerationTime = 0.0;
|
if (currentSpeed > endSpeed) {
|
decelerationTime = (currentSpeed - endSpeed) / maxDeceleration;
|
decelerationDistance = endSpeed * decelerationTime + 0.5 * maxDeceleration * decelerationTime * decelerationTime;
|
}
|
|
// 阶段3: 匀速巡航
|
if (remainingDistance > decelerationDistance) {
|
double cruisingDistance = remainingDistance - decelerationDistance;
|
double cruisingTime = cruisingDistance / currentSpeed;
|
totalTime += cruisingTime;
|
}
|
|
// 添加减速时间
|
totalTime += decelerationTime;
|
|
return totalTime;
|
}
|
|
/**
|
* 计算短距离移动时间（无法完整加速减速的情况）
|
*/
|
private double calculateTimeForShortDistance(double distance, double startSpeed, double endSpeed) {
|
// 使用运动学公式: s = v0*t + 0.5*a*t^2
|
// 其中 v_f = v0 + a*t，求解 t
|
double avgAcceleration = (endSpeed > startSpeed) ? maxAcceleration : -maxDeceleration;
|
|
// 如果加速度很小或为0，使用平均速度
|
if (Math.abs(avgAcceleration) < 0.01) {
|
return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
|
}
|
|
// 求解二次方程: 0.5*a*t^2 + v0*t - s = 0
|
double a = 0.5 * avgAcceleration;
|
double b = startSpeed;
|
double c = -distance;
|
|
double discriminant = b * b - 4 * a * c;
|
if (discriminant < 0) {
|
// 无解，使用平均速度
|
return distance / Math.max(0.1, (startSpeed + endSpeed) / 2.0);
|
}
|
|
double t1 = (-b + Math.sqrt(discriminant)) / (2 * a);
|
double t2 = (-b - Math.sqrt(discriminant)) / (2 * a);
|
|
// 选择正的时间值
|
double time = (t1 > 0) ? t1 : t2;
|
return Math.max(0.1, time); // 确保时间为正
|
}
|
|
/**
|
* 基于实际坐标计算两点间的精确距离（米）
|
*
|
* @param coord1 起点坐标 [x_mm, y_mm]
|
* @param coord2 终点坐标 [x_mm, y_mm]
|
* @return 距离（米）
|
*/
|
public static double calculateRealDistance(double[] coord1, double[] coord2) {
|
if (coord1 == null || coord2 == null || coord1.length < 2 || coord2.length < 2) {
|
return 0.0;
|
}
|
|
// 坐标单位为毫米，转换为米
|
double dx = (coord2[0] - coord1[0]) / 1000.0;
|
double dy = (coord2[1] - coord1[1]) / 1000.0;
|
|
return Math.sqrt(dx * dx + dy * dy);
|
}
|
|
/**
|
* 计算方向改变角度
|
*
|
* @param coord1 起点坐标
|
* @param coord2 中间点坐标
|
* @param coord3 终点坐标
|
* @return 转向角度（度）
|
*/
|
public static double calculateTurnAngle(double[] coord1, double[] coord2, double[] coord3) {
|
if (coord1 == null || coord2 == null || coord3 == null) {
|
return 0.0;
|
}
|
|
// 计算两个向量
|
double[] vec1 = {coord2[0] - coord1[0], coord2[1] - coord1[1]};
|
double[] vec2 = {coord3[0] - coord2[0], coord3[1] - coord2[1]};
|
|
// 计算向量长度
|
double len1 = Math.sqrt(vec1[0] * vec1[0] + vec1[1] * vec1[1]);
|
double len2 = Math.sqrt(vec2[0] * vec2[0] + vec2[1] * vec2[1]);
|
|
if (len1 < 1e-6 || len2 < 1e-6) {
|
return 0.0;
|
}
|
|
// 计算夹角
|
double dot = vec1[0] * vec2[0] + vec1[1] * vec2[1];
|
double cosAngle = dot / (len1 * len2);
|
|
// 限制cosAngle在[-1, 1]范围内
|
cosAngle = Math.max(-1.0, Math.min(1.0, cosAngle));
|
|
double angle = Math.acos(cosAngle);
|
return Math.toDegrees(angle);
|
}
|
|
@Override
|
public String toString() {
|
return "CTUPhysicalConfig{" +
|
"maxSpeed=" + maxSpeed +
|
", normalSpeed=" + normalSpeed +
|
", maxAcceleration=" + maxAcceleration +
|
", maxDeceleration=" + maxDeceleration +
|
", turnTime90=" + turnTime90 +
|
", turnTime180=" + turnTime180 +
|
", minSafetyDistance=" + minSafetyDistance +
|
", minFollowingDistance=" + minFollowingDistance +
|
", ctuLength=" + ctuLength +
|
", ctuWidth=" + ctuWidth +
|
", startupTime=" + startupTime +
|
", stopTime=" + stopTime +
|
", standardPointDistance=" + standardPointDistance +
|
'}';
|
}
|
}
|
