fix: preserve trusted raw path curvature

2026-07-31 15:28:49 +08:00
parent 9173f9bfb5
commit 272a847b2a
3 changed files with 362 additions and 13 deletions
@@ -1,4 +1,6 @@
+using System;
 using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
@@ -27,7 +29,7 @@ internal static class RawPathBaselineBuilder
        if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
            return false;

-        if (!validator.TryValidate(
+        if (validator.TryValidate(
                analysis.Path,
                analysis.Segments,
                preparedPath,
@@ -37,6 +39,23 @@ internal static class RawPathBaselineBuilder
                out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearanceMeters,
                out reason))
+        {
+            baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
+            return true;
+        }
+
+        if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
+            !TryCreateTrustedRawAnalysis(request, out analysis, out reason) ||
+            !validator.TryValidate(
+                analysis.Path,
+                analysis.Segments,
+                preparedPath,
+                request.Map,
+                request.Vehicle,
+                maximumCollisionCheckStepMeters,
+                out safePath,
+                out minimumClearanceMeters,
+                out reason))
        {
            return false;
        }
@@ -45,6 +64,143 @@ internal static class RawPathBaselineBuilder
        return true;
    }

+    private static bool TryCreateTrustedRawAnalysis(
+        PathSmoothingRequest request,
+        out PathGeometryAnalysis analysis,
+        out string reason)
+    {
+        analysis = null;
+        reason = string.Empty;
+        if (request.CoarsePath == null || request.Segments == null ||
+            request.CoarsePath.Count == 0 || request.Segments.Count == 0)
+        {
+            reason = "原始粗路径基线缺少可信路径或方向分段。";
+            return false;
+        }
+
+        var path = new List<SmoothedPathPoint>(request.CoarsePath.Count);
+        var segments = new List<SmoothedPathSegment>(request.Segments.Count);
+        double maximumAbsoluteVehicleCurvature = 0d;
+        double maximumAbsoluteVehicleCurvatureDerivative = 0d;
+        double curvatureSquareSum = 0d;
+        int curvatureSampleCount = 0;
+        double totalCurvatureVariation = 0d;
+        double curvatureVariationEnergy = 0d;
+        double minimumClearance = double.PositiveInfinity;
+
+        for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
+        {
+            PathSegment segment = request.Segments[segmentIndex];
+            if (segment == null || segment.SegmentIndex != segmentIndex ||
+                segment.StartIndex != path.Count || segment.EndIndex < segment.StartIndex ||
+                segment.EndIndex >= request.CoarsePath.Count)
+            {
+                reason = "原始粗路径基线方向分段无效。";
+                return false;
+            }
+
+            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
+            {
+                CoarsePathPoint point = request.CoarsePath[pointIndex];
+                if (point == null || point.Direction != segment.Direction ||
+                    !IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.Heading) ||
+                    !IsFinite(point.UnwrappedHeading) || !IsFinite(point.ArcLength) || point.ArcLength < 0d ||
+                    !IsFinite(point.VehicleCurvature) || !IsFinite(point.BodyClearance) || point.BodyClearance < 0d)
+                {
+                    reason = "原始粗路径基线包含非法可信点。";
+                    return false;
+                }
+
+                double derivative = EstimateVehicleCurvatureDerivative(request.CoarsePath, segment, pointIndex, out bool validDerivative);
+                if (!validDerivative)
+                {
+                    reason = "原始粗路径基线曲率导数弧长无效。";
+                    return false;
+                }
+
+                double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
+                double geometricCurvature = directionSign * point.VehicleCurvature;
+                SmoothedPathPointSource source = point.IsGearSwitchPoint
+                    ? SmoothedPathPointSource.GearSwitch
+                    : SmoothedPathPointSource.CoarsePathFallback;
+                path.Add(new SmoothedPathPoint(
+                    point.X,
+                    point.Y,
+                    point.Heading,
+                    point.UnwrappedHeading,
+                    point.ArcLength,
+                    point.Direction,
+                    geometricCurvature,
+                    point.VehicleCurvature,
+                    derivative,
+                    point.BodyClearance,
+                    point.IsGearSwitchPoint,
+                    source));
+
+                maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(point.VehicleCurvature));
+                maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
+                    maximumAbsoluteVehicleCurvatureDerivative,
+                    Math.Abs(derivative));
+                curvatureSquareSum += point.VehicleCurvature * point.VehicleCurvature;
+                curvatureSampleCount++;
+                minimumClearance = Math.Min(minimumClearance, point.BodyClearance);
+                if (pointIndex > segment.StartIndex)
+                {
+                    CoarsePathPoint previous = request.CoarsePath[pointIndex - 1];
+                    double deltaArc = point.ArcLength - previous.ArcLength;
+                    double deltaCurvature = geometricCurvature -
+                        (directionSign * previous.VehicleCurvature);
+                    totalCurvatureVariation += Math.Abs(deltaCurvature);
+                    curvatureVariationEnergy +=
+                        (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
+                }
+            }
+
+            segments.Add(new SmoothedPathSegment(
+                segment.SegmentIndex,
+                segment.Direction,
+                segment.StartIndex,
+                segment.EndIndex,
+                segment.StartsAtGearSwitch,
+                segment.EndsAtGearSwitch));
+        }
+
+        double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
+        analysis = new PathGeometryAnalysis(
+            path,
+            segments,
+            request.CoarsePath[request.CoarsePath.Count - 1].ArcLength,
+            maximumAbsoluteVehicleCurvature,
+            maximumAbsoluteVehicleCurvatureDerivative,
+            rmsCurvature,
+            totalCurvatureVariation,
+            curvatureVariationEnergy,
+            minimumClearance);
+        return true;
+    }
+
+    private static double EstimateVehicleCurvatureDerivative(
+        IReadOnlyList<CoarsePathPoint> path,
+        PathSegment segment,
+        int pointIndex,
+        out bool valid)
+    {
+        valid = true;
+        if (segment.StartIndex == segment.EndIndex) return 0d;
+
+        int leftIndex = pointIndex == segment.StartIndex ? pointIndex : pointIndex - 1;
+        int rightIndex = pointIndex == segment.EndIndex ? pointIndex : pointIndex + 1;
+        double deltaArc = path[rightIndex].ArcLength - path[leftIndex].ArcLength;
+        if (!IsFinite(deltaArc) || deltaArc <= 0d)
+        {
+            valid = false;
+            return 0d;
+        }
+        return (path[rightIndex].VehicleCurvature - path[leftIndex].VehicleCurvature) / deltaArc;
+    }
+
+    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
+
    private static PathQualityMetrics CreateMetrics(
        PathGeometryAnalysis analysis,
        double minimumClearanceMeters)