From d58d5e1baf9390d4119f48c4af3fbcf308da10a3 Mon Sep 17 00:00:00 2001
From: 826220679@qq.com <826220679@qq.com>
Date: 星期六, 27 十二月 2025 22:46:10 +0800
Subject: [PATCH] 进一步优化了异形无障碍物路径规划算法
---
src/bianjie/BoundaryAlgorithm.java | 60 ++++++++++++++++++++++++++++++++----------------------------
1 files changed, 32 insertions(+), 28 deletions(-)
diff --git a/src/bianjie/BoundaryAlgorithm.java b/src/bianjie/BoundaryAlgorithm.java
index a38f076..7b47660 100644
--- a/src/bianjie/BoundaryAlgorithm.java
+++ b/src/bianjie/BoundaryAlgorithm.java
@@ -116,30 +116,21 @@
// 楂樼骇澶勭悊杈圭晫鏁版嵁鏂规硶
public List<Coordinate> processBoundaryDataAdvanced(String gnggaData, double baseLat, double baseLon,
BoundaryParameters params) {
- System.out.println("寮�濮嬪鐞嗚竟鐣屾暟鎹�...");
- System.out.println("鍩哄噯绔欏潗鏍�: " + baseLat + ", " + baseLon);
- System.out.println("鍙傛暟: 闂撮殧=" + params.interval + "绫�, 瑙掑害闃堝��=" + params.angleThreshold + "搴�");
-
// 1. 瑙ｆ瀽GNGGA鏁版嵁
List<Coordinate> rawPoints = parseGNGGA(gnggaData, baseLat, baseLon);
- System.out.println("瑙ｆ瀽鍒板師濮嬬偣: " + rawPoints.size() + " 涓�");
if (rawPoints.size() < 3) {
- System.out.println("鏁版嵁鐐逛笉瓒筹紝鏃犳硶褰㈡垚杈圭晫");
return new ArrayList<>();
}
// 2. 杩囨护鍜屽钩婊戞暟鎹�
List<Coordinate> filteredPoints = filterAndSmoothPoints(rawPoints);
- System.out.println("杩囨护鍚庣偣: " + filteredPoints.size() + " 涓�");
// 3. 楂樼骇杈圭晫鐐逛紭鍖�
List<Coordinate> optimizedPoints = optimizeBoundaryPointsAdvanced(filteredPoints, params);
- System.out.println("浼樺寲鍚庤竟鐣岀偣: " + optimizedPoints.size() + " 涓�");
// 4. 杈圭晫璐ㄩ噺璇勪及
BoundaryQuality quality = evaluateBoundaryQuality(optimizedPoints);
- System.out.println(quality.toString());
return optimizedPoints;
}
@@ -167,7 +158,6 @@
// 5. 鑷姩閫夋嫨棰勮鍦烘櫙
selectPresetAutomatically(analysis);
- System.out.println(analysis.toString());
return analysis;
}
@@ -287,8 +277,6 @@
} else {
analysis.suggestedPreset = "澶嶆潅灏忓尯鍩�";
}
-
- System.out.println("鑷姩鍦烘櫙閫夋嫨寰楀垎: " + String.format("%.2f", score) + " -> " + analysis.suggestedPreset);
}
// 鏍规嵁鍦烘櫙鍚嶇О鑾峰彇鍙傛暟
@@ -312,7 +300,6 @@
}
String[] records = gnggaData.split("\\$GNGGA");
- System.out.println("鎵惧埌GNGGA璁板綍: " + (records.length - 1) + " 鏉�");
for (String record : records) {
try {
@@ -325,7 +312,6 @@
String[] fields = trimmedRecord.split(",");
if (fields.length < 7) {
- System.out.println("璁板綍瀛楁涓嶈冻: " + trimmedRecord);
continue;
}
@@ -334,12 +320,10 @@
try {
fixStatus = Integer.parseInt(fields[6]);
} catch (NumberFormatException e) {
- System.out.println("瀹氫綅鐘舵�佹牸寮忛敊璇�: " + fields[6]);
continue;
}
if (fixStatus != 4) {
- System.out.println("璺宠繃闈為珮绮惧害瀹氫綅鐐癸紝鐘舵��: " + fixStatus);
continue;
}
@@ -415,8 +399,6 @@
double speed = calculateDistance(points.get(i-1), points.get(i));
if (speed < 5.0) { // 鏈�澶у悎鐞嗛�熷害涓�5绫�/绉�
filtered.add(points.get(i));
- } else {
- System.out.println("绉婚櫎楂橀�熺偣: 閫熷害=" + speed + "绫�/绉�");
}
}
@@ -462,7 +444,6 @@
// 1. 棣栧厛杩涜閬撴牸鎷夋柉-鏅厠绠楁硶绠�鍖�
List<Coordinate> simplified = douglasPeuckerSimplification(points, params.simplificationTolerance);
- System.out.println("閬撴牸鎷夋柉-鏅厠绠�鍖栧悗: " + simplified.size() + " 涓偣");
// 2. 鍩轰簬璺濈鍜岃搴︾殑杩涗竴姝ヤ紭鍖�
optimized.add(simplified.get(0));
@@ -488,19 +469,22 @@
if (!optimized.get(0).equals(optimized.get(optimized.size() - 1))) {
Coordinate first = optimized.get(0);
Coordinate last = optimized.get(optimized.size() - 1);
- double closingDistance = calculateDistance(first, last);
-
+ double closingDistance = calculateDistance(last, first);
+
if (closingDistance > params.interval) {
- // 濡傛灉棣栧熬璺濈杈冭繙锛屾坊鍔犱腑闂寸偣
- List<Coordinate> interpolated = interpolateBoundary(
- List.of(last, first), params.interval
- );
- optimized.addAll(interpolated.subList(1, interpolated.size() - 1));
+ int segments = (int) Math.ceil(closingDistance / params.interval);
+ for (int i = 1; i < segments; i++) {
+ double ratio = (double) i / segments;
+ Coordinate interpolatedPoint = interpolate(last, first, ratio);
+ if (!interpolatedPoint.equals(last)) {
+ optimized.add(interpolatedPoint);
+ }
+ }
}
optimized.add(first);
}
-
- return optimized;
+
+ return removeConsecutiveDuplicates(optimized);
}
// 閬撴牸鎷夋柉-鏅厠绠楁硶
@@ -580,6 +564,26 @@
return interpolated;
}
+ private List<Coordinate> removeConsecutiveDuplicates(List<Coordinate> points) {
+ if (points == null || points.isEmpty()) {
+ return points;
+ }
+
+ List<Coordinate> cleaned = new ArrayList<>();
+ Coordinate previous = null;
+ for (Coordinate point : points) {
+ if (previous == null || !point.equals(previous)) {
+ cleaned.add(point);
+ previous = point;
+ }
+ }
+
+ if (!cleaned.isEmpty() && !cleaned.get(0).equals(cleaned.get(cleaned.size() - 1))) {
+ cleaned.add(cleaned.get(0));
+ }
+ return cleaned;
+ }
+
// 绾挎�ф彃鍊�
private Coordinate interpolate(Coordinate p1, Coordinate p2, double ratio) {
double x = p1.x + (p2.x - p1.x) * ratio;
--
Gitblit v1.10.0