From d808837cd368c3772962be591aa6532bcc0cf3e4 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期一, 22 九月 2025 12:11:37 +0800
Subject: [PATCH] 更新版本
---
algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java | 161 +++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 153 insertions(+), 8 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java b/algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java
index a63d923..d201612 100644
--- a/algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java
+++ b/algo-zkd/src/main/java/com/algo/model/CTUPhysicalConfig.java
@@ -71,16 +71,16 @@
*/
private double standardPointDistance;
- // 榛樿鏋勯�犲嚱鏁� - 浣跨敤鎺ㄨ崘鐨勯粯璁ゅ��
+ // 榛樿鏋勯�犲嚱鏁� - 浣跨敤鍩轰簬瀹為檯鍦哄湴鐨勭墿鐞嗗弬鏁�
public CTUPhysicalConfig() {
- this.maxSpeed = 2.0; // 2绫�/绉�
- this.normalSpeed = 1.5; // 1.5绫�/绉�
- this.maxAcceleration = 1.0; // 1绫�/绉捖�
- this.maxDeceleration = 1.5; // 1.5绫�/绉捖�
- this.turnTime90 = 2.0; // 90搴﹁浆鍚戦渶瑕�2绉�
- this.turnTime180 = 4.0; // 180搴﹁浆鍚戦渶瑕�4绉�
+ 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 = 2.0; // 鏈�灏忚窡闅忚窛绂�2绫�
+ this.minFollowingDistance = 3.0; // 鏈�灏忚窡闅忚窛绂�3绫�
this.ctuLength = 1.5; // CTU闀垮害1.5绫�
this.ctuWidth = 1.0; // CTU瀹藉害1绫�
this.startupTime = 1.0; // 鍚姩鏃堕棿1绉�
@@ -301,6 +301,151 @@
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{" +
--
Gitblit v1.9.1